timor2542

[4 DOF Robot Arm Keyestudio][Lab 02][AX-TinySAM][PCA9685] Servo 1 Test

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