5-servo arm demo

Crickits/robot_arm/code.py

@@ -0,0 +1,51 @@
+#import time
+from busio import I2C
+from adafruit_seesaw.seesaw import Seesaw
+from adafruit_seesaw.pwmout import PWMOut
+from adafruit_motor import servo
+import board
+
+print("5 Servo Robot Arm Demo!")
+
+# Create seesaw object
+i2c = I2C(board.SCL, board.SDA)
+seesaw = Seesaw(i2c)
+
+pots = (2, 3, 40, 41, 11)
+
+# Create servos list
+servos = []
+for ss_pin in (17, 16, 15, 14, 22):
+    pwm = PWMOut(seesaw, ss_pin)
+    pwm.frequency = 50
+    _servo = servo.Servo(pwm)
+    servos.append(_servo)
+
+# Maps a number from one range to another.
+def map_range(x, in_min, in_max, out_min, out_max):
+    mapped = (x-in_min) * (out_max - out_min) / (in_max-in_min) + out_min
+    if out_min <= out_max:
+        return max(min(mapped, out_max), out_min)
+    return min(max(mapped, out_max), out_min)
+
+servo_angles = [90] * 4
+
+while True:
+    readings = []
+    angles = []
+    for i in range(len(pots)):
+	# Read 5 potentiometers
+        reading = seesaw.analog_read(pots[i])
+        readings.append(reading)
+	# Map 10-bit value to servo angle
+        if i == 5:
+	    # The 5th servo is for the claw and it doesnt need full range
+            angle = int(map_range(reading, 0, 1023, 0, 50))
+        else:
+	    # Other 4 servos are for motion, map to 180 degrees!
+            angle = int(map_range(reading, 0, 1023, 0, 180))
+        angles.append(angle)
+	# set the angle
+        servos[i].angle = angle
+    # For our debugging!
+    print(readings, angles)