Integrated IK and logging

Author: danic85

arduino_sketch/.vscode/arduino.json

@@ -1,4 +1,6 @@
 {
     "board": "arduino:avr:nano",
-    "port": "/dev/ttyUSB0"
+    "port": "/dev/ttyUSB0",
+    "programmer": "avrispmkii",
+    "sketch": "arduino_sketch.ino"
 }

arduino_sketch/Config.h

@@ -22,8 +22,7 @@
 #define SERVO_SPEED_MIN 50
 #define SERVO_SPEED_MAX 100
 
-// If DEBUG is set to true, the arduino will send back all the received messages
-#define DEBUG false
+#define DEBUG
 
 #define MAX_EASING_SERVOS 9
 
@@ -37,6 +36,12 @@
 #define S8_REST 90  // Neck tilt
 #define S9_REST 90  // Neck pan
 
+#define LEG_IK_MIN 140
+#define LEG_IK_MAX 170
+
+#define NOVAL 1000
+
+
 // Arrays to store servo min / max positions to avoid mechanical issues due
 // NOTE: attach() disregards this, set PosSleep to be within range of the servo's physical boundaries
 int PosMin[MAX_EASING_SERVOS] = {20, 5, 40, 20, 5, 15, 40, 60, 20};
@@ -49,12 +54,12 @@ int PrepSleepFromRest[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST,
 int PosRest[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, S8_REST, S9_REST};
 
 // Poses
-int PosStand[MAX_EASING_SERVOS] = {110, 110, 110, 70, 70, 70, S7_REST, S8_REST, S9_REST};
-int PosLookLeft[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, S8_REST, 180};
-int PosLookRight[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, S8_REST, 0};
-int PosLookRandom[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, -1, -1}; // Made random by calling the function moveRandom() if the value is -1
-int PosLookUp[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, 60, S9_REST};
-int PosLookDown[MAX_EASING_SERVOS] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, 120, S9_REST};
+int PosStand[MAX_EASING_SERVOS] = {110, 110, 110, 70, 70, 70, NOVAL, NOVAL, NOVAL};
+int PosLookLeft[MAX_EASING_SERVOS] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 180};
+int PosLookRight[MAX_EASING_SERVOS] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 0};
+int PosLookRandom[MAX_EASING_SERVOS] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, -1}; // Made random by calling the function moveRandom() if the value is -1
+int PosLookUp[MAX_EASING_SERVOS] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 60, S9_REST};
+int PosLookDown[MAX_EASING_SERVOS] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 120, S9_REST};
 
 // Array of poses except PosRest and PosSleep (which are used for initialization and reset of position)
 int *Poses[] = {PosStand, PosLookLeft, PosLookRight, PosLookUp, PosLookDown, PosLookRandom};
@@ -67,4 +72,21 @@ void blinkLED()
     delay(100);
 }
 
+/**
+ * Custom logging function to avoid having to wrap every Serial.println() in #ifdef DEBUG
+*/
+void cLog(String message, boolean newline = true)
+{
+    #ifdef DEBUG
+    if (newline)
+    {
+        Serial.println(message);
+    }
+    else
+    {
+        Serial.print(message);
+    }
+    #endif
+}
+
 #endif

arduino_sketch/InverseKinematics.h

