Merge pull request #21 from dmt-labs/feature/v3serial

Serial servo control
makerforgetech · Jul 18, 2023 · c6b23fe · c6b23fe
2 parents ebbef47 + 66e0883
commit c6b23fe
Show file tree

Hide file tree

Showing 28 changed files with 687 additions and 117 deletions.
diff --git a/.vscode/arduino.json b/.vscode/arduino.json
@@ -1,3 +1,7 @@
 {
-    "port": "/dev/ttyS0"
+    "port": "/dev/ttyS0",
+    "configuration": "cpu=16MHzatmega328",
+    "board": "arduino:avr:pro",
+    "programmer": "avrisp",
+    "sketch": "arduino_sketch/arduino_sketch.ino"
 }
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
diff --git a/animations/head_nod.json b/animations/head_nod.json
@@ -2,11 +2,11 @@
   {"servo:tilt:mv": -10},
   {"sleep": 0.5},
   {"servo:tilt:mv": 20},
-  {"sleep": 0.2},
+  {"sleep": 0.5},
   {"servo:tilt:mv": -20},
-  {"sleep": 0.2},
+  {"sleep": 0.5},
   {"servo:tilt:mv": 20},
-  {"sleep": 0.2},
+  {"sleep": 0.5},
   {"servo:tilt:mv": -10},
   {"sleep": 0.5}
 ]
diff --git a/animations/head_nod_abs.json b/animations/head_nod_abs.json
@@ -0,0 +1,12 @@
+[
+  {"servo:tilt:mvabs": 70},
+  {"sleep": 0.4},
+  {"servo:tilt:mvabs": 30},
+  {"sleep": 0.4},
+  {"servo:tilt:mvabs": 70},
+  {"sleep": 0.4},
+  {"servo:tilt:mvabs": 30},
+  {"sleep": 0.4},
+  {"servo:tilt:mvabs": 50},
+  {"sleep": 0.5}
+]
diff --git a/animations/head_shake.json b/animations/head_shake.json
@@ -1,12 +1,12 @@
 [
-  {"servo:pan:mv": -10},
-  {"sleep": 0.5},
   {"servo:pan:mv": 20},
-  {"sleep": 0.2},
-  {"servo:pan:mv": -20},
-  {"sleep": 0.2},
+  {"sleep": 0.5},
+  {"servo:pan:mv": -40},
+  {"sleep": 0.5},
+  {"servo:pan:mv": 40},
+  {"sleep": 0.5},
+  {"servo:pan:mv": -40},
+  {"sleep": 0.5},
   {"servo:pan:mv": 20},
-  {"sleep": 0.2},
-  {"servo:pan:mv": -10},
-  {"sleep": 0.5}
+  {"sleep": 2}
 ]
diff --git a/animations/head_shake_abs.json b/animations/head_shake_abs.json
@@ -0,0 +1,12 @@
+[
+  {"servo:pan:mvabs": 40},
+  {"sleep": 0.4},
+  {"servo:pan:mvabs": 60},
+  {"sleep": 0.4},
+  {"servo:pan:mvabs": 40},
+  {"sleep": 0.4},
+  {"servo:pan:mvabs": 60},
+  {"sleep": 0.4},
+  {"servo:pan:mvabs": 50},
+  {"sleep": 2}
+]
diff --git a/animations/look_down.json b/animations/look_down.json
@@ -1,8 +1,6 @@
 [
-  {"servo:pan:mvabs": 60},
+  {"servo:pan:mvabs": 50},
   {"sleep": 0.5},
-  {"servo:neck:mvabs": 0},
-  {"sleep": 0.5},
-  {"servo:tilt:mvabs": 100},
+  {"servo:tilt:mvabs": 20},
   {"sleep": 1}
 ]
diff --git a/animations/look_up.json b/animations/look_up.json
@@ -1,6 +1,6 @@
 [
-  {"servo:neck:mvabs": 40},
+  {"servo:pan:mvabs": 50},
   {"sleep": 0.5},
-  {"servo:tilt:mvabs": 0},
+  {"servo:tilt:mvabs": 80},
   {"sleep": 1}
 ]
diff --git a/animations/neck_forward.json b/animations/neck_forward.json
diff --git a/animations/raised.json b/animations/raised.json
@@ -1,9 +1,3 @@
 [
-  {"servo:leg_l_hip:mvabs": 65},
-  {"servo:leg_r_hip:mvabs": 35},
-  {"servo:leg_l_knee:mvabs": 55},
-  {"servo:leg_r_knee:mvabs": 55},
-  {"servo:leg_l_ankle:mvabs": 65},
-  {"servo:leg_r_ankle:mvabs": 35},
   {"sleep": 1}
 ]
