Skip to content

Instantly share code, notes, and snippets.

@danic85
Created February 26, 2023 19:07
Show Gist options
  • Save danic85/bb05070f675a1dd5738a7206e5c8493c to your computer and use it in GitHub Desktop.
Save danic85/bb05070f675a1dd5738a7206e5c8493c to your computer and use it in GitHub Desktop.
Test script for robotic legs
/*
* 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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment