Arduino Servo Modes - Set Via Analog Input

arduino_sketch/Config.h

@@ -5,7 +5,7 @@
  * @brief Configuration file for the Arduino sketch.
  * @details This file contains the configuration for the Arduino sketch.
  */
-//#define SERVO_CALIBRATION_ENABLED // Enable servo calibration (see ServoManager::calibrate())
+// #define SERVO_CALIBRATION_ENABLED // Enable servo calibration (see ServoManager::calibrate())
 //#define SERVO_CALIBRATION_SYMMETRICAL // Calculate and apply equivelant changes on other leg.
 
 #define SERVO_PIN_OFFSET 2 // Legacy, used to identify pin from pi communication
@@ -34,7 +34,7 @@
 
 #define EASING_TYPE EASE_QUADRATIC_IN_OUT
 #define ENABLE_EASE_QUADRATIC
-#define SERVO_SPEED_MIN 20
+#define SERVO_SPEED_MIN 30 // Was 20
 #define SERVO_SPEED_MAX 80
 
 // #define DEBUG
@@ -50,6 +50,10 @@
 //#define MPU6050_DEBUG // Debug in serial plotter
 #define ANIMATE_ENABLED // Enable random animations
 
+#define SERVO_MODE_PIN_ENABLED // Enable behavior related to servoModePin
+// #define SERVO_MODE_OVERRIDE 3 // Override input from pin and set specific mode for debugging
+#define RESTRAIN_PIN_ENABLED // Enable behavior related to restrainPin
+
 // 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
 int PosMin[SERVO_COUNT] = {0, 0, 5, 0, 0, 20, 60, 30};
@@ -62,15 +66,16 @@ 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, 20, 90, 90};
 
-int PosBackpack[SERVO_COUNT] = {45, 90, 165, 135, 90, 20, 90, 90}; // straighten legs and point feet to fit in backpack upright
+int PosBackpack[SERVO_COUNT] = {30, 5, 90, 130, 120, 160, 90, 90}; // Position legs to support when mounted to backpack
+int PosStraight[SERVO_COUNT] = {45, 90, 165, 135, 90, 20, 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, 20, 90, 90};
 
 int PosConfig[SERVO_COUNT] = {90, 90, 90, 90, 90, 90, 90, 90};
 
 // Poses
-int PosStand[SERVO_COUNT] = {45, 75, 80, 135, 105, 100, NOVAL, NOVAL};
+int PosStand[SERVO_COUNT] = {45, 70, 80, 135, 110, 100, 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, -1}; // Made random by calling the function moveRandom() if the value is -1
@@ -80,11 +85,20 @@ int PosLookDown[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 120, 9
 // 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 = PIN_A1;
-bool backpack = false;
+int *StartingPos = PosStart;
+int servoMode = 2; // Default to standing pose
+
+#ifdef SERVO_MODE_PIN_ENABLED
+int servoModePin = PIN_A1;
+// Define 5 threshold values between 0 and 1024 for the 5 leg modes
+// vals: 0, 545, 617, 711, 839
+int servoModeThresholds[5] = {0, 550, 650, 750, 850};
+String servoModeNames[] = {"Disabled", "Sit", "Stand", "BackPack", "Straight"};
+int *servoModePoses[] = {PosStart, PosStart, PosStart, PosBackpack, PosStraight};
+#endif
 
 int restrainPin = 12;
-bool restrainingBolt = false;
+bool restrainingBolt = false; // Needed in either case as it is checked by ServoManager
 
 void blinkLED()
 {

arduino_sketch/ServoManager.h

@@ -25,26 +25,17 @@ class ServoManager
     double currentX, currentY;
     void doInit()
     {
+        initInputs();
+
         // IMPORTANT: Communication from Pi uses index, these must be attached in the same order as they are referenced in the pi config
-        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]);
+        ServoLLH.attach(PIN_SLLH, StartingPos[0]);
+        ServoLLK.attach(PIN_SLLK, StartingPos[1]);
+        ServoLLA.attach(PIN_SLLA, StartingPos[2]);
+        ServoRLH.attach(PIN_SRLH, StartingPos[3]);
+        ServoRLK.attach(PIN_SRLK, StartingPos[4]);
+        ServoRLA.attach(PIN_SRLA, StartingPos[5]);
+        ServoNT.attach(PIN_SNT, StartingPos[6]);
+        ServoNP.attach(PIN_SNP, StartingPos[7]);
 
         // Initialise IK with min and max angles and leg section lengths
         ik.doInit(PosMin[3], PosMax[3], PosMin[4], PosMax[4], PosMin[5], PosMax[5], LEG_LENGTH_THIGH, LEG_LENGTH_SHIN, LEG_LENGTH_FOOT);
@@ -56,9 +47,58 @@ class ServoManager
             ServoEasing::ServoEasingArray[tIndex]->setEasingType(EASING_TYPE);
             ServoEasing::ServoEasingArray[tIndex]->setMinMaxConstraint(PosMin[tIndex], PosMax[tIndex]);
         }
