timor2542

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

Aug 2nd, 2021
1,069
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>  // ผนวกไลบารี่การใช้งานเซอร์โวมอเตอร์
  2. Servo myservo1;     // ประกาศตัวแปรแทน Servo ตัวที่ 1
  3. Servo myservo2;     // ประกาศตัวแปรแทน Servo ตัวที่ 2
  4. Servo myservo3;     // ประกาศตัวแปรแทน Servo ตัวที่ 3
  5. Servo myservo4;     // ประกาศตัวแปรแทน Servo ตัวที่ 4
  6.  
  7. int pos1=80, pos2=60, pos3=130, pos4=0;  // กำหนดค่าเริ่มต้นตำแหน่งแกนมุมของเซอร์โวมอเตอร์ของแต่ละตัว
  8.  
  9. const int left_X = A3; // กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน X คือขา A3
  10. const int left_Y = A4;  // กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน X คือขา A4
  11. const int left_key = 8; // กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน Z (ปุ่มกดลง) คือขา 8
  12.  
  13. const int right_X = A2; // กำหนดขาสัญญาณ Joystick ด้านขวาแกน X คือขา A2
  14. const int right_Y = A5; // กำหนดขาสัญญาณ Joystick ด้านขวาแกน Y คือขา A5
  15. const int right_key = 7; // กำหนดขาสัญญาณ Joystick ด้านขวาแกน Z (ปุ่มกดลง) คือขา 7
  16.  
  17. int x1,y1,z1;  // กำหนดตัวแปร เพื่อใช้เก็บค่าจากการอ่านค่าสัญญาณของ Joystick
  18. int x2,y2,z2;
  19.  
  20.  
  21. void setup()
  22. {
  23.   myservo1.attach(A1);  // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ต่อกับ i-Duino UNO R3B
  24.   myservo2.attach(A0);  // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ต่อกับ i-Duino UNO R3B
  25.   myservo3.attach(6);   // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ต่อกับ i-Duino UNO R3B
  26.   myservo4.attach(9);   // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ต่อกับ i-Duino UNO R3B
  27.  
  28.   // ตั้งท่าเริ่มต้น
  29.   myservo1.write(pos1); // ขับเซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ให้แกนหมุนไปยังตำแหน่ง pos1
  30.   delay(3000);
  31.   myservo2.write(pos2); // ขับเซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ให้แกนหมุนไปยังตำแหน่ง pos2
  32.   myservo3.write(pos3); // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  33.   myservo4.write(pos4); // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  34.   delay(3000);      // หน่วงเวลา 1.5 วินาที
  35.  
  36.   pinMode(left_key, INPUT);  //กำหนดขาสัญญาณ Joystick ด้านซ้ายแกน Z (ปุ่มกดลง) เป็นอินพุตดิจิตอล
  37.   pinMode(right_key, INPUT); //กำหนดขาสัญญาณ Joystick ด้านขวาแกน Z (ปุ่มกดลง) เป็นอินพุตดิจิตอล
  38. }
  39.  
  40. void loop()
  41. {
  42.   x2 = analogRead(right_X); // อ่านค่าสัญญาณแอนาล็อกจากแกน X ของ Joystick ด้านขวา
  43.   y2 = analogRead(right_Y); // อ่านค่าสัญญาณแอนาล็อกจากแกน Y ของ Joystick ด้านขวา
  44.   z2 = digitalRead(right_key);  // อ่านค่าสัญญาณดิจิตอลจากแกน X ของ Joystick ด้านขวา
  45.  
  46.   x1 = analogRead(left_X); // อ่านค่าสัญญาณแอนาล็อกจากแกน X ของ Joystick ด้านซ้าย
  47.   y1 = analogRead(left_Y); // อ่านค่าสัญญาณแอนาล็อกจากแกน Y ของ Joystick ด้านซ้าย
  48.   z1 = digitalRead(left_key); // อ่านค่าสัญญาณแอนาล็อกจากแกน Z ของ Joystick ด้านซ้าย
  49.  
  50.   // ควบคุมตัวหนีบจับ (Servo4)
  51.   claw();
  52.   // ควบคุมการหมุนฐานหุ่นยนต์ (Servo1)
  53.   turn();
  54.   // ควบคุม Upper arm (Servo2)
  55.   upperarm();
  56.   // ควบคุม Lower arm or Right arm (Servo3)
  57.   lowerarm();
  58. }
  59.  
  60. // ฟังก์ชันควบคุมตัวหนีบจับ (Servo4)
  61. void claw()
  62. {
  63.   if(x1<50) // ถ้าดันจอยสติ๊กด้านซ้ายไปทางขวาสุด
  64.   {
  65.       pos4 -= 2; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 4 ลดลงทีละ 2 องศา
  66.       pos4 = (pos4 < 0) ? 0 : pos4; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  67.       myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  68.       delay(20); // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  69.    }
  70.   if(x1>1000) // ถ้าดันจอยสติ๊กด้านซ้ายไปทางซ้ายสุด
  71.   {
  72.       pos4 += 8;  // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 4 เพิ่มขึ้นทีละ 8 องศา
  73.       pos4 = (pos4 > 108) ? 108 : pos4;  // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 108 องศา
  74.       myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ กางหรือคลาย
  75.       delay(20); // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  76.   }
  77. }
  78. //******************************************************
  79. // ฟังก์ชันควบคุมการหมุนฐานหุ่นยนต์  (Servo1)
  80. void turn()
  81. {
  82.   if(x2<50)   // ถ้าดันจอยสติ๊กด้านขวาไปทางขวาสุด
  83.   {
  84.     pos1--; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 1 องศา
  85.     pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  86.     myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
  87.     delay(10);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  88.   }
  89.   if(x2>1000)  // ถ้าดันจอยสติ๊กด้านขวาไปทางซ้ายสุด
  90.   {
  91.     pos1++; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 1 องศา
  92.     pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  93.     myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
  94.     delay(10);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  95.   }
  96. }
  97.  
  98. //**********************************************************/
  99. // ฟังก์ชันควบคุมการยกขึ้น/วาลงของแขนส่วนบน (Servo2)
  100. void upperarm()
  101. {
  102.   if(y2<50) // ถ้าดันจอยสติ๊กด้านขวาขึ้นบนสุด
  103.   {
  104.     pos2--; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  105.     pos2 = (pos2 < 0) ? 0 : pos2; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  106.     myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ ยกขึ้น
  107.     delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  108.   }
  109.   if(y2>1000)  // ถ้าดันจอยสติ๊กด้านขวาลงล่างสุด
  110.   {
  111.     pos2++; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 เพิ่มขึ้นทีละ 1 องศา
  112.     pos2 = (pos2 > 90) ? 90 : pos2; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  113.     myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ วางลง  
  114.     delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  115.   }
  116. }
  117.  
  118. //*************************************************************/
  119. // ฟังก์ชันควบคุมการยึดออก/หดกลับของแขนส่วนล่าง (Servo3)
  120. void lowerarm()
  121. {
  122.   if(y1<50) // ถ้าดันจอยสติ๊กด้านซ้ายขึ้นบนสุด
  123.   {
  124.     pos3++; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 3 เพิ่มขึ้นทีละ 1 องศา
  125.     pos3 = (pos3 > 100) ? 100 : pos3; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 100 องศา
  126.     myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ หดกลับ
  127.     delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  128.   }
  129.   if(y1>1000) // ถ้าดันจอยสติ๊กด้านซ้ายลงล่างสุด
  130.   {
  131.     pos3--; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  132.     pos3 = (pos3 < 30) ? 30 : pos3; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 30 องศา
  133.     myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ ยืดออก
  134.     delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  135.   }
  136. }
  137.  
RAW Paste Data