timor2542

[4 DOF Robot Arm Keyestudio][Lab 04][i-Duino UNO R3B] Auto Pickup and Paste Down

Aug 2nd, 2021
1,221
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. int pos1=80, pos2=60, pos3=130, pos4=0; // กำหนดค่าเริ่มต้นตำแหน่งแกนมุมของเซอร์โวมอเตอร์ของแต่ละตัว
  7.  
  8. void setup()
  9. {
  10.   delay(3000);      // หน่วงเวลา 3 วินาที
  11.   myservo1.attach(A1);  // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ต่อกับ i-Duino UNO R3B
  12.   myservo2.attach(A0);  // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ต่อกับ i-Duino UNO R3B
  13.   myservo3.attach(6);   // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ต่อกับ i-Duino UNO R3B
  14.   myservo4.attach(9);   // กำหนดขาสัญญาณเอาต์พุตที่เซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ต่อกับ i-Duino UNO R3B
  15.  
  16.   delay(5000);      // หน่วงเวลา 5 วินาที
  17.   myservo1.write(pos1); // ขับเซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ให้แกนหมุนไปยังตำแหน่ง pos1
  18.   delay(3000);      // หน่วงเวลา 3 วินาที
  19.   myservo2.write(pos2); // ขับเซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ให้แกนหมุนไปยังตำแหน่ง pos2
  20.   myservo3.write(pos3); // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  21.   myservo4.write(pos4); // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  22.   delay(3000);      // หน่วงเวลา 3 วินาที
  23. }
  24.  
  25. void loop()
  26. {
  27.   // หมุนไปทางขวา
  28.   for(pos1;pos1>0;pos1--) // ปรับปรุงค่า pos1
  29.   {
  30.     myservo1.write(pos1); // ขับเซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ให้แกนหมุนไปยังตำแหน่ง pos1
  31.     delay(5);      // หน่วงเวลา 5 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  32.   }
  33.   delay(1000);    // หน่วงเวลา 1 วินาที
  34.  
  35.   // คลายตัวมือจับ
  36.   for(pos4;pos4<100;pos4++) // ปรับปรุงค่า pos4
  37.   {
  38.     myservo4.write(pos4); // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  39.   }
  40.   delay(1000);    // หน่วงเวลา 1 วินาที
  41.  
  42.   // ขับเซอร์โวมอเตอร์ด้านขวาหมุนไปยังตำแหน่ง 50 องศา เพื่อยกระดับความสูงของตัวมือจับชิ้นส่วน
  43.   for(pos2;pos2>50;pos2--) // ปรับปรุงค่า pos2
  44.   {
  45.     myservo2.write(pos2); // ขับเซอร์โวมอเตอร์ตัวที่ 2 (Servo2) ให้แกนหมุนไปยังตำแหน่ง pos2
  46.     delay(20); // หน่วงเวลา 20 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  47.   }
  48.   // ขับเซอร์โวมอเตอร์ด้านซ้ายหมุนไปยังตำแหน่ง 25 องศา เพื่อยืดแขนออก
  49.   for(pos3;pos3>25;pos3--)
  50.   {
  51.     myservo3.write(pos3); // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  52.     delay(20); // หน่วงเวลา 20 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  53.   }
  54.   delay(1500);    // หน่วงเวลา 1.5 วินาที
  55.  
  56.   // หนีบตัวมือจับ
  57.   for(pos4;pos4>0;pos4--)   // ปรับปรุงค่า pos4
  58.   {
  59.     myservo4.write(pos4);  // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  60.   }
  61.   delay(1000);    // หน่วงเวลา 1 วินาที
  62.  
  63.   // ขับเซอร์โวมอเตอร์ด้านซ้ายหมุนไปยังตำแหน่ง 120 องศา เพื่อหดแขนกลับ
  64.   for(pos3;pos3<120;pos3++)
  65.   {
  66.     myservo3.write(pos3);  // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  67.     delay(20); // หน่วงเวลา 20 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  68.   }
  69.   delay(1000);    // หน่วงเวลา 1 วินาที
  70.  
  71.   // หมุนไปทางซ้าย
  72.   for(pos1;pos1<180;pos1++) // ปรับปรุงค่า pos1
  73.   {
  74.     myservo1.write(pos1); // ขับเซอร์โวมอเตอร์ตัวที่ 1 (Servo1) ให้แกนหมุนไปยังตำแหน่ง pos1
  75.     delay(5);      // หน่วงเวลา 5 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  76.   }
  77.   delay(1000);    // หน่วงเวลา 1 วินาที
  78.  
  79.   // ขับเซอร์โวมอเตอร์ด้านซ้ายหมุนไปยังตำแหน่ง 25 องศา เพื่อยืดแขนออก
  80.   for(pos3;pos3>25;pos3--)  // ปรับปรุงค่า pos3
  81.   {
  82.     myservo3.write(pos3);  // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  83.     delay(20); // หน่วงเวลา 20 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  84.   }
  85.   delay(1000);    // หน่วงเวลา 1 วินาที
  86.  
  87.   // คลายตัวมือจับ
  88.   for(pos4;pos4<100;pos4++)  // ปรับปรุงค่า pos4
  89.   {
  90.     myservo4.write(pos4); // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  91.   }
  92.   delay(1000);
  93.  
  94.   // ขับเซอร์โวมอเตอร์ด้านซ้ายหมุนไปยังตำแหน่ง 120 องศา เพื่อหดแขนกลับ
  95.   for(pos3;pos3<120;pos3++)
  96.   {
  97.     myservo3.write(pos3);  // ขับเซอร์โวมอเตอร์ตัวที่ 3 (Servo3) ให้แกนหมุนไปยังตำแหน่ง pos3
  98.     delay(20); // หน่วงเวลา 20 มิลลิวินาที (เพื่อปรับความเร็วของเซอร์โวมอเตอร์)
  99.   }
  100.   delay(1000);    // หน่วงเวลา 1 วินาที
  101.  
  102.   // หนีบตัวมือจับ
  103.   for(pos4;pos4>0;pos4--)   // ปรับปรุงค่า pos4
  104.   {
  105.     myservo4.write(pos4);  // ขับเซอร์โวมอเตอร์ตัวที่ 4 (Servo4) ให้แกนหมุนไปยังตำแหน่ง pos4
  106.   }
  107.   delay(1000);    // หน่วงเวลา 1 วินาที
  108. }
  109.  
RAW Paste Data