[4 DOF Robot Arm Keyestudio][Lab 12][i-Duino UNO R3B] Simple Trajectory

/*
 * SymmetricEasing.cpp
 *
 *  Shows symmetric (end movement is mirror of start movement) linear, quadratic and cubic movements for 3 servos synchronously.
 *
 *  Copyright (C) 2019-2021  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>.
 */


/*
 * To generate the Arduino plotter output, you must activate the line #define PRINT_FOR_SERIAL_PLOTTER in ServoEasing.h
 */

/*
 * Pin mapping table for different platforms
 *
 * Platform     Servo1      Servo2      Servo3      Analog
 * -------------------------------------------------------
 * AVR + SAMD   9           10          11          A0
 * ESP8266      14 // D5    12 // D6    13 // D7    0
 * ESP32        5           18          19          A0
 * BluePill     PB7         PB8         PB9         PA0
 * APOLLO3      11          12          13          A3
 * ATX-2        13          14          15          knob
 */

#include "ServoEasing.h"

#define SERVO1_PIN A1
#define SERVO2_PIN A0
#define SERVO3_PIN 6
#define SERVO4_PIN 9

#define START_SERVO1_DEGREE_VALUE 80
#define START_SERVO2_DEGREE_VALUE 60
#define START_SERVO3_DEGREE_VALUE 100
#define START_SERVO4_DEGREE_VALUE 85

ServoEasing Servo1;
ServoEasing Servo2;
ServoEasing Servo3;
ServoEasing Servo4;
// ServoEasing Servo4;

#define START_DEGREE_VALUE 90

void setup() {
    delay(5000);
    Serial.begin(115200);
    /************************************************************
     * Attach servo to pin and set servos to start position.
     * This is the position where the movement starts.
     *
     * The order of the attach() determine the position
     * of the Servos in internal ServoEasing::ServoEasingArray[]
     ***********************************************************/
     Serial.println(F("Waiting..."));
     while(!Serial){
      }
     Serial.println(F("OK."));
#ifndef PRINT_FOR_SERIAL_PLOTTER
    Serial.print(F("Attach servo at pin "));
    Serial.println(SERVO1_PIN);
#endif
    if (Servo1.attach(SERVO1_PIN, START_SERVO1_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
    DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
        Serial.println(F("Error attaching Servo1"));
        while(1){}
    }
    Serial.println(F("Servo1: PASS"));

#ifndef PRINT_FOR_SERIAL_PLOTTER
    Serial.print(F("Attach servo at pin "));
    Serial.println(SERVO2_PIN);
#endif
    if (Servo2.attach(SERVO2_PIN, START_SERVO2_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
    DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
        Serial.println(F("Error attaching Servo2"));
        while(1){}
    }
    Serial.println(F("Servo2: PASS"));
    /*
     * Check at least the last call to attach()
     */
#ifndef PRINT_FOR_SERIAL_PLOTTER
    Serial.print(F("Attach servo at pin "));
    Serial.println(SERVO3_PIN);
#endif
    if (Servo3.attach(SERVO3_PIN, START_SERVO3_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
    DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
        Serial.println(F("Error attaching Servo3"));
        while(1){}
    }
    Serial.println(F("Servo3: PASS"));

#ifndef PRINT_FOR_SERIAL_PLOTTER
    Serial.print(F("Attach servo at pin "));
    Serial.println(SERVO3_PIN);
#endif
    if (Servo4.attach(SERVO4_PIN, START_SERVO4_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
    DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
        Serial.println(F("Error attaching Servo4"));
        while(1){}
    }
    Serial.println(F("Servo4: PASS"));
    delay(2000);

#ifdef PRINT_FOR_SERIAL_PLOTTER
    // Legend for Arduino Serial plotter
    Serial.println();
    Serial.println("Quadratic");
#endif

#ifndef PRINT_FOR_SERIAL_PLOTTER
    Serial.println(F("Move from 90 to 45 degree in 1 second"));
#endif
    Servo1.startEaseToD(80, 1000);
    Servo2.startEaseToD(60, 1000);
    Servo3.startEaseToD(100, 1000);
    Servo4.startEaseToD(85, 1000);
    delay(1000);

    Servo1.setEasingType(EASE_QUADRATIC_IN_OUT);
    Servo2.setEasingType(EASE_QUADRATIC_IN_OUT);
    Servo3.setEasingType(EASE_QUADRATIC_IN_OUT);
    Servo4.setEasingType(EASE_QUADRATIC_IN_OUT);

    delay(500);
}
void loop() {
    setSpeedForAllServos(100);
    __4PosTraj(80, 60, 100, 165);
    __4PosTraj(150, 60, 80, 165);
    __4PosTraj(150, 60, 80, 85);
    __4PosTraj(80, 60, 100, 85);
    __4PosTraj(10, 60, 80, 85);
    __4PosTraj(10, 60, 80, 165);
    __4PosTraj(80, 60, 100, 165);
    __4PosTraj(10, 60, 80, 165);
    __4PosTraj(10, 60, 80, 85);
    __4PosTraj(80, 60, 100, 85);
    __4PosTraj(150, 60, 80, 85);
    __4PosTraj(150, 60, 80, 165);
    __4PosTraj(80, 60, 100, 165);
    __4PosTraj(80, 60, 100, 85);
}

void __4PosTraj(int pos1, int pos2, int pos3, int pos4)
{
    setDegreeForAllServos(4, pos1, pos2, pos3, pos4);
    setEaseToForAllServos();
    synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below

    do {
        // here you can call your own program
        delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
    } while (!updateAllServos());
    delay(500);
}