Refactor Arduino Sketch

arduino_sketch/Animation.h

@@ -0,0 +1,53 @@
+#pragma once
+#include "IServoManager.h"
+
+namespace ModularBiped {
+    class Animation {
+        bool isResting;
+        unsigned long bootTime; // boot time in millis
+        unsigned long sleepTime; // wait start time in millis
+        IServoManager* servoManager; // Pointer to IServoManager
+
+        public:
+            Animation(IServoManager* servoManager)
+            {
+                this->sleepTime = 0;
+                this->bootTime = millis();      
+                this->isResting = false;
+                this->servoManager = servoManager;
+            }
+
+            void animateRandomly(bool enabled)
+            {
+                if (enabled == false)
+                    return;
+
+                // if not sleeping, animate randomly
+                // Orders from serial will set sleep time so that the animation does not take precedence
+                if (!this->isSleeping())
+                {
+                    if (this->isResting)
+                    {
+                        cLog("Animating Randomly");
+                        servoManager->moveServos(Config::PosLookRandom);
+                    }
+                    else
+                    {
+                        this->isResting = true;
+                        setSleep(random(1000, 5000));
+                    }
+                }
+            }
+
+        private:
+            void setSleep(unsigned long length)
+            {
+                sleepTime = millis() - bootTime + length;
+            }
+
+            bool isSleeping()
+            {
+                return millis() - bootTime < sleepTime;
+            }
+    };
+}

arduino_sketch/Config.h