+
+        setSpeed(SERVO_SPEED_MIN);
+
+        #ifdef SERVO_CALIBRATION_ENABLED
+        servoManager.calibrate(); // Must be in servoMode 2
+        #endif
+
         // Wait for servos to reach start position.
         delay(3000);
         cLog("Servos initialised");
+
+        if (servoMode == 2)
+        {
+            moveServos(PosStand); // Stand once
+        }
+
+    }
+
+    void initInputs()
+    {
+        #ifdef SERVO_MODE_PIN_ENABLED
+            pinMode(servoModePin, INPUT); // sets the digital pin as input
+            int servoModeVal = analogRead(servoModePin);
+            // Serial.print("Servo Mode Val:");
+            // Serial.println(servoModeVal);
+            for (int i = 0; i < 5; i++)
+            {
+                if (servoModeVal <= servoModeThresholds[i])
+                {
+                    servoMode = i;
+                    break;
+                }
+            }
+            // Serial.print("Servo mode: ");
+            // Serial.println(servoMode);
+            #ifdef SERVO_MODE_OVERRIDE
+            servoMode = SERVO_MODE_OVERRIDE; // Define and set in Config.h if appropriate
+            // Serial.print("Servo mode override: ");
+            // Serial.println(servoMode);
+            #endif
+
+            StartingPos = servoModePoses[servoMode];
+        #endif
+
+        #ifdef RESTRAIN_PIN_ENABLED
+            pinMode(restrainPin, INPUT_PULLUP); // sets the digital pin as input
+            if (digitalRead(restrainPin) == LOW) // Check once on startup
+            {
+                restrainingBolt = true;
+                StartingPos = PosStart; // Override starting position @todo remove once selector is installed
+            }
+        #endif
     }
 
     // Adjust hips to compensate for angle of body
@@ -129,9 +169,9 @@ class ServoManager
 
     void moveSingleServo(uint8_t pServoIndex, int pPos, boolean isRelative)
     {
-        if ((backpack == true || restrainingBolt == true) && pServoIndex < 6)
+        if (((servoMode != 2 || restrainingBolt == true) && pServoIndex < 6) || servoMode == 0)
         {
-            // Serial.println("Backpack or restrained mode, skipping leg servos");
+            // Serial.println("Restrained mode, skipping some / all servos");
         }
         else if (isRelative)
         {

arduino_sketch/arduino_sketch.ino

@@ -46,24 +46,8 @@ void setup()
   // Init serial
   Serial.begin(115200);
 
-
-  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;
-  }
-
   // Init ServoManager
   servoManager.doInit();
-  servoManager.setSpeed(SERVO_SPEED_MIN);
 
   #ifdef MPU6050_ENABLED
   tilt.doInit();
@@ -80,6 +64,7 @@ void setup()
   // Custom log message (enable DEBUG in Config.h to see this)
   cLog("Start loop");
 }
+
 /**
  * @brief Set all servos to 90 degrees for mechanical calibration. Wait for 20 seconds.
  */
