คำอธิบาย
Biped Robot เรียนรู้การเขียนโปรแกรมควบคุมหุ่นยนต์ เดิน 2 ขา ที่มีอินพุทเป็น อัลตราโซนิคเซนเซอร์ ใช้ Micro servo motors (SG90 9G) 4ชิ้น 4DOF บอร์ดควบคุมเป็น Biped Robot Arduino Nano
ฟังก์ชั่นพื้นฐาน: การเดิน 2ขา, การเดินหลบหลีกสิ่งกีดขวาง, การควบคุมไร้สายด้วยไออาร์ รีโมท IR Remote
รองรับการเขียนโปรแกรมหลากหลายรูปแบบ
- Arduino IDE, Graphical programming แบบลากและวาง ตามแพลทฟอร์ม Blockly แพลทฟอร์ม Scratch เช่น Mixly เป็นต้น
- เหมาะสำหรับ อายุ 8 ปีขึ้นไป เด็กๆ จะได้เรียนรู้ หุ่นยนต์ ในรูปแบบของ STEM Education ด้วยไปพร้อมๆกัน
รายละเอียด:
- Microcontroller: Arduino Nano
- Power: AA ฺBattery 4 ก้อน
- Micro servo motors (SG90 9G) 4ชิ้น 4 degree of freedom (DOF)
มีอะไรเตรียมไว้ให้ในชุด
- Biped Robot หุ่นยนต์เดิน 2 ขา ประกอบแล้ว (ไม่รวมแบตเตอรี่)
- Arduino Nano Compatible
- Robot Shield
- Sensor อัลตร้าโซนิค
- IR รีโมทคอนโทรล และ ตัวรับ IR
- สายโปรแกรม USB
ตัวอย่าง การเขียนโปรแกรม Arduino ควบคุมหุ่นยนต์ สามารถใช้โปรแกรมอาดูโน่ได้ด้วย
โดยโปรแกรมตัวอย่างประกอบด้วย
4 DOF Servo
- RU upper-right servo to pin 3
- RL lower-right servo to pin 5
- LU upper-left servo to pin 6
- LL lower-left servo to pin 9
IR remote อินฟราเรด
- ir_run = 0x00FF629D; / ปุ่ม CH
- ir_back = 0x00FFA857; / ปุ่ม +
- ir_left = 0x00FF22DD; / ปุ่ม <<
- ir_right = 0x00FFC23D; // ปุ่ม> ||
- ir_stop = 0x00FF02FD; / ปุ่มปุ่ม >> |
- ir_left_turn = 0x00ffE01F; / ปุ่ม –
- ir_right_turn = 0x00FF906F; / ปุ่ม EQ
- โปรแกรมการเซ็ตซีโร่เซอร์โว
- โปรแกรมตัวอย่างเดินหน้าถอยหลังเลี้ยวซ้ายเลี้ยว
- โปรแกรมหุ่นยนต์แดนซ์
- โปรแกรมหุ่นยนต์เดินหลบหลีกสิ่งกีดขวางโดยใช้เซ็นเซอร์อัลตร้าโซนิค
- โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้รีโมทคอนโทรล
- โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้คอนเดนเซอร์ไมโครโฟน
- โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้ Sensor LDR
ทดสอบ ไออาร์รีโมทก่อนเริ่มควบคุมหุ่นยนต์
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 |
/* * IRremote: IRrecvDemo - demonstrates receiving IR codes with IRrecv * An IR detector/demodulator must be connected to the input RECV_PIN. * Version 0.1 July, 2009 * Copyright 2009 Ken Shirriff * http://arcfn.com */ #include <IRremote.h> int RECV_PIN = A4; IRrecv irrecv(RECV_PIN); decode_results results; void setup() { Serial.begin(9600); // In case the interrupt driver crashes on setup, give a clue // to the user what's going on. Serial.println("Enabling IRin"); irrecv.enableIRIn(); // Start the receiver Serial.println("Enabled IRin"); } void loop() { if (irrecv.decode(&results)) { Serial.println(results.value, HEX); irrecv.resume(); // Receive the next value } delay(100); } |
ค่า Data ของข้อมูลปุ่มกดที่ได้จาก ไออาร์รีโมท (รีโมทแต่ละรุ่น Data จะไม่เหมือนกัน)
| KEY | DATA |
| CH+ | E318261B |
| CH | 511DBB |
| CH- | EE886D7F |
| |<< | 52A3D41F |
| >>| | D7E84B1B |
| >|| | 20FE4DBB |
| – | F076C13B |
| + | A3C8EDDB |
| EQ | E5CFBD7F |
| 0 | C101E57B |
| 100 | 97483BFB |
| 200 | F0C41643 |
| 1 | 9716BE3F |
| 2 | 3D9AE3F7 |
| 3 | 6182021B |
| 4 | 8C22657B |
| 5 | 488F3CBB |
| 6 | 449E79F |
| 7 | 32C6FDF7 |
| 8 | 1BC0157B |
| 9 | 3EC3FC1B |
ตัวอย่าง การเขียนโปรแกรม Arduino ควบคุมหุ่นยนต์ แบบใช้ IR Remote
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 |
// IR Remote #include <IRremote.h>//Contains infrared library key points #include "VarSpeedServo.h" // VarSpeedServo library int RECV_PIN = A4;//Port declaration IRrecv irrecv(RECV_PIN); decode_results results;//Structure statement int on = 0;//Flag bit unsigned long last = millis(); long ir_run_car = 0x511DBB;//button CH long ir_back_car = 0xA3C8EDDB;//button + long ir_left_car = 0x52A3D41F;//button << long ir_right_car = 0x20FE4DBB;//button >|| long ir_stop_car = 0xD7E84B1B;//button >>| long ir_left_turn = 0xF076C13B;//button - long ir_right_turn = 0xE5CFBD7F;//button EQ //============================== VarSpeedServo RU; //Right Upper VarSpeedServo RL; //Right Lower VarSpeedServo LU; //Left Upper VarSpeedServo LL; //Left Lower int beep=A3;//Define the buzzer Digital A3 interface int Echo = A1; // Echo (P2.0) int Trig = A0; // Trig (P2.1) int Distance = 0; //vel(min), delay_Forward(max) = (5, 2000) const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750) const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50) //wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500) const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL ) const int delay_tim = 300; //Delay 750ms int RU_Degree = 0, LU_Degree = array_cal[2] + 5; const int num1 = 6; const int array_forward[num1][4] = { {0,-40,0,-20}, {30,-40,40,-20}, {30,0,40,0}, {0,20,0,40}, {-30,20,-30,40}, {-30,0,-30,0}, }; const int num2 = 6; const int array_back[num2][4] ={ {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {40,0,50,0}, {40,-30,50,-40}, {0,-30,0,-40}, }; const int num3 = 5; // Define the array number 5 /* When I encountered obstacles, turned around. Each set requires 5 sets of data. Angle change of each group of 4 servo systems*/ const int array_left[num3][4] = { {-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees. {-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance {0,30,0,30}, {30,0,30,0}, {0,0,0,0}, }; //Define 5 arrays, each containing 4 elements to store the current angular value of the servo。 const int num4 = 5; const int array_right[num4][4] = { {40,0,50,0}, {40,-30,50,-40}, {0,-30,0,-40}, {-30,0,-40,0}, {0,0,0,0}, }; //#define INSTALL //#define CALIBRATION #define RUN void Servo_Init() { RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9 RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10 LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11 LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12 } void Adjust() // Avoid the servo's fast spinning in initialization { // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) { RU.write(RU_Degree); // in steps of 1 degree LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree' delay(15); // waits 15ms for the servo to reach the RU_Degreeition } } void TooClose() { digitalWrite(Trig, LOW); // Give the trigger pin low 2μs delayMicroseconds(2); digitalWrite(Trig, HIGH); // Give the trigger pin a high level of 10μs, here at least 10μs delayMicroseconds(10); digitalWrite(Trig, LOW); // Continue to low power to the trigger pin float Fdistance = pulseIn(Echo, HIGH); // Read high time (unit: microsecond) Fdistance= Fdistance/58; // divide by 58 equals centimeter, Y m = (X seconds * 344)/2 // X seconds = ( 2 * Y meters) / 344 = = "X seconds = 0.0058 * Y meters = = "cm = microseconds / 58 Serial.print("Distance:"); //Output distance (unit: cm) Serial.println(Fdistance); //Display distance Distance = Fdistance; } void Forward(unsigned int n_delay) { for(int x=0; x<num1; x++) { RU.slowmove (array_cal[0] + array_forward[x][0] , vel); RL.slowmove (array_cal[1] + array_forward[x][1] , vel); LU.slowmove (array_cal[2] + array_forward[x][2] , vel); LL.slowmove (array_cal[3] + array_forward[x][3] , vel); delay(n_delay); //delay_Forward :700 } } void stop() { RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(delay_tim); } void Backward(unsigned int n_delay) { for(int y=0; y<num2; y++) { RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back); delay(n_delay); //delay_Forward:700 } } void left_turn(unsigned int n_delay) { for(int z=0; z<2; z++) { for(int y=0; y<num3; y++) { RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back); delay(n_delay); //delay_Back:750 } } } void right_turn(unsigned int n_delay) { for(int z=0; z<2; z++) { for(int y=0; y<num4; y++) { RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back); delay(n_delay); //delay_Back:750 } } } void n_Forward(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { Forward(n_delay); } } void n_Backward(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { Backward(n_delay); } } void n_left_turn(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { left_turn(n_delay); } } void n_right_turn(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { right_turn(n_delay); } } void setup() { #ifdef INSTALL Servo_Init(); RU.slowmove (90 , vel); RL.slowmove (90 , vel); LU.slowmove (90 , vel); LL.slowmove (90 , vel); while(1); #endif #ifdef CALIBRATION Servo_Init(); RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo. RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); while(1); #endif #ifdef RUN Servo_Init(); RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); #endif irrecv.enableIRIn(); // tart the infrared remote control pinMode(Echo, INPUT); // Define the ultrasonic input pin pinMode(Trig, OUTPUT); // Define the ultrasonic output pin pinMode(beep,OUTPUT); // buzzer Serial.begin(9600); } void dump(decode_results *results) { int count = results->rawlen; if (results->decode_type == UNKNOWN) { //Serial.println("Could not decode message"); // brake(); } } void loop() { if (irrecv.decode(&results)) //Call library function: decode { // If it's been at least 1/4 second since the last // IR received, toggle the relay if (millis() - last > 250) //Determine the signal received { on = !on;//Mark position digitalWrite(13, on ? HIGH : LOW);//The signal received on the board flashesled dump(&results);//Decoding infrared signal } if (results.value == ir_run_car )//buttonCH { n_Forward(4,250); //go ahead } if (results.value == ir_back_car )//button+ { n_Backward(2,650);//Retreat } if (results.value == ir_left_car )//button<< { n_left_turn(1,650);//Turn left } if (results.value == ir_right_car )//button>|| { n_right_turn(1,650);//Turn right } if (results.value == ir_stop_car )//button>>| { stop();//stop } last = millis(); irrecv.resume(); // Receive the next value } } |
ตัวอย่าง Upload โปรแกรม Arduino ให้หุ่นเต้น (Dance)
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 |
//dance #include "VarSpeedServo.h" VarSpeedServo RU; VarSpeedServo RL; VarSpeedServo LU; VarSpeedServo LL; const int vel = 50, vel_Back = 15, vel_turn= 15; const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; int vel_Dance1 = 30,vel_Dance2 = 25, vel_Dance3 = 40; int delay_Dance1 = 300,delay_Dance2 = 750, delay_Dance3 = 200; int vel_Dance4 = 20, delay_Dance4 = 400; const int array_cal[4] = {90,90,90,90}; int RU_Degree = 0, LU_Degree = array_cal[2] + 5; const int num1 = 6; const int array_forward[num1][4] = { {0,-40,0,-20}, {30,-40,30,-20}, {30,0,30,0}, {0,20,0,40}, {-30,20,-30,40}, {-30,0,-30,0}, }; const int num3 = 5; const int array_left[num3][4] = { {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {30,0,30,0}, {0,0,0,0}, }; const int num4 = 5; const int array_right[num4][4] = { {40,0,20,0}, {40,-30,20,-20}, {0,-30,0,-20}, {-30,0,-30,0}, {0,0,0,0}, }; const int num_dance1 = 10; const int array_dance1[num_dance1][4] ={ {0,-20,0,0}, {0,-40,0,20}, {0,-20,0,40}, {0,0,0,20}, {0,0,0,0}, {0,0,0,20}, {0,-20,0,40}, {0,-40,0,20}, {0,-20,0,0}, {0,0,0,0}, }; const int num_dance2 = 32; const int array_dance2[num_dance2][4] = { {20,0,40,0}, {20,-30,10,-30}, {20,-30,10,-30}, {20,-30,40,-30}, {20,-30,10,-30}, {20,-30,40,-30}, {20,0,40,-30}, {20,80,40,-30}, {20,0,40,-30}, {20,-80,40,-30}, {20,0,40,-30}, {20,80,40,-30}, {20,0,40,-30}, {20,-30,40,-30}, {20,0,40,0}, {0,0,0,0}, {-40,0,-20,0}, {-40,40,-20,30}, {-20,40,-20,30}, {-40,40,-20,30}, {-20,40,-20,30}, {-40,40,-20,30}, {-40,40,-20,0}, {-40,40,-20,-80}, {-40,40,-20,0}, {-40,40,-20,80}, {-40,40,-20,0}, {-40,40,-20,-80}, {-40,40,-20,0}, {-40,40,-20,30}, {-40,0,-20,0}, {0,0,0,0}, }; const int num_dance4 = 20; const int array_dance4[num_dance4][4] = { {0,-20,0,0}, {0,-40,0,20}, {0,0,0,0}, {0,-20,0,20}, {0,0,0,0}, {0,-20,0,20}, {0,0,0,0}, {0,-20,0,20}, {0,0,0,0}, {0,-30,0,30}, {0,0,0,0}, {0,-30,0,30}, {0,0,0,0}, {0,-30,0,30}, {0,0,0,0}, {0,-40,0,40}, {0,-40,0,40}, {0,-50,0,50}, {0,0,0,0,}, }; const int num_dance3 = 8; const int array_dance3[num_dance3][4] ={ {0,-40,0,0}, {20,-30,20,20}, {40,0,40,30}, {0,0,0,40}, {-20,-20,-20,30}, {-40,-30,-40,0}, {0,-40,0,0}, {0,0,0,0}, }; const int num2 = 6; const int array_back[num2][4] ={ {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {40,0,20,0}, {40,-30,20,-30},{0,-30,0,-30}, }; #define RUN void Servo_Init() { RU.attach(3); RL.attach(5); LU.attach(6); LL.attach(9); } void Adjust() { for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) { RU.write(RU_Degree); LU.write(LU_Degree--); delay(15); } } //bool TooClose() //{ //int tooclose = 0; //for(int a=0; a<5; a++) { // delay(50); //int din = sonar.ping_in(); //if (din < 7 && din > 0) tooclose++; // } //if (tooclose < 5) return 1; //return 0; //} void Forward() { for(int x=0; x<num1; x++) { RU.slowmove (array_cal[0] + array_forward[x][0] , vel); RL.slowmove (array_cal[1] + array_forward[x][1] , vel); LU.slowmove (array_cal[2] + array_forward[x][2] , vel); LL.slowmove (array_cal[3] + array_forward[x][3] , vel); delay(delay_Forward); } } void Turnleft() { for(int z=0; z<2; z++) { for(int y=0; y<num3; y++) { RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back); delay(delay_Back); } } } void Turnright(){ for(int z=0; z<2; z++) { for(int y=0; y<num4; y++) { RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back); delay(delay_Back); } } } void Slide_2_Left(int times) { for(int time1 = 0; time1 < times; time1++) { for(int z=0; z<5; z++) { vel_Dance2 = 50; RU.slowmove (array_cal[0] + array_dance1[z][0] , vel_Dance1); LU.slowmove (array_cal[2] + array_dance1[z][2] , vel_Dance1); LL.slowmove (array_cal[3] + array_dance1[z][3] , vel_Dance1); delay(delay_Dance1);} } } void Left_Foot_Support(){ for(int z=0; z<16; z++) { if ( z > 5 && z < 14) { vel_Dance2 = 50;delay_Dance2 = 200; } else { vel_Dance2 = 25; delay_Dance2 = 750; } RU.slowmove (array_cal[0] + array_dance2[z][0] , vel_Dance2); RL.slowmove (array_cal[1] + array_dance2[z][1] , vel_Dance2); LU.slowmove (array_cal[2] + array_dance2[z][2] , vel_Dance2); LL.slowmove (array_cal[3] + array_dance2[z][3] , vel_Dance2); delay(delay_Dance2); } } void Dancing1_2() { Slide_2_Left(2); Left_Foot_Support(); } void Dancing4() { for(int z=0; z<num_dance4; z++) { if ( z > 17) { vel_Dance4 = 10; delay_Dance4 = 1500;} else { vel_Dance4 = 40; delay_Dance4 = 400;} RU.slowmove (array_cal[0] + array_dance4[z][0] , vel_Dance4); RL.slowmove (array_cal[1] + array_dance4[z][1] , vel_Dance4); LU.slowmove (array_cal[2] + array_dance4[z][2] , vel_Dance4); LL.slowmove (array_cal[3] + array_dance4[z][3] , vel_Dance4); delay(delay_Dance4); } } void Dancing3(int Times = 1, int Vel = 40, int Delay = 250, int low = 0, int high = 0) { for(int time3=0; time3<Times; time3++) { for(int z=0; z<6; z++) { if ( time3 > 1 && time3 < 4) { vel_Dance3 = Vel; delay_Dance3 = Delay; } else { vel_Dance3 = 40; delay_Dance3 = 200; } RU.slowmove (array_cal[0] + array_dance3[z][0] , vel_Dance3); RL.slowmove (array_cal[1] + array_dance3[z][1] , vel_Dance3); LU.slowmove (array_cal[2] + array_dance3[z][2] , vel_Dance3); LL.slowmove (array_cal[3] + array_dance3[z][3] , vel_Dance3); delay(delay_Dance3); } } for(int z=6; z<8; z++) { RU.slowmove (array_cal[0] + array_dance3[z][0] , vel_Dance3); RL.slowmove (array_cal[1] + array_dance3[z][1] , vel_Dance3); LU.slowmove (array_cal[2] + array_dance3[z][2] , vel_Dance3); LL.slowmove (array_cal[3] + array_dance3[z][3] , vel_Dance3); delay(delay_Dance3); } } void Backward(){ for(int y=0; y<num2; y++) { RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back); delay(delay_Forward); } } void setup() { #ifdef RUN Servo_Init(); Adjust(); RL.slowmove (array_cal[1] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); #endif } void loop() { Dancing3(5,20,400); } |
โปรแกรม Arduino เซตซีโร่ เซอโวร์
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 |
// 1. Biped robot servo zero adjustment program #include "VarSpeedServo.h" //contain VarSpeedServo library VarSpeedServo RU; //Right Upper VarSpeedServo RL; //Right Lower VarSpeedServo LU; //Left Upper VarSpeedServo LL; //Left Lower int beep=A3;//Define the buzzer Digital A3 interface int Echo = A1; // Echo pin(P2.0) int Trig = A0; // Trig pin(P2.1) int Distance = 0; //vel(min), delay_Forward(max) = (5, 2000) const int vel = 20, vel_Back = 10; //vel(mid), delay_Forward(mid) = (20, 750) const int delay_Forward = 750, delay_Back = 1000; //vel(max), delay_Forward(min)= (256, 50) //wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500) const int array_cal[4] = {90,80,100,90}; // Define the angular adjustment of servo (RU, RL, LU, LL ) const int delay_tim = 300; //Delay 750ms int RU_Degree = 0, LU_Degree = array_cal[2] + 5; const int num1 = 6; const int array_forward[num1][4] = { {0,-40,0,-20}, {30,-40,30,-20}, {30,0,30,0}, {0,20,0,40}, {-30,20,-30,40}, {-30,0,-30,0}, }; const int num2 = 6; const int array_back[num2][4] ={ {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {40,0,20,0}, {40,-30,20,-30}, {0,-30,0,-30}, }; const int num3 = 5; // Define the array number to be 5. /* Turned when you encountered an obstacle. Each set requires 5 sets of data. Angle change of 4 servo systems per group*/ const int array_left[num3][4] = { {-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees. {-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance {0,30,0,30}, {30,0,30,0}, {0,0,0,0}, }; // Define 5 arrays, each containing 4 elements to store the current angle value of the servo.) const int num4 = 5; const int array_right[num4][4] = { {40,0,20,0}, {40,-30,20,-20}, {0,-30,0,-20}, {-30,0,-30,0}, {0,0,0,0}, }; //#define INSTALL #define CALIBRATION //#define RUN void Servo_Init() { RU.attach(3); // Connect the signal wire of the upper-right servo to pin 3 RL.attach(5); // Connect the signal wire of the lower-right servo to pin 5 LU.attach(6); // Connect the signal wire of the upper-left servo to pin 6 LL.attach(9); // Connect the signal wire of the lower-left servo to pin 9 } void Adjust() // Avoid the servo's fast spinning in initialization { // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) { RU.write(RU_Degree); // in steps of 1 degree LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree' delay(15); // waits 15ms for the servo to reach the RU_Degreeition } } void TooClose() { digitalWrite(Trig, LOW); // Give the trigger pin low level 2μs delayMicroseconds(2); digitalWrite(Trig, HIGH); // Give the trigger pin a high level of 10μs, here at least 10μs delayMicroseconds(10); digitalWrite(Trig, LOW); // Continue to low power to the trigger pin float Fdistance = pulseIn(Echo, HIGH); // Read high time (unit: microsecond) Fdistance= Fdistance/58; //Why divide by 58 equals centimeter, Y m = (X seconds * 344)/2 // X seconds = ( 2 * Y meters) / 344 = = "X seconds = 0.0058 * Y meters = = "cm = microseconds / 58 Serial.print("Distance:"); // Output distance (unit: cm)) Serial.println(Fdistance); //Display distance Distance = Fdistance; } void Forward() { for(int x=0; x<num1; x++) { RU.slowmove (array_cal[0] + array_forward[x][0] , vel); RL.slowmove (array_cal[1] + array_forward[x][1] , vel); LU.slowmove (array_cal[2] + array_forward[x][2] , vel); LL.slowmove (array_cal[3] + array_forward[x][3] , vel); delay(delay_Forward); } } void Backward() { for(int y=0; y<num2; y++) { RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back); delay(delay_Back); } } void left_turn() { for(int y=0; y<num3; y++) // When turning, the angle and speed of the 4 servos with every step. { RU.slowmove (array_cal[0] + array_left[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead. RL.slowmove (array_cal[1] + array_left[y][1] , vel); LU.slowmove (array_cal[2] + array_left[y][2] , vel); LL.slowmove (array_cal[3] + array_left[y][3] , vel); delay(delay_tim); // Delay delay_tim } } void right_turn() { for(int y=0; y<num4; y++) // When turning, the angle and speed of the 4 servos with every step. { RU.slowmove (array_cal[0] + array_right[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead. RL.slowmove (array_cal[1] + array_right[y][1] , vel); LU.slowmove (array_cal[2] + array_right[y][2] , vel); LL.slowmove (array_cal[3] + array_right[y][3] , vel); delay(delay_tim); // Delay delay_tim } } void setup() { #ifdef INSTALL Servo_Init(); RU.slowmove (90 , vel); RL.slowmove (90 , vel); LU.slowmove (90 , vel); LL.slowmove (90 , vel); while(1); #endif #ifdef CALIBRATION Servo_Init(); RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo. RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); while(1); #endif #ifdef RUN Servo_Init(); RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); #endif pinMode(Echo, INPUT); // Define ultrasonic input feet pinMode(Trig, OUTPUT); // Define the ultrasonic output pin pinMode(beep,OUTPUT); //buzzer } void loop() { TooClose(); if(Distance>30) { Forward(); } else { digitalWrite(beep,HIGH); //Buzzer for(int z=0; z<4; z++) //Turn right to turn 4, and adjust your steering value even more. { right_turn(); digitalWrite(beep,LOW); } } } |
โปรแกรม Arduino ควบคุมหุ่นยนต์เดินด้วยเสียง โดยใช้คอนเดนเซอร์ไมโครโฟน
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 |
// Sound #include "VarSpeedServo.h" // VarSpeedServo library //============================== VarSpeedServo RU; //Right Upper VarSpeedServo RL; //Right Lower VarSpeedServo LU; //Left Upper VarSpeedServo LL; //Left Lower int beep=A3;//Define the buzzer Digital A3 interface int Echo = A1; // Echo (P2.0) int Trig = A0; // Trig (P2.1) int Distance = 0; int sensorValue = 0; //vel(min), delay_Forward(max) = (5, 2000) const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750) const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50) //wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500) const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL ) const int delay_tim = 300; //Delay 750ms int RU_Degree = 0, LU_Degree = array_cal[2] + 5; const int num1 = 6; const int array_forward[num1][4] = { {0,-40,0,-20}, {30,-40,40,-20}, {30,0,40,0}, {0,20,0,40}, {-30,20,-30,40}, {-30,0,-30,0}, }; const int num2 = 6; const int array_back[num2][4] ={ {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {40,0,50,0}, {40,-30,50,-40}, {0,-30,0,-40}, }; const int num3 = 5; // Define the array number 5 /* When I encountered obstacles, turned around. Each set requires 5 sets of data. Angle change of each group of 4 servo systems*/ const int array_left[num3][4] = { {-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees. {-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance {0,30,0,30}, {30,0,30,0}, {0,0,0,0}, }; //Define 5 arrays, each containing 4 elements to store the current angular value of the servo。 const int num4 = 5; const int array_right[num4][4] = { {40,0,50,0}, {40,-30,50,-40}, {0,-30,0,-40}, {-30,0,-40,0}, {0,0,0,0}, }; #define RUN void Servo_Init() { RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9 RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10 LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11 LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12 } void Adjust() // Avoid the servo's fast spinning in initialization { // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) { RU.write(RU_Degree); // in steps of 1 degree LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree' delay(15); // waits 15ms for the servo to reach the RU_Degreeition } } void Forward(unsigned int n_delay) { for(int x=0; x<num1; x++) { RU.slowmove (array_cal[0] + array_forward[x][0] , vel); RL.slowmove (array_cal[1] + array_forward[x][1] , vel); LU.slowmove (array_cal[2] + array_forward[x][2] , vel); LL.slowmove (array_cal[3] + array_forward[x][3] , vel); delay(n_delay); //delay_Forward :700 } } void stop() { RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(delay_tim); } void Backward(unsigned int n_delay) { for(int y=0; y<num2; y++) { RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back); delay(n_delay); //delay_Forward:700 } } void left_turn(unsigned int n_delay) { for(int z=0; z<2; z++) { for(int y=0; y<num3; y++) { RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back); delay(n_delay); //delay_Back:750 } } } void right_turn(unsigned int n_delay) { for(int z=0; z<2; z++) { for(int y=0; y<num4; y++) { RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back); delay(n_delay); //delay_Back:750 } } } void n_Forward(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { Forward(n_delay); } } void n_Backward(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { Backward(n_delay); } } void n_left_turn(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { left_turn(n_delay); } } void n_right_turn(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { right_turn(n_delay); } } void setup() { Servo_Init(); RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); Serial.begin(9600); } void loop() { sensorValue = analogRead(A6); if (sensorValue > 20 )//buttonCH { n_Forward(4,250); //go ahead delay(2000); } } |
โปรแกรม Arduino ควบคุมหุ่นยนต์เดินด้วยแสง โดยใช้เซนเซอร์ แอลดีอาร์
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 |
// light #include "VarSpeedServo.h" // VarSpeedServo library //============================== VarSpeedServo RU; //Right Upper VarSpeedServo RL; //Right Lower VarSpeedServo LU; //Left Upper VarSpeedServo LL; //Left Lower int beep=A3;//Define the buzzer Digital A3 interface int sensorValue = 0; //vel(min), delay_Forward(max) = (5, 2000) const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750) const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50) //wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500) const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL ) const int delay_tim = 300; //Delay 750ms int RU_Degree = 0, LU_Degree = array_cal[2] + 5; const int num1 = 6; const int array_forward[num1][4] = { {0,-40,0,-20}, {30,-40,40,-20}, {30,0,40,0}, {0,20,0,40}, {-30,20,-30,40}, {-30,0,-30,0}, }; const int num2 = 6; const int array_back[num2][4] ={ {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {40,0,50,0}, {40,-30,50,-40}, {0,-30,0,-40}, }; const int num3 = 5; // Define the array number 5 /* When I encountered obstacles, turned around. Each set requires 5 sets of data. Angle change of each group of 4 servo systems*/ const int array_left[num3][4] = { {-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees. {-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance {0,30,0,30}, {30,0,30,0}, {0,0,0,0}, }; //Define 5 arrays, each containing 4 elements to store the current angular value of the servo。 const int num4 = 5; const int array_right[num4][4] = { {40,0,50,0}, {40,-30,50,-40}, {0,-30,0,-40}, {-30,0,-40,0}, {0,0,0,0}, }; #define RUN void Servo_Init() { RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9 RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10 LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11 LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12 } void Adjust() // Avoid the servo's fast spinning in initialization { // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) { RU.write(RU_Degree); // in steps of 1 degree LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree' delay(15); // waits 15ms for the servo to reach the RU_Degreeition } } void Forward(unsigned int n_delay) { for(int x=0; x<num1; x++) { RU.slowmove (array_cal[0] + array_forward[x][0] , vel); RL.slowmove (array_cal[1] + array_forward[x][1] , vel); LU.slowmove (array_cal[2] + array_forward[x][2] , vel); LL.slowmove (array_cal[3] + array_forward[x][3] , vel); delay(n_delay); //delay_Forward :700 } } void stop() { RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(delay_tim); } void Backward(unsigned int n_delay) { for(int y=0; y<num2; y++) { RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back); delay(n_delay); //delay_Forward:700 } } void left_turn(unsigned int n_delay) { for(int z=0; z<2; z++) { for(int y=0; y<num3; y++) { RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back); delay(n_delay); //delay_Back:750 } } } void right_turn(unsigned int n_delay) { for(int z=0; z<2; z++) { for(int y=0; y<num4; y++) { RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back); delay(n_delay); //delay_Back:750 } } } void n_Forward(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { Forward(n_delay); } } void n_Backward(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { Backward(n_delay); } } void n_left_turn(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { left_turn(n_delay); } } void n_right_turn(unsigned char n_step,unsigned int n_delay) { unsigned char z; for(z = 0;z < n_step; z++) { right_turn(n_delay); } } void setup() { Servo_Init(); RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); Serial.begin(9600); } void loop() { sensorValue = analogRead(A5); if (sensorValue > 900 )//ค่าเซนเซอร์มากกว่า 900 ให้เดินหน้า { n_Forward(4,250); //go ahead delay(2000); } } |
โปรแกรม Arduino ควบคุมหุ่นยนต์เดินด้วย อัลตร้าโซนิคเซ็นเซอร์ Ultrasonic sensor
int Trig = A0; // Trig (P2.1)
int Echo = A1; // Echo (P2.0)
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 |
//4. Biped robot ultrasonic obstacle avoidance #include "VarSpeedServo.h" // VarSpeedServo library VarSpeedServo RU; //Right Upper VarSpeedServo RL; //Right Lower VarSpeedServo LU; //Left Upper VarSpeedServo LL; //Left Lower int beep=A3;//Define the buzzer Digital A3 interface int Trig = A0; // Trig (P2.1) int Echo = A1; // Echo (P2.0) int Distance = 0; //vel(min), delay_Forward(max) = (5, 2000) const int vel = 20, vel_Back = 10; //vel(mid), delay_Forward(mid) = (20, 750) const int delay_Forward = 750, delay_Back = 1000; //vel(max), delay_Forward(min)= (256, 50) //wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500) const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL ) const int delay_tim = 300; //Delay 750ms int RU_Degree = 0, LU_Degree = array_cal[2] + 5; const int num1 = 6; const int array_forward[num1][4] = { {0,-40,0,-20}, {30,-40,30,-20}, {30,0,30,0}, {0,20,0,40}, {-30,20,-30,40}, {-30,0,-30,0}, }; const int num2 = 6; const int array_back[num2][4] ={ {-40,0,-20,0}, {-40,30,-20,30}, {0,30,0,30}, {40,0,20,0}, {40,-30,20,-30}, {0,-30,0,-30}, }; const int num3 = 5; // Define the array number to be 5. const int array_left[num3][4] = { {-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees. {-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance {0,30,0,30}, {30,0,30,0}, {0,0,0,0}, }; const int num4 = 5; const int array_right[num4][4] = { {40,0,20,0}, {40,-30,20,-20}, {0,-30,0,-20}, {-30,0,-30,0}, {0,0,0,0}, }; //#define INSTALL //#define CALIBRATION #define RUN void Servo_Init() { RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9 RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10 LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11 LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12 } void Adjust() // Avoid the servo's fast spinning in initialization { // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) { RU.write(RU_Degree); // in steps of 1 degree LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree' delay(15); // waits 15ms for the servo to reach the RU_Degreeition } } void TooClose() { digitalWrite(Trig, LOW); delayMicroseconds(2); digitalWrite(Trig, HIGH); delayMicroseconds(10); digitalWrite(Trig, LOW); float Fdistance = pulseIn(Echo, HIGH); Fdistance= Fdistance/58; Serial.print("Distance:"); Serial.println(Fdistance); Distance = Fdistance; } void Forward() { for(int x=0; x<num1; x++) { RU.slowmove (array_cal[0] + array_forward[x][0] , vel); RL.slowmove (array_cal[1] + array_forward[x][1] , vel); LU.slowmove (array_cal[2] + array_forward[x][2] , vel); LL.slowmove (array_cal[3] + array_forward[x][3] , vel); delay(delay_Forward); } } void Backward() { for(int y=0; y<num2; y++) { RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back); RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back); LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back); LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back); delay(delay_Back); } } void left_turn() { for(int y=0; y<num3; y++) // When turning, the angle and speed of the 4 servos with every step. { RU.slowmove (array_cal[0] + array_left[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead. RL.slowmove (array_cal[1] + array_left[y][1] , vel); LU.slowmove (array_cal[2] + array_left[y][2] , vel); LL.slowmove (array_cal[3] + array_left[y][3] , vel); delay(delay_tim); // Delay delay_tim } } void right_turn() { for(int y=0; y<num4; y++) // When turning, the angle and speed of the 4 servos with every step. { RU.slowmove (array_cal[0] + array_right[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead. RL.slowmove (array_cal[1] + array_right[y][1] , vel); LU.slowmove (array_cal[2] + array_right[y][2] , vel); LL.slowmove (array_cal[3] + array_right[y][3] , vel); delay(delay_tim); // Delay delay_tim } } void setup() { #ifdef INSTALL Servo_Init(); RU.slowmove (90 , vel); RL.slowmove (90 , vel); LU.slowmove (90 , vel); LL.slowmove (90 , vel); while(1); #endif #ifdef CALIBRATION Servo_Init(); RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo. RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); while(1); #endif #ifdef RUN Servo_Init(); RU.slowmove (array_cal[0] , vel); RL.slowmove (array_cal[1] , vel); LU.slowmove (array_cal[2] , vel); LL.slowmove (array_cal[3] , vel); delay(2000); #endif pinMode(Echo, INPUT); pinMode(Trig, OUTPUT); pinMode(beep,OUTPUT); } void loop() { TooClose(); if(Distance>30) { Forward(); } else { digitalWrite(beep,HIGH); for(int z=0; z<4; z++) { right_turn(); digitalWrite(beep,LOW); } } } |
เริ่มต้นใช้งาน Mixly กับ Robot Biped หุ่นยนต์เดิน 2ขา ด้วย Arduino Nano
รีวิว
ยังไม่มีบทวิจารณ์