@@ -1,128 +1,133 @@
-#ifndef CONFIG_H
-#define CONFIG_H
+#pragma once
 /**
  * @file config.h
  * @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_SYMMETRICAL // Calculate and apply equivelant changes on other leg.
-
-#define SERVO_PIN_OFFSET 2 // Legacy, used to identify pin from pi communication
-// Left Leg
-#define PIN_SLLH 9 // Servo left leg hip
-#define PIN_SLLK 10 // Servo left leg knee
-#define PIN_SLLA 11 // Servo left leg ankle
-// Right Leg
-#define PIN_SRLH 6 // Servo right leg hip
-#define PIN_SRLK 7 // Servo right leg knee
-#define PIN_SRLA 8 // Servo right leg ankle
-// Neck
-#define PIN_SNT 2 // Servo neck tilt
-#define PIN_SNP 3 // Servo neck pan
-// Sway
-#define PIN_SLLHP 4 // Servo left leg hip pivot (unused)
-#define PIN_SRLHP 5 // Servo right leg hip pivot (unused)
-
-#define LEG_LENGTH_SHIN 94.0 // Length of shin
-#define LEG_LENGTH_THIGH 94.0 // Length of thigh
-#define LEG_LENGTH_FOOT 28.0 // Length of foot
-
-#define HIP_ADJUSTMENT 120 // Starting angle of hip relative to IK model
-#define KNEE_ADJUSTMENT 90
-#define ANKLE_ADJUSTMENT 85
-
+namespace ModularBiped
+{
+    namespace Config
+    {
+        // Left Leg
+        constexpr int PIN_SLLH = 9;  // Servo left leg hip
+        constexpr int PIN_SLLK = 10; // Servo left leg knee
+        constexpr int PIN_SLLA = 11; // Servo left leg ankle
+
+        // Right Leg
+        constexpr int PIN_SRLH = 6; // Servo right leg hip
+        constexpr int PIN_SRLK = 7; // Servo right leg knee
+        constexpr int PIN_SRLA = 8; // Servo right leg ankle
+
+        // Neck
+        constexpr int PIN_SNT = 2; // Servo neck tilt
+        constexpr int PIN_SNP = 3; // Servo neck pan
+
+        // Sway (currently unused)
+        constexpr int PIN_SLLHP = 4; // Servo left leg hip pivot
+        constexpr int PIN_SRLHP = 5; // Servo right leg hip pivot
+
+        // Leg dimensions
+        constexpr double LEG_LENGTH_SHIN = 94.0;  // Length of shin
+        constexpr double LEG_LENGTH_THIGH = 94.0; // Length of thigh
+        constexpr double LEG_LENGTH_FOOT = 28.0;  // Length of foot
+
+        // Servo adjustments
+        constexpr int HIP_ADJUSTMENT = 120; // Starting angle of hip relative to IK model
+        constexpr int KNEE_ADJUSTMENT = 90;
+        constexpr int ANKLE_ADJUSTMENT = 85;
+
+// Servo easing configuration
 #define EASING_TYPE EASE_QUADRATIC_IN_OUT
-#define ENABLE_EASE_QUADRATIC
-#define SERVO_SPEED_MIN 30 // Was 20
-#define SERVO_SPEED_MAX 80
-
-// #define DEBUG
+        constexpr int SERVO_SPEED_MIN = 30; // Minimum servo speed
+        constexpr int SERVO_SPEED_MAX = 80; // Maximum servo speed
 
-#define SERVO_COUNT 10 // Number of servos to be controlled by ServoEasing
+        // Servo count
+        constexpr int SERVO_COUNT = 10; // 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 180 // Max solvable height of leg between hip and ankle
+        // Inverse kinematics limits
+        constexpr int LEG_IK_MIN = 75;  // Minimum solvable height of leg between hip and ankle
+        constexpr int LEG_IK_MAX = 180; // Maximum solvable height of leg between hip and ankle
 
-#define NOVAL 1000
+        // Special values
+        constexpr int NOVAL = 1000;
 
+// Feature flags
 #define MPU6050_ENABLED // Enable MPU6050
-//#define MPU6050_DEBUG // Debug in serial plotter
-#define ANIMATE_ENABLED // Enable random animations
-
+// #define MPU6050_DEBUG // Debug in serial plotter
+// #define DEBUG // Main debug via cLog method
 #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, 0, 0};
-int PosMax[SERVO_COUNT] = {180, 180, 165, 180, 180, 180, 120, 150, 180, 180};
-int PosSleep[SERVO_COUNT] = {40, 60, 95, 140, 120, 85, PosMax[7], 90, 180, 0};
-//int PrepRestFromSleep[SERVO_COUNT] = {80, PosMin[1], PosMax[2], 100, PosMax[4], PosMin[5], S7_REST, 80, S9_REST};
-//int PrepSleepFromRest[SERVO_COUNT] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, 80, S9_REST};
+        // 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, 0, 0};
+        int PosMax[SERVO_COUNT] = {180, 180, 165, 180, 180, 180, 120, 150, 180, 180};
+        int PosSleep[SERVO_COUNT] = {40, 60, 95, 140, 120, 85, PosMax[7], 90, 180, 0};
+        // int PrepRestFromSleep[SERVO_COUNT] = {80, PosMin[1], PosMax[2], 100, PosMax[4], PosMin[5], S7_REST, 80, S9_REST};
+        // int PrepSleepFromRest[SERVO_COUNT] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, 80, S9_REST};
 
-// Starting positions
-//0, 3 = HIP
-int PosStart[SERVO_COUNT] = {60, 0, 165, 120, 180, 30, 90, 90, 180, 0};
+        // Starting positions
+        // 0, 3 = HIP
+        int PosStart[SERVO_COUNT] = {60, 0, 165, 120, 180, 30, 90, 90, 180, 0};
 
-int PosBackpack[SERVO_COUNT] = {30, 5, 90, 130, 120, 160, 90, 90, 180, 0}; // Position legs to support when mounted to backpack
-int PosStraight[SERVO_COUNT] = {45, 90, 165, 135, 90, 20, 90, 90, 180, 0}; // straighten legs and point feet to fit in backpack upright
+        int PosBackpack[SERVO_COUNT] = {30, 5, 90, 130, 120, 160, 90, 90, 180, 0}; // Position legs to support when mounted to backpack
+        int PosStraight[SERVO_COUNT] = {45, 90, 165, 135, 90, 20, 90, 90, 180, 0}; // 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, 180, 0};
+        // 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, 180, 0};
 
-int PosConfig[SERVO_COUNT] = {90, 90, 90, 90, 90, 90, 90, 90, 180, 0};
+        int PosConfig[SERVO_COUNT] = {90, 90, 90, 90, 90, 90, 90, 90, 180, 0};
 
-// Poses
-int PosStand[SERVO_COUNT] = {45, 70, 80, 135, 110, 100, NOVAL, NOVAL, 180, 0};
-int PosLookLeft[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 180, 180, 0};
-int PosLookRight[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL,  NOVAL, NOVAL, 0, 180, 0};
-int PosLookRandom[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, -1, 180, 0}; // Made random by calling the function moveRandom() if the value is -1
-int PosLookUp[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 60, 90, 180, 0};
-int PosLookDown[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 120, 90, 180, 0};
+        // Poses
+        int PosStand[SERVO_COUNT] = {45, 70, 80, 135, 110, 100, NOVAL, NOVAL, 180, 0};
+        int PosLookLeft[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 180, 180, 0};
+        int PosLookRight[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 0, 180, 0};
+        int PosLookRandom[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, -1, 180, 0}; // Made random by calling the function moveRandom() if the value is -1
+        int PosLookUp[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 60, 90, 180, 0};
+        int PosLookDown[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 120, 90, 180, 0};
 
-// Array of poses except PosRest and PosSleep (which are used for initialization and reset of position)
-int *Poses[] = {PosStand, PosLookLeft, PosLookRight, PosLookUp, PosLookDown, PosLookRandom};
+        // 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 *StartingPos = PosStart;
-int servoMode = 2; // Default to standing pose
+        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};
+        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; // Needed in either case as it is checked by ServoManager
-
-void blinkLED()
-{
-    digitalWrite(LED_BUILTIN, HIGH);
-    delay(100);
-    digitalWrite(LED_BUILTIN, LOW);
-    delay(100);
-}
+        int restrainPin = 12;
+        bool restrainingBolt = false; // Needed in either case as it is checked by ServoManager
+    }
 
-/**
- * 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)
+    void blinkLED()
     {
-        Serial.println(message);
+        digitalWrite(LED_BUILTIN, HIGH);
+        delay(100);
+        digitalWrite(LED_BUILTIN, LOW);
+        delay(100);
     }
-    else
+
+    /**
+     * Custom logging function to avoid having to wrap every Serial.println() in #ifdef DEBUG
+     */
+    void cLog(String message, boolean newline = true)
     {
-        Serial.print(message);
+#ifdef DEBUG
+        if (newline)
+        {
+            Serial.println(message);
+        }
+        else
+        {
+            Serial.print(message);
+        }
+#endif
     }
-    #endif
 }
-
-#endif

arduino_sketch/Dependencies.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include <Arduino.h>
+#include "Config.h"
+
+#include "SerialConnection.h"
+
+#include "ServoManager.h"
+
+#include "Animation.h"

arduino_sketch/IServoManager.h

@@ -0,0 +1,7 @@
+#pragma once
+
+class IServoManager {
+public:
+    virtual void moveSingleServoByPercentage(uint8_t pServoIndex, int pPercent, boolean isRelative) = 0;
+    virtual void moveServos(int *Pos) = 0;
+};