diff --git a/animations/sit.json b/animations/sit.json
@@ -1,9 +1,3 @@
 [
-  {"servo:leg_l_hip:mvabs": 80},
-  {"servo:leg_r_hip:mvabs": 20},
-  {"servo:leg_l_knee:mvabs": 5},
-  {"servo:leg_r_knee:mvabs": 100},
-  {"servo:leg_l_ankle:mvabs": 35},
-  {"servo:leg_r_ankle:mvabs": 65},
   {"sleep": 1}
 ]
diff --git a/animations/sleep.json b/animations/sleep.json
@@ -1,6 +1,5 @@
 [
-  {"servo:neck:mvabs": 85},
-  {"servo:tilt:mvabs": 55},
-  {"servo:pan:mvabs": 60},
+  {"servo:tilt:mvabs": 0},
+  {"servo:pan:mvabs": 90},
   {"sleep": 1}
 ]
diff --git a/animations/stand.json b/animations/stand.json
@@ -1,9 +1,3 @@
 [
-  {"servo:leg_l_ankle:mvabs": 60},
-  {"servo:leg_r_ankle:mvabs": 40},
-  {"servo:leg_l_knee:mvabs": 30},
-  {"servo:leg_r_knee:mvabs": 75},
-  {"servo:leg_l_hip:mvabs": 80},
-  {"servo:leg_r_hip:mvabs": 20},
   {"sleep": 1}
 ]
diff --git a/animations/tiptoes.json b/animations/tiptoes.json
@@ -1,9 +1,3 @@
 [
-  {"servo:leg_l_hip:mvabs": 65},
-  {"servo:leg_r_hip:mvabs": 35},
-  {"servo:leg_l_knee:mvabs": 70},
-  {"servo:leg_r_knee:mvabs": 40},
-  {"servo:leg_l_ankle:mvabs": 80},
-  {"servo:leg_r_ankle:mvabs": 20},
   {"sleep": 1}
 ]
diff --git a/animations/wake.json b/animations/wake.json
@@ -1,6 +1,5 @@
 [
-  {"servo:neck:mvabs": 40},
-  {"servo:tilt:mvabs": 40},
-  {"servo:pan:mvabs": 60},
+  {"servo:tilt:mvabs": 50},
+  {"servo:pan:mvabs": 50},
   {"sleep": 1}
 ]
diff --git a/arduino_sketch/Order.h b/arduino_sketch/Order.h
@@ -5,7 +5,7 @@
 enum Order {
   HELLO = 0,
   SERVO = 1,
-  MOTOR = 2,
+  SERVO_RELATIVE = 2,
   ALREADY_CONNECTED = 3,
   ERROR = 4,
   RECEIVED = 5,

diff --git a/arduino_sketch/ServoManager.h b/arduino_sketch/ServoManager.h
@@ -22,12 +22,13 @@ ServoEasing ServoNP;
 
 InverseKinematics ik;
 
-
 class ServoManager
 {
     public:
     void doInit()
     {
+
+        // IMPORTANT: Communication from Pi uses index, these must be attached in the same order as they are referenced in the pi config
         ServoLLH.attach(PIN_SLLH, PosStart[0]);
         ServoLLK.attach(PIN_SLLK, PosStart[1]);
         ServoLLA.attach(PIN_SLLA, PosStart[2]);
@@ -61,18 +62,31 @@ class ServoManager
               continue;
             }
             if (Pos[tIndex] != -1)
-                moveSingleServo(tIndex, Pos[tIndex]);
+                moveSingleServo(tIndex, Pos[tIndex], false);
             else 
-                moveSingleServo(tIndex, moveRandom(tIndex)); // If scripted value is -1, generate random position based on range of currently indexed servo
+                moveSingleServo(tIndex, moveRandom(tIndex), false); // If scripted value is -1, generate random position based on range of currently indexed servo
         }
         //setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed); 
     }
 
     // @todo just make this pass an array in to moveServos.
-    void moveSingleServo(uint8_t pServoIndex, int pPos)
+    void moveSingleServo(uint8_t pServoIndex, int pPos, boolean isRelative)
     {
-        ServoEasing::ServoEasingNextPositionArray[pServoIndex] = pPos;
-        //setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
+        if (isRelative)
+        {
+            // Assuming pPos is a percentage, map to PosMin and PosMax and add to current position
+            int realChange = map(abs(pPos), 0, 100, PosMin[pServoIndex], PosMax[pServoIndex]);
+            if (pPos < 0)
+            {
+                realChange = -realChange;
+            }
+            ServoEasing::ServoEasingNextPositionArray[pServoIndex] = ServoEasing::ServoEasingNextPositionArray[pServoIndex] + realChange;
+        }
+        else
+        {
+            ServoEasing::ServoEasingNextPositionArray[pServoIndex] = pPos;
+        }
+        setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
     }
 
     void moveLegsAndStore(int x, int y, int *store)

