timor2542

[4 DOF Robot Arm Keyestudio][Lab 09][i-Duino UNO R3B] Bluetooth HC-06 Test on 4DOF Robot Arm

Aug 2nd, 2021
1,150
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>  // add the servo libraries
  2. Servo myservo1;  // create servo object to control a servo
  3. Servo myservo2;
  4. Servo myservo3;
  5. Servo myservo4;
  6. int pos1=80, pos2=60, pos3=100, pos4=85;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
  7. char val;
  8.  
  9. void setup()
  10. {
  11.    // boot posture
  12.   myservo1.write(pos1);  
  13.   delay(1000);
  14.   myservo2.write(pos2);
  15.   myservo3.write(pos3);
  16.   myservo4.write(pos4);
  17.   delay(1500);
  18.  
  19.   Serial.begin(9600); //  set the baud rate to 9600
  20. }
  21.  
  22. void loop()
  23. {
  24.   myservo1.attach(A1);  // set the control pin of servo 1 to A1
  25.   myservo2.attach(A0);  // set the control pin of servo 2 to A0
  26.   myservo3.attach(6);   // set the control pin of servo 3 to D6
  27.   myservo4.attach(9);   // set the control pin of servo 4 to D9
  28.  
  29.   if(Serial.available())   // if receive the data
  30.   {
  31.     val=Serial.read();    // read the received data
  32.     Serial.println(val);
  33.     switch(val)  
  34.     {
  35.       case 0x38:  Arm2LiftUp();  break;   // execute the corresponding function when receive the value
  36.       case 0x45:  Arm1TurnLeft();  break;
  37.       case 0x46:  Arm1TurnRight();  break;
  38.       case 0x32:  Arm2GoDown();  break;
  39.       case 0x36:  Arm3DrawBack();  break;
  40.       case 0x44:  Arm4OpenClaw();  break;
  41.       case 0x43:  Arm4CloseClaw();  break;
  42.       case 0x34:  Arm3StretchedOut();  break;
  43.     }
  44.   }  
  45. }
  46. //**************************************************
  47. // turn left
  48. void Arm1TurnLeft()
  49. {
  50.     pos1 += 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 เพิ่มขึ้นทีละ 10 องศา
  51.     pos1 = (pos1 > 180) ? 180 : pos1; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  52.     myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางซ้าย
  53.     delay(5);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  54. }
  55. //turn right
  56. void Arm1TurnRight()
  57. {
  58.     pos1 -= 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 1 ลดลงทีละ 10 องศา
  59.     pos1 = (pos1 < 0) ? 0 : pos1; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  60.     myservo1.write(pos1);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 1 ให้ฐานหุ่นยนต์จะค่อย ๆ หมุนไปทางขวา
  61.     delay(5);  // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  62. }
  63. //********************************************
  64. //open the claw
  65. void Arm4OpenClaw()
  66. {
  67.       pos4 = 165;
  68.       myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  69.       delay(10); // หน่วงเวลา 10 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  70. }
  71. // close the claw
  72. void Arm4CloseClaw()
  73. {
  74.       pos4 = 85;
  75.       myservo4.write(pos4);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 4 ให้ตัวมือจับจะค่อย ๆ หุบหรือหนีบ
  76.       delay(10); // หน่วงเวลา 10 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  77. }
  78.  
  79. //******************************************
  80. // the upper arm will lift up
  81. void Arm2LiftUp()
  82. {
  83.     pos2 -= 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  84.     pos2 = (pos2 < 30) ? 30 : pos2; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 0 องศา
  85.     myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ ยกขึ้น
  86.     delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  87. }
  88. // the upper arm will go down
  89. void Arm2GoDown()
  90. {
  91.     pos2 += 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 เพิ่มขึ้นทีละ 1 องศา
  92.     pos2 = (pos2 > 90) ? 90 : pos2; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 180 องศา
  93.     myservo2.write(pos2);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 2 ให้แขนส่วนบน จะค่อย ๆ วางลง  
  94.     delay(10);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  95. }
  96.  
  97. //***************************************
  98. // the lower arm will stretch out
  99. void Arm3DrawBack()
  100. {
  101.     pos3 -= 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 2 ลดลงทีละ 1 องศา
  102.     pos3 = (pos3 < 30) ? 30 : pos3; // จำกัดลดตำแหน่งแกนมุมลงต่ำสุดถึง 30 องศา
  103.     myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ ยืดออก
  104.     delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  105. }
  106. // the lower arm will draw back
  107. void Arm3StretchedOut()
  108. {
  109.     pos3 += 10; // ปรับค่าตัวแปรตำแหน่งแกนมุมปัจจุบันของเซอโวมอเตอร์ตัวที่ 3 เพิ่มขึ้นทีละ 1 องศา
  110.     pos3 = (pos3 > 100) ? 100 : pos3; // จำกัดเพิ่มตำแหน่งแกนมุมขึ้นสูงสุดถึง 100 องศา
  111.     myservo3.write(pos3);  // ควบคุมเซอร์โวมอเตอร์ตัวที่ 3 ให้แขนส่วนล่าง จะค่อย ๆ หดกลับ
  112.     delay(20);   // หน่วงเวลา 5 มิลลิวินาที่ เพื่อควบคุมความเร็วเซอโวมอเตอร์
  113. }
  114.  
RAW Paste Data