[4 DOF Robot Arm Keyestudio][Lab 12][ATX-2] ZX-PSX2 Servo Control

/*
   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.
*/

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

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

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

   เพิ่มเติม - ต่อเซอร์โวสำหรับหนีบจับวัตถุเข้าที่ช่อง SV0 และต่อเซอรืโวสำหรับยกแขนจับเข้าที่ช่อง SV1
*/

#include <ATX2.h>                                // เรียกใช้งานไลบรารีสำหรับ ATX-2
#include <PS2X_lib.h>                            // เรียกใช้งานไลบรารีสำหรับ PS2 Controller

#define PS2_DAT        24                        // กำหนดขา Data    เป็นขา 16
#define PS2_CMD        25                        // กำหนดขา Command เป็นขา 17
#define PS2_SEL        26                        // กำหนดขา Select  เป็นขา 18
#define PS2_CLK        27                        // กำหนดขา Clock   เป็นขา 19

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

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()
{
  XIO();
  delay(1000);                                   // หน่วงเวลา 1 วินาทีเพื่อรอให้บอร์ดพร้อมทำงาน
  servo(1, pos1);
  delay(1000);
  servo(2, pos2);
  servo(3, pos3);
  servo(4, pos4);
  delay(1500);
  glcdClear();                                   // เคลียร์ภาพทั้งหมดบนหน้าจอแสดงผล
  glcdMode(1);                                   // กำหนดให้หน้าจอแสดงผลเป็นแนวนอน
  delay(200);                                    // หน่วงเวลา 200 มิลลิวินาที

  setTextSize(2);
  glcd(0, 0, "Connecting");                      // แสดงข้อความเพื่อให้รู้ว่ากำลังทำการเชื่อมต่อกับ PS2 Controller

  while (true)                                   // วนการทำงานเพื่อรอการเชื่อมต่อกับ PS2 Controller
  {
    // กำหนดขาที่จะเชื่อมต่กับ PS2 Controller โดยมีการเก็บค่าที่ส่งกลับมาเป็น Integer เพื่อรู้ได้ว่าเชื่อมต่อได้หรือไม่
    int error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);

    if (error == 0)                              // กรณีที่เชื่อมต่อได้ ไม่มีปัญหาอะไร (Error = 0)
    {
      glcd(0, 0, "OK        ");               // แสดงข้อความว่าเชื่อมต่อกับ PS2 Controller เรียบร้อยแล้ว
      delay(1000);                               // หน่วงเวลา 1 วินาที
      glcdClear();                               // เคลียร์ภาพทั้งหมดบนหน้าจอแสดงผล
      glcd(1,0,"Servo1:");
      glcd(2,0,"Servo2:");
      glcd(3,0,"Servo3:");
      glcd(4,0,"Servo4:");
      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_TRIANGLE))                 // เมื่อปุ่มสามเหลี่ยมถูกกด
  {
    servo(1, -1);                                // ผ่อนเซอร์โวทั้งสองตัว
    servo(2, -1);
    servo(3, -1);
    servo(4, -1);
  }

  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))                 // เมื่อปุ่มสามเหลี่ยมถูกกด
  {
    glcd(1,7,"%d   ",pos1);
    glcd(2,7,"%d   ",pos2);
    glcd(3,7,"%d   ",pos3);
    glcd(4,7,"%d   ",pos4);
  }
  delay(50);                                     // หน่วงเวลา 50 มิลลิวินาที
}
//**************************************************
// turn left
void Arm1TurnLeft()
{
  pos1 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 10 องศา
  pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  servo(1, pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
}
//turn right
void Arm1TurnRight()
{
  pos1 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 10 องศา
  pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  servo(1, pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
}
//********************************************
//open the claw
void Arm4OpenClaw()
{
  pos4 += 2;
  pos4 = (pos4 > 166) ? 166 : pos4; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 165 องศา
  servo(4, pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
}
// close the claw
void Arm4CloseClaw()
{
  pos4 -= 2;
  pos4 = (pos4 < 84) ? 84 : pos4;
  servo(4, pos4);   // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
}

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

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