timor2542

[4 DOF Robot Arm Keyestudio][Lab 09][ATX-2] 4DOF Robot Arm Move01

Aug 2nd, 2021
759
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(1000);
  9. }
  10.  
  11.  
  12. void loop() {
  13.   OK();
  14.   delay(1500);
  15.   Arm1TurnRight();
  16.   delay(1500);
  17.   Arm4Open();
  18.   delay(1500);
  19.   Arm3StretchedOut();
  20.   delay(1500);
  21.   Arm4Close();
  22.   delay(1500);
  23.   Arm3DrawBack();
  24.   delay(1500);
  25.   Arm2Up();
  26.   delay(1500);
  27.   Arm1TurnLeft();
  28.   delay(1500);
  29.   Arm2Down();
  30.   delay(1500);
  31.   Arm3StretchedOut();
  32.   delay(1500);
  33.   Arm4Open();
  34.   delay(1500);
  35.   Arm3DrawBack();
  36.   delay(1500);
  37. }
  38. void FourServoSet(signed int sv0_angle, signed int sv1_angle, signed int sv2_angle, signed int sv3_angle)
  39. {
  40.   servo(1, sv0_angle);
  41.   servo(2, sv1_angle);
  42.   servo(3, sv2_angle);
  43.   servo(4, sv3_angle);
  44. }
  45. void Arm1TurnLeft()
  46. {
  47.   for(pos1;pos1<115;pos1++)
  48.   {
  49.     servo(1, pos1);
  50.     delay(TIME);
  51.   }
  52. }
  53. void Arm1TurnRight()
  54. {
  55.   for(pos1;pos1>45;pos1--)
  56.   {
  57.     servo(1, pos1);
  58.     delay(TIME);
  59.   }
  60. }
  61. void Arm2Up()
  62. {
  63.   for(pos2;pos2>30;pos2--)
  64.   {
  65.     servo(2, pos2);
  66.     delay(TIME);
  67.   }
  68. }
  69. void Arm2Down()
  70. {
  71.   for(pos2;pos2<60;pos2++)
  72.   {
  73.     servo(2, pos2);
  74.     delay(TIME);
  75.   }
  76. }
  77. void Arm3StretchedOut()
  78. {
  79.   for(pos3;pos3>30;pos3--)
  80.   {
  81.     servo(3, pos3);
  82.     delay(TIME);
  83.   }
  84. }
  85. void Arm3DrawBack()
  86. {
  87.   for(pos3;pos3<100;pos3++)
  88.   {
  89.     servo(3, pos3);
  90.     delay(TIME);
  91.   }
  92. }
  93. void Arm4Close()
  94. {
  95.   for(pos4;pos4>85;pos4--)
  96.   {
  97.     servo(4, pos4);
  98.     delay(TIME);
  99.   }
  100. }
  101. void Arm4Open()
  102. {
  103.   for(pos4;pos4<165;pos4++)
  104.   {
  105.     servo(4, pos4);
  106.     delay(TIME);
  107.   }
  108. }
  109.  
RAW Paste Data