[4 DOF Robot Arm Keyestudio][Lab 07][i-Duino UNO R3B] Basic for Joystick Module Control

#include <Servo.h>  // ผนวกไลบารี่การใช้งานเซอร์โวมอเตอร์
Servo myservo1;     // ประกาศตัวแปรแทน Servo ตัวที่ 1
Servo myservo2;     // ประกาศตัวแปรแทน Servo ตัวที่ 2
Servo myservo3;     // ประกาศตัวแปรแทน Servo ตัวที่ 3
Servo myservo4;     // ประกาศตัวแปรแทน Servo ตัวที่ 4

int pos1=80, pos2=60, pos3=130, pos4=0;  // กำหนดค่าเริ่มต้นตำแหน่งแกนมุมของเซอร์โวมอเตอร์ของแต่ละตัว

const int left_X = A3; // กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน X คือขา A3
const int left_Y = A4;  // กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน X คือขา A4
const int left_key = 8; // กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน Z (ปุ่มกดลง) คือขา 8

const int right_X = A2; // กำหนดขาสัญญาณ Joystick ด้านขวาแกน X คือขา A2
const int right_Y = A5; // กำหนดขาสัญญาณ Joystick ด้านขวาแกน Y คือขา A5
const int right_key = 7; // กำหนดขาสัญญาณ Joystick ด้านขวาแกน Z (ปุ่มกดลง) คือขา 7

int x1,y1,z1;  // กำหนดตัวแปร เพื่อใช้เก็บค่าจากการอ่านค่าสัญญาณของ Joystick
int x2,y2,z2;


void setup()
{
  myservo1.attach(A1);  // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ต่อกับ i-Duino UNO R3B
  myservo2.attach(A0);  // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ต่อกับ i-Duino UNO R3B
  myservo3.attach(6);   // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ต่อกับ i-Duino UNO R3B
  myservo4.attach(9);   // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ต่อกับ i-Duino UNO R3B

  // ตั้งท่าเริ่มต้น
  myservo1.write(pos1); // ขับเซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ให้แกนหมุนไปยังตำแหน่ง pos1
  delay(3000);
  myservo2.write(pos2); // ขับเซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ให้แกนหมุนไปยังตำแหน่ง pos2
  myservo3.write(pos3); // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  myservo4.write(pos4); // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  delay(3000);      // หน่วงเวลา 1.5 วินาที

  pinMode(left_key, INPUT);  //กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน Z (ปุ่มกดลง) เป็นอินพุตดิจิตอล
  pinMode(right_key, INPUT); //กำหนดขาสัญญาณ Joystick ด้านขวาแกน Z (ปุ่มกดลง) เป็นอินพุตดิจิตอล
}

void loop()
{
  x2 = analogRead(right_X); // อ่านค่าสัญญาณแอนาล็อกจากแกน X ของ Joystick ด้านขวา
  y2 = analogRead(right_Y); // อ่านค่าสัญญาณแอนาล็อกจากแกน Y ของ Joystick ด้านขวา
  z2 = digitalRead(right_key);  // อ่านค่าสัญญาณดิจิตอลจากแกน X ของ Joystick ด้านขวา

  x1 = analogRead(left_X); // อ่านค่าสัญญาณแอนาล็อกจากแกน X ของ Joystick ด้านซ้าย
  y1 = analogRead(left_Y); // อ่านค่าสัญญาณแอนาล็อกจากแกน Y ของ Joystick ด้านซ้าย
  z1 = digitalRead(left_key); // อ่านค่าสัญญาณแอนาล็อกจากแกน Z ของ Joystick ด้านซ้าย

  // ควบคุมตัวหนีบจับ (Servo4)
  claw();
  // ควบคุมการหมุนฐานหุ่นยนต์ (Servo1)
  turn();
  // ควบคุม Upper arm (Servo2)
  upperarm();
  // ควบคุม Lower arm or Right arm (Servo3)
  lowerarm();
}

// ฟังก์ชันควบคุมตัวหนีบจับ (Servo4)
void claw()
{
  if(x1<50) // ถ้าดันจอยสติ๊กด้านซ้ายไปทางขวาสุด
  {
      pos4 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 4 ลดลงทีละ 2 องศา
      pos4 = (pos4 < 0) ? 0 : pos4; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
      myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
      delay(20); // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
   }
  if(x1>1000) // ถ้าดันจอยสติ๊กด้านซ้ายไปทางซ้ายสุด
  {
      pos4 += 8;  // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 4 เพิ่มขึ้นทีละ 8 องศา
      pos4 = (pos4 > 108) ? 108 : pos4;  // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 108 องศา
      myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ กางหรือคลาย
      delay(20); // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
}
//******************************************************
// ฟังก์ชันควบคุมการหมุนฐานหุ่นยนต์  (Servo1)
void turn()
{
  if(x2<50)   // ถ้าดันจอยสติ๊กด้านขวาไปทางขวาสุด
  {
    pos1--; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 1 องศา
    pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
    myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
    delay(10);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
  if(x2>1000)  // ถ้าดันจอยสติ๊กด้านขวาไปทางซ้ายสุด
  {
    pos1++; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 1 องศา
    pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
    myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
    delay(10);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
}

//**********************************************************/
// ฟังก์ชันควบคุมการยกขึ้น/วาลงของแขนส่วนบน (Servo2)
void upperarm()
{
  if(y2<50) // ถ้าดันจอยสติ๊กด้านขวาขึ้นบนสุด
  {
    pos2--; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
    pos2 = (pos2 < 0) ? 0 : pos2; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
    myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ ยกขึ้น
    delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
  if(y2>1000)  // ถ้าดันจอยสติ๊กด้านขวาลงล่างสุด
  {
    pos2++; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 เพิ่มขึ้นทีละ 1 องศา
    pos2 = (pos2 > 90) ? 90 : pos2; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
    myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ วางลง
    delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
}

//*************************************************************/
// ฟังก์ชันควบคุมการยึดออก/หดกลับของแขนส่วนล่าง (Servo3)
void lowerarm()
{
  if(y1<50) // ถ้าดันจอยสติ๊กด้านซ้ายขึ้นบนสุด
  {
    pos3++; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 3 เพิ่มขึ้นทีละ 1 องศา
    pos3 = (pos3 > 100) ? 100 : pos3; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 100 องศา
    myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ หดกลับ
    delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
  if(y1>1000) // ถ้าดันจอยสติ๊กด้านซ้ายลงล่างสุด
  {
    pos3--; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
    pos3 = (pos3 < 30) ? 30 : pos3; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 30 องศา
    myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ ยืดออก
    delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  }
}