Improvements, backpack + restraining bolt support

Author: danic85

animations/look_down.json

@@ -1,6 +1,6 @@
 [
   {"servo:pan:mvabs": 50},
   {"sleep": 0.5},
-  {"servo:tilt:mvabs": 20},
+  {"servo:tilt:mvabs": 80},
   {"sleep": 1}
 ]

animations/look_up.json

@@ -1,6 +1,6 @@
 [
   {"servo:pan:mvabs": 50},
   {"sleep": 0.5},
-  {"servo:tilt:mvabs": 80},
+  {"servo:tilt:mvabs": 20},
   {"sleep": 1}
 ]

arduino_sketch/Config.h

@@ -37,19 +37,18 @@
 #define SERVO_SPEED_MIN 20
 #define SERVO_SPEED_MAX 80
 
-//#define DEBUG
+// #define DEBUG
 
 #define SERVO_COUNT 8 // Number of servos to be controlled by ServoEasing
 
 #define LEG_IK_MIN 75 // Min solvable height of leg between hip and ankle
-#define LEG_IK_MAX 170 // Max solvable height of leg between hip and ankle
+#define LEG_IK_MAX 180 // Max solvable height of leg between hip and ankle
 
 #define NOVAL 1000
 
-//#define MPU6050_ENABLED // Enable MPU6050
+// #define MPU6050_ENABLED // Enable MPU6050
 //#define MPU6050_DEBUG // Debug in serial plotter
-//#define ANIMATE_ENABLED // Enable random animations
-
+#define ANIMATE_ENABLED // Enable random animations
 
 // Arrays to store servo min / max positions to avoid mechanical issues due
 // NOTE: attach() disregards this, set PosRest to be within range of the servo's physical boundaries
@@ -63,24 +62,30 @@ int PosSleep[SERVO_COUNT] = {40, 60, 95, 140, 120, 85, PosMax[7], 90};
 //0, 3 = HIP
 int PosStart[SERVO_COUNT] = {60, 0, 165, 120, 180, 10, 90, 90};
 
-
+int PosBackpack[SERVO_COUNT] = {45, 90, 165, 135, 90, 10, 90, 90}; // straighten legs and point feet to fit in backpack upright
 
 //int PosRest[SERVO_COUNT] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, S8_REST, S9_REST};
 int PosRest[SERVO_COUNT] = {60, 0, 165, 120, 180, 10, 90, 90};
 
 int PosConfig[SERVO_COUNT] = {90, 90, 90, 90, 90, 90, 90, 90};
 
 // Poses
-int PosStand[SERVO_COUNT] = {40, 60, 95, 140, 120, 85, NOVAL, NOVAL};
+int PosStand[SERVO_COUNT] = {45, 75, 95, 135, 105, 85, NOVAL, NOVAL};
 int PosLookLeft[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 180};
 int PosLookRight[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL,  NOVAL, NOVAL, 0};
-int PosLookRandom[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, NOVAL}; // Made random by calling the function moveRandom() if the value is -1
+int PosLookRandom[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, -1}; // Made random by calling the function moveRandom() if the value is -1
 int PosLookUp[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 60, 90};
 int PosLookDown[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 120, 90};
 
 // Array of poses except PosRest and PosSleep (which are used for initialization and reset of position)
 int *Poses[] = {PosStand, PosLookLeft, PosLookRight, PosLookUp, PosLookDown, PosLookRandom};
 
+int backpackPin = 12;
+bool backpack = false;
+
+int restrainPin = PIN_A1;
+bool restrainingBolt = false;
+
 void blinkLED()
 {
     digitalWrite(LED_BUILTIN, HIGH);

arduino_sketch/ServoManager.h

@@ -26,12 +26,23 @@ class ServoManager
     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]);
-        ServoRLH.attach(PIN_SRLH, PosStart[3]);
-        ServoRLK.attach(PIN_SRLK, PosStart[4]);
-        ServoRLA.attach(PIN_SRLA, PosStart[5]);
+        if (backpack == true)
+        {
+            ServoLLH.attach(PIN_SLLH, PosBackpack[0]);
+            ServoLLK.attach(PIN_SLLK, PosBackpack[1]);
+            ServoLLA.attach(PIN_SLLA, PosBackpack[2]);
+            ServoRLH.attach(PIN_SRLH, PosBackpack[3]);
+            ServoRLK.attach(PIN_SRLK, PosBackpack[4]);
+            ServoRLA.attach(PIN_SRLA, PosBackpack[5]);
+        }
+        else {
+            ServoLLH.attach(PIN_SLLH, PosStart[0]);
+            ServoLLK.attach(PIN_SLLK, PosStart[1]);
+            ServoLLA.attach(PIN_SLLA, PosStart[2]);
+            ServoRLH.attach(PIN_SRLH, PosStart[3]);
+            ServoRLK.attach(PIN_SRLK, PosStart[4]);
+            ServoRLA.attach(PIN_SRLA, PosStart[5]);
+        }
         ServoNT.attach(PIN_SNT, PosStart[6]);
         ServoNP.attach(PIN_SNP, PosStart[7]);
 
