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

#include <ATX2.h>
#define TIME 10
signed int pos1=80, pos2=60, pos3=100, pos4=85; // กำหนดค่าเริ่มต้นตำแหน่งแกนมุมของเซอร์โวมอเตอร์ของแต่ละตัว
void setup() {
  OK();
  delay(3000);
  FourServoSet(pos1, pos2, pos3, pos4);
  delay(3000);
}


void loop() {
  OK();
  delay(1000);
  Arm4Open();
  delay(1000);
  Arm3StretchedOut();
  delay(1000);
  Arm4Close();
  delay(1000);
  Arm3DrawBack();
  delay(1000);
  Arm2Up();
  delay(1000);
  Arm1TurnLeft();
  delay(1000);
  Arm2Down();
  delay(1000);
  Arm3StretchedOut();
  delay(1000);
  Arm4Open();
  delay(1000);
  Arm3DrawBack();
  delay(1000);
  Arm1TurnRight();
  delay(1000);
  Arm4Open();
  delay(1000);
  Arm3StretchedOut();
  delay(1000);
  Arm4Close();
  delay(1000);
  Arm3DrawBack();
  delay(1000);
  Arm2Up();
  delay(1000);
  Arm1TurnRtoCenter();
  delay(1000);
  Arm2Down();
  delay(1000);
  Arm3StretchedOut();
  delay(1000);
  Arm4Open();
  delay(1000);
  Arm3DrawBack();
  delay(1000);
  Arm4Close();
  delay(1000);

}
void FourServoSet(signed int sv0_angle, signed int sv1_angle, signed int sv2_angle, signed int sv3_angle)
{
  servo(1, sv0_angle);
  servo(2, sv1_angle);
  servo(3, sv2_angle);
  servo(4, sv3_angle);
}
void Arm1TurnLeft()
{
  for(pos1;pos1<115;pos1++)
  {
    servo(1, pos1);
    delay(TIME);
  }
}
void Arm1TurnRight()
{
  for(pos1;pos1>45;pos1--)
  {
    servo(1, pos1);
    delay(TIME);
  }
}
void Arm1TurnRtoCenter()
{
  for(pos1;pos1<80;pos1++)
  {
    servo(1, pos1);
    delay(TIME);
  }
}
void Arm2Up()
{
  for(pos2;pos2>30;pos2--)
  {
    servo(2, pos2);
    delay(TIME);
  }
}
void Arm2Down()
{
  for(pos2;pos2<60;pos2++)
  {
    servo(2, pos2);
    delay(TIME);
  }
}
void Arm3StretchedOut()
{
  for(pos3;pos3>25;pos3--)
  {
    servo(3, pos3);
    delay(TIME);
  }
}
void Arm3DrawBack()
{
  for(pos3;pos3<100;pos3++)
  {
    servo(3, pos3);
    delay(TIME);
  }
}
void Arm4Close()
{
  for(pos4;pos4>85;pos4--)
  {
    servo(4, pos4);
    delay(TIME);
  }
}
void Arm4Open()
{
  for(pos4;pos4<165;pos4++)
  {
    servo(4, pos4);
    delay(TIME);
  }
}