[4 DOF Robot Arm Keyestudio][Lab 09][i-Duino UNO R3B] Bluetooth HC-06 Test on 4DOF Robot Arm

#include <Servo.h>  // add the servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;
int pos1=80, pos2=60, pos3=100, pos4=85;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
char val;

void setup()
{
   // boot posture
  myservo1.write(pos1);
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);

  Serial.begin(9600); //  set the baud rate to 9600
}

void loop()
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   // set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9

  if(Serial.available())   // if receive the data
  {
    val=Serial.read();    // read the received data
    Serial.println(val);
    switch(val)
    {
      case 0x38:  Arm2LiftUp();  break;   // execute the corresponding function when receive the value
      case 0x45:  Arm1TurnLeft();  break;
      case 0x46:  Arm1TurnRight();  break;
      case 0x32:  Arm2GoDown();  break;
      case 0x36:  Arm3DrawBack();  break;
      case 0x44:  Arm4OpenClaw();  break;
      case 0x43:  Arm4CloseClaw();  break;
      case 0x34:  Arm3StretchedOut();  break;
    }
  }
}
//**************************************************
// turn left
void Arm1TurnLeft()
{
    pos1 += 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 10 องศา
    pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
    myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
    delay(5);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}
//turn right
void Arm1TurnRight()
{
    pos1 -= 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 10 องศา
    pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
    myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
    delay(5);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}
//********************************************
//open the claw
void Arm4OpenClaw()
{
      pos4 = 165;
      myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
      delay(10); // หน่วงเวลา 10 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}
// close the claw
void Arm4CloseClaw()
{
      pos4 = 85;
      myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
      delay(10); // หน่วงเวลา 10 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}

//******************************************
// the upper arm will lift up
void Arm2LiftUp()
{
    pos2 -= 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
    pos2 = (pos2 < 30) ? 30 : pos2; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
    myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ ยกขึ้น
    delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}
// the upper arm will go down
void Arm2GoDown()
{
    pos2 += 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 เพิ่มขึ้นทีละ 1 องศา
    pos2 = (pos2 > 90) ? 90 : pos2; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
    myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ วางลง
    delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}

//***************************************
// the lower arm will stretch out
void Arm3DrawBack()
{
    pos3 -= 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
    pos3 = (pos3 < 30) ? 30 : pos3; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 30 องศา
    myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ ยืดออก
    delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}
// the lower arm will draw back
void Arm3StretchedOut()
{
    pos3 += 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 3 เพิ่มขึ้นทีละ 1 องศา
    pos3 = (pos3 > 100) ? 100 : pos3; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 100 องศา
    myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ หดกลับ
    delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
}