API tools faq
paste
Advertisement
timor2542

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

timor2542
Jul 25th, 2021
1,426
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 7.05 KB | None | 0 0
raw download clone embed print report
  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.  *  [email protected]
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!