@@ -165,9 +150,7 @@ void hipAdjust()
 
 void loop()
 {
-  #ifdef SERVO_CALIBRATION_ENABLED
-    servoManager.calibrate();
-  #endif
+
 
   #ifdef MPU6050_ENABLED
   hipAdjust();
@@ -237,10 +220,10 @@ void animateRandomly()
   float headTiltOffset = ServoEasing::ServoEasingNextPositionArray[7] - 90;
   // Attempt to compensate movement of head by adjusting leg height
   // 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);
+  // float legHeight = map(headTiltOffset, 180, 0, LEG_IK_MIN, LEG_IK_MAX);
   // Move legs to that height
   // servoManager.moveLegs(LEG_IK_MAX, 0);
-  servoManager.moveServos(PosStand);
+  // servoManager.moveServos(PosStand);
   shouldMove = true;
 #ifdef DEBUG
   Serial.print("Moving legs ");

config/animate.yml

@@ -1,3 +1,3 @@
 animate:
-  enabled: false
+  enabled: true
   path: modules.animate.Animate    

config/buzzer.yml

@@ -1,5 +1,5 @@
 buzzer:
-  enabled: false
+  enabled: true
   path: "modules.audio.buzzer.Buzzer"
   config:
     pin: 27

config/motion.yml

@@ -1,5 +1,5 @@
 motion:
-  enabled: false
+  enabled: true
   path: 'modules.sensor.Sensor'
   config:
     pin: 26

config/neopixel.yml

@@ -1,9 +1,9 @@
 neopixel:
-  enabled: false
+  enabled: true
   path: 'modules.neopixel.neopx.NeoPx'
   config:
     pin: 12 # Only used for GPIO. GPIO2 and 3 are use for i2c, GPIO10 is used for SPI, GPIO12 is used for GPIO
-    protocol: 'SPI' # choose between GPIO, I2C and SPI
+    protocol: 'I2C' # choose between GPIO, I2C and SPI
     count: 12
     positions: {
       'status1': 0,

config/personality.yml

@@ -1,5 +1,5 @@
 personality:
-  enabled: false
+  enabled: true
   path: modules.personality.Personality
   config:
     mode: 6 # Config.MODE_LIVE

config/piservo.yml

@@ -1,5 +1,5 @@
 piservo:
-  enabled: false
+  enabled: true
   path: modules.actuators.piservo.PiServo
   instances:
     - name: "ear"

config/serial.yml

@@ -1,5 +1,5 @@
 serial:
-  enabled: false
+  enabled: true
   path: "modules.network.arduinoserial.ArduinoSerial"
   config:
     port: '/dev/ttyAMA0'

config/servos.yml

@@ -1,5 +1,5 @@
 servos:
-  enabled: false
+  enabled: true
   path: "modules.actuators.servo.Servo" # Include class name here
   instances:
     - name: "leg_l_hip"

config/tracking.yml

@@ -1,7 +1,7 @@
 tracking:
-  enabled: false
+  enabled: true
   path: 'modules.vision.imx500.tracking.Tracking'
   config:
-    active: True
+    active: true
     name: 'tracking'
     filter: 'person'

config/vision.yml

@@ -1,5 +1,5 @@
 vision:
-  enabled: false
+  enabled: true
   path: 'modules.vision.imx500.vision.Vision'
   config:
     preview: False

modules/actuators/servo.py

@@ -52,11 +52,11 @@ def __init__(self, **kwargs):
             self.pi = kwargs.get('pi', pigpio.pi())
             self.pi.set_mode(self.pin, pigpio.OUTPUT)
 
-        self.move(self.start)
-
     def setup_messaging(self):
         self.subscribe('servo:' + self.identifier + ':mvabs', self.move)
         self.subscribe('servo:' + self.identifier + ':mv', self.move_relative)
+
+        self.move(self.start) # @todo move to a more appropriate place, can't be in init because messaging is not set up yet
 
     def __del__(self):
         pass #self.reset()