timor2542

[4 DOF Robot Arm Keyestudio][Lab 04][AX-TinySAM][PCA9685] 4 Servo Demo Test

Aug 3rd, 2021
1,172
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Arduino.h>
  2. #include <PCA9685.h>
  3.  
  4. #include "Constants.h"
  5.  
  6.  
  7. #define TIME 7
  8. PCA9685 pca9685;
  9. int pos0 = 80;
  10. int pos1 = 60;
  11. int pos2 = 100;
  12. int pos3 = 73;
  13. void setup()
  14. {
  15.   pinMode(LED_BUILTIN, OUTPUT);
  16.   pinMode(3, OUTPUT);
  17.   pinMode(10, OUTPUT);
  18.  
  19.   digitalWrite(LED_BUILTIN, LOW);
  20.   servoPCA9685Init(0x40);
  21.   servoPCA9685Degree(0, -1);
  22.   servoPCA9685Degree(1, -1);
  23.   servoPCA9685Degree(2, -1);
  24.   servoPCA9685Degree(3, -1);
  25.   sw1_press();
  26.   servoPCA9685Degree(0, pos0);
  27.   servoPCA9685Degree(1, pos1);
  28.   servoPCA9685Degree(2, pos2);
  29.   servoPCA9685Degree(3, pos3);
  30.   delay(500);
  31.  
  32. }
  33.  
  34. void loop()
  35. {
  36.   sw1_press();
  37.   delay(400);
  38.   Arm1TurnRight();
  39.   delay(400);
  40.   Arm4Open();
  41.   delay(400);
  42.   Arm3StretchedOut();
  43.   delay(400);
  44.   Arm4Close();
  45.   delay(400);
  46.   Arm3DrawBack();
  47.   delay(400);
  48.   Arm2Up();
  49.   delay(400);
  50.   Arm1TurnLeft();
  51.   delay(400);
  52.   Arm2Down();
  53.   delay(400);
  54.   Arm3StretchedOut();
  55.   delay(400);
  56.   Arm4Open();
  57.   delay(400);
  58.   Arm3DrawBack();
  59.   delay(400);
  60.   Arm4Open();
  61.   delay(400);
  62.   Arm3StretchedOut();
  63.   delay(400);
  64.   Arm4Close();
  65.   delay(400);
  66.   Arm3DrawBack();
  67.     delay(400);
  68.   Arm2Up();
  69.   delay(400);
  70.   Arm1TurnRight();
  71.   delay(400);
  72.   Arm2Down();
  73.   delay(400);
  74.   Arm3StretchedOut();
  75.   delay(400);
  76.   Arm4Open();
  77.   delay(400);
  78.   Arm3DrawBack();
  79.   delay(400);
  80. }
  81. void sw1_press()
  82. {
  83.   while(analogRead(2)>10){}
  84.   while(analogRead(2)<=10){}
  85.  
  86.   tone(10, 1000, 100);
  87.   delay(100);
  88.  
  89.   noTone(10);
  90.   digitalWrite(LED_BUILTIN, HIGH);
  91.   for(int i = 0; i < 3; i++)
  92.   {
  93.     digitalWrite(3, HIGH);
  94.     delay(500);
  95.     digitalWrite(3, LOW);
  96.     delay(500);
  97.   }
  98.   digitalWrite(3, HIGH);
  99. }
  100. void servoPCA9685Init(const uint8_t _address)
  101. {
  102.   pca9685.setupSingleDevice(Wire,_address);
  103.   pca9685.setToServoFrequency();
  104. }
  105. void servoPCA9685Degree(uint8_t servo,signed int angle)
  106. {
  107.   if(servo == 0)
  108.   {
  109.     if(angle == -1)
  110.     {
  111.       pca9685.setChannelServoPulseDuration(0,0);
  112.     }
  113.     else
  114.     {
  115.       pca9685.setChannelServoPulseDuration(0,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  116.     }
  117.   }
  118.     if(servo == 1)
  119.   {
  120.     if(angle == -1)
  121.     {
  122.       pca9685.setChannelServoPulseDuration(1,0);
  123.     }
  124.     else
  125.     {
  126.       pca9685.setChannelServoPulseDuration(1,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  127.     }
  128.   }
  129.     if(servo == 2)
  130.   {
  131.     if(angle == -1)
  132.     {
  133.       pca9685.setChannelServoPulseDuration(2,0);
  134.     }
  135.     else
  136.     {
  137.       pca9685.setChannelServoPulseDuration(2,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  138.     }
  139.   }
  140.     if(servo == 3)
  141.   {
  142.     if(angle == -1)
  143.     {
  144.       pca9685.setChannelServoPulseDuration(3,0);
  145.     }
  146.     else
  147.     {
  148.       pca9685.setChannelServoPulseDuration(3,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  149.     }
  150.   }
  151.     if(servo == 4)
  152.   {
  153.     if(angle == -1)
  154.     {
  155.       pca9685.setChannelServoPulseDuration(4,0);
  156.     }
  157.     else
  158.     {
  159.       pca9685.setChannelServoPulseDuration(4,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  160.     }
  161.   }
  162.       if(servo == 5)
  163.   {
  164.     if(angle == -1)
  165.     {
  166.       pca9685.setChannelServoPulseDuration(5,0);
  167.     }
  168.     else
  169.     {
  170.       pca9685.setChannelServoPulseDuration(5,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  171.     }
  172.   }
  173. }
  174. void Arm1TurnLeft()
  175. {
  176.   for(pos0;pos0<150;pos0++)
  177.   {
  178.     servoPCA9685Degree(0, pos0);
  179.     delay(TIME);
  180.   }
  181. }
  182. void Arm1TurnRight()
  183. {
  184.   for(pos0;pos0>10;pos0--)
  185.   {
  186.     servoPCA9685Degree(0, pos0);
  187.     delay(TIME);
  188.   }
  189. }
  190. void Arm2Up()
  191. {
  192.   for(pos1;pos1>30;pos1--)
  193.   {
  194.     servoPCA9685Degree(1, pos1);
  195.     delay(TIME);
  196.   }
  197. }
  198. void Arm2Down()
  199. {
  200.   for(pos1;pos1<60;pos1++)
  201.   {
  202.     servoPCA9685Degree(1, pos1);
  203.     delay(TIME);
  204.   }
  205. }
  206. void Arm3StretchedOut()
  207. {
  208.   for(pos2;pos2>80;pos2--)
  209.   {
  210.     servoPCA9685Degree(2, pos2);
  211.     delay(TIME);
  212.   }
  213. }
  214. void Arm3DrawBack()
  215. {
  216.   for(pos2;pos2<100;pos2++)
  217.   {
  218.     servoPCA9685Degree(2, pos2);
  219.     delay(TIME);
  220.   }
  221. }
  222. void Arm4Close()
  223. {
  224.   for(pos3;pos3>73;pos3--)
  225.   {
  226.     servoPCA9685Degree(3, pos3);
  227.   }
  228. }
  229. void Arm4Open()
  230. {
  231.   for(pos3;pos3<165;pos3++)
  232.   {
  233.     servoPCA9685Degree(3, pos3);
  234.   }
  235. }
RAW Paste Data