API tools faq
paste
Advertisement
timor2542

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

timor2542
Aug 3rd, 2021
1,285
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 5.83 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.  * SymmetricEasing.cpp
  3.  *
  4.  *  Shows symmetric (end movement is mirror of start movement) linear, quadratic and cubic movements for 3 servos synchronously.
  5.  *
  6.  *  Copyright (C) 2019-2021  Armin Joachimsmeyer
  7.  *  [email protected]
  8.  *
  9.  *  This file is part of ServoEasing https://github.com/ArminJo/ServoEasing.
  10.  *
  11.  *  ServoEasing is free software: you can redistribute it and/or modify
  12.  *  it under the terms of the GNU General Public License as published by
  13.  *  the Free Software Foundation, either version 3 of the License, or
  14.  *  (at your option) any later version.
  15.  *
  16.  *  This program is distributed in the hope that it will be useful,
  17.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  18.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19.  *  GNU General Public License for more details.
  20.  *
  21.  *  You should have received a copy of the GNU General Public License
  22.  *  along with this program.  If not, see <http://www.gnu.org/licenses/gpl.html>.
  23.  */
  24.  
  25.  
  26. /*
  27.  * To generate the Arduino plotter output, you must activate the line #define PRINT_FOR_SERIAL_PLOTTER in ServoEasing.h
  28.  */
  29.  
  30. /*
  31.  * Pin mapping table for different platforms
  32.  *
  33.  * Platform     Servo1      Servo2      Servo3      Analog
  34.  * -------------------------------------------------------
  35.  * AVR + SAMD   9           10          11          A0
  36.  * ESP8266      14 // D5    12 // D6    13 // D7    0
  37.  * ESP32        5           18          19          A0
  38.  * BluePill     PB7         PB8         PB9         PA0
  39.  * APOLLO3      11          12          13          A3
  40.  * ATX-2        13          14          15          knob
  41.  */
  42.  
  43. #include "ServoEasing.h"
  44.  
  45. #define SERVO1_PIN A1
  46. #define SERVO2_PIN A0
  47. #define SERVO3_PIN 6
  48. #define SERVO4_PIN 9
  49.  
  50. #define START_SERVO1_DEGREE_VALUE 80
  51. #define START_SERVO2_DEGREE_VALUE 60
  52. #define START_SERVO3_DEGREE_VALUE 100
  53. #define START_SERVO4_DEGREE_VALUE 85
  54.  
  55. ServoEasing Servo1;
  56. ServoEasing Servo2;
  57. ServoEasing Servo3;
  58. ServoEasing Servo4;
  59. // ServoEasing Servo4;
  60.  
  61. #define START_DEGREE_VALUE 90
  62.  
  63. void setup() {
  64.     delay(5000);
  65.     Serial.begin(115200);
  66.     /************************************************************
  67.      * Attach servo to pin and set servos to start position.
  68.      * This is the position where the movement starts.
  69.      *
  70.      * The order of the attach() determine the position
  71.      * of the Servos in internal ServoEasing::ServoEasingArray[]
  72.      ***********************************************************/
  73.      Serial.println(F("Waiting..."));
  74.      while(!Serial){
  75.       }
  76.      Serial.println(F("OK."));
  77. #ifndef PRINT_FOR_SERIAL_PLOTTER
  78.     Serial.print(F("Attach servo at pin "));
  79.     Serial.println(SERVO1_PIN);
  80. #endif
  81.     if (Servo1.attach(SERVO1_PIN, START_SERVO1_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  82.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  83.         Serial.println(F("Error attaching Servo1"));
  84.         while(1){}
  85.     }
  86.     Serial.println(F("Servo1: PASS"));
  87.    
  88. #ifndef PRINT_FOR_SERIAL_PLOTTER
  89.     Serial.print(F("Attach servo at pin "));
  90.     Serial.println(SERVO2_PIN);
  91. #endif
  92.     if (Servo2.attach(SERVO2_PIN, START_SERVO2_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  93.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  94.         Serial.println(F("Error attaching Servo2"));
  95.         while(1){}
  96.     }
  97.     Serial.println(F("Servo2: PASS"));
  98.     /*
  99.      * Check at least the last call to attach()
  100.      */
  101. #ifndef PRINT_FOR_SERIAL_PLOTTER
  102.     Serial.print(F("Attach servo at pin "));
  103.     Serial.println(SERVO3_PIN);
  104. #endif
  105.     if (Servo3.attach(SERVO3_PIN, START_SERVO3_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  106.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  107.         Serial.println(F("Error attaching Servo3"));
  108.         while(1){}
  109.     }
  110.     Serial.println(F("Servo3: PASS"));
  111.    
  112. #ifndef PRINT_FOR_SERIAL_PLOTTER
  113.     Serial.print(F("Attach servo at pin "));
  114.     Serial.println(SERVO3_PIN);
  115. #endif
  116.     if (Servo4.attach(SERVO4_PIN, START_SERVO4_DEGREE_VALUE, DEFAULT_MICROSECONDS_FOR_0_DEGREE,
  117.     DEFAULT_MICROSECONDS_FOR_180_DEGREE) == INVALID_SERVO) {
  118.         Serial.println(F("Error attaching Servo4"));
  119.         while(1){}
  120.     }
  121.     Serial.println(F("Servo4: PASS"));
  122.     delay(2000);
  123.  
  124. #ifdef PRINT_FOR_SERIAL_PLOTTER
  125.     // Legend for Arduino Serial plotter
  126.     Serial.println();
  127.     Serial.println("Quadratic");
  128. #endif
  129.  
  130. #ifndef PRINT_FOR_SERIAL_PLOTTER
  131.     Serial.println(F("Move from 90 to 45 degree in 1 second"));
  132. #endif
  133.     Servo1.startEaseToD(80, 1000);
  134.     Servo2.startEaseToD(60, 1000);
  135.     Servo3.startEaseToD(100, 1000);
  136.     Servo4.startEaseToD(85, 1000);
  137.     delay(1000);
  138.  
  139.     Servo1.setEasingType(EASE_QUADRATIC_IN_OUT);
  140.     Servo2.setEasingType(EASE_QUADRATIC_IN_OUT);
  141.     Servo3.setEasingType(EASE_QUADRATIC_IN_OUT);
  142.     Servo4.setEasingType(EASE_QUADRATIC_IN_OUT);
  143.  
  144.     delay(500);
  145. }
  146. void loop() {
  147.     setSpeedForAllServos(100);
  148.     __4PosTraj(80, 60, 100, 165);
  149.     __4PosTraj(150, 60, 80, 165);
  150.     __4PosTraj(150, 60, 80, 85);
  151.     __4PosTraj(80, 60, 100, 85);
  152.     __4PosTraj(10, 60, 80, 85);
  153.     __4PosTraj(10, 60, 80, 165);
  154.     __4PosTraj(80, 60, 100, 165);
  155.     __4PosTraj(10, 60, 80, 165);
  156.     __4PosTraj(10, 60, 80, 85);
  157.     __4PosTraj(80, 60, 100, 85);
  158.     __4PosTraj(150, 60, 80, 85);
  159.     __4PosTraj(150, 60, 80, 165);
  160.     __4PosTraj(80, 60, 100, 165);
  161.     __4PosTraj(80, 60, 100, 85);
  162. }
  163.  
  164. void __4PosTraj(int pos1, int pos2, int pos3, int pos4)
  165. {
  166.     setDegreeForAllServos(4, pos1, pos2, pos3, pos4);
  167.     setEaseToForAllServos();
  168.     synchronizeAllServosAndStartInterrupt(false); // false, since we call updateAllServos() manually below
  169.  
  170.     do {
  171.         // here you can call your own program
  172.         delay(REFRESH_INTERVAL / 1000); // optional 20ms delay - REFRESH_INTERVAL is in Microseconds
  173.     } while (!updateAllServos());
  174.     delay(500);
  175. }
  176.  
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!