timor2542

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

Aug 3rd, 2021
853
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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.  *  armin.joachimsmeyer@gmail.com
  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.  
RAW Paste Data