1  # SPDX-FileCopyrightText: 2020 Limor Fried for Adafruit Industries
 2  #
 3  # SPDX-License-Identifier: MIT
 4  
 5  import busio
 6  import board
 7  from digitalio import DigitalInOut, Direction
 8  from adafruit_pca9685 import PCA9685
 9  from adafruit_motor import servo
10  
11  led = DigitalInOut(board.P16)
12  led.direction = Direction.OUTPUT
13  
14  i2c = busio.I2C(board.SCL, board.SDA)
15  pca = PCA9685(i2c, address=0x68)
16  pca.frequency = 50
17  servoSetInit = (1000, 630, 500, 600, 240, 600, 1000, 720)
18  servoAngle = [1000, 630, 500, 600, 240, 600, 1000, 720]
19  motionSpeed = 15
20  servos = []
21  for c in range(8):
22      servos.append(servo.Servo(pca.channels[c], min_pulse=800, max_pulse=2200))
23  
24  def servoInitialSet():
25      print("Initialize servos")
26      for i in range(8):
27          servos[i].angle = servoSetInit[i] / 10
28  
29  def servoFree(n = None):
30      if n:
31          print("Release servo #", n)
32          servos[n].angle = None
33      else:
34          print("Release all servos")
35          for s in servos:
36              s.angle = None
37  
38  def servoWrite(num, degrees):
39      degrees = min(max(degrees, 0), 180)
40      servos[num].angle = degrees
41  
42  # angle is a list of 8 target values
43  def setAngle(angle, msec):
44      step = [0, 0, 0, 0, 0, 0, 0, 0]
45      msec = msec / motionSpeed # now 15//default 10; //speedy 20   Speed Adj
46      for val in range(8):
47          target = servoSetInit[val] - angle[val]
48          target = min(max(target, 0), 1800)
49          if target != servoAngle[val]: # Target != Present
50              step[val] = (target - servoAngle[val]) / msec
51      #print(step)
52      for _ in range(msec):
53          for val in range(8):
54              servoAngle[val] += step[val]
55              #print("setting servo %d to %d" % (val, int(servoAngle[val] / 10)))
56              servoWrite(val, servoAngle[val] / 10)
57          #time.sleep(msec / 1000)
58      print(servoAngle)
59  
60  servoInitialSet()
61  
62  led.value = False
63  setAngle([0, 0, -200, 0, 0, 0, 0, 0], 500)
64  setAngle([0, 0, -1800, 0, 0, 0, 1800, 0], 500)
65  setAngle([900, 0, -1800, 0, -900, 0, 1800, 0], 500)
66  setAngle([900, 0, -200, 0, -900, 0, 0, 0], 500)
67  setAngle([900, 0, -1800, 0, -900, 0, 1800, 0], 500)
68  setAngle([900, 0, -200, 0, -900, 0, 0, 0], 500)
69  setAngle([0, 0, -200, 0, 0, 0, 0, 0], 500)
70  led.value = True
71  
72  servoFree()