timor2542

[4 DOF Robot Arm Keyestudio][Lab 08][ATX-2] Gripper Move Auto Test

Aug 2nd, 2021
741
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ATX2.h>
  2. #define TIME 10
  3. signed int pos1=80, pos2=60, pos3=100, pos4=85; // กำหนดค่าเริ่มต้นตำแหน่งแกนมุมของเซอร์โวมอเตอร์ของแต่ละตัว
  4. void setup() {
  5.   OK();
  6.   delay(3000);
  7.   FourServoSet(pos1, pos2, pos3, pos4);
  8.   delay(3000);
  9. }
  10.  
  11.  
  12. void loop() {
  13.   OK();
  14.   delay(1500);
  15.   Arm4Open();
  16.   delay(1500);
  17.   Arm3StretchedOut();
  18.   delay(1500);
  19.   Arm4Close();
  20.   delay(1500);
  21.   Arm3DrawBack();
  22.   delay(1500);
  23.   Arm2Up();
  24.   delay(1500);
  25.   Arm2Down();
  26.   delay(1500);
  27.   Arm3StretchedOut();
  28.   delay(1500);
  29.   Arm4Open();
  30.   delay(1500);
  31.   Arm3DrawBack();
  32.   delay(1500);
  33.   Arm4Close();
  34.   delay(1500);
  35. }
  36. void FourServoSet(signed int sv0_angle, signed int sv1_angle, signed int sv2_angle, signed int sv3_angle)
  37. {
  38.   servo(1, sv0_angle);
  39.   servo(2, sv1_angle);
  40.   servo(3, sv2_angle);
  41.   servo(4, sv3_angle);
  42. }
  43. void Arm2Up()
  44. {
  45.   for(pos2;pos2>30;pos2--)
  46.   {
  47.     servo(2, pos2);
  48.     delay(TIME);
  49.   }
  50. }
  51. void Arm2Down()
  52. {
  53.   for(pos2;pos2<60;pos2++)
  54.   {
  55.     servo(2, pos2);
  56.     delay(TIME);
  57.   }
  58. }
  59. void Arm3StretchedOut()
  60. {
  61.   for(pos3;pos3>30;pos3--)
  62.   {
  63.     servo(3, pos3);
  64.     delay(TIME);
  65.   }
  66. }
  67. void Arm3DrawBack()
  68. {
  69.   for(pos3;pos3<100;pos3++)
  70.   {
  71.     servo(3, pos3);
  72.     delay(TIME);
  73.   }
  74. }
  75. void Arm4Close()
  76. {
  77.   for(pos4;pos4>85;pos4--)
  78.   {
  79.     servo(4, pos4);
  80.     delay(TIME);
  81.   }
  82. }
  83. void Arm4Open()
  84. {
  85.   for(pos4;pos4<165;pos4++)
  86.   {
  87.     servo(4, pos4);
  88.     delay(TIME);
  89.   }
  90. }
  91.  
RAW Paste Data