timor2542

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

Jul 25th, 2021
939
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2.    Copyright (c) 2014 Innovative Experiment Co.,Ltd.
  3.  
  4.    Permission is hereby granted, free of charge, to any person obtaining a copy
  5.    of this software and associated documentation files (the "Software"), to deal
  6.    in the Software without restriction, including without limitation the rights
  7.    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8.    copies of the Software, and to permit persons to whom the Software is
  9.    furnished to do so, subject to the following conditions:
  10.  
  11.    The above copyright notice and this permission notice shall be included in
  12.    all copies or substantial portions of the Software.
  13.  
  14.    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15.    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16.    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17.    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18.    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19.    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  20.    THE SOFTWARE.
  21. */
  22.  
  23. /*
  24.    ตัวอย่างการใช้งานบอร์ด ATX-2 ร่วมกับ PS2 Controller แบบใช้คันโยกอะนาล็อกเพื่อควบคุม Gripper
  25.  
  26.    โปรแกรมควบคุมหุ่นยนต์ ATX-2 แบบมี Gripper สำหรับการทำภารกิจที่ต้องหนีบจับวัตถุ
  27.  
  28.    ควบคุมหุ่นยนต์ ATX-2 ด้วยปุ่มทิศทางขึ้น ลง ซ้าย ขวา โดยที่ปุ่มขึ้นจะเป็นการเดินหน้า
  29.    และปุ่มลงจะเป็นการถอยหลัง ส่วนปุ่มซ้ายและขวาจะเป็นการหมุนตัวไปในทิศทางนั้นๆ
  30.    สำหรับการควบคุมแขนจับจะใช้ปุ่ม R1 R2 L1 และ L2 โดยที่ R1 และ R2 จะควบคุมเซอร์โวสำหรับหนีบจับวัตถุ
  31.    โดยที่กด R1 เพื่อหยีบวัตถุ และกด R2 เพื่อปล่อยวัตถ ส่วน L1 และ L2 จะใช้ควบคุมเซอร์โวสำหรับยกแขนจับ
  32.    โดยที่กด L1 เพื่อปล่อยแขนจับให้ต่ำลง และกด L2 เพื่อยกแขนจับขึ้น
  33.  
  34.    เพิ่มเติม - ต่อเซอร์โวสำหรับหนีบจับวัตถุเข้าที่ช่อง SV0 และต่อเซอรืโวสำหรับยกแขนจับเข้าที่ช่อง SV1
  35. */
  36.  
  37. #include <ATX2.h>                                // เรียกใช้งานไลบรารีสำหรับ ATX-2
  38. #include <PS2X_lib.h>                            // เรียกใช้งานไลบรารีสำหรับ PS2 Controller
  39.  
  40. #define PS2_DAT        24                        // กำหนดขา Data    เป็นขา 16
  41. #define PS2_CMD        25                        // กำหนดขา Command เป็นขา 17
  42. #define PS2_SEL        26                        // กำหนดขา Select  เป็นขา 18
  43. #define PS2_CLK        27                        // กำหนดขา Clock   เป็นขา 19
  44.  
  45. PS2X ps2x;                                       // ประกาศตัวแปรสำหรับ PS2 Controller
  46.  
  47. int pos1 = 80;
  48. int pos2 = 60;
  49. int pos3 = 100;
  50. int pos4 = 84;
  51.  
  52. boolean isLB = false;
  53. boolean isRB = false;
  54. boolean isUB = false;
  55. boolean isDB = false;
  56.  
  57. boolean isR1 = false;                            // สภานะของปุ่ม R1
  58. boolean isR2 = false;                            // สภานะของปุ่ม R2
  59.  
  60. boolean isL1 = false;                            // สภานะของปุ่ม L1
  61. boolean isL2 = false;                            // สภานะของปุ่ม L2
  62.  
  63. boolean isSquare = false;
  64. boolean isCircle = false;
  65.  
  66. void setup()
  67. {
  68.   XIO();
  69.   delay(1000);                                   // หน่วงเวลา 1 วินาทีเพื่อรอให้บอร์ดพร้อมทำงาน
  70.   servo(1, pos1);
  71.   delay(1000);
  72.   servo(2, pos2);
  73.   servo(3, pos3);
  74.   servo(4, pos4);
  75.   delay(1500);
  76.   glcdClear();                                   // เคลียร์ภาพทั้งหมดบนหน้าจอแสดงผล
  77.   glcdMode(1);                                   // กำหนดให้หน้าจอแสดงผลเป็นแนวนอน
  78.   delay(200);                                    // หน่วงเวลา 200 มิลลิวินาที
  79.  
  80.   setTextSize(2);
  81.   glcd(0, 0, "Connecting");                      // แสดงข้อความเพื่อให้รู้ว่ากำลังทำการเชื่อมต่อกับ PS2 Controller
  82.  
  83.   while (true)                                   // วนการทำงานเพื่อรอการเชื่อมต่อกับ PS2 Controller
  84.   {
  85.     // กำหนดขาที่จะเชื่อมต่กับ PS2 Controller โดยมีการเก็บค่าที่ส่งกลับมาเป็น Integer เพื่อรู้ได้ว่าเชื่อมต่อได้หรือไม่
  86.     int error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);
  87.  
  88.     if (error == 0)                              // กรณีที่เชื่อมต่อได้ ไม่มีปัญหาอะไร (Error = 0)
  89.     {
  90.       glcd(0, 0, "OK        ");               // แสดงข้อความว่าเชื่อมต่อกับ PS2 Controller เรียบร้อยแล้ว
  91.       delay(1000);                               // หน่วงเวลา 1 วินาที
  92.       glcdClear();                               // เคลียร์ภาพทั้งหมดบนหน้าจอแสดงผล
  93.       glcd(1,0,"Servo1:");
  94.       glcd(2,0,"Servo2:");
  95.       glcd(3,0,"Servo3:");
  96.       glcd(4,0,"Servo4:");
  97.       break;                                     // ออกจาก while(true)
  98.     }
  99.     delay(500);                                  // หน่วงเวลา 500 มิลลิวินาทีเพื่อรอการเชื่อมต่อครั้งต่อไปในกรณีที่เชื่อมต่อไม่สำเร็จ
  100.   }
  101. }
  102.  
  103. void loop()
  104. {
  105.   ps2x.read_gamepad(false, false);               // อ่านข้อมูลจาก PS2 Controller
  106.  
  107.   if (ps2x.Button(PSB_PAD_LEFT)) {
  108.     isLB = true;
  109.   }
  110.   else {
  111.     isLB = false;
  112.   }
  113.  
  114.   if (ps2x.Button(PSB_PAD_RIGHT)) {
  115.     isRB = true;
  116.   }
  117.   else {
  118.     isRB = false;
  119.   }
  120.  
  121.   if (ps2x.Button(PSB_PAD_UP)) {
  122.     isUB = true;
  123.   }
  124.   else {
  125.     isUB = false;
  126.   }
  127.  
  128.   if (ps2x.Button(PSB_PAD_DOWN)) {
  129.     isDB = true;
  130.   }
  131.   else {
  132.     isDB = false;
  133.   }
  134.  
  135.   if (ps2x.Button(PSB_R1)) {                      // ถ้าปุ่ม R1 ถูกกด
  136.     isR1 = true;
  137.   }                                // กำหนดสถานะของ isR1 เป็น True
  138.   else   {                                        // ถ้าปุ่ม R1 ไม่ถูกกด
  139.     isR1 = false;
  140.   }                               // กำหนดสถานะของ isR1 เป็น False
  141.  
  142.   if (ps2x.Button(PSB_R2)) {                      // ถ้าปุ่ม R2 ถูกกด
  143.     isR2 = true;
  144.   }                                // กำหนดสถานะของ isR2 เป็น True
  145.   else {                                          // ถ้าปุ่ม R2 ไม่ถูกกด
  146.     isR2 = false;
  147.   }                               // กำหนดสถานะของ isR2 เป็น False
  148.  
  149.   if (ps2x.Button(PSB_L1)) {                      // ถ้าปุ่ม L1 ถูกกด
  150.     isL1 = true;
  151.   }                                 // กำหนดสถานะของ isL1 เป็น True
  152.   else    {                                       // ถ้าปุ่ม L1 ไม่ถูกกด
  153.     isL1 = false;
  154.   }                              // กำหนดสถานะของ isL1 เป็น False
  155.  
  156.   if (ps2x.Button(PSB_L2)) {                      // ถ้าปุ่ม L2 ถูกกด
  157.     isL2 = true;
  158.   }                               // กำหนดสถานะของ isL2 เป็น True
  159.   else     {                                      // ถ้าปุ่ม L2 ไม่ถูกกด
  160.     isL2 = false;
  161.   }                            // กำหนดสถานะของ isL2 เป็น False
  162.  
  163.   if (ps2x.Button(PSB_TRIANGLE))                 // เมื่อปุ่มสามเหลี่ยมถูกกด
  164.   {
  165.     servo(1, -1);                                // ผ่อนเซอร์โวทั้งสองตัว
  166.     servo(2, -1);
  167.     servo(3, -1);
  168.     servo(4, -1);
  169.   }
  170.  
  171.   if (ps2x.Button(PSB_SQUARE)) {
  172.     isSquare = true;
  173.   }
  174.   else {
  175.     isSquare = false;
  176.   }
  177.  
  178.   if (ps2x.Button(PSB_CIRCLE)) {
  179.     isCircle = true;
  180.   }
  181.   else {
  182.     isCircle = false;
  183.   }
  184.  
  185.   if (isR1)                                      // เมื่อสถานะของ isR1 เป็น True (ถูกกด) : หนีบวัตถุ
  186.   {
  187.     Arm2LiftUp();
  188.   }
  189.   else if (isR2)                                 // เมื่อสถานะของ isR2 เป็น True (ถูกกด) : ปล่อยวัตถุ
  190.   {
  191.     Arm2GoDown();
  192.   }
  193.   else if (isL1)                                 // เมื่อสถานะของ isL1 เป็น True (ถูกกด) : ปล่อยแขนจับให้ต่ำลง
  194.   {
  195.     Arm3DrawBack();
  196.   }
  197.   else if (isL2)                                 // เมื่อสถานะของ isL2 เป็น True (ถูกกด) : ยกแขนจับขึ้น
  198.   {
  199.     Arm3StretchedOut();
  200.   }
  201.  
  202.   if (isLB) {
  203.     Arm1TurnLeft();
  204.   }
  205.   else if (isRB) {
  206.     Arm1TurnRight();
  207.   }
  208.  
  209.   if (isSquare) {
  210.     Arm4OpenClaw();
  211.   }
  212.   else if (isCircle) {
  213.     Arm4CloseClaw();
  214.   }
  215.  
  216.   if (ps2x.Button(PSB_CROSS))                 // เมื่อปุ่มสามเหลี่ยมถูกกด
  217.   {
  218.     glcd(1,7,"%d   ",pos1);
  219.     glcd(2,7,"%d   ",pos2);
  220.     glcd(3,7,"%d   ",pos3);
  221.     glcd(4,7,"%d   ",pos4);
  222.   }
  223.   delay(50);                                     // หน่วงเวลา 50 มิลลิวินาที
  224. }
  225. //**************************************************
  226. // turn left
  227. void Arm1TurnLeft()
  228. {
  229.   pos1 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 10 องศา
  230.   pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  231.   servo(1, pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
  232. }
  233. //turn right
  234. void Arm1TurnRight()
  235. {
  236.   pos1 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 10 องศา
  237.   pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  238.   servo(1, pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
  239. }
  240. //********************************************
  241. //open the claw
  242. void Arm4OpenClaw()
  243. {
  244.   pos4 += 2;
  245.   pos4 = (pos4 > 166) ? 166 : pos4; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 165 องศา
  246.   servo(4, pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  247. }
  248. // close the claw
  249. void Arm4CloseClaw()
  250. {
  251.   pos4 -= 2;
  252.   pos4 = (pos4 < 84) ? 84 : pos4;
  253.   servo(4, pos4);   // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  254. }
  255.  
  256. //******************************************
  257. // the upper arm will lift up
  258. void Arm2LiftUp()
  259. {
  260.   pos2 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  261.   pos2 = (pos2 < 30) ? 30 : pos2; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  262.   servo(2, pos2);   // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ ยกขึ้น
  263. }
  264. // the upper arm will go down
  265. void Arm2GoDown()
  266. {
  267.   pos2 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 เพิ่มขึ้นทีละ 1 องศา
  268.   pos2 = (pos2 > 90) ? 90 : pos2; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  269.   servo(2, pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ วางลง
  270. }
  271.  
  272. //***************************************
  273. // the lower arm will stretch out
  274. void Arm3DrawBack()
  275. {
  276.   pos3 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  277.   pos3 = (pos3 < 30) ? 30 : pos3; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 30 องศา
  278.   servo(3, pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ ยืดออก
  279. }
  280. // the lower arm will draw back
  281. void Arm3StretchedOut()
  282. {
  283.   pos3 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 3 เพิ่มขึ้นทีละ 1 องศา
  284.   pos3 = (pos3 > 100) ? 100 : pos3; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 100 องศา
  285.   servo(3, pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ หดกลับ
  286. }
RAW Paste Data