timor2542

[4 DOF Robot Arm Keyestudio][Lab 10][ATX-2] 4DOF Robot Arm Move02

Aug 2nd, 2021
769
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(1000);
  15.   Arm4Open();
  16.   delay(1000);
  17.   Arm3StretchedOut();
  18.   delay(1000);
  19.   Arm4Close();
  20.   delay(1000);
  21.   Arm3DrawBack();
  22.   delay(1000);
  23.   Arm2Up();
  24.   delay(1000);
  25.   Arm1TurnLeft();
  26.   delay(1000);
  27.   Arm2Down();
  28.   delay(1000);
  29.   Arm3StretchedOut();
  30.   delay(1000);
  31.   Arm4Open();
  32.   delay(1000);
  33.   Arm3DrawBack();
  34.   delay(1000);
  35.   Arm1TurnRight();
  36.   delay(1000);
  37.   Arm4Open();
  38.   delay(1000);
  39.   Arm3StretchedOut();
  40.   delay(1000);
  41.   Arm4Close();
  42.   delay(1000);
  43.   Arm3DrawBack();
  44.   delay(1000);
  45.   Arm2Up();
  46.   delay(1000);
  47.   Arm1TurnRtoCenter();
  48.   delay(1000);
  49.   Arm2Down();
  50.   delay(1000);
  51.   Arm3StretchedOut();
  52.   delay(1000);
  53.   Arm4Open();
  54.   delay(1000);
  55.   Arm3DrawBack();
  56.   delay(1000);
  57.   Arm4Close();
  58.   delay(1000);
  59.  
  60. }
  61. void FourServoSet(signed int sv0_angle, signed int sv1_angle, signed int sv2_angle, signed int sv3_angle)
  62. {
  63.   servo(1, sv0_angle);
  64.   servo(2, sv1_angle);
  65.   servo(3, sv2_angle);
  66.   servo(4, sv3_angle);
  67. }
  68. void Arm1TurnLeft()
  69. {
  70.   for(pos1;pos1<115;pos1++)
  71.   {
  72.     servo(1, pos1);
  73.     delay(TIME);
  74.   }
  75. }
  76. void Arm1TurnRight()
  77. {
  78.   for(pos1;pos1>45;pos1--)
  79.   {
  80.     servo(1, pos1);
  81.     delay(TIME);
  82.   }
  83. }
  84. void Arm1TurnRtoCenter()
  85. {
  86.   for(pos1;pos1<80;pos1++)
  87.   {
  88.     servo(1, pos1);
  89.     delay(TIME);
  90.   }
  91. }
  92. void Arm2Up()
  93. {
  94.   for(pos2;pos2>30;pos2--)
  95.   {
  96.     servo(2, pos2);
  97.     delay(TIME);
  98.   }
  99. }
  100. void Arm2Down()
  101. {
  102.   for(pos2;pos2<60;pos2++)
  103.   {
  104.     servo(2, pos2);
  105.     delay(TIME);
  106.   }
  107. }
  108. void Arm3StretchedOut()
  109. {
  110.   for(pos3;pos3>25;pos3--)
  111.   {
  112.     servo(3, pos3);
  113.     delay(TIME);
  114.   }
  115. }
  116. void Arm3DrawBack()
  117. {
  118.   for(pos3;pos3<100;pos3++)
  119.   {
  120.     servo(3, pos3);
  121.     delay(TIME);
  122.   }
  123. }
  124. void Arm4Close()
  125. {
  126.   for(pos4;pos4>85;pos4--)
  127.   {
  128.     servo(4, pos4);
  129.     delay(TIME);
  130.   }
  131. }
  132. void Arm4Open()
  133. {
  134.   for(pos4;pos4<165;pos4++)
  135.   {
  136.     servo(4, pos4);
  137.     delay(TIME);
  138.   }
  139. }
  140.  
RAW Paste Data