@@ -12,7 +12,7 @@ class InverseKinematics
 {
     // constructor
     public:
-        InverseKinematics(float hipMinAngle, float hipMaxAngle, float kneeMinAngle, float kneeMaxAngle, float ankleMinAngle, float ankleMaxAngle, float thighLength, float shinLength, float footLength)
+        void doInit(float hipMinAngle, float hipMaxAngle, float kneeMinAngle, float kneeMaxAngle, float ankleMinAngle, float ankleMaxAngle, float thighLength, float shinLength, float footLength)
         {
             this->hipMinAngle = hipMinAngle;
             this->hipMaxAngle = hipMaxAngle;

arduino_sketch/PiConnect.h

@@ -25,20 +25,20 @@ class PiConnect
         }
         void setConnected(boolean connected)
         {
-            Serial.print(F("Serial connected: "));
-            Serial.println(connected);
+            cLog(F("Serial connected: "), false);
+            cLog((String) connected);
             this->serialConnected = connected;
         }
         static boolean checkForConnection()
         {
-            Serial.print(F("Checking for connection:"));
+            cLog(F("Checking for connection"));
             write_order(HELLO);
             wait_for_bytes(1, 1000);
         }
 
         static Order read_order()
         {
-            Serial.println("Reading order");
+            cLog("Reading order");
             return (Order) Serial.read();
         }
 

arduino_sketch/ServoManager.h

@@ -22,7 +22,7 @@ ServoEasing Servo8;
 // Neck pan
 ServoEasing Servo9;
 
-InverseKinematics ik(PosMin[3], PosMax[3], PosMin[4], PosMax[4], PosMin[5], PosMax[5], 94.0, 94.0, 28.0);
+InverseKinematics ik;
 
 
 class ServoManager
@@ -40,6 +40,8 @@ class ServoManager
         Servo8.attach(SERVO8_PIN, PosSleep[7]);
         Servo9.attach(SERVO9_PIN, PosSleep[8]);
 
+        ik.doInit(PosMin[3], PosMax[3], PosMin[4], PosMax[4], PosMin[5], PosMax[5], 94.0, 94.0, 28.0);
+
         // Loop over ServoEasing::ServoEasingArray and attach each servo
         for (uint8_t tIndex = 0; tIndex < MAX_EASING_SERVOS; ++tIndex)
         {
@@ -49,38 +51,62 @@ class ServoManager
         }
         // Wait for servos to reach start position.
         delay(3000); 
-        Serial.println("Servos initialised");
+        cLog("Servos initialised");
     }
 
     void moveServos(int *Pos)
     {
         for (uint8_t tIndex = 0; tIndex < MAX_EASING_SERVOS; ++tIndex)
         {
+            if (Pos[tIndex] == NOVAL) {
+              //Serial.println("NOVAL FOUND");
+              continue;
+            }
             if (Pos[tIndex] != -1)
-                ServoEasing::ServoEasingNextPositionArray[tIndex] = Pos[tIndex];
+                moveSingleServo(tIndex, Pos[tIndex]);
             else 
-                ServoEasing::ServoEasingNextPositionArray[tIndex] = moveRandom(tIndex); // If scripted value is -1, generate random position based on range of currently indexed servo
+                moveSingleServo(tIndex, moveRandom(tIndex)); // 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)
     {
         ServoEasing::ServoEasingNextPositionArray[pServoIndex] = pPos;
         //setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
     }
 
+    void moveLegsAndStore(int x, int y, int *store)
+    {
+        moveLegs(x, y);
+        for (uint8_t tIndex = 0; tIndex < MAX_EASING_SERVOS; ++tIndex)
+        {
+            store[tIndex] = ServoEasing::ServoEasingNextPositionArray[tIndex];
+        }
+    }
+
+    // function moveLegs that returns an int array
+    void moveLegs(int x, int y)
+    {
+        float hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR;
+        // solve left leg
+        ik.inverseKinematics2D(x, y, hipAngleR, kneeAngleR, ankleAngleR);
+        // solve other leg
+        ik.calculateOtherLeg(hipAngleR, kneeAngleR, ankleAngleR, hipAngleL, kneeAngleL, ankleAngleL);
+        // Assign angles and move servos
+        int thisMove[MAX_EASING_SERVOS] = {hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR, NOVAL, NOVAL, NOVAL};
+        moveServos(thisMove);
+    }
+
     void demoAll()
     {
-        //Serial.println(F("Demo all poses at minimum speed"));
         setSpeed(SERVO_SPEED_MIN);
         // Cycle through all poses and move all servos to them
         for (uint8_t tPoseIndex = 0; tPoseIndex < sizeof(Poses) / sizeof(Poses[0]); ++tPoseIndex)
         {
-            //Serial.print(F("Pose: "));
-            //Serial.println(tPoseIndex);
             moveServos(Poses[tPoseIndex]);
             delay(1000);
-            //Serial.println(F("Rest"));
             moveServos(PosRest);
             delay(2000);
         }
@@ -90,8 +116,8 @@ class ServoManager
     {
         tSpeed = pSpeed;
         setSpeedForAllServos(tSpeed);
-        //Serial.print(F("Speed set: "));
-        //Serial.println(tSpeed);
+//        cLog(F("Speed set: "));
+//        cLog((String) tSpeed);
     }
 
     uint16_t getSpeed()
@@ -116,7 +142,9 @@ class ServoManager
         // Solve inverse kinematics for left leg
         if (!ik.inverseKinematics2D(x, 0, hipAngleL, kneeAngleL, ankleAngleL)) 
         {
+            #ifdef IK_DEBUG
             Serial.println("No solution");
+            #endif
             return;
         }
 

arduino_sketch/arduino_sketch.ino

@@ -25,26 +25,42 @@ unsigned long sleepTime;
 
 void setup()
 {
+    // Init LED pin
     pinMode(LED_BUILTIN, OUTPUT);
     pinMode(11, OUTPUT);   // sets the digital pin as output
     digitalWrite(11, LOW); // sets the digital pin on
+    // Init boot time for sleep function
     bootTime = millis();
+    // Init serial
     Serial.begin(115200);
-    //delay(2000);
+    // Init ServoManager
     servoManager.doInit();
     servoManager.setSpeed(SERVO_SPEED_MIN);
 
     // Seed random number generator
     randomSeed(analogRead(0));
 
-    servoManager.moveServos(PrepRestFromSleep); // Move hips and head to try and balance
-    doRest();
-    
-    Serial.println(F("Start loop"));
+    // Move from start position to rest position
+    // servoManager.moveServos(PrepRestFromSleep); // Move hips and head to try and balance
+    //doRest();
+
+    // Mid value between LEG_IK_MIN and LEG_IK_MAX
+    float mid = LEG_IK_MIN + ((LEG_IK_MAX - LEG_IK_MIN) / 2);
+    servoManager.moveLegsAndStore(mid, 0, PosRest); // Move legs and store as rest position
+    // iterate over PosRest and output values:
+    for (int i = 0; i < 9; i++)
+    {
+        Serial.print(PosRest[i]);
+        Serial.print(", ");
+    }
+    Serial.println();
+
+    // Custom log message (enable DEBUG in Config.h to see this)
+    cLog("Start loop");
 }
 void doRest()
 {
-  Serial.println("Resting");
+  cLog("Resting");
   isResting = true;
   // Reset to slow speed
   servoManager.setSpeed(SERVO_SPEED_MIN);
@@ -58,11 +74,8 @@ void loop()
   {
       blinkLED();
   }
-  //Serial.println("Looping");
-  // Check for serial connection and connect, or wait for orders if connected
-//  if (pi.isConnected() == false)
-  //  PiConnect::checkForConnection(); // Say hello and wait for response
-  
+
+  // Check for orders from pi
   getOrdersFromPi();
 
   // if not sleeping, animate randomly
@@ -89,33 +102,53 @@ void setSleep(unsigned long length)
 
 boolean isSleeping()
 {
-    //Serial.println(".");
     return millis() - bootTime < sleepTime;
 }
 
 void animateRandomly()
 {
-    Serial.println("Animating");
+    cLog("Animating");
     isResting = false;
-    // Set tSpeed to between SERVO_SPEED_MIN and SERVO_SPEED_MAX
     servoManager.setSpeed(random(SERVO_SPEED_MIN, SERVO_SPEED_MAX));
-    // Radomly select a pose and move to it (not stand)
-    // uint8_t tPoseIndex = random(1, sizeof(Poses) / sizeof(Poses[0]));
+    // Look around randomly or move legs randomly
+    switch (random(0, 4))
+    {
+      case 0:
+        servoManager.moveServos(PosLookRandom);
 
-    //Serial.print(F("Pose: "));
-    //Serial.println(tPoseIndex);
-    //moveServos(Poses[tPoseIndex]);
-    servoManager.moveServos(PosLookRandom);
+        // If head looks down, move legs up, and vice versa
+        float headTiltOffset = ServoEasing::ServoEasingNextPositionArray[7] - 90;
+        // Scale headTiltOffset value between 0 and 180 to scale of LEG_IK_MIN and LEG_IK_MAX 
+        float legHeight = map(headTiltOffset, -90, 90, LEG_IK_MIN, LEG_IK_MAX); 
+        // Move legs to that height
+        servoManager.moveLegs(legHeight, 0);
+        Serial.print("Moving legs ");
+        Serial.print(legHeight);
+        Serial.print(" as head tilt was ");
+        Serial.println(headTiltOffset);
+        break;
+      case 1:
+        servoManager.moveLegs(random(LEG_IK_MIN, LEG_IK_MAX), 0);
+        break;
+      case 2:
+        servoManager.moveServos(PosLookRandom);
+        break;
+      case 3:
+        // do both
+        servoManager.moveServos(PosLookRandom);
+        servoManager.moveLegs(random(LEG_IK_MIN, LEG_IK_MAX), 0);
+        break;
+    }
 }
 
 void getOrdersFromPi()
 {
   if(Serial.available() == 0) return;
-  Serial.print("Order received: ");
+  cLog("Order received: ", false);
 
   // The first byte received is the instruction
   Order order_received = PiConnect::read_order();
-  Serial.println(order_received);
+  cLog((String) order_received);
   if(order_received == HELLO)
   {
     // If the cards haven't say hello, check the connection