@@ -118,20 +129,18 @@ class ServoManager
 
     void moveSingleServo(uint8_t pServoIndex, int pPos, boolean isRelative)
     {
-        if (isRelative)
+        if ((backpack == true || restrainingBolt == true) && pServoIndex < 6)
+        {
+            // Serial.println("Backpack or restrained mode, skipping leg servos");
+        }
+        else if (isRelative)
         {
-            // Serial.print(" Moving from: ");
-            // Serial.print(ServoEasing::ServoEasingNextPositionArray[pServoIndex]);
-            // Serial.print(" to: ");
-            // Serial.println(ServoEasing::ServoEasingNextPositionArray[pServoIndex] + pPos);
             ServoEasing::ServoEasingNextPositionArray[pServoIndex] = ServoEasing::ServoEasingNextPositionArray[pServoIndex] + pPos;
         }
         else
         {
             ServoEasing::ServoEasingNextPositionArray[pServoIndex] = pPos;
         }
-        // setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
-        
         // Return actual value to Pi
         PiConnect::write_i16(ServoEasing::ServoEasingNextPositionArray[pServoIndex]);
     }

arduino_sketch/arduino_sketch.ino

@@ -33,6 +33,8 @@ boolean calibrateRest = false;
 
 bool shouldMove = false;
 
+bool piControl = false;
+
 void setup()
 {
   // Init LED pin
@@ -47,6 +49,21 @@ void setup()
   servoManager.doInit();
   servoManager.setSpeed(SERVO_SPEED_MIN);
 
+
+  pinMode(backpackPin, INPUT_PULLUP); // sets the digital pin as input
+  if (digitalRead(backpackPin) == LOW) // Check once on startup
+  {
+    Serial.println("Backpack detected");
+    backpack = true;
+  }
+
+  pinMode(restrainPin, INPUT_PULLUP); // sets the digital pin as input
+  if (digitalRead(restrainPin) == LOW) // Check once on startup
+  {
+    Serial.println("Restraint detected");
+    restrainingBolt = true;
+  }
+
   #ifdef MPU6050_ENABLED
   tilt.doInit();
   #endif
@@ -57,7 +74,7 @@ void setup()
   // allTo90();
 
   // Move to rest position + calculate IK and store as rest position
-  //doRest();
+  // doRest();
 
   // Custom log message (enable DEBUG in Config.h to see this)
   cLog("Start loop");
@@ -83,7 +100,7 @@ void allTo90()
 void doRest()
 {
   cLog("Resting");
-  isResting = true;
+
   // Reset to slow speed
   servoManager.setSpeed(SERVO_SPEED_MIN);
   if (calibrateRest == false)
@@ -163,6 +180,7 @@ void loop()
   bool receiving = getOrdersFromPi();
   while(receiving) 
   {
+    piControl = true;
     delay(50); // Wait a short time for any other orders
     receiving = getOrdersFromPi();
     shouldMove = !receiving; // Only move when there are no other orders
@@ -179,17 +197,17 @@ void loop()
   // Orders from pi will set sleep time so that the animation does not take precedence
   if (!isSleeping())
   {
-    if (isResting)
+    if (isResting && piControl == false) // Only do this if the Pi is not in control (i.e. switched off)
     {
       #ifdef ANIMATE_ENABLED
       animateRandomly();
       #endif
-      setSleep(random(3000, 5000));
     }
     else
     {
       // doRest();
-      setSleep(random(3000, 20000));
+      isResting = true;
+      setSleep(random(1000, 5000));
     }
   }
 
@@ -219,7 +237,8 @@ void animateRandomly()
   // Scale headTiltOffset value between 0 and 180 (inverted) to scale of LEG_IK_MIN and LEG_IK_MAX
   float legHeight = map(headTiltOffset, 180, 0, LEG_IK_MIN, LEG_IK_MAX);
   // Move legs to that height
-  servoManager.moveLegs(legHeight, 0);
+  // servoManager.moveLegs(LEG_IK_MAX, 0);
+  servoManager.moveServos(PosStand);
   shouldMove = true;
 #ifdef DEBUG
   Serial.print("Moving legs ");