timor2542

[4 DOF Robot Arm Keyestudio][Lab 13][ATX-2] Simple Trajectory

Jul 25th, 2021
1,168
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. /*
  3.  * SymmetricEasing.cpp
  4.  *
  5.  *  Shows symmetric (end movement is mirror of start movement) linear, quadratic and cubic movements for 3 servos synchronously.
  6.  *
  7.  *  Copyright (C) 2019-2021  Armin Joachimsmeyer
  8.  *  armin.joachimsmeyer@gmail.com
  9.  *
  10.  *  This file is part of ServoEasing https://github.com/ArminJo/ServoEasing.
  11.  *
  12.  *  ServoEasing is free software: you can redistribute it and/or modify
  13.  *  it under the terms of the GNU General Public License as published by
  14.  *  the Free Software Foundation, either version 3 of the License, or
  15.  *  (at your option) any later version.
  16.  *
  17.  *  This program is distributed in the hope that it will be useful,
  18.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  19.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  20.  *  GNU General Public License for more details.
  21.  *
  22.  *  You should have received a copy of the GNU General Public License
  23.  *  along with this program.  If not, see <http://www.gnu.org/licenses/gpl.html>.
  24.  */
  25.  
  26.  
  27. /*
  28.  * To generate the Arduino plotter output, you must activate the line #define PRINT_FOR_SERIAL_PLOTTER in ServoEasing.h
  29.  */
  30.  
  31. /*
  32.  * Pin mapping table for different platforms
  33.  *
  34.  * Platform     Servo1      Servo2      Servo3      Analog
  35.  * -------------------------------------------------------
  36.  * AVR + SAMD   9           10          11          A0
  37.  * ESP8266      14 // D5    12 // D6    13 // D7    0
  38.  * ESP32        5           18          19          A0
  39.  * BluePill     PB7         PB8         PB9         PA0
  40.  * APOLLO3      11          12          13          A3
  41.  * ATX-2        13          14          15          knob
  42.  */
  43.  
  44. #include <ATX2.h>
  45. #include "ServoEasing.h"
  46.  
  47. #define SERVO1_PIN 13
  48. #define SERVO2_PIN 14
  49. #define SERVO3_PIN 15
  50. #define SERVOCH4 4
  51.  
  52. #define START_SERVO1_DEGREE_VALUE 80
  53. #define START_SERVO2_DEGREE_VALUE 60
  54. #define START_SERVO3_DEGREE_VALUE 100
  55. #define START_SERVO4_DEGREE_VALUE 85
  56.  
  57. ServoEasing Servo1;
  58. ServoEasing Servo2;
  59. ServoEasing Servo3;
  60. // ServoEasing Servo4;
  61.  
  62. #define START_DEGREE_VALUE 90
  63.  
  64. void setup() {
  65.     OK();
  66.     glcdClear();
  67.     glcdMode(1);
  68.     setTextSize(2);
  69.  
  70.     setTextColor(GLCD_WHITE);
  71.     glcd(0,0,"Check status.");
  72.     glcd(1,0,"UART0:");
  73.     glcd(2,0,"Servo1:");
  74.     glcd(3,0,"Servo2:");
  75.     glcd(4,0,"Servo3:");
  76.     glcd(5,0,"Servo4:");
  77.     setTextColor(GLCD_WHITE);
  78.     Serial.begin(115200);
  79.     /************************************************************
  80.      * Attach servo to pin and set servos to start position.
  81.      * This is the position where the movement starts.
  82.      *
  83.      * The order of the attach() determine the position
  84.      * of the Servos in internal ServoEasing::ServoEasingArray[]
  85.      ***********************************************************/
  86.      setTextColor(GLCD_YELLOW);
  87.      glcd(1,6,"Waiting");
  88.      while(!Serial){
  89.       }
  90.      setTextColor(GLCD_GREEN);
  91.      glcd(1,6," PASS  ");
  92.      setTextColor(GLCD_WHITE);
  93. #ifndef PRINT_FOR_SERIAL_PLOTTER
  94.     Serial.print(F("Attach servo at pin "));
  95.     Serial.println(SERVO1_PIN);
  96. #endif
  97.     if (Servo1.attach(SERVO1_PIN, START_SERVO1_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  98.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  99.         Serial.println(F("Error attaching servo"));
  100.         setTextColor(GLCD_RED);
  101.         glcd(2,7,"FAIL");
  102.         while(1){}
  103.     }
  104.     setTextColor(GLCD_GREEN);
  105.     glcd(2,7,"PASS  ");
  106.  
  107. #ifndef PRINT_FOR_SERIAL_PLOTTER
  108.     Serial.print(F("Attach servo at pin "));
  109.     Serial.println(SERVO2_PIN);
  110. #endif
  111.     if (Servo2.attach(SERVO2_PIN, START_SERVO2_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  112.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  113.         Serial.println(F("Error attaching servo"));
  114.         setTextColor(GLCD_RED);
  115.         glcd(3,7,"FAIL");
  116.         while(1){}
  117.     }
  118.     setTextColor(GLCD_GREEN);
  119.     glcd(3,7,"PASS  ");
  120.     /*
  121.      * Check at least the last call to attach()
  122.      */
  123. #ifndef PRINT_FOR_SERIAL_PLOTTER
  124.     Serial.print(F("Attach servo at pin "));
  125.     Serial.println(SERVO3_PIN);
  126. #endif
  127.     if (Servo3.attach(SERVO3_PIN, START_SERVO3_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  128.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  129.         Serial.println(F("Error attaching servo"));
  130.         setTextColor(GLCD_RED);
  131.         glcd(4,7,"FAIL");
  132.         while(1){}
  133.     }
  134.     setTextColor(GLCD_GREEN);
  135.     glcd(4,7,"PASS  ");
  136.     // Wait for servos to reach start position.
  137.     servo(4, START_SERVO4_DEGREE_VALUE);
  138.     glcd(5,7,"PASS  ");
  139.     delay(2000);
  140.  
  141. #ifdef PRINT_FOR_SERIAL_PLOTTER
  142.     // Legend for Arduino Serial plotter
  143.     Serial.println();
  144.     Serial.println("Linear, Quadratic, Cubic");
  145. #endif
  146.  
  147. #ifndef PRINT_FOR_SERIAL_PLOTTER
  148.     Serial.println(F("Move from 90 to 45 degree in 1 second"));
  149. #endif
  150.     Servo1.startEaseToD(80, 1000);
  151.     Servo2.startEaseToD(60, 1000);
  152.     Servo3.startEaseToD(100, 1000);
  153.     delay(1000);
  154.  
  155.     Servo1.setEasingType(EASE_QUADRATIC_IN_OUT);
  156.     Servo2.setEasingType(EASE_QUADRATIC_IN_OUT);
  157.     Servo3.setEasingType(EASE_QUADRATIC_IN_OUT);
  158.  
  159.     delay(500);
  160. }
  161.  
  162. void loop() {
  163.     uint16_t tSpeed = knob(100, 150);
  164.     setSpeedForAllServos(tSpeed);
  165.  
  166.     /*
  167.      * Move three servos synchronously without interrupt handler
  168.      */
  169.     /*
  170.      * Here we use the allServos functions
  171.      */
  172.     setDegreeForAllServos(3, 135, 60, 30);
  173.     setEaseToForAllServos();
  174.     synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below
  175.     servo(4, 165);
  176.     do {
  177.         // here you can call your own program
  178.         delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
  179.     } while (!updateAllServos());
  180.     servo(4, -1);
  181.     delay(500);
  182.     servo(4, 85);
  183.     delay(500);
  184.     setDegreeForAllServos(3, 80, 60, 100);
  185.     setEaseToForAllServos();
  186.     synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below
  187.  
  188.     do {
  189.         // here you can call your own program
  190.         delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
  191.     } while (!updateAllServos());
  192.    
  193.     setDegreeForAllServos(3, 25, 60, 30);
  194.     setEaseToForAllServos();
  195.     synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below
  196.  
  197.     do {
  198.         // here you can call your own program
  199.         delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
  200.     } while (!updateAllServos());
  201.     delay(500);
  202.     servo(4, 165);
  203.     delay(500);
  204.     servo(4, -1);
  205.     delay(500);
  206.    
  207.     setDegreeForAllServos(3, 80, 60, 130);
  208.     setEaseToForAllServos();
  209.     synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below
  210.  
  211.     do {
  212.         // here you can call your own program
  213.         delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
  214.     } while (!updateAllServos());
  215.     delay(500);
  216.     servo(4, 85);
  217.     delay(500);
  218.     servo(4, -1);
  219.     delay(500);
  220.  
  221. }
RAW Paste Data