Test script for robotic legs

RobotLeg.ino

/*
 * ThreeServos.cpp
 *
 *  Shows smooth movement from one servo position to another for 3 servos synchronously.
 *  Demonstrates the use of ServoEasingArray and ServoEasingNextPositionArray.
 *
 *  Copyright (C) 2019-2022  Armin Joachimsmeyer
 *  armin.joachimsmeyer@gmail.com
 *
 *  This file is part of ServoEasing https://github.com/ArminJo/ServoEasing.
 *
 *  ServoEasing is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *  See the GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/gpl.html>.
 */

#include <Arduino.h>

// Must specify this before the include of "ServoEasing.hpp"
//#define USE_PCA9685_SERVO_EXPANDER    // Activating this enables the use of the PCA9685 I2C expander chip/board.
//#define USE_SERVO_LIB                 // If USE_PCA9685_SERVO_EXPANDER is defined, Activating this enables force additional using of regular servo library.
//#define PROVIDE_ONLY_LINEAR_MOVEMENT  // Activating this disables all but LINEAR movement. Saves up to 1540 bytes program memory.
//#define DISABLE_COMPLEX_FUNCTIONS     // Activating this disables the SINE, CIRCULAR, BACK, ELASTIC, BOUNCE and PRECISION easings. Saves up to 1850 bytes program memory.
#define MAX_EASING_SERVOS 6
//#define DISABLE_MICROS_AS_DEGREE_PARAMETER // Activating this disables microsecond values as (target angle) parameter. Saves 128 bytes program memory.
//#define DISABLE_MIN_AND_MAX_CONSTRAINTS    // Activating this disables constraints. Saves 4 bytes RAM per servo but strangely enough no program memory.
//#define DISABLE_PAUSE_RESUME               // Activating this disables pause and resume functions. Saves 5 bytes RAM per servo.
//#define DEBUG                              // Activating this enables generate lots of lovely debug output for this library.

//#define PRINT_FOR_SERIAL_PLOTTER           // Activating this enables generate the Arduino plotter output from ServoEasing.hpp.
#include "ServoEasing.hpp"


#define SERVO1_PIN 6
#define SERVO2_PIN 7
#define SERVO3_PIN 8
#define SERVO4_PIN 9
#define SERVO5_PIN 10
#define SERVO6_PIN 11
#define SPEED_IN_PIN A0
#define MODE_ANALOG_INPUT_PIN A1

#define SERVO_UNDER_TEST_PIN SERVO1_PIN

#define START_DEGREE_VALUE  60 // The degree value written to the servo at time of attach.
#define EASING_TYPE EASE_QUADRATIC_IN_OUT
#define EASING_TYPE2 EASE_QUADRATIC_IN_OUT
#define ENABLE_EASE_QUADRATIC
#define SERVO_SPEED 50


ServoEasing Servo1;
ServoEasing Servo2;
ServoEasing Servo3;
ServoEasing Servo4;
ServoEasing Servo5;
ServoEasing Servo6;

// Arrays to store servo min / max positions to avoid mechanical issues due
// NOTE: PosStart disregards this, set the PosStart to be within range of the servo's physical boundaries
int PosMin[MAX_EASING_SERVOS] = { 20, 5, 15, 20, 5, 15 };
int PosMax[MAX_EASING_SERVOS] = { 160, 175, 180, 160, 175, 180 };
// Starting positions
int PosStart[MAX_EASING_SERVOS] = { 160, 180, 90, 20, 0, 90 };
// Poses
//int PosStand[MAX_EASING_SERVOS] = { 0, 150, 100, 60, 20, 90 };
//int PosSit[MAX_EASING_SERVOS] = { 180, 60, 130, 60, 90, 80 };
int PosStand[MAX_EASING_SERVOS] = { 90, 90, 110, 90, 90, 70 };
int PosSit[MAX_EASING_SERVOS] = { 180, 180, 90, 0, 0, 90 };
void blinkLED();

uint16_t tSpeed;

void setup() {
    pinMode(LED_BUILTIN, OUTPUT);
    Serial.begin(115200);
    tSpeed = SERVO_SPEED;

    Servo1.attach(SERVO1_PIN, PosStart[0]);
    Servo2.attach(SERVO2_PIN, PosStart[1]);
    Servo3.attach(SERVO3_PIN, PosStart[2]);
    Servo4.attach(SERVO4_PIN, PosStart[3]);
    Servo5.attach(SERVO5_PIN, PosStart[4]);
    Servo6.attach(SERVO6_PIN, PosStart[5]);

    // Loop over ServoEasing::ServoEasingArray and attach each servo
    for (uint8_t tIndex = 0; tIndex < MAX_EASING_SERVOS; ++tIndex) {
        //ServoEasing s;
        //s.attach(SERVO1_PIN + tIndex, PosStart[tIndex]);
        // Set easing type to EASING_TYPE
        if (tIndex < 3) ServoEasing::ServoEasingArray[tIndex]->setEasingType(EASING_TYPE);
        else ServoEasing::ServoEasingArray[tIndex]->setEasingType(EASING_TYPE2);
        ServoEasing::ServoEasingArray[tIndex]->setMinMaxConstraint(PosMin[tIndex],PosMax[tIndex]);     
    }
    // Wait for servos to reach start position.
    delay(3000);
    Serial.println(F("Start loop"));
    

}

void blinkLED() {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(100);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
}

void loop() {
    

    setSpeedForAllServos(tSpeed);

    //Loop all servos in ServoEasingArray and set ServoEasingNextPositionArray value to PosStand
    for (uint8_t tIndex = 0; tIndex < MAX_EASING_SERVOS; ++tIndex) {
        ServoEasing::ServoEasingNextPositionArray[tIndex] = PosSit[tIndex];
        
    }     
    setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
    /*
     * Now you can run your program while the servos are moving.
     * Just let the LED blink until servos stop.
     * Since all servos stops at the same time I have to check only one
     */
    while (ServoEasing::areInterruptsActive()) {
        blinkLED();
    }

    delay(1000);

    //Loop all servos in ServoEasingArray and set ServoEasingNextPositionArray value to PosStand
    for (uint8_t tIndex = 0; tIndex < MAX_EASING_SERVOS; ++tIndex) {
        ServoEasing::ServoEasingNextPositionArray[tIndex] = PosStand[tIndex];
        
    }     
    setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
    /*
     * Now you can run your program while the servos are moving.
     * Just let the LED blink until servos stop.
     * Since all servos stops at the same time I have to check only one
     */
    while (ServoEasing::areInterruptsActive()) {
        blinkLED();
    }

    //tSpeed += 10;
    //if (tSpeed > 350) tSpeed = SERVO_SPEED;
    //Serial.println(tSpeed);
}