timor2542

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

Aug 3rd, 2021
975
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.    ตัวอย่างการใช้งานบอร์ด R3B ร่วมกับ PS2 Controller แบบใช้คันโยกอะนาล็อกเพื่อควบคุม Gripper
  25.  
  26.    โปรแกรมควบคุมหุ่นยนต์ R3B แบบมี Gripper สำหรับการทำภารกิจที่ต้องหนีบจับวัตถุ
  27.  
  28.    ควบคุมหุ่นยนต์ R3B ด้วยปุ่มทิศทางขึ้น ลง ซ้าย ขวา โดยที่ปุ่มขึ้นจะเป็นการเดินหน้า
  29.    และปุ่มลงจะเป็นการถอยหลัง ส่วนปุ่มซ้ายและขวาจะเป็นการหมุนตัวไปในทิศทางนั้นๆ
  30.    สำหรับการควบคุมแขนจับจะใช้ปุ่ม R1 R2 L1 และ L2 โดยที่ R1 และ R2 จะควบคุมเซอร์โวสำหรับหนีบจับวัตถุ
  31.    โดยที่กด R1 เพื่อหยีบวัตถุ และกด R2 เพื่อปล่อยวัตถ ส่วน L1 และ L2 จะใช้ควบคุมเซอร์โวสำหรับยกแขนจับ
  32.    โดยที่กด L1 เพื่อปล่อยแขนจับให้ต่ำลง และกด L2 เพื่อยกแขนจับขึ้น
  33.  
  34.    เพิ่มเติม - ต่อเซอร์โวสำหรับหนีบจับวัตถุเข้าที่ช่อง SV0 และต่อเซอรืโวสำหรับยกแขนจับเข้าที่ช่อง SV1
  35. */
  36. #include <Servo.h>
  37. #include <PS2X_lib.h>                            // เรียกใช้งานไลบรารีสำหรับ PS2 Controller
  38.  
  39. PS2X ps2x;                                       // ประกาศตัวแปรสำหรับ PS2 Controller
  40.  
  41. Servo myservo1;  // create servo object to control a servo
  42. Servo myservo2;
  43. Servo myservo3;
  44. Servo myservo4;  
  45.  
  46. int pos1 = 80;
  47. int pos2 = 60;
  48. int pos3 = 100;
  49. int pos4 = 84;
  50.  
  51. boolean isLB = false;
  52. boolean isRB = false;
  53. boolean isUB = false;
  54. boolean isDB = false;
  55.  
  56. boolean isR1 = false;                            // สภานะของปุ่ม R1
  57. boolean isR2 = false;                            // สภานะของปุ่ม R2
  58.  
  59. boolean isL1 = false;                            // สภานะของปุ่ม L1
  60. boolean isL2 = false;                            // สภานะของปุ่ม L2
  61.  
  62. boolean isSquare = false;
  63. boolean isCircle = false;
  64.  
  65. void setup()
  66. {
  67.   myservo1.attach(A1);  // set the control pin of servo 1 to A1
  68.   myservo2.attach(A0);  // set the control pin of servo 2 to A0
  69.   myservo3.attach(6);   //set the control pin of servo 3 to D6
  70.   myservo4.attach(9);   //set the control pin of servo 4 to D9
  71.   delay(1000);                                   // หน่วงเวลา 1 วินาทีเพื่อรอให้บอร์ดพร้อมทำงาน
  72.   myservo1.write(pos1);
  73.   delay(1000);
  74.   myservo2.write(pos2);
  75.   myservo3.write(pos3);
  76.   myservo4.write(pos4);
  77.   delay(1500);
  78.   Serial.begin(115200);
  79.   while(!Serial){
  80.     }
  81.   Serial.println("Connecting...");       // แสดงข้อความเพื่อให้รู้ว่ากำลังทำการเชื่อมต่อกับ PS2 Controller
  82.  
  83.   while (true)                                   // วนการทำงานเพื่อรอการเชื่อมต่อกับ PS2 Controller
  84.   {
  85.     // กำหนดขาที่จะเชื่อมต่กับ PS2 Controller โดยมีการเก็บค่าที่ส่งกลับมาเป็น Integer เพื่อรู้ได้ว่าเชื่อมต่อได้หรือไม่
  86.     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
  87.     if (error == 0)                              // กรณีที่เชื่อมต่อได้ ไม่มีปัญหาอะไร (Error = 0)
  88.     {
  89.       Serial.println("OK");       /// แสดงข้อความว่าเชื่อมต่อกับ PS2 Controller เรียบร้อยแล้ว
  90.       delay(1000);                               // หน่วงเวลา 1 วินาที
  91.       break;                                     // ออกจาก while(true)
  92.     }
  93.     delay(500);                                  // หน่วงเวลา 500 มิลลิวินาทีเพื่อรอการเชื่อมต่อครั้งต่อไปในกรณีที่เชื่อมต่อไม่สำเร็จ
  94.   }
  95. }
  96.  
  97. void loop()
  98. {
  99.   ps2x.read_gamepad(false, false);               // อ่านข้อมูลจาก PS2 Controller
  100.  
  101.   if (ps2x.Button(PSB_PAD_LEFT)) {
  102.     isLB = true;
  103.   }
  104.   else {
  105.     isLB = false;
  106.   }
  107.  
  108.   if (ps2x.Button(PSB_PAD_RIGHT)) {
  109.     isRB = true;
  110.   }
  111.   else {
  112.     isRB = false;
  113.   }
  114.  
  115.   if (ps2x.Button(PSB_PAD_UP)) {
  116.     isUB = true;
  117.   }
  118.   else {
  119.     isUB = false;
  120.   }
  121.  
  122.   if (ps2x.Button(PSB_PAD_DOWN)) {
  123.     isDB = true;
  124.   }
  125.   else {
  126.     isDB = false;
  127.   }
  128.  
  129.   if (ps2x.Button(PSB_R1)) {                      // ถ้าปุ่ม R1 ถูกกด
  130.     isR1 = true;
  131.   }                                // กำหนดสถานะของ isR1 เป็น True
  132.   else   {                                        // ถ้าปุ่ม R1 ไม่ถูกกด
  133.     isR1 = false;
  134.   }                               // กำหนดสถานะของ isR1 เป็น False
  135.  
  136.   if (ps2x.Button(PSB_R2)) {                      // ถ้าปุ่ม R2 ถูกกด
  137.     isR2 = true;
  138.   }                                // กำหนดสถานะของ isR2 เป็น True
  139.   else {                                          // ถ้าปุ่ม R2 ไม่ถูกกด
  140.     isR2 = false;
  141.   }                               // กำหนดสถานะของ isR2 เป็น False
  142.  
  143.   if (ps2x.Button(PSB_L1)) {                      // ถ้าปุ่ม L1 ถูกกด
  144.     isL1 = true;
  145.   }                                 // กำหนดสถานะของ isL1 เป็น True
  146.   else    {                                       // ถ้าปุ่ม L1 ไม่ถูกกด
  147.     isL1 = false;
  148.   }                              // กำหนดสถานะของ isL1 เป็น False
  149.  
  150.   if (ps2x.Button(PSB_L2)) {                      // ถ้าปุ่ม L2 ถูกกด
  151.     isL2 = true;
  152.   }                               // กำหนดสถานะของ isL2 เป็น True
  153.   else     {                                      // ถ้าปุ่ม L2 ไม่ถูกกด
  154.     isL2 = false;
  155.   }                            // กำหนดสถานะของ isL2 เป็น False
  156.  
  157.   if (ps2x.Button(PSB_SQUARE)) {
  158.     isSquare = true;
  159.   }
  160.   else {
  161.     isSquare = false;
  162.   }
  163.  
  164.   if (ps2x.Button(PSB_CIRCLE)) {
  165.     isCircle = true;
  166.   }
  167.   else {
  168.     isCircle = false;
  169.   }
  170.  
  171.   if (isR1)                                      // เมื่อสถานะของ isR1 เป็น True (ถูกกด) : หนีบวัตถุ
  172.   {
  173.     Arm2LiftUp();
  174.   }
  175.   else if (isR2)                                 // เมื่อสถานะของ isR2 เป็น True (ถูกกด) : ปล่อยวัตถุ
  176.   {
  177.     Arm2GoDown();
  178.   }
  179.   else if (isL1)                                 // เมื่อสถานะของ isL1 เป็น True (ถูกกด) : ปล่อยแขนจับให้ต่ำลง
  180.   {
  181.     Arm3DrawBack();
  182.   }
  183.   else if (isL2)                                 // เมื่อสถานะของ isL2 เป็น True (ถูกกด) : ยกแขนจับขึ้น
  184.   {
  185.     Arm3StretchedOut();
  186.   }
  187.  
  188.   if (isLB) {
  189.     Arm1TurnLeft();
  190.   }
  191.   else if (isRB) {
  192.     Arm1TurnRight();
  193.   }
  194.  
  195.   if (isSquare) {
  196.     Arm4OpenClaw();
  197.   }
  198.   else if (isCircle) {
  199.     Arm4CloseClaw();
  200.   }
  201.  
  202.   if (ps2x.Button(PSB_CROSS))                 // เมื่อปุ่มสามเหลี่ยมถูกกด
  203.   {
  204.     Serial.println(pos1, DEC);
  205.     Serial.println(pos2, DEC);
  206.     Serial.println(pos3, DEC);
  207.     Serial.println(pos4, DEC);
  208.   }
  209.   delay(50);                                     // หน่วงเวลา 50 มิลลิวินาที
  210. }
  211. //**************************************************
  212. // turn left
  213. void Arm1TurnLeft()
  214. {
  215.   pos1 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 10 องศา
  216.   pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  217.   myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
  218. }
  219. //turn right
  220. void Arm1TurnRight()
  221. {
  222.   pos1 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 10 องศา
  223.   pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  224.   myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
  225. }
  226. //********************************************
  227. //open the claw
  228. void Arm4OpenClaw()
  229. {
  230.   pos4 += 2;
  231.   pos4 = (pos4 > 166) ? 166 : pos4; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 165 องศา
  232.   myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  233. }
  234. // close the claw
  235. void Arm4CloseClaw()
  236. {
  237.   pos4 -= 2;
  238.   pos4 = (pos4 < 84) ? 84 : pos4;
  239.   myservo4.write(pos4);   // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  240. }
  241.  
  242. //******************************************
  243. // the upper arm will lift up
  244. void Arm2LiftUp()
  245. {
  246.   pos2 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  247.   pos2 = (pos2 < 30) ? 30 : pos2; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  248.   myservo2.write(pos2);   // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ ยกขึ้น
  249. }
  250. // the upper arm will go down
  251. void Arm2GoDown()
  252. {
  253.   pos2 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 เพิ่มขึ้นทีละ 1 องศา
  254.   pos2 = (pos2 > 90) ? 90 : pos2; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  255.   myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ วางลง
  256. }
  257.  
  258. //***************************************
  259. // the lower arm will stretch out
  260. void Arm3DrawBack()
  261. {
  262.   pos3 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  263.   pos3 = (pos3 < 30) ? 30 : pos3; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 30 องศา
  264.   myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ ยืดออก
  265. }
  266. // the lower arm will draw back
  267. void Arm3StretchedOut()
  268. {
  269.   pos3 += 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 3 เพิ่มขึ้นทีละ 1 องศา
  270.   pos3 = (pos3 > 100) ? 100 : pos3; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 100 องศา
  271.   myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ หดกลับ
  272. }
  273.  
RAW Paste Data