[4 DOF Robot Arm Keyestudio][Lab 11][i-Duino UNO R3B] PSX2 4 Servo Controller

/*
   Copyright (c) 2014 Innovative Experiment Co.,Ltd.

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
*/

/*
   ตัวอย่างการใช้งานบอร์ด R3B ร่วมกับ PS2 Controller แบบใช้คันโยกอะนาล็อกเพื่อควบคุม Gripper

   โปรแกรมควบคุมหุ่นยนต์ R3B แบบมี Gripper สำหรับการทำภารกิจที่ต้องหนีบจับวัตถุ

   ควบคุมหุ่นยนต์ R3B ด้วยปุ่มทิศทางขึ้น ลง ซ้าย ขวา โดยที่ปุ่มขึ้นจะเป็นการเดินหน้า
   และปุ่มลงจะเป็นการถอยหลัง ส่วนปุ่มซ้ายและขวาจะเป็นการหมุนตัวไปในทิศทางนั้นๆ
   สำหรับการควบคุมแขนจับจะใช้ปุ่ม R1 R2 L1 และ L2 โดยที่ R1 และ R2 จะควบคุมเซอร์โวสำหรับหนีบจับวัตถุ
   โดยที่กด R1 เพื่อหยีบวัตถุ และกด R2 เพื่อปล่อยวัตถ ส่วน L1 และ L2 จะใช้ควบคุมเซอร์โวสำหรับยกแขนจับ
   โดยที่กด L1 เพื่อปล่อยแขนจับให้ต่ำลง และกด L2 เพื่อยกแขนจับขึ้น

   เพิ่มเติม - ต่อเซอร์โวสำหรับหนีบจับวัตถุเข้าที่ช่อง SV0 และต่อเซอรืโวสำหรับยกแขนจับเข้าที่ช่อง SV1
*/
#include <Servo.h>
#include <PS2X_lib.h>                            // เรียกใช้งานไลบรารีสำหรับ PS2 Controller

PS2X ps2x;                                       // ประกาศตัวแปรสำหรับ PS2 Controller

Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;

int pos1 = 80;
int pos2 = 60;
int pos3 = 100;
int pos4 = 84;

boolean isLB = false;
boolean isRB = false;
boolean isUB = false;
boolean isDB = false;

boolean isR1 = false;                            // สภานะของปุ่ม R1
boolean isR2 = false;                            // สภานะของปุ่ม R2

boolean isL1 = false;                            // สภานะของปุ่ม L1
boolean isL2 = false;                            // สภานะของปุ่ม L2

boolean isSquare = false;
boolean isCircle = false;

void setup()
{
  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
  delay(1000);                                   // หน่วงเวลา 1 วินาทีเพื่อรอให้บอร์ดพร้อมทำงาน
  myservo1.write(pos1);
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);
  Serial.begin(115200);
  while(!Serial){
    }
  Serial.println("Connecting...");       // แสดงข้อความเพื่อให้รู้ว่ากำลังทำการเชื่อมต่อกับ PS2 Controller

  while (true)                                   // วนการทำงานเพื่อรอการเชื่อมต่อกับ PS2 Controller
  {
    // กำหนดขาที่จะเชื่อมต่กับ PS2 Controller โดยมีการเก็บค่าที่ส่งกลับมาเป็น Integer เพื่อรู้ได้ว่าเชื่อมต่อได้หรือไม่
    int error = ps2x.config_gamepad(13,11,10,12, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
    if (error == 0)                              // กรณีที่เชื่อมต่อได้ ไม่มีปัญหาอะไร (Error = 0)
    {
      Serial.println("OK");       /// แสดงข้อความว่าเชื่อมต่อกับ PS2 Controller เรียบร้อยแล้ว
      delay(1000);                               // หน่วงเวลา 1 วินาที
      break;                                     // ออกจาก while(true)
    }
    delay(500);                                  // หน่วงเวลา 500 มิลลิวินาทีเพื่อรอการเชื่อมต่อครั้งต่อไปในกรณีที่เชื่อมต่อไม่สำเร็จ
  }
}

