[4 DOF Robot Arm Keyestudio][Lab 13][ATX-2] 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 <ATX2.h>
#include "ServoEasing.h"

#define SERVO1_PIN 13
#define SERVO2_PIN 14
#define SERVO3_PIN 15
#define SERVOCH4 4

#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;

#define START_DEGREE_VALUE 90

void setup() {
    OK();
    glcdClear();
    glcdMode(1);
    setTextSize(2);

    setTextColor(GLCD_WHITE);
    glcd(0,0,"Check status.");
    glcd(1,0,"UART0:");
    glcd(2,0,"Servo1:");
    glcd(3,0,"Servo2:");
    glcd(4,0,"Servo3:");
    glcd(5,0,"Servo4:");
    setTextColor(GLCD_WHITE);
    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[]
     ***********************************************************/
     setTextColor(GLCD_YELLOW);
     glcd(1,6,"Waiting");
     while(!Serial){
      }
     setTextColor(GLCD_GREEN);
     glcd(1,6," PASS  ");
     setTextColor(GLCD_WHITE);
#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 servo"));
        setTextColor(GLCD_RED);
        glcd(2,7,"FAIL");
        while(1){}
    }
    setTextColor(GLCD_GREEN);
    glcd(2,7,"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 servo"));
        setTextColor(GLCD_RED);
        glcd(3,7,"FAIL");
        while(1){}
    }
    setTextColor(GLCD_GREEN);
    glcd(3,7,"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 servo"));
        setTextColor(GLCD_RED);
        glcd(4,7,"FAIL");
        while(1){}
    }
    setTextColor(GLCD_GREEN);
    glcd(4,7,"PASS  ");
    // Wait for servos to reach start position.
    servo(4, START_SERVO4_DEGREE_VALUE);
    glcd(5,7,"PASS  ");
    delay(2000);

#ifdef PRINT_FOR_SERIAL_PLOTTER
    // Legend for Arduino Serial plotter
    Serial.println();
    Serial.println("Linear, Quadratic, Cubic");
#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);
    delay(1000);

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

    delay(500);
}

void loop() {
    uint16_t tSpeed = knob(100, 150);
    setSpeedForAllServos(tSpeed);

    /*
     * Move three servos synchronously without interrupt handler
     */
    /*
     * Here we use the allServos functions
     */
    setDegreeForAllServos(3, 135, 60, 30);
    setEaseToForAllServos();
    synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below
    servo(4, 165);
    do {
        // here you can call your own program
        delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
    } while (!updateAllServos());
    servo(4, -1);
    delay(500);
    servo(4, 85);
    delay(500);
    setDegreeForAllServos(3, 80, 60, 100);
    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());

    setDegreeForAllServos(3, 25, 60, 30);
    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);
    servo(4, 165);
    delay(500);
    servo(4, -1);
    delay(500);

    setDegreeForAllServos(3, 80, 60, 130);
    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);
    servo(4, 85);
    delay(500);
    servo(4, -1);
    delay(500);

}