From f7c70a8ef667ec586d12d1d8d6c52d262c9c53ee Mon Sep 17 00:00:00 2001
From: ladyada <limor@ladyada.net>
Date: Thu, 14 Jun 2018 21:22:10 -0400
Subject: [PATCH] 5-servo arm demo

---
 Crickits/robot_arm/code.py | 51 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 51 insertions(+)
 create mode 100644 Crickits/robot_arm/code.py

diff --git a/Crickits/robot_arm/code.py b/Crickits/robot_arm/code.py
new file mode 100644
index 00000000..c15f9644
--- /dev/null
+++ b/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)