void loop()
{
  ps2x.read_gamepad(false, false);               // อ่านข้อมูลจาก PS2 Controller

  if (ps2x.Button(PSB_PAD_LEFT)) {
    isLB = true;
  }
  else {
    isLB = false;
  }

  if (ps2x.Button(PSB_PAD_RIGHT)) {
    isRB = true;
  }
  else {
    isRB = false;
  }

  if (ps2x.Button(PSB_PAD_UP)) {
    isUB = true;
  }
  else {
    isUB = false;
  }

  if (ps2x.Button(PSB_PAD_DOWN)) {
    isDB = true;
  }
  else {
    isDB = false;
  }

  if (ps2x.Button(PSB_R1)) {                      // ถ้าปุ่ม R1 ถูกกด
    isR1 = true;
  }                                // กำหนดสถานะของ isR1 เป็น True
  else   {                                        // ถ้าปุ่ม R1 ไม่ถูกกด
    isR1 = false;
  }                               // กำหนดสถานะของ isR1 เป็น False

  if (ps2x.Button(PSB_R2)) {                      // ถ้าปุ่ม R2 ถูกกด
    isR2 = true;
  }                                // กำหนดสถานะของ isR2 เป็น True
  else {                                          // ถ้าปุ่ม R2 ไม่ถูกกด
    isR2 = false;
  }                               // กำหนดสถานะของ isR2 เป็น False

  if (ps2x.Button(PSB_L1)) {                      // ถ้าปุ่ม L1 ถูกกด
    isL1 = true;
  }                                 // กำหนดสถานะของ isL1 เป็น True
  else    {                                       // ถ้าปุ่ม L1 ไม่ถูกกด
    isL1 = false;
  }                              // กำหนดสถานะของ isL1 เป็น False

  if (ps2x.Button(PSB_L2)) {                      // ถ้าปุ่ม L2 ถูกกด
    isL2 = true;
  }                               // กำหนดสถานะของ isL2 เป็น True
  else     {                                      // ถ้าปุ่ม L2 ไม่ถูกกด
    isL2 = false;
  }                            // กำหนดสถานะของ isL2 เป็น False

  if (ps2x.Button(PSB_SQUARE)) {
    isSquare = true;
  }
  else {
    isSquare = false;
  }

  if (ps2x.Button(PSB_CIRCLE)) {
    isCircle = true;
  }
  else {
    isCircle = false;
  }

  if (isR1)                                      // เมื่อสถานะของ isR1 เป็น True (ถูกกด) : หนีบวัตถุ
  {
    Arm2LiftUp();
  }
  else if (isR2)                                 // เมื่อสถานะของ isR2 เป็น True (ถูกกด) : ปล่อยวัตถุ
  {
    Arm2GoDown();
  }
  else if (isL1)                                 // เมื่อสถานะของ isL1 เป็น True (ถูกกด) : ปล่อยแขนจับให้ต่ำลง
  {
    Arm3DrawBack();
  }
  else if (isL2)                                 // เมื่อสถานะของ isL2 เป็น True (ถูกกด) : ยกแขนจับขึ้น
  {
    Arm3StretchedOut();
  }

  if (isLB) {
    Arm1TurnLeft();
  }
  else if (isRB) {
    Arm1TurnRight();
  }

  if (isSquare) {
    Arm4OpenClaw();
  }
  else if (isCircle) {
    Arm4CloseClaw();
  }

  if (ps2x.Button(PSB_CROSS))                 // เมื่อปุ่มสามเหลี่ยมถูกกด
  {
    Serial.println(pos1, DEC);
    Serial.println(pos2, DEC);
    Serial.println(pos3, DEC);
    Serial.println(pos4, DEC);
  }
  delay(50);                                     // หน่วงเวลา 50 มิลลิวินาที
}
//**************************************************
// turn left
void Arm1TurnLeft()
{
  pos1 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 10 องศา
  pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
}
//turn right
void Arm1TurnRight()
{
  pos1 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 10 องศา
  pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
}
//********************************************
//open the claw
void Arm4OpenClaw()
{
  pos4 += 2;
  pos4 = (pos4 > 166) ? 166 : pos4; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 165 องศา
  myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
}
// close the claw
void Arm4CloseClaw()
{
  pos4 -= 2;
  pos4 = (pos4 < 84) ? 84 : pos4;
  myservo4.write(pos4);   // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
}

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

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