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