diff --git a/arduino_sketch/arduino_sketch.ino b/arduino_sketch/arduino_sketch.ino
@@ -189,16 +189,19 @@ void getOrdersFromPi()
     switch(order_received)
     {
       case SERVO:
+      case SERVO_RELATIVE:
       {
         int servo_identifier = PiConnect::read_i8();
         int servo_angle = PiConnect::read_i16();
         #ifdef DEBUG
           PiConnect::write_order(SERVO);
+          PiConnect::write_i8(servo_identifier);
           PiConnect::write_i16(servo_angle);
         #endif
         // sleep animations for 2 seconds to allow pi to control servos
         setSleep(2000);
-        servoManager.moveSingleServo(servo_identifier - SERVO_PIN_OFFSET, servo_angle);
+        servoManager.moveSingleServo(servo_identifier, servo_angle, order_received == SERVO_RELATIVE);
+        // delay(2000);
         break;
       }
       case PIN:

diff --git a/main.py b/main.py
@@ -76,10 +76,10 @@ def main():
     servos = dict()
     for key in Config.servos:
         s = Config.servos[key]
-        servos[key] = Servo(s['pin'], key, s['range'], start_pos=s['start'])
+        servos[key] = Servo(s['pin'], key, s['range'], s['id'], start_pos=s['start'])
 
     # POWER
-    power = Power(Config.POWER_ENABLE_PIN)
+    # power = Power(Config.POWER_ENABLE_PIN)
 
     led = LED(Config.LED_COUNT)
     tts = TTS()

diff --git a/modules/actuators/servo.py b/modules/actuators/servo.py
@@ -7,9 +7,10 @@
 
 class Servo:
 
-    def __init__(self, pin, identifier, pwm_range, **kwargs):
+    def __init__(self, pin, identifier, pwm_range, index, **kwargs):
         self.pin = pin
         self.identifier = identifier
+        self.index = index
         self.range = pwm_range
         self.power = kwargs.get('power', True)
 
@@ -33,18 +34,25 @@ def __del__(self):
         pass #self.reset()
 
     def move_relative(self, percentage, safe=True):
-        new = self.pos + (self.translate(percentage) - self.range[0])
-        if new > self.range[1] and safe:
-            new = self.range[1]
-        elif new < self.range[0] and safe:
-            new = self.range[0]
-        if self.range[0] <= new <= self.range[1]:
-            this_move = self.calculate_move(self.pos, new)
-            self.execute_move(this_move)
-            self.pos = new
+        # Only calculate relative position if not using serial
+        # Otherwise it is done by the arduino
+        if not self.serial:
+            new = self.pos + (self.translate(percentage) - self.range[0])
+
+
+            if new > self.range[1] and safe:
+                new = self.range[1]
+            elif new < self.range[0] and safe:
+                new = self.range[0]
+            if self.range[0] <= new <= self.range[1]:
+                this_move = self.calculate_move(self.pos, new)
+                self.execute_move(this_move, True)
+                self.pos = new
+            else:
+                pub.sendMessage('log:error', '[Servo] Percentage %d out of range' % percentage)
+                raise ValueError('Percentage %d out of range' % percentage)
         else:
-            pub.sendMessage('log:error', '[Servo] Percentage %d out of range' % percentage)
-            raise ValueError('Percentage %d out of range' % percentage)
+            self.execute_move([(percentage, 0)], True)
 
     def move(self, percentage, safe=True):
         if 0 <= percentage <= 100 or safe:
@@ -59,7 +67,7 @@ def move(self, percentage, safe=True):
             pub.sendMessage('log:error', '[Servo] Percentage %d out of range' % percentage)
             raise ValueError('Percentage %d out of range' % percentage)
 
-    def execute_move(self, sequence):
+    def execute_move(self, sequence, is_relative=False):
         """
         Recursive function to handle each movement in sequence.
         Move to first position then set thread.Timer for next movement after delay specified in movement 1.
@@ -77,8 +85,13 @@ def execute_move(self, sequence):
         if self.power:
             pub.sendMessage('power:use')
         if self.serial:
-            # print(int(s[0]))
-            pub.sendMessage('serial', type=ArduinoSerial.DEVICE_SERVO, identifier=self.pin, message=s[0])
+            # just move the pan servo for now. Remove after debugging
+            if self.index != 7 and self.index != 6:
+                return;
+            type = ArduinoSerial.DEVICE_SERVO
+            if is_relative:
+                type = ArduinoSerial.DEVICE_SERVO_RELATIVE
+            pub.sendMessage('serial', type=type, identifier=self.index, message=s[0])
         else:
             self.pi.set_servo_pulsewidth(self.pin, s[0])
         if len(sequence) > 1: