diff --git a/arduino_sketch/Animation.h b/arduino_sketch/Animation.h new file mode 100644 index 0000000..9b01852 --- /dev/null +++ b/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; + } + }; +} \ No newline at end of file diff --git a/arduino_sketch/Config.h b/arduino_sketch/Config.h index 1c8bb90..eeb82bb 100644 --- a/arduino_sketch/Config.h +++ b/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 diff --git a/arduino_sketch/Dependencies.h b/arduino_sketch/Dependencies.h new file mode 100644 index 0000000..d018b0f --- /dev/null +++ b/arduino_sketch/Dependencies.h @@ -0,0 +1,10 @@ +#pragma once + +#include +#include "Config.h" + +#include "SerialConnection.h" + +#include "ServoManager.h" + +#include "Animation.h" diff --git a/arduino_sketch/IServoManager.h b/arduino_sketch/IServoManager.h new file mode 100644 index 0000000..167fe6c --- /dev/null +++ b/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; +}; \ No newline at end of file diff --git a/arduino_sketch/InverseKinematics.h b/arduino_sketch/InverseKinematics.h index 57bd603..9628198 100644 --- a/arduino_sketch/InverseKinematics.h +++ b/arduino_sketch/InverseKinematics.h @@ -1,165 +1,162 @@ -#ifndef INVERSE_KINEMATICS_H -#define INVERSE_KINEMATICS_H +#pragma once //#define IK_DEBUG +namespace ModularBiped { + /** + * @file inverse_kinematics.h + * @brief Inverse kinematics library for biped robots + * @details This library provides inverse kinematics for a biped robot with 3 degrees of freedom. + */ + class InverseKinematics + { + // constructor + public: + double hipAdjustment = Config::HIP_ADJUSTMENT; // Angle to adjust hip, will be modified for balance + 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; + this->kneeMinAngle = kneeMinAngle; + this->kneeMaxAngle = kneeMaxAngle; + this->ankleMinAngle = ankleMinAngle; + this->ankleMaxAngle = ankleMaxAngle; + this->thighLength = thighLength; + this->shinLength = shinLength; + this->footLength = footLength; // Not currently used + } + + void hipAdjust(float angle) + { + this->hipAdjustment = angle; + } + + /** + * @brief Calculate the inverse kinematics for a bipedal leg on a 2d plane + * @param x The x coordinate of the point + * @param y The y coordinate of the point (not in use) + * @param hipAngle The angle of the hip servo (pointer) + * @param kneeAngle The angle of the knee servo (pointer) + * @param ankleAngle The angle of the ankle servo (pointer) + * + */ + boolean inverseKinematics2D(float x, float y, float &hipAngle, float &kneeAngle, float &ankleAngle) + { + float a = this->thighLength; + float b = this->shinLength; + float c = x; + // Law of cosines to sides A+ B + //γ = acos((a² + b² − c²)/(2ab)) + float hip = acos((sq(a) + sq(c) - sq(b)) / (2*a*c)); + // Calculate knee from hip + float knee = PI - (hip * 2) ; + // Calculate remaining angle (ankle) + float ankle = d2r(180) - knee - hip; + + // if IK_DEBUG is defined, print debug info + #ifdef IK_DEBUG + Serial.print(hip); + Serial.print(" + "); + Serial.print(knee); + Serial.print(" + "); + Serial.print(ankle); + Serial.print(" = "); + Serial.println(hip + knee + ankle); + + Serial.print(r2d(hip)); + Serial.print(" - "); + Serial.print(r2d(knee)); + Serial.print(" - "); + Serial.print(r2d(ankle)); + Serial.print(" = "); + Serial.println(r2d(hip + knee + ankle)); + #endif + + // return false if angles are not numbers + if (isnan(hip) || isnan(knee) || isnan(ankle)) + { + return false; + } + + // Convert the angles from radians to degrees + // Adjust to compensate for servo's actual position + hipAngle = r2d(d2r(180) - (hip + d2r(hipAdjustment))); // Inverse and offset by 90 degrees + kneeAngle = r2d(knee - d2r(Config::KNEE_ADJUSTMENT)); // Offset 90 to compensate + ankleAngle = r2d(ankle + d2r(Config::ANKLE_ADJUSTMENT)); // Offset 70 to compensate + + // if IK_DEBUG is defined, print debug info + #ifdef IK_DEBUG + Serial.print(hipAngle); + Serial.print(" - "); + Serial.print(kneeAngle); + Serial.print(" - "); + Serial.println(ankleAngle); + #endif + + return anglesWithinLimits(hipAngle, kneeAngle, ankleAngle); + + } -/** - * @file inverse_kinematics.h - * @brief Inverse kinematics library for biped robots - * @details This library provides inverse kinematics for a biped robot with 3 degrees of freedom. - */ -class InverseKinematics -{ - // constructor - public: - double hipAdjustment = HIP_ADJUSTMENT; // Angle to adjust hip, will be modified for balance - 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; - this->kneeMinAngle = kneeMinAngle; - this->kneeMaxAngle = kneeMaxAngle; - this->ankleMinAngle = ankleMinAngle; - this->ankleMaxAngle = ankleMaxAngle; - this->thighLength = thighLength; - this->shinLength = shinLength; - this->footLength = footLength; // Not currently used - } - - void hipAdjust(float angle) - { - this->hipAdjustment = angle; - } - - /** - * @brief Calculate the inverse kinematics for a bipedal leg on a 2d plane - * @param x The x coordinate of the point - * @param y The y coordinate of the point (not in use) - * @param hipAngle The angle of the hip servo (pointer) - * @param kneeAngle The angle of the knee servo (pointer) - * @param ankleAngle The angle of the ankle servo (pointer) - * - */ - boolean inverseKinematics2D(float x, float y, float &hipAngle, float &kneeAngle, float &ankleAngle) - { - float a = this->thighLength; - float b = this->shinLength; - float c = x; - // Law of cosines to sides A+ B - //γ = acos((a² + b² − c²)/(2ab)) - float hip = acos((sq(a) + sq(c) - sq(b)) / (2*a*c)); - // Calculate knee from hip - float knee = PI - (hip * 2) ; - // Calculate remaining angle (ankle) - float ankle = d2r(180) - knee - hip; - - // if IK_DEBUG is defined, print debug info - #ifdef IK_DEBUG - Serial.print(hip); - Serial.print(" + "); - Serial.print(knee); - Serial.print(" + "); - Serial.print(ankle); - Serial.print(" = "); - Serial.println(hip + knee + ankle); - - Serial.print(r2d(hip)); - Serial.print(" - "); - Serial.print(r2d(knee)); - Serial.print(" - "); - Serial.print(r2d(ankle)); - Serial.print(" = "); - Serial.println(r2d(hip + knee + ankle)); - #endif - - // return false if angles are not numbers - if (isnan(hip) || isnan(knee) || isnan(ankle)) + /** + * @brief Check if the angles are within the limits of the servos + * @param hipAngle The angle of the hip servo + * @param kneeAngle The angle of the knee servo + * @param ankleAngle The angle of the ankle servo + * @return True if the angles are within the limits, false otherwise + */ + boolean anglesWithinLimits(float hipAngle, float kneeAngle, float ankleAngle) { - return false; + if (hipAngle < this->hipMinAngle || hipAngle > this->hipMaxAngle || + kneeAngle < this->kneeMinAngle || kneeAngle > this->kneeMaxAngle || + ankleAngle < this->ankleMinAngle || ankleAngle > this->ankleMaxAngle) + { + return false; + } + return true; } - // Convert the angles from radians to degrees - // Adjust to compensate for servo's actual position - hipAngle = r2d(d2r(180) - (hip + d2r(hipAdjustment))); // Inverse and offset by 90 degrees - kneeAngle = r2d(knee - d2r(KNEE_ADJUSTMENT)); // Offset 90 to compensate - ankleAngle = r2d(ankle + d2r(ANKLE_ADJUSTMENT)); // Offset 70 to compensate - - // if IK_DEBUG is defined, print debug info - #ifdef IK_DEBUG - Serial.print(hipAngle); - Serial.print(" - "); - Serial.print(kneeAngle); - Serial.print(" - "); - Serial.println(ankleAngle); - #endif - - return anglesWithinLimits(hipAngle, kneeAngle, ankleAngle); - - } - - /** - * @brief Check if the angles are within the limits of the servos - * @param hipAngle The angle of the hip servo - * @param kneeAngle The angle of the knee servo - * @param ankleAngle The angle of the ankle servo - * @return True if the angles are within the limits, false otherwise - */ - boolean anglesWithinLimits(float hipAngle, float kneeAngle, float ankleAngle) - { - if (hipAngle < this->hipMinAngle || hipAngle > this->hipMaxAngle || - kneeAngle < this->kneeMinAngle || kneeAngle > this->kneeMaxAngle || - ankleAngle < this->ankleMinAngle || ankleAngle > this->ankleMaxAngle) + /** + * @brief Calculate the angles for the other leg assuming it is identical but mirrored + * @param lHip The angle of the hip servo of the left leg + * @param lKnee The angle of the knee servo of the left leg + * @param lAnkle The angle of the ankle servo of the left leg + * @param rHip The angle of the hip servo of the right leg (pointer) + * @param rKnee The angle of the knee servo of the right leg (pointer) + * @param rAnkle The angle of the ankle servo of the right leg (pointer) + */ + void calculateOtherLeg(float lHip, float lKnee, float lAnkle, float &rHip, float &rKnee, float &rAnkle) { - return false; + rHip = 180 - lHip; + rKnee = 180 - lKnee; + rAnkle = 180 - lAnkle; } - return true; - } - - /** - * @brief Calculate the angles for the other leg assuming it is identical but mirrored - * @param lHip The angle of the hip servo of the left leg - * @param lKnee The angle of the knee servo of the left leg - * @param lAnkle The angle of the ankle servo of the left leg - * @param rHip The angle of the hip servo of the right leg (pointer) - * @param rKnee The angle of the knee servo of the right leg (pointer) - * @param rAnkle The angle of the ankle servo of the right leg (pointer) - */ - void calculateOtherLeg(float lHip, float lKnee, float lAnkle, float &rHip, float &rKnee, float &rAnkle) - { - rHip = 180 - lHip; - rKnee = 180 - lKnee; - rAnkle = 180 - lAnkle; - } - - - /** - * @brief Convert radians to degrees - */ - float r2d(float rad) - { - return (rad * 180 / PI); - } - - /** - * @brief Convert degrees to radians - */ - float d2r(float deg) - { - return (deg * PI / 180); - } - - private: - float hipMinAngle; - float hipMaxAngle; - float kneeMinAngle; - float kneeMaxAngle; - float ankleMinAngle; - float ankleMaxAngle; - float thighLength; - float shinLength; - float footLength; -}; - - -#endif + + + /** + * @brief Convert radians to degrees + */ + float r2d(float rad) + { + return (rad * 180 / PI); + } + + /** + * @brief Convert degrees to radians + */ + float d2r(float deg) + { + return (deg * PI / 180); + } + + private: + float hipMinAngle; + float hipMaxAngle; + float kneeMinAngle; + float kneeMaxAngle; + float ankleMinAngle; + float ankleMaxAngle; + float thighLength; + float shinLength; + float footLength; + }; +} \ No newline at end of file diff --git a/arduino_sketch/Mpu6050.h b/arduino_sketch/Mpu6050.h index 801a90e..723f7c1 100644 --- a/arduino_sketch/Mpu6050.h +++ b/arduino_sketch/Mpu6050.h @@ -1,74 +1,71 @@ -#ifndef MPU6050_H -#define MPU6050_H +#pragma once #include #include #include -Adafruit_MPU6050 mpu; -boolean mpuReady = false; - -uint8_t address; -double pitch,roll; - -class Mpu6050 -{ - public: - - void doInit(uint8_t addr = 0x68) +namespace ModularBiped { + class Mpu6050 { - address = addr; - // Try to initialize - if (!mpu.begin(address)) { - cLog("Failed to find MPU6050 chip"); - return; - } + public: + Adafruit_MPU6050 mpu; + boolean mpuReady = false; - mpu.setAccelerometerRange(MPU6050_RANGE_16_G); - mpu.setGyroRange(MPU6050_RANGE_250_DEG); - mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); - mpuReady = true; + uint8_t address; + double pitch,roll; - cLog("MPU6050 initialised"); - } - - double getPitch() - { - return pitch; - } - - double getRoll() - { - return roll; - } + void doInit(uint8_t addr = 0x68) + { + address = addr; + // Try to initialize + if (!mpu.begin(address)) { + cLog("Failed to find MPU6050 chip"); + return; + } - void read() - { - if (mpuReady) { - sensors_event_t a, g, temp; - mpu.getEvent(&a, &g, &temp); + mpu.setAccelerometerRange(MPU6050_RANGE_16_G); + mpu.setGyroRange(MPU6050_RANGE_250_DEG); + mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); + mpuReady = true; + + cLog("MPU6050 initialised"); + } - pitch = atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180 / PI; - roll = atan2(a.acceleration.y, sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180 / PI; + double getPitch() + { + return pitch; } - } - void debug() - { - if (mpuReady) { - Serial.print("0x"); - Serial.print(address, HEX); - Serial.print("Pitch:"); - Serial.print(pitch); - Serial.print(","); - Serial.print("0x"); - Serial.print(address, HEX); - Serial.print("Roll:"); - Serial.println(roll); + double getRoll() + { + return roll; } - delay(10); - } -}; + void read() + { + if (mpuReady) { + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); + + pitch = atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180 / PI; + roll = atan2(a.acceleration.y, sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180 / PI; + } + } -#endif + void debug() + { + if (mpuReady) { + Serial.print("0x"); + Serial.print(address, HEX); + Serial.print("Pitch:"); + Serial.print(pitch); + Serial.print(","); + Serial.print("0x"); + Serial.print(address, HEX); + Serial.print("Roll:"); + Serial.println(roll); + } + delay(10); + } + }; +} \ No newline at end of file diff --git a/arduino_sketch/Order.h b/arduino_sketch/Order.h index 0edaeeb..d16c4f8 100644 --- a/arduino_sketch/Order.h +++ b/arduino_sketch/Order.h @@ -1,20 +1 @@ -#ifndef ORDER_H -#define ORDER_H - -// Define the orders that can be sent and received -enum Order { - HELLO = 0, - SERVO = 1, - SERVO_RELATIVE = 2, - ALREADY_CONNECTED = 3, - ERROR = 4, - RECEIVED = 5, - STOP = 6, - LED = 7, - PIN = 8, - READ = 9 -}; - -typedef enum Order Order; - -#endif +// Incorporated into SerialConnection.h. Will delete. diff --git a/arduino_sketch/PiConnect.h b/arduino_sketch/PiConnect.h index 3832da2..d2492ab 100644 --- a/arduino_sketch/PiConnect.h +++ b/arduino_sketch/PiConnect.h @@ -1,113 +1 @@ -#ifndef PI_CONNECT_H -#define PI_CONNECT_H - -#include "Order.h" - -class PiConnect -{ - bool serialConnected; - // boot time in millis - unsigned long bootTime; - // wait start time in millis - unsigned long sleepTime; - - - public: - PiConnect() - { - this->serialConnected = false; - this->bootTime = millis(); - this->sleepTime = 0; - } - boolean isConnected() - { - return this->serialConnected; - } - void setConnected(boolean connected) - { - cLog(F("Serial connected: "), false); - cLog((String) connected); - this->serialConnected = connected; - } - static boolean checkForConnection() - { - cLog(F("Checking for connection")); - write_order(HELLO); - wait_for_bytes(1, 1000); - } - - static Order read_order() - { - cLog("Reading order"); - return (Order) Serial.read(); - } - - static void wait_for_bytes(int num_bytes, unsigned long timeout) - { - unsigned long startTime = millis(); - //Wait for incoming bytes or exit if timeout - while ((Serial.available() < num_bytes) && (millis() - startTime < timeout)){} - } - - // NOTE : Serial.readBytes is SLOW - // this one is much faster, but has no timeout - static void read_signed_bytes(int8_t* buffer, size_t n) - { - size_t i = 0; - int c; - while (i < n) - { - c = Serial.read(); - if (c < 0) break; - *buffer++ = (int8_t) c; // buffer[i] = (int8_t)c; - i++; - } - } - - static int8_t read_i8() - { - wait_for_bytes(1, 100); // Wait for 1 byte with a timeout of 100 ms - return (int8_t) Serial.read(); - } - - static int16_t read_i16() - { - int8_t buffer[2]; - wait_for_bytes(2, 100); // Wait for 2 bytes with a timeout of 100 ms - read_signed_bytes(buffer, 2); - return (((int16_t) buffer[0]) & 0xff) | (((int16_t) buffer[1]) << 8 & 0xff00); - } - - static int32_t read_i32() - { - int8_t buffer[4]; - wait_for_bytes(4, 200); // Wait for 4 bytes with a timeout of 200 ms - read_signed_bytes(buffer, 4); - return (((int32_t) buffer[0]) & 0xff) | (((int32_t) buffer[1]) << 8 & 0xff00) | (((int32_t) buffer[2]) << 16 & 0xff0000) | (((int32_t) buffer[3]) << 24 & 0xff000000); - } - - static void write_order(enum Order myOrder) - { - uint8_t* Order = (uint8_t*) &myOrder; - Serial.write(Order, sizeof(uint8_t)); - } - - static void write_i8(int8_t num) - { - Serial.write(num); - } - - static void write_i16(int16_t num) - { - int8_t buffer[2] = {(int8_t) (num & 0xff), (int8_t) (num >> 8)}; - Serial.write((uint8_t*)&buffer, 2*sizeof(int8_t)); - } - - static void write_i32(int32_t num) - { - int8_t buffer[4] = {(int8_t) (num & 0xff), (int8_t) (num >> 8 & 0xff), (int8_t) (num >> 16 & 0xff), (int8_t) (num >> 24 & 0xff)}; - Serial.write((uint8_t*)&buffer, 4*sizeof(int8_t)); - } - -}; -#endif +// Renamed to SerialConnection.h - will delete diff --git a/arduino_sketch/SerialConnection.h b/arduino_sketch/SerialConnection.h new file mode 100644 index 0000000..a944cf7 --- /dev/null +++ b/arduino_sketch/SerialConnection.h @@ -0,0 +1,222 @@ +#pragma once +#include "IServoManager.h" + +namespace ModularBiped { + // Define the orders that can be sent and received + enum Order { + HELLO = 0, + SERVO = 1, + SERVO_RELATIVE = 2, + ALREADY_CONNECTED = 3, + ERROR = 4, + RECEIVED = 5, + STOP = 6, + LED = 7, + PIN = 8, + READ = 9 + }; + typedef enum Order Order; + + class SerialConnection + { + bool serialConnected; + // boot time in millis + unsigned long bootTime; + // wait start time in millis + unsigned long sleepTime; + bool ordersReceived; + IServoManager* servoManager; // Pointer to IServoManager + + + public: + SerialConnection(IServoManager* servoManager) + : servoManager(servoManager), serialConnected(false), bootTime(millis()), sleepTime(0), ordersReceived(false) {} + + boolean getOrdersReceived() + { + return this->ordersReceived; + } + + boolean isConnected() + { + return this->serialConnected; + } + void setConnected(boolean connected) + { + cLog(F("Serial connected: "), false); + cLog((String) connected); + this->serialConnected = connected; + } + static boolean checkForConnection() + { + cLog(F("Checking for connection")); + write_order(HELLO); + wait_for_bytes(1, 1000); + } + + void getAllOrdersFromSerial() + { + // Check for orders from serial + bool receiving = true; + while(receiving) + { + receiving = getOrdersFromSerial(); + if (receiving) { + delay(50); // Wait a short time for any other orders + } + } + } + + boolean getOrdersFromSerial() + { + if (Serial.available() == 0) + { + //SerialConnection::write_order(ERROR); + return false; + } + + cLog("Order received: ", false); + + // The first byte received is the instruction + Order order_received = SerialConnection::read_order(); + cLog((String)order_received); + if (order_received == HELLO) + { + // If the cards haven't say hello, check the connection + if (!this->isConnected()) + { + this->setConnected(true); + SerialConnection::write_order(HELLO); + } + else + { + // If we are already connected do not send "hello" to avoid infinite loop + SerialConnection::write_order(ALREADY_CONNECTED); + } + } + else if (order_received == ALREADY_CONNECTED) + { + this->setConnected(true); + } + else + { + switch (order_received) + { + case SERVO: + case SERVO_RELATIVE: + { + int servo_identifier = SerialConnection::read_i8(); + int servo_angle_percent = SerialConnection::read_i16(); + #ifdef DEBUG + SerialConnection::write_order(SERVO); + SerialConnection::write_i8(servo_identifier); + SerialConnection::write_i16(servo_angle_percent); + #endif + // Use the injected servoManager instance + servoManager->moveSingleServoByPercentage(servo_identifier, servo_angle_percent, order_received == SERVO_RELATIVE); + this->ordersReceived = true; + return true; + } + case PIN: + { + int pin = SerialConnection::read_i8(); + int value = SerialConnection::read_i8(); + pinMode(pin, OUTPUT); + digitalWrite(pin, value); + break; + } + case READ: + { + int pin = SerialConnection::read_i8(); + pinMode(pin, INPUT); + long value = analogRead(pin); + SerialConnection::write_i16(value); + break; + } + // Unknown order + default: + { + SerialConnection::write_order(order_received); + // SerialConnection::write_i16(404); + } + } + } + return false; + } + + static Order read_order() + { + cLog("Reading order"); + return (Order) Serial.read(); + } + + static void wait_for_bytes(int num_bytes, unsigned long timeout) + { + unsigned long startTime = millis(); + //Wait for incoming bytes or exit if timeout + while ((Serial.available() < num_bytes) && (millis() - startTime < timeout)){ + delay(1); + } + } + + // NOTE : Serial.readBytes is SLOW + // this one is much faster, but has no timeout + static void read_signed_bytes(int8_t* buffer, size_t n) + { + size_t i = 0; + int c; + while (i < n) + { + c = Serial.read(); + if (c < 0) break; + *buffer++ = (int8_t) c; // buffer[i] = (int8_t)c; + i++; + } + } + + static int8_t read_i8() + { + wait_for_bytes(1, 100); // Wait for 1 byte with a timeout of 100 ms + return (int8_t) Serial.read(); + } + + static int16_t read_i16() + { + int8_t buffer[2]; + wait_for_bytes(2, 100); // Wait for 2 bytes with a timeout of 100 ms + read_signed_bytes(buffer, 2); + return (((int16_t) buffer[0]) & 0xff) | (((int16_t) buffer[1]) << 8 & 0xff00); + } + + static int32_t read_i32() + { + int8_t buffer[4]; + wait_for_bytes(4, 200); // Wait for 4 bytes with a timeout of 200 ms + read_signed_bytes(buffer, 4); + return (((int32_t) buffer[0]) & 0xff) | (((int32_t) buffer[1]) << 8 & 0xff00) | (((int32_t) buffer[2]) << 16 & 0xff0000) | (((int32_t) buffer[3]) << 24 & 0xff000000); + } + + static void write_order(enum Order myOrder) + { + uint8_t* Order = (uint8_t*) &myOrder; + Serial.write(Order, sizeof(uint8_t)); + } + + static void write_i8(int8_t num) + { + Serial.write(num); + } + + static void write_i16(int16_t num) + { + int8_t buffer[2] = {(int8_t) (num & 0xff), (int8_t) (num >> 8)}; + Serial.write((uint8_t*)&buffer, 2*sizeof(int8_t)); + } + + static void write_i32(int32_t num) + { + int8_t buffer[4] = {(int8_t) (num & 0xff), (int8_t) (num >> 8 & 0xff), (int8_t) (num >> 16 & 0xff), (int8_t) (num >> 24 & 0xff)}; + Serial.write((uint8_t*)&buffer, 4*sizeof(int8_t)); + } + }; +}; \ No newline at end of file diff --git a/arduino_sketch/ServoManager.h b/arduino_sketch/ServoManager.h index fda8322..1baa478 100644 --- a/arduino_sketch/ServoManager.h +++ b/arduino_sketch/ServoManager.h @@ -1,375 +1,437 @@ -#ifndef ServoManager_h -#define ServoManager_h +#pragma once -#include "Config.h" +#include "ServoEasing.hpp" #include "InverseKinematics.h" +#ifdef MPU6050_ENABLED +#include "Mpu6050.h" +#endif +#include "IServoManager.h" -// Left Leg -ServoEasing ServoLLH; // Hip -ServoEasing ServoLLK; // Knee -ServoEasing ServoLLA; // Ankle -// Right Leg -ServoEasing ServoRLH; // Hip -ServoEasing ServoRLK; // Knee -ServoEasing ServoRLA; // Ankle -// Neck tilt -ServoEasing ServoNT; -// Neck pan -ServoEasing ServoNP; -// Left Leg Hip Pivot -ServoEasing ServoLLHP; // Hip pivot -// Right Leg Hip Pivot -ServoEasing ServoRLHP; // Hip pivot - -InverseKinematics ik; - -class ServoManager +namespace ModularBiped { -public: - double currentX, currentY; - void doInit() + class ServoManager : public IServoManager { - initInputs(); - - // 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, 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]); - ServoLLHP.attach(PIN_SLLHP, StartingPos[8]); - ServoRLHP.attach(PIN_SRLHP, StartingPos[9]); - - // 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); - - // Loop over ServoEasing::ServoEasingArray and attach each servo - for (uint8_t tIndex = 0; tIndex < SERVO_COUNT; ++tIndex) + public: + double currentX, currentY; + boolean calibrateRest; + + // Left Leg + ServoEasing ServoLLH; // Hip + ServoEasing ServoLLK; // Knee + ServoEasing ServoLLA; // Ankle + // Right Leg + ServoEasing ServoRLH; // Hip + ServoEasing ServoRLK; // Knee + ServoEasing ServoRLA; // Ankle + // Neck tilt + ServoEasing ServoNT; + // Neck pan + ServoEasing ServoNP; + // Left Leg Hip Pivot + ServoEasing ServoLLHP; // Hip pivot + // Right Leg Hip Pivot + ServoEasing ServoRLHP; // Hip pivot + + InverseKinematics ik; +#ifdef MPU6050_ENABLED + Mpu6050 tilt; +#endif + + void doInit() { - // Set easing type to EASING_TYPE - ServoEasing::ServoEasingArray[tIndex]->setEasingType(EASING_TYPE); - ServoEasing::ServoEasingArray[tIndex]->setMinMaxConstraint(PosMin[tIndex], PosMax[tIndex]); - } + initInputs(); + this->calibrateRest = false; - setSpeed(SERVO_SPEED_MIN); +#ifdef MPU6050_ENABLED + tilt.doInit(); +#endif - #ifdef SERVO_CALIBRATION_ENABLED - servoManager.calibrate(); // Must be in servoMode 2 - #endif + // IMPORTANT: Communication from Pi uses index, these must be attached in the same order as they are referenced in the pi config + this->ServoLLH.attach(Config::PIN_SLLH, Config::StartingPos[0]); + this->ServoLLK.attach(Config::PIN_SLLK, Config::StartingPos[1]); + this->ServoLLA.attach(Config::PIN_SLLA, Config::StartingPos[2]); + this->ServoRLH.attach(Config::PIN_SRLH, Config::StartingPos[3]); + this->ServoRLK.attach(Config::PIN_SRLK, Config::StartingPos[4]); + this->ServoRLA.attach(Config::PIN_SRLA, Config::StartingPos[5]); + this->ServoNT.attach(Config::PIN_SNT, Config::StartingPos[6]); + this->ServoNP.attach(Config::PIN_SNP, Config::StartingPos[7]); + this->ServoLLHP.attach(Config::PIN_SLLHP, Config::StartingPos[8]); + this->ServoRLHP.attach(Config::PIN_SRLHP, Config::StartingPos[9]); + + // Initialise IK with min and max angles and leg section lengths + ik.doInit(Config::PosMin[3], Config::PosMax[3], Config::PosMin[4], Config::PosMax[4], Config::PosMin[5], Config::PosMax[5], Config::LEG_LENGTH_THIGH, Config::LEG_LENGTH_SHIN, Config::LEG_LENGTH_FOOT); + + // Loop over ServoEasing::ServoEasingArray and attach each servo + for (uint8_t tIndex = 0; tIndex < Config::SERVO_COUNT; ++tIndex) + { + // Set easing type to EASING_TYPE + ServoEasing::ServoEasingArray[tIndex]->setEasingType(EASING_TYPE); + ServoEasing::ServoEasingArray[tIndex]->setMinMaxConstraint(Config::PosMin[tIndex], Config::PosMax[tIndex]); + } - // Wait for servos to reach start position. - delay(3000); - cLog("Servos initialised"); + setSpeed(Config::SERVO_SPEED_MIN); - if (servoMode == 2) - { - moveServos(PosStand); // Stand once - } +#ifdef SERVO_CALIBRATION_ENABLED + calibrate(); // Must be in servoMode 2 +#endif - } + // Wait for servos to reach start position. + delay(3000); + cLog("Servos initialised"); - 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); + if (Config::servoMode == 2) + { + // stationarySteps(); // Move to standing pose + moveServos(Config::PosStand); // Stand once + } + } + + void initInputs() + { +#ifdef SERVO_MODE_PIN_ENABLED + pinMode(Config::servoModePin, INPUT); // sets the digital pin as input + int servoModeVal = analogRead(Config::servoModePin); for (int i = 0; i < 5; i++) { - if (servoModeVal <= servoModeThresholds[i]) + if (servoModeVal < Config::servoModeThresholds[i]) { - servoMode = i; + Config::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 +// Serial.print("Servo mode: "); +// Serial.println(servoMode); +#ifdef SERVO_MODE_OVERRIDE + Config::servoMode = SERVO_MODE_OVERRIDE; // Define and set in Config.h if appropriate +// Serial.print("Servo mode override: "); +// Serial.println(servoMode); +#endif + + Config::StartingPos = Config::servoModePoses[Config::servoMode]; +#endif + +#ifdef RESTRAIN_PIN_ENABLED + pinMode(Config::restrainPin, INPUT_PULLUP); // sets the digital pin as input + if (digitalRead(Config::restrainPin) == LOW) // Check once on startup { - restrainingBolt = true; - StartingPos = PosStart; // Override starting position @todo remove once selector is installed + Config::restrainingBolt = true; + Config::StartingPos = Config::PosStart; // Override starting position @todo remove once selector is installed } - #endif - } +#endif + } - // Adjust hips to compensate for angle of body - // Read pitch (input from MPU6050) and adjust hips accordingly - void hipAdjust(double pitch) - { - double startingOffset = 3.5; - - double threshold = 5.0; // Do not move if less than this - #ifdef MPU6050_DEBUG - Serial.print("Pitch:"); - Serial.print(pitch); - Serial.print(" TH:"); - Serial.print(threshold+startingOffset); - Serial.print(" TL:"); - Serial.println(-threshold+startingOffset); - #endif - if (pitch < threshold+startingOffset && pitch > -threshold+startingOffset) + void servoLoop() { - return; +#ifdef MPU6050_ENABLED + hipAdjust(); +#endif + handlePendingMovement(); } - if (abs(pitch) > threshold) { - pitch = pitch*0.5; + void handlePendingMovement() + { + setEaseToForAllServosSynchronizeAndStartInterrupt(getSpeed()); + while (ServoEasing::areInterruptsActive()) + { + blinkLED(); + } } - ik.hipAdjust(ik.hipAdjustment - pitch); - moveSingleServo(0, pitch, true); - moveSingleServo(3, -pitch, true); - } - void moveServos(int *Pos) - { - for (uint8_t tIndex = 0; tIndex < SERVO_COUNT; ++tIndex) + void moveServos(int *Pos) { - if (Pos[tIndex] == NOVAL) + for (uint8_t tIndex = 0; tIndex < Config::SERVO_COUNT; ++tIndex) { - // Serial.println("NOVAL FOUND"); - continue; + if (Pos[tIndex] == Config::NOVAL) + { + // Serial.println("NOVAL FOUND"); + continue; + } + if (Pos[tIndex] != -1) + moveSingleServo(tIndex, Pos[tIndex], false); + else + moveSingleServo(tIndex, moveRandom(tIndex), false); // If scripted value is -1, generate random position based on range of currently indexed servo } - if (Pos[tIndex] != -1) - moveSingleServo(tIndex, Pos[tIndex], false); - else - moveSingleServo(tIndex, moveRandom(tIndex), false); // If scripted value is -1, generate random position based on range of currently indexed servo - } - // setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed); - } - - void moveSingleServoByPercentage(uint8_t pServoIndex, int pPercent, boolean isRelative) { - // Serial.print("PosMin: "); - // Serial.print(PosMin[pServoIndex]); - // Serial.print(" PosMax: "); - // Serial.print(PosMax[pServoIndex]); - // Serial.print(" Relative pos change in %: "); - // Serial.print(pPercent); - int realChange = map(abs(pPercent), 0, 100, PosMin[pServoIndex], PosMax[pServoIndex]); - // Serial.print(" in degrees: "); - // Serial.print(realChange); - if (isRelative) { - realChange = realChange - PosMin[pServoIndex]; // Account for min value ONLY if relative + // setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed); } - if (pPercent < 0) + + void moveSingleServoByPercentage(uint8_t pServoIndex, int pPercent, boolean isRelative) { - realChange = -realChange; + // Serial.print("PosMin: "); + // Serial.print(PosMin[pServoIndex]); + // Serial.print(" PosMax: "); + // Serial.print(PosMax[pServoIndex]); + // Serial.print(" Relative pos change in %: "); + // Serial.print(pPercent); + int realChange = map(abs(pPercent), 0, 100, Config::PosMin[pServoIndex], Config::PosMax[pServoIndex]); + // Serial.print(" in degrees: "); + // Serial.print(realChange); + if (isRelative) + { + realChange = realChange - Config::PosMin[pServoIndex]; // Account for min value ONLY if relative + } + if (pPercent < 0) + { + realChange = -realChange; + } + // SerialConnection::write_i16(realChange); + moveSingleServo(pServoIndex, realChange, isRelative); } - // PiConnect::write_i16(realChange); - moveSingleServo(pServoIndex, realChange, isRelative); - } - void moveSingleServo(uint8_t pServoIndex, int pPos, boolean isRelative) - { - if (((servoMode != 2 || restrainingBolt == true) && pServoIndex < 6) || servoMode == 0) + void moveSingleServo(uint8_t pServoIndex, int pPos, boolean isRelative) { - // Serial.println("Restrained mode, skipping some / all servos"); + if (((Config::servoMode != 2 || Config::restrainingBolt == true) && pServoIndex < 6) || Config::servoMode == 0) + { + // Serial.println("Restrained mode, skipping some / all servos"); + } + else if (isRelative) + { + ServoEasing::ServoEasingNextPositionArray[pServoIndex] = ServoEasing::ServoEasingNextPositionArray[pServoIndex] + pPos; + } + else + { + ServoEasing::ServoEasingNextPositionArray[pServoIndex] = pPos; + } + // Return actual value to Pi + SerialConnection::write_i16(ServoEasing::ServoEasingNextPositionArray[pServoIndex]); } - else if (isRelative) + + void moveLegsAndStore(int x, int y, int *store) { - ServoEasing::ServoEasingNextPositionArray[pServoIndex] = ServoEasing::ServoEasingNextPositionArray[pServoIndex] + pPos; + moveLegs(x, y); + for (uint8_t tIndex = 0; tIndex < Config::SERVO_COUNT; ++tIndex) + { + store[tIndex] = ServoEasing::ServoEasingNextPositionArray[tIndex]; + } } - else + + // function moveLegs that returns an int array + void moveLegs(int x, int y) { - ServoEasing::ServoEasingNextPositionArray[pServoIndex] = pPos; + float hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR; + currentX = x; + currentY = y; + // solve left leg + ik.inverseKinematics2D(x, y, hipAngleL, kneeAngleL, ankleAngleL); + // solve other leg + ik.calculateOtherLeg(hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR); + // Assign angles and move servos (cast float to int) + int thisMove[Config::SERVO_COUNT] = {(int)hipAngleL, (int)kneeAngleL, (int)ankleAngleL, (int)hipAngleR, (int)kneeAngleR, (int)ankleAngleR, Config::NOVAL, Config::NOVAL}; + moveServos(thisMove); } - // Return actual value to Pi - PiConnect::write_i16(ServoEasing::ServoEasingNextPositionArray[pServoIndex]); - } - void moveLegsAndStore(int x, int y, int *store) - { - moveLegs(x, y); - for (uint8_t tIndex = 0; tIndex < SERVO_COUNT; ++tIndex) + void demoAll() { - store[tIndex] = ServoEasing::ServoEasingNextPositionArray[tIndex]; + setSpeed(Config::SERVO_SPEED_MIN); + // Cycle through all poses and move all servos to them + for (uint8_t tPoseIndex = 0; tPoseIndex < sizeof(Config::Poses) / sizeof(Config::Poses[0]); ++tPoseIndex) + { + moveServos(Config::Poses[tPoseIndex]); + delay(1000); + moveServos(Config::PosRest); + delay(2000); + } } - } - // function moveLegs that returns an int array - void moveLegs(int x, int y) - { - float hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR; - currentX = x; - currentY = y; - // solve left leg - ik.inverseKinematics2D(x, y, hipAngleL, kneeAngleL, ankleAngleL); - // solve other leg - ik.calculateOtherLeg(hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR); - // Assign angles and move servos (cast float to int) - int thisMove[SERVO_COUNT] = {(int)hipAngleL, (int)kneeAngleL, (int)ankleAngleL, (int)hipAngleR, (int)kneeAngleR, (int)ankleAngleR, NOVAL, NOVAL}; - moveServos(thisMove); - } - - void demoAll() - { - 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) + void setSpeed(uint16_t pSpeed) { - moveServos(Poses[tPoseIndex]); - delay(1000); - moveServos(PosRest); - delay(2000); + tSpeed = pSpeed; + setSpeedForAllServos(tSpeed); + // cLog(F("Speed set: ")); + // cLog((String) tSpeed); } - } - void setSpeed(uint16_t pSpeed) - { - tSpeed = pSpeed; - setSpeedForAllServos(tSpeed); - // cLog(F("Speed set: ")); - // cLog((String) tSpeed); - } + uint16_t getSpeed() + { + return tSpeed; + } - uint16_t getSpeed() - { - return tSpeed; - } - - /** - * Manual servo calibration - * - * Move servos to 90 degree position (mid range). - * - * Iterate over each servo and allow the position to be entered manually. - * Store in an array. If user enters '', move to the next servo. - * - * Following calibration of all servos, output the arrays to the serial monitor - * and loop through the process again. - * - * Output instructions in the serial monitor to guide the user through the process. - */ - void calibrate() { - Serial.println("Calibration Mode Activated."); - Serial.println("WARNING: All min/max constraints removed, be careful!"); - // Array for storing positions - int positions[SERVO_COUNT] = {PosConfig[0], PosConfig[1], PosConfig[2], PosConfig[3], PosConfig[4], PosConfig[5], PosConfig[6], PosConfig[7]}; - moveServos(positions); - while(true) { - for (int i = 0; i < SERVO_COUNT; i++) + /** + * Manual servo calibration + * + * Move servos to 90 degree position (mid range). + * + * Iterate over each servo and allow the position to be entered manually. + * Store in an array. If user enters '', move to the next servo. + * + * Following calibration of all servos, output the arrays to the serial monitor + * and loop through the process again. + * + * Output instructions in the serial monitor to guide the user through the process. + */ + void calibrate() + { + Serial.println("Calibration Mode Activated."); + Serial.println("WARNING: All min/max constraints removed, be careful!"); + // Array for storing positions + int positions[Config::SERVO_COUNT] = {Config::PosConfig[0], Config::PosConfig[1], Config::PosConfig[2], Config::PosConfig[3], Config::PosConfig[4], Config::PosConfig[5], Config::PosConfig[6], Config::PosConfig[7]}; + moveServos(positions); + while (true) { - ServoEasing::ServoEasingArray[i]->setMinMaxConstraint(0, 180); // Remove min/max constraints - - Serial.print("Calibrating servo "); - Serial.println(i); - Serial.print("Starting position: "); - Serial.println(positions[i]); - Serial.println("Enter position (0-180) or 'n' to move to next servo"); - while (Serial.available() == 0) - { - delay(100); // Wait for input - } - String input = Serial.readString(); - // If input is a digit, move to that position - while (isDigit(input.charAt(0))) + for (int i = 0; i < Config::SERVO_COUNT; i++) { - int pos = input.toInt(); - Serial.print("Moving to "); - Serial.println(pos); - // Store position in array - positions[i] = pos; - moveSingleServo(i, pos, false); - #ifdef SERVO_CALIBRATION_SYMMETRICAL - // Calculate and move equivelent servo in other leg - int otherPos = i+3; - if (otherPos > 5) otherPos = i-3; - moveSingleServo(otherPos, 180-pos, false); - #endif - setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed); - while (ServoEasing::areInterruptsActive()) + ServoEasing::ServoEasingArray[i]->setMinMaxConstraint(0, 180); // Remove min/max constraints + + Serial.print("Calibrating servo "); + Serial.println(i); + Serial.print("Starting position: "); + Serial.println(positions[i]); + Serial.println("Enter position (0-180) or 'n' to move to next servo"); + while (Serial.available() == 0) { - blinkLED(); + delay(100); // Wait for input } - while (Serial.available() == 0) + String input = Serial.readString(); + // If input is a digit, move to that position + while (isDigit(input.charAt(0))) { - delay(100); + int pos = input.toInt(); + Serial.print("Moving to "); + Serial.println(pos); + // Store position in array + positions[i] = pos; + moveSingleServo(i, pos, false); +#ifdef SERVO_CALIBRATION_SYMMETRICAL + // Calculate and move equivelent servo in other leg + int otherPos = i + 3; + if (otherPos > 5) + otherPos = i - 3; + moveSingleServo(otherPos, 180 - pos, false); +#endif + setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed); + while (ServoEasing::areInterruptsActive()) + { + blinkLED(); + } + while (Serial.available() == 0) + { + delay(100); + } + input = Serial.readString(); } - input = Serial.readString(); + Serial.println("Moving to next servo"); + } + Serial.println("New positions:"); + // output all servo positions + for (int i = 0; i < Config::SERVO_COUNT; i++) + { + Serial.print(positions[i]); + Serial.print(", "); + } + Serial.println(); + } + } + /** + * @brief Set all servos to 90 degrees for mechanical calibration. Wait for 20 seconds. + */ + void allTo90() + { + cLog("All at 90"); + moveServos(Config::PosConfig); + handlePendingMovement(); + delay(20000); + } + + /** + * @brief Move to rest position. Either using stored values or by calculating using inverse kinematics and storing result for next time. + */ + void doRest() + { + cLog("Resting"); + + // Reset to slow speed + setSpeed(Config::SERVO_SPEED_MIN); + if (calibrateRest == false) + { + moveServos(Config::PosRest); + } + else + { + // Mid value between LEG_IK_MIN and LEG_IK_MAX + float mid = Config::LEG_IK_MIN + ((Config::LEG_IK_MAX - Config::LEG_IK_MIN) / 2); + moveLegsAndStore(mid, 0, Config::PosRest); // Move legs and store as rest position +// iterate over PosRest and output values: +#ifdef DEBUG + for (int i = 0; i < 9; i++) + { + Serial.print(Config::PosRest[i]); + Serial.print(", "); } - Serial.println("Moving to next servo"); + Serial.println(); +#endif + calibrateRest = false; } - Serial.println("New positions:"); - // output all servo positions - for (int i = 0; i < SERVO_COUNT; i++) + // setEaseToForAllServosSynchronizeAndStartInterrupt(getSpeed()); + } + + void stationarySteps() + { + int left[Config::SERVO_COUNT] = {Config::PosStart[0], Config::PosStart[1], Config::PosStart[2], Config::PosStand[3], Config::PosStand[4], Config::PosStand[5], Config::NOVAL, Config::NOVAL, Config::PosMax[8], Config::PosMax[9]}; + int right[Config::SERVO_COUNT] = {Config::PosStand[0], Config::PosStand[1], Config::PosStand[2], Config::PosStart[3], Config::PosStart[4], Config::PosStart[5], Config::NOVAL, Config::NOVAL, Config::PosMax[8], Config::PosMax[9]}; + uint16_t speed = Config::SERVO_SPEED_MIN; // 20 - 60 recommended + unsigned long delayTime = 2000; + setSpeed(speed); + boolean moveLeft = true; + int stepNum = 2; + for (int i = 0; i <= stepNum; i++) { - Serial.print(positions[i]); - Serial.print(", "); + if (moveLeft) + { + // Serial.println("Moving left"); + moveServos(left); + moveLeft = false; + } + else + { + // Serial.println("Moving left"); + moveServos(right); + moveLeft = true; + } + + handlePendingMovement(); + + delay(delayTime); } - Serial.println(); } - } -private: - uint16_t tSpeed; +#ifdef MPU6050_ENABLED + void hipAdjust() + { + tilt.read(); + double pitch = tilt.getPitch(); + double startingOffset = 3.5; + double threshold = 5.0; // Do not move if less than this +#ifdef MPU6050_DEBUG + Serial.print("Pitch:"); + Serial.print(pitch); + Serial.print(" TH:"); + Serial.print(threshold + startingOffset); + Serial.print(" TL:"); + Serial.println(-threshold + startingOffset); +#endif + if (pitch < threshold + startingOffset && pitch > -threshold + startingOffset) + { + return; + } - long moveRandom(int index) - { - int middle = (PosMin[index] + PosMax[index]) / 2; - int range = 15; // Reduce range of motion to avoid extreme movement - return random(middle - range, middle + range); - } - - // void solve2dInverseK(int x) - // { - // float hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR; - // int thisMove[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL}; - - // // Solve inverse kinematics for left leg - // if (!ik.inverseKinematics2D(x, 0, hipAngleL, kneeAngleL, ankleAngleL)) - // { - // #ifdef IK_DEBUG - // Serial.println("No solution"); - // #endif - // return; - // } - - // // Solve right leg, assuming identical but mirrored - // ik.calculateOtherLeg(hipAngleL, kneeAngleL, ankleAngleL, hipAngleR, kneeAngleR, ankleAngleR); - - // #ifdef IK_DEBUG - // Serial.print("X: "); - // Serial.print(x); - // Serial.print(" Y: "); - // Serial.println(y); - // Serial.print(" - L Hip: "); - // Serial.print(hipAngleL); - // Serial.print(" Knee: "); - // Serial.print(kneeAngleL); - // Serial.print(" Ankle: "); - // Serial.println(ankleAngleL); - // Serial.print(" - R Hip: "); - // Serial.print(hipAngleR); - // Serial.print(" Knee: "); - // Serial.print(kneeAngleR); - // Serial.print(" Ankle: "); - // Serial.println(ankleAngleR); - // #endif - - // // Define new positions and move servos - // thisMove[0] = hipAngleR; - // thisMove[1] = kneeAngleR; - // thisMove[2] = ankleAngleR; - // thisMove[3] = hipAngleL; - // thisMove[4] = kneeAngleL; - // thisMove[5] = ankleAngleL; - // moveServos(thisMove); - // delay(1000); - // } -}; + if (abs(pitch) > threshold) + { + pitch = pitch * 0.5; + } + ik.hipAdjust(ik.hipAdjustment - pitch); + moveSingleServo(0, pitch, true); + moveSingleServo(3, -pitch, true); + } #endif + + private: + uint16_t tSpeed; + + long moveRandom(int index) + { + int middle = (Config::PosMin[index] + Config::PosMax[index]) / 2; + int range = 15; // Reduce range of motion to avoid extreme movement + return random(middle - range, middle + range); + } + }; +}; \ No newline at end of file diff --git a/arduino_sketch/arduino_sketch.ino b/arduino_sketch/arduino_sketch.ino index 5d19cf1..85e3e06 100644 --- a/arduino_sketch/arduino_sketch.ino +++ b/arduino_sketch/arduino_sketch.ino @@ -1,66 +1,36 @@ + + /** - * @file arduino_sketch2.ino - * @brief Arduino sketch to control a 9 servo robot - * @details This sketch animates a 9 servo bipedal robot using the ServoEasing library. + * @file arduino_sketch.ino + * @brief Arduino sketch to control a bipedal robot + * @details This sketch animates a bipedal robot using the ServoEasing library. + * + * Config.h should be used to configure implementation specific values. * See the README.md file for more information. */ -#include -#include "ServoEasing.hpp" // Must be here otherwise method fails: setEaseToForAllServosSynchronizeAndStartInterrupt -#include "Config.h" -#include "Order.h" -#include "PiConnect.h" -#include "ServoManager.h" -#ifdef MPU6050_ENABLED -#include "Mpu6050.h" -#endif +#include "Dependencies.h" -// #define DEBUG +using namespace ModularBiped; -PiConnect pi; ServoManager servoManager; -#ifdef MPU6050_ENABLED -Mpu6050 tilt; -#endif - -bool isResting = false; -// boot time in millis -unsigned long bootTime; -// wait start time in millis -unsigned long sleepTime; - -boolean calibrateRest = false; - -bool shouldMove = false; - -bool piControl = false; +SerialConnection bipedSerial(&servoManager); +Animation animator(&servoManager); 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); // Init ServoManager servoManager.doInit(); - #ifdef MPU6050_ENABLED - tilt.doInit(); - #endif - // Seed random number generator randomSeed(analogRead(0)); - // allTo90(); - - // Move to rest position + calculate IK and store as rest position - // doRest(); - // Custom log message (enable DEBUG in Config.h to see this) cLog("Start loop"); @@ -69,257 +39,17 @@ void setup() // stationarySteps(); // servoManager.moveServos(PosStand); // } - -} - -/** - * @brief Set all servos to 90 degrees for mechanical calibration. Wait for 20 seconds. - */ -void allTo90() -{ - cLog("All at 90"); - servoManager.moveServos(PosConfig); - setEaseToForAllServosSynchronizeAndStartInterrupt(servoManager.getSpeed()); - while (ServoEasing::areInterruptsActive()) - { - blinkLED(); - } - delay(20000); -} - -/** - * @brief Move to rest position. Either using stored values or by calculating using inverse kinematics and storing result for next time. - */ -void doRest() -{ - cLog("Resting"); - - // Reset to slow speed - servoManager.setSpeed(SERVO_SPEED_MIN); - if (calibrateRest == false) - { - servoManager.moveServos(PosRest); - } - else - { - // 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: -#ifdef DEBUG - for (int i = 0; i < 9; i++) - { - Serial.print(PosRest[i]); - Serial.print(", "); - } - Serial.println(); -#endif - calibrateRest = false; - } - shouldMove = true; - // setEaseToForAllServosSynchronizeAndStartInterrupt(servoManager.getSpeed()); -} - -void stationarySteps() { - int right[SERVO_COUNT] = {PosStart[0], PosStart[1], PosStart[2], PosStand[3], PosStand[4], PosStand[5], NOVAL, NOVAL, PosMax[8], PosMax[9]}; - int left[SERVO_COUNT] = {PosStand[0], PosStand[1], PosStand[2], PosStart[3], PosStart[4], PosStart[5], NOVAL, NOVAL, PosMin[8], PosMin[9]}; - uint16_t speed = SERVO_SPEED_MAX; // 20 - 60 recommended - unsigned long delayTime = 2000; - servoManager.setSpeed(speed); - boolean moveLeft = true; - int stepNum = 2; - for (int i = 0; i <= stepNum; i++) - { - // Serial.println("Step " + String(i)); - if (moveLeft) - { - // Serial.println("Moving left"); - servoManager.moveServos(left); - moveLeft = false; - } - else - { - // Serial.println("Moving left"); - servoManager.moveServos(right); - moveLeft = true; - } - - setEaseToForAllServosSynchronizeAndStartInterrupt(servoManager.getSpeed()); - - while (ServoEasing::areInterruptsActive()) - { - blinkLED(); - } - - delay(delayTime); - } -} - -#ifdef MPU6050_ENABLED -void hipAdjust() -{ - tilt.read(); - //Serial.println(tilt.getPitch()); - servoManager.hipAdjust(tilt.getPitch()); - setEaseToForAllServosSynchronizeAndStartInterrupt(servoManager.getSpeed()); } -#endif void loop() { - - - #ifdef MPU6050_ENABLED - hipAdjust(); - #endif - // This needs to be here rather than in the ServoManager, otherwise it doesn't work. - while (ServoEasing::areInterruptsActive()) - { - blinkLED(); - } - - // Check for orders from pi - 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 - } - - // Once all orders received (or animating without orders) - if (shouldMove) - { - setEaseToForAllServosSynchronizeAndStartInterrupt(servoManager.getSpeed()); - shouldMove = false; - } - - // if not sleeping, animate randomly - // Orders from pi will set sleep time so that the animation does not take precedence - if (!isSleeping()) - { - if (isResting && piControl == false) // Only do this if the Pi is not in control (i.e. switched off) - { - #ifdef ANIMATE_ENABLED - animateRandomly(); - #endif - } - else - { - // doRest(); - isResting = true; - setSleep(random(1000, 5000)); - } - } - -} - -void setSleep(unsigned long length) -{ - sleepTime = millis() - bootTime + length; -} - -boolean isSleeping() -{ - return millis() - bootTime < sleepTime; -} - -void animateRandomly() -{ - cLog("Animating"); - isResting = false; - servoManager.setSpeed(SERVO_SPEED_MIN); - - // Look around randomly and move legs to react - servoManager.moveServos(PosLookRandom); - // If head looks down, move legs up, and vice versa - 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); - // Move legs to that height - // servoManager.moveLegs(LEG_IK_MAX, 0); - // servoManager.moveServos(PosStand); - shouldMove = true; -#ifdef DEBUG - Serial.print("Moving legs "); - Serial.print(legHeight); - Serial.print(" as head tilt was "); - Serial.println(headTiltOffset); -#endif -} - -boolean getOrdersFromPi() -{ - if (Serial.available() == 0) - //PiConnect::write_order(ERROR); - return false; - cLog("Order received: ", false); + bipedSerial.getAllOrdersFromSerial(); + servoManager.servoLoop(); // Handle pending movement and adjust hips + animator.animateRandomly(!bipedSerial.getOrdersReceived()); // Only if serial control is not active - // The first byte received is the instruction - Order order_received = PiConnect::read_order(); - cLog((String)order_received); - if (order_received == HELLO) - { - // If the cards haven't say hello, check the connection - if (!pi.isConnected()) - { - pi.setConnected(true); - PiConnect::write_order(HELLO); - } - else - { - // If we are already connected do not send "hello" to avoid infinite loop - PiConnect::write_order(ALREADY_CONNECTED); - } - } - else if (order_received == ALREADY_CONNECTED) - { - pi.setConnected(true); - } - else - { - switch (order_received) - { - case SERVO: - case SERVO_RELATIVE: - { - int servo_identifier = PiConnect::read_i8(); - int servo_angle_percent = PiConnect::read_i16(); - #ifdef DEBUG - PiConnect::write_order(SERVO); - PiConnect::write_i8(servo_identifier); - PiConnect::write_i16(servo_angle_percent); - #endif - // sleep animations for 2 seconds to allow pi to control servos - setSleep(2000); - servoManager.moveSingleServoByPercentage(servo_identifier, servo_angle_percent, order_received == SERVO_RELATIVE); - return true; - } - case PIN: - { - int pin = PiConnect::read_i8(); - int value = PiConnect::read_i8(); - pinMode(pin, OUTPUT); - digitalWrite(pin, value); - break; - } - case READ: - { - int pin = PiConnect::read_i8(); - pinMode(pin, INPUT); - long value = analogRead(pin); - PiConnect::write_i16(value); - break; - } - // Unknown order - default: - { - PiConnect::write_order(order_received); - // PiConnect::write_i16(404); - } - } - } - return false; -} + // This needs to be here rather than in the ServoManager, otherwise it doesn't work. @TODO confirm + // while (ServoEasing::areInterruptsActive()) + // { + // blinkLED(); + // } +} \ No newline at end of file