timor2542

[4 DOF Robot Arm Keyestudio][Lab 03][AX-TinySAM][PCA9685] Servo 2 and 3 Test

Aug 3rd, 2021
865
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. PCA9685 pca9685;
  8. int pos1 = 60;
  9. int pos2 = 100;
  10. //uint16_t servo_pulse_duration;
  11.  
  12. void setup()
  13. {
  14.   pinMode(LED_BUILTIN, OUTPUT);
  15.   pinMode(3, OUTPUT);
  16.   pinMode(10, OUTPUT);
  17.  
  18.   digitalWrite(LED_BUILTIN, LOW);
  19.   sw1_press();
  20.   servoPCA9685Init(0x40);
  21.   servoPCA9685Degree(1, pos1);
  22.   servoPCA9685Degree(2, pos2);
  23.   delay(1000);
  24.  
  25. }
  26.  
  27. void loop()
  28. {
  29.   for(pos2; pos2 > 30; pos2--)
  30.   {
  31.     servoPCA9685Degree(2, pos2);
  32.     delay(7);
  33.   }
  34.   delay(1000);
  35.     for(pos2 = 30; pos2 < 100; pos2++)
  36.   {
  37.     servoPCA9685Degree(2, pos2);
  38.     delay(7);
  39.   }
  40.   delay(1000);
  41. }
  42. void sw1_press()
  43. {
  44.   while(analogRead(2)>10){}
  45.   while(analogRead(2)<=10){}
  46.  
  47.   tone(10, 1000, 100);
  48.   delay(100);
  49.  
  50.   noTone(10);
  51.   digitalWrite(LED_BUILTIN, HIGH);
  52.   for(int i = 0; i < 3; i++)
  53.   {
  54.     digitalWrite(3, HIGH);
  55.     delay(500);
  56.     digitalWrite(3, LOW);
  57.     delay(500);
  58.   }
  59.   digitalWrite(3, HIGH);
  60. }
  61. void servoPCA9685Init(const uint8_t _address)
  62. {
  63.   pca9685.setupSingleDevice(Wire,_address);
  64.   pca9685.setToServoFrequency();
  65. }
  66. void servoPCA9685Degree(uint8_t servo,signed int angle)
  67. {
  68.   if(servo == 0)
  69.   {
  70.     if(angle == -1)
  71.     {
  72.       pca9685.setChannelServoPulseDuration(0,0);
  73.     }
  74.     else
  75.     {
  76.       pca9685.setChannelServoPulseDuration(0,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  77.     }
  78.   }
  79.     if(servo == 1)
  80.   {
  81.     if(angle == -1)
  82.     {
  83.       pca9685.setChannelServoPulseDuration(1,0);
  84.     }
  85.     else
  86.     {
  87.       pca9685.setChannelServoPulseDuration(1,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  88.     }
  89.   }
  90.     if(servo == 2)
  91.   {
  92.     if(angle == -1)
  93.     {
  94.       pca9685.setChannelServoPulseDuration(2,0);
  95.     }
  96.     else
  97.     {
  98.       pca9685.setChannelServoPulseDuration(2,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  99.     }
  100.   }
  101.     if(servo == 3)
  102.   {
  103.     if(angle == -1)
  104.     {
  105.       pca9685.setChannelServoPulseDuration(3,0);
  106.     }
  107.     else
  108.     {
  109.       pca9685.setChannelServoPulseDuration(3,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  110.     }
  111.   }
  112.     if(servo == 4)
  113.   {
  114.     if(angle == -1)
  115.     {
  116.       pca9685.setChannelServoPulseDuration(4,0);
  117.     }
  118.     else
  119.     {
  120.       pca9685.setChannelServoPulseDuration(4,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  121.     }
  122.   }
  123.       if(servo == 5)
  124.   {
  125.     if(angle == -1)
  126.     {
  127.       pca9685.setChannelServoPulseDuration(5,0);
  128.     }
  129.     else
  130.     {
  131.       pca9685.setChannelServoPulseDuration(5,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
  132.     }
  133.   }
  134. }
  135.  
RAW Paste Data