Components and supplies
3
Ultrasonic Sensor - HC-SR04 (Generic)
5
MG90S Metal Geared Micro Servo
1
Portable Power Pack for Smartphones (2 Channels: 1A, and 2.1A)
1
Arduino Pro Mini 328 - 5V/16MHz
4
Photodiode
4
Resistor 100k ohm
Tools and machines
1
Soldering iron (generic)
1
Hot glue gun (generic)
Apps and platforms
1
Arduino IDE
Project description
Code
WALTER.ino
arduino
1/* 2WALTER - THE 4 LEGGED PHOTOVORE 3 4This Arduino sketch is my attemp to build a 5 servo quadruped (4 legged) robot named "WALTER". 5 6In order to use this sketch, you might need to change some values for your convenient or to adjust to your own hardware set up. 7Find (Ctrl + F) these marks to easily search which values might need to be changed: 8 - **** : These marks mean they are servos' center potition and have to calibrated (your robot legs' potitioning when it's idle). 9 - *** : These marks mean arduino pins asignment (sensors & servos connection to arduino). Refer to this when you build the robot. 10 - ** : These marks mean that the values optionally can be changed according to your taste (legs' step width, how much to turn when sensing light/obstacle, etc). Just leave it as is if you don't know what you were doing. 11 12You can use this sketch at your own risk.. and, it's provided as is and.. uh.. 13What're those all about copyrights stuff people use to write at published source code?? 14The point is I don't want to be held responsible if something bad happened when you were using this codes. 15Have fun! 16 17Yohanes Martedi - 2015 18*/ 19 20#include <Servo.h> 21 22// ****Calibrate servos' center angle (in microseconds because we'll use "xx.writeMicroseconds();" command). Start with 1500. 23const int ANGLE_mid_Shaft = 1520; 24const int ANGLE_mid_FLeft = 1550; 25const int ANGLE_mid_FRight = 1570; 26const int ANGLE_mid_BLeft = 1450; 27const int ANGLE_mid_BRight = 1450; 28 29const int ANGLE_sweep = 250; // **Set this value (in microseconds) to determine how wide the servos will sweep (legs' step width). Bigger value means wider sweep angle. 30const int ANGLE_res = 10; // **Set the servo movement resolution (in microseconds) at least at minimum servo's default dead band width (highest resolution) or more (less resolution). Example: SG90 servo dead band width is 10 microseconds. 31int sweepSPEED; // variable to determine how fast the servos will sweep. 32int sweepSPEED_Rand[3] = {4, 6, 8}; // **Servo speed (gait speed) will change randomly in 3 modes. Set the speed (in miliseconds) for each mode. Smaller value means faster. 33 34const int ANGLE_turnMAX = ANGLE_sweep * 1.5; // **Set this value to determine how much maximum the bot will turn toward light. Bigger value means bigger turn. 35const int ANGLE_turnNARROW = ANGLE_sweep * 0.25; // **Set this value to determine how much maximum the bot will turn avoiding objects at its sides in narrow space. Bigger value means bigger turn. 36 37const int SONAR_sum = 3; // Amount of sonars used. 38const int PHOTO_sum = 4; // Amount of photodiodes used. 39int PIN_trig[SONAR_sum] = {13, 11, 8}; // ***Set arduino pins connected to ultrasonic sensors' trigger pins; {front, left, right}. 40int PIN_ec[SONAR_sum] = {12, 10, 7}; // ***Set arduino pins connected to ultrasonic sensors' echo pins; {front, left, right}. 41int PIN_PHOTO[PHOTO_sum] = {2, 3, 1, 0}; // ***Set arduino analog input pins connected to photodiodes; {front left, front right, back left, back right}. 42 43const int distRotate = 25; // **Configure minimum distance (in cm) between the robot and obstacle before the robot avoid it by rotating. 44const int distRetreat = 10; // **Configure minimum distance (in cm) between the robot and obstacle before the robot avoid it by retreating. 45const int distTurn = 20; // **Configure minimum distance (in cm) between the robot and obstacle before the robot avoid it by turning. 46const int counter_gait_max = 8; // **Configure how many steps the robot will take to avoid obstacle (when rotating or retreating). 47 48// **Configure how long the bot rest & run (in milliseconds). 49const int RUN_time = 25000; 50const int REST_time = 3000; 51 52// ID's for sonars: 53int SONAR_id; 54const int FRONT = 0; 55const int LEFT = 1; 56const int RIGHT = 2; 57 58// ID's for photodiodes: 59const int FRONT_LEFT = 0; 60const int FRONT_RIGHT = 1; 61const int BACK_LEFT = 2; 62const int BACK_RIGHT = 3; 63 64// Variables for photodiodes reading: 65int PHOTO_Front_Left; 66int PHOTO_Front_Right; 67int PHOTO_Back_Left; 68int PHOTO_Back_Right; 69 70const int SONAR_TrigSig = 10; // Duration (in S) of trigger signal the sensors needed to produce ultrasonic sound (already specified by the products, don't change this value). 71const unsigned long SONAR_MaxEc = 50000; // Maximum duration (in S) of the echo signal given by the sensors (already specified by the products, don't change this value). 72const float SOUND_speed = 0.034; // The speed of sound on air in S/cm (already specified by sciene, avatar Aang is needed to do air bending if this value is wanted to be changed). 73int distance[SONAR_sum]; // Results of the calculation of distance. 74 75// Ddeclaration of servos: 76Servo SERVO_shaft; 77Servo SERVO_front_left; 78Servo SERVO_front_right; 79Servo SERVO_back_left; 80Servo SERVO_back_right; 81 82// Variables for each servos' angles: 83int ANGLE_shaft = ANGLE_mid_Shaft; 84int ANGLE_front_left = ANGLE_mid_FLeft; 85int ANGLE_front_right = ANGLE_mid_FRight; 86int ANGLE_back_left = ANGLE_mid_BLeft; 87int ANGLE_back_right = ANGLE_mid_BRight; 88 89// Angle manipulation for middle servo (shaft). 90const int ANGLE_max_Shaft = ANGLE_mid_Shaft + ANGLE_sweep; 91const int ANGLE_min_Shaft = ANGLE_mid_Shaft - ANGLE_sweep; 92int ANGLE_sweep_val; 93 94// Variables for recording current angles of each servos: 95int ANGLE_shaft_record; 96int ANGLE_front_left_record; 97int ANGLE_front_right_record; 98int ANGLE_back_left_record; 99int ANGLE_back_right_record; 100 101// Variables for servos angles correction according to light detection: 102int LIGHT_left; 103int LIGHT_right; 104// Variables for servos angles correction according to sonar detection: 105int SONAR_left; 106int SONAR_right; 107 108// That things such as flags, counters, records that I'm always not sure how to explain. :( 109int ANGLE_prev; 110int flag_shaft_reverse; 111int flag_transition_rotate; 112int flag_transition_start = 1; 113int flag_rest = 0; 114int flag_RUN_time = 0; 115int rotate_random; 116int counter_gait; 117 118void setup() { 119 // Serial.begin(9600); // Serial.. you know, checking & debugging.. 120 121 SERVO_shaft.attach(2); // ***Set up horizontal (shaft) servo's signal pin on arduino. 122 SERVO_front_left.attach(4); // ***Set up front-left servo's signal pin on arduino. 123 SERVO_front_right.attach(3); // ***Set up front-right servo's signal pin on arduino. 124 SERVO_back_left.attach(6); // ***Set up back-left servo's signal pin on arduino. 125 SERVO_back_right.attach(5); // ***Set up back-right servo's signal pin on arduino. 126 127 // Get the servos ready at their middle angles. 128 SERVO_shaft.writeMicroseconds(ANGLE_mid_Shaft); 129 SERVO_front_left.writeMicroseconds(ANGLE_mid_FLeft); 130 SERVO_front_right.writeMicroseconds(ANGLE_mid_FRight); 131 SERVO_back_left.writeMicroseconds(ANGLE_mid_BLeft); 132 SERVO_back_right.writeMicroseconds(ANGLE_mid_BRight); 133 134 // Setting pins for sonars, both pinMode and value. 135 for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) { 136 pinMode(PIN_trig[SONAR_id],OUTPUT); 137 pinMode(PIN_ec[SONAR_id],INPUT); 138 digitalWrite(PIN_trig[SONAR_id], LOW); 139 } 140 141 randomSeed(analogRead(5)); // Using analog pin 5 to generate random values to be used later. 142 SONAR_READ_ALL(); // Initiate first sonar reading before doing anything, 143 SONAR_READ_ALL(); // and actually I forget why I have to call it twice for the first time. But believe me, it's needed. 144 START(); // This function make sure which legs are lifted randomly at the first step. 145 146 delay(3000); // **dum.. daa.. dum.. 147} 148 149void loop() { 150 int state = random(0,2); 151 152 if(state == 0) { 153 REST(); 154 } 155 else { 156 int randomSPEED = random(0,3); 157 sweepSPEED = sweepSPEED_Rand[randomSPEED]; 158 RUN(); 159 flag_rest = 0; 160 } 161} 162 163void REST() { 164 if(flag_rest == 0) { 165 TRANSITION_GAIT(); 166 flag_rest = 1; 167 } 168 delay(REST_time); 169} 170 171void RUN() { 172 unsigned long TIMER_state = millis(); 173 while((millis() - TIMER_state) <= RUN_time) { 174 if(distance[FRONT] > distRotate) { 175 flag_RUN_time = 0; 176 while(flag_RUN_time == 0) { 177 FORWARD(); 178 } 179 } 180 181 while(distance[FRONT] > distRetreat && distance[FRONT] <= distRotate) { 182 while(distance[LEFT] > distance[RIGHT]) { 183 ROTATE_LEFT_AVOID(); 184 break; 185 } 186 while(distance[LEFT] < distance[RIGHT]) { 187 ROTATE_RIGHT_AVOID(); 188 break; 189 } 190 while(distance[LEFT] == distance[RIGHT]) { 191 ROTATE_RANDOM_AVOID(); 192 break; 193 } 194 } 195 196 while(distance[FRONT] <= distRetreat) { 197 RETREAT_AVOID(); 198 } 199 } 200} 201 202/*________________________________________________________________________________________ 203########################################## GAIT ########################################*/ 204 205/*=================================== SHAFT MOVEMENT ===================================*/ 206 207void SHAFT() { 208 unsigned long TIMER_servo = millis(); 209 while((millis() - TIMER_servo) <= sweepSPEED) { 210 while(ANGLE_shaft == ANGLE_mid_Shaft) { 211 counter_gait++; 212 SONAR_READ_ALL(); 213 LIGHT_COMPARE_EXECUTE(); 214 SIDE_AVOID(); 215 flag_RUN_time = 1; 216 break; 217 } 218 } 219 220 if(ANGLE_prev < ANGLE_shaft && ANGLE_shaft < ANGLE_max_Shaft) { 221 ANGLE_prev = ANGLE_shaft; 222 ANGLE_shaft += ANGLE_res; 223 } 224 else if(ANGLE_shaft >= ANGLE_max_Shaft) { 225 ANGLE_prev = ANGLE_shaft; 226 ANGLE_shaft -= ANGLE_res; 227 } 228 else if(ANGLE_prev > ANGLE_shaft && ANGLE_shaft > ANGLE_min_Shaft) { 229 ANGLE_prev = ANGLE_shaft; 230 ANGLE_shaft -= ANGLE_res; 231 } 232 else if(ANGLE_shaft <= ANGLE_min_Shaft) { 233 ANGLE_prev = ANGLE_shaft; 234 ANGLE_shaft += ANGLE_res; 235 } 236 SERVO_shaft.writeMicroseconds(ANGLE_shaft); 237} 238 239void SHAFT_REVERSE() { 240 if(ANGLE_prev < ANGLE_shaft) { 241 ANGLE_prev = ANGLE_shaft + 1; 242 } 243 else if(ANGLE_prev > ANGLE_shaft) { 244 ANGLE_prev = ANGLE_shaft - 1; 245 } 246} 247 248/*================================ END OF SHAFT MOVEMENT ================================*/ 249 250 251/*===================================== TRANSITION =====================================*/ 252 253void TRANSITION_GAIT() { 254 ANGLE_front_left_record = ANGLE_front_left; 255 ANGLE_front_right_record = ANGLE_front_right; 256 ANGLE_back_left_record = ANGLE_back_left; 257 ANGLE_back_right_record = ANGLE_back_right; 258 ANGLE_shaft_record = ANGLE_shaft; 259 260 int flag = HIGH; 261 int counter = 0; 262 while(flag == HIGH) { 263 SHAFT(); 264 LIGHT_left = 0; 265 LIGHT_right = 0; 266 267 counter++; 268 269 ANGLE_front_left = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_left_record, ANGLE_mid_FLeft); 270 ANGLE_front_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_right_record, ANGLE_mid_FRight); 271 ANGLE_back_left = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_left_record, ANGLE_mid_BLeft); 272 ANGLE_back_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_right_record, ANGLE_mid_BRight); 273 274 SERVO_shaft.writeMicroseconds(ANGLE_shaft); 275 SERVO_front_left.writeMicroseconds(ANGLE_front_left); 276 SERVO_front_right.writeMicroseconds(ANGLE_front_right); 277 SERVO_back_left.writeMicroseconds(ANGLE_back_left); 278 SERVO_back_right.writeMicroseconds(ANGLE_back_right); 279 280 if(counter == ((ANGLE_sweep * 2) / ANGLE_res)) { 281 flag = LOW; 282 START(); 283 flag_transition_rotate = 0; 284 } 285 } 286} 287 288void TRANSITION_START() { 289 if(ANGLE_shaft == ANGLE_mid_Shaft || (ANGLE_shaft > ANGLE_mid_Shaft && ANGLE_shaft > ANGLE_prev) || (ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_shaft < ANGLE_prev)) { 290 ANGLE_sweep_val = 0; 291 } 292 else { 293 flag_transition_start = 0; 294 } 295} 296 297void START() { 298 ANGLE_prev = random((ANGLE_shaft), (ANGLE_shaft + 2)); 299 if(ANGLE_prev == ANGLE_shaft) { 300 ANGLE_prev -= 1; 301 } 302 flag_transition_start = 1; 303} 304 305int ROTATE_RANDOM() { 306 return random(0, 2); 307} 308 309/*================================== END OF TRANSITION ==================================*/ 310 311 312/*======================================== WALK ========================================*/ 313 314void FORWARD() { 315 while(flag_transition_rotate == 2) { 316 TRANSITION_GAIT(); 317 } 318 flag_transition_rotate = 1; 319 320 while(flag_shaft_reverse == 0) { 321 SHAFT_REVERSE(); 322 break; 323 } 324 flag_shaft_reverse = 1; 325 326 while(flag_transition_start == 1) { 327 SHAFT(); 328 TRANSITION_START(); 329 WALK(); 330 } 331 332 SHAFT(); 333 ANGLE_sweep_val = ANGLE_sweep; 334 WALK(); 335} 336 337void RETREAT() { 338 while(flag_transition_rotate == 2) { 339 TRANSITION_GAIT(); 340 } 341 flag_transition_rotate = 1; 342 343 while(flag_shaft_reverse == 1) { 344 SHAFT_REVERSE(); 345 break; 346 } 347 flag_shaft_reverse = 0; 348 349 while(flag_transition_start == 1) { 350 SHAFT(); 351 LIGHT_left = 0; 352 LIGHT_right = 0; 353 TRANSITION_START(); 354 WALK(); 355 } 356 357 SHAFT(); 358 LIGHT_left = 0; 359 LIGHT_right = 0; 360 ANGLE_sweep_val = (ANGLE_sweep * -1); 361 WALK(); 362} 363 364void WALK() { 365 if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) { 366 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left), ANGLE_mid_FLeft); 367 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), ANGLE_mid_FRight); 368 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_BLeft); 369 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_BRight); 370 } 371 else if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 372 ANGLE_front_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left)); 373 ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right)); 374 ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left)); 375 ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right)); 376 } 377 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 378 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_FLeft); 379 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_FRight); 380 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left), ANGLE_mid_BLeft); 381 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), ANGLE_mid_BRight); 382 } 383 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) { 384 ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left)); 385 ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right)); 386 ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left)); 387 ANGLE_back_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right)); 388 } 389 390 SERVO_front_left.writeMicroseconds(ANGLE_front_left); 391 SERVO_front_right.writeMicroseconds(ANGLE_front_right); 392 SERVO_back_left.writeMicroseconds(ANGLE_back_left); 393 SERVO_back_right.writeMicroseconds(ANGLE_back_right); 394} 395 396/*===================================== END OF WALK =====================================*/ 397 398 399/*======================================= ROTATE =======================================*/ 400 401void ROTATE_LEFT() { 402 while(flag_transition_rotate == 1) { 403 TRANSITION_GAIT(); 404 } 405 flag_transition_rotate = 2; 406 407 while(flag_shaft_reverse == 2) { 408 SHAFT_REVERSE(); 409 break; 410 } 411 flag_shaft_reverse = 3; 412 413 while(flag_transition_start == 1) { 414 SHAFT(); 415 LIGHT_left = 0; 416 LIGHT_right = 0; 417 TRANSITION_START(); 418 ROTATE(); 419 } 420 421 SHAFT(); 422 LIGHT_left = 0; 423 LIGHT_right = 0; 424 ANGLE_sweep_val = ANGLE_sweep; 425 ROTATE(); 426} 427 428void ROTATE_RIGHT() { 429 while(flag_transition_rotate == 1) { 430 TRANSITION_GAIT(); 431 } 432 flag_transition_rotate = 2; 433 434 while(flag_shaft_reverse == 3) { 435 SHAFT_REVERSE(); 436 break; 437 } 438 flag_shaft_reverse = 2; 439 440 while(flag_transition_start == 1) { 441 SHAFT(); 442 LIGHT_left = 0; 443 LIGHT_right = 0; 444 TRANSITION_START(); 445 ROTATE(); 446 } 447 448 SHAFT(); 449 LIGHT_left = 0; 450 LIGHT_right = 0; 451 ANGLE_sweep_val = (ANGLE_sweep * -1); 452 ROTATE(); 453} 454 455void ROTATE() { 456 if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) { 457 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FLeft + ANGLE_sweep_val), ANGLE_mid_FLeft); 458 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FRight - ANGLE_sweep_val), ANGLE_mid_FRight); 459 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BLeft - ANGLE_sweep_val), ANGLE_mid_BLeft); 460 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BRight + ANGLE_sweep_val), ANGLE_mid_BRight); 461 } 462 else if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 463 ANGLE_front_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft - ANGLE_sweep_val)); 464 ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight + ANGLE_sweep_val)); 465 ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft + ANGLE_sweep_val)); 466 ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight - ANGLE_sweep_val)); 467 } 468 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 469 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FLeft - ANGLE_sweep_val), ANGLE_mid_FLeft); 470 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FRight + ANGLE_sweep_val), ANGLE_mid_FRight); 471 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BLeft + ANGLE_sweep_val), ANGLE_mid_BLeft); 472 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BRight - ANGLE_sweep_val), ANGLE_mid_BRight); 473 } 474 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) { 475 ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft + ANGLE_sweep_val)); 476 ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight - ANGLE_sweep_val)); 477 ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft - ANGLE_sweep_val)); 478 ANGLE_back_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight + ANGLE_sweep_val)); 479 } 480 481 SERVO_front_left.writeMicroseconds(ANGLE_front_left); 482 SERVO_front_right.writeMicroseconds(ANGLE_front_right); 483 SERVO_back_left.writeMicroseconds(ANGLE_back_left); 484 SERVO_back_right.writeMicroseconds(ANGLE_back_right); 485} 486 487/*==================================== END OF ROTATE ====================================*/ 488 489/*_________________________________________________________________________________________ 490/*##################################### END OF GAIT #####################################*/ 491 492 493/*================================= ULTRASONIC READING =================================*/ 494 495void SONAR_READ_ALL() { 496 for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) { 497 distance[SONAR_id] = int (SONAR_READ(SONAR_id)); 498 } 499} 500 501float SONAR_READ(int index) { 502 float SONAR_distance; 503 504 digitalWrite(PIN_trig[index], HIGH); 505 delayMicroseconds(SONAR_TrigSig); 506 digitalWrite(PIN_trig[index], LOW); 507 508 float SONAR_EcInterval = float (pulseIn(PIN_ec[index], HIGH, SONAR_MaxEc)); 509 510 while(SONAR_EcInterval > 0.0) { 511 SONAR_distance = SONAR_EcInterval * (SOUND_speed / 2.0); 512 break; 513 } 514 while(SONAR_EcInterval == 0.0) { 515 SONAR_distance = 501.0; 516 break; 517 } 518 return SONAR_distance; 519} 520 521/*============================= END OF ULTRASONIC READING =============================*/ 522 523 524/*==================================== LIGHT DETECT ====================================*/ 525 526void LIGHT_COMPARE_EXECUTE() { 527 //PHOTO_FLeft_RAW = analogRead(PIN_PHOTO[FRONT_LEFT]); 528 //PHOTO_FRight_RAW = analogRead(PIN_PHOTO[FRONT_RIGHT]); 529 PHOTO_Front_Left = analogRead(PIN_PHOTO[FRONT_LEFT]); 530 PHOTO_Front_Right = analogRead(PIN_PHOTO[FRONT_RIGHT]); 531 PHOTO_Back_Left = analogRead(PIN_PHOTO[BACK_LEFT]); 532 PHOTO_Back_Right = analogRead(PIN_PHOTO[BACK_RIGHT]); 533 534 if((PHOTO_Front_Left + PHOTO_Front_Right) >= (PHOTO_Back_Left + PHOTO_Back_Right)) { 535 int LIGHT_Sensitivity = 50; 536 if(LIGHT_COMPARE() > LIGHT_Sensitivity) { 537 LIGHT_left = LIGHT_COMPARE(); 538 LIGHT_right = 0; 539 } 540 else if(LIGHT_COMPARE() < -LIGHT_Sensitivity) { 541 LIGHT_left = 0; 542 LIGHT_right = LIGHT_COMPARE(); 543 } 544 else { 545 LIGHT_left = 0; 546 LIGHT_right = 0; 547 } 548 } 549 else { 550 if(PHOTO_Back_Left > PHOTO_Back_Right) { 551 LIGHT_right = 0; 552 LIGHT_left = ANGLE_turnMAX; 553 } 554 else if(PHOTO_Back_Left < PHOTO_Back_Right) { 555 LIGHT_right = -ANGLE_turnMAX; 556 LIGHT_left = 0; 557 } 558 else { 559 int RANDOM_back_light = random(0,2); 560 561 if(RANDOM_back_light == 0) { 562 LIGHT_right = 0; 563 LIGHT_left = ANGLE_turnMAX; 564 } 565 else if(RANDOM_back_light == 1) { 566 LIGHT_right = -ANGLE_turnMAX; 567 LIGHT_left = 0; 568 } 569 } 570 } 571} 572 573int LIGHT_COMPARE() { 574 int LIGHT_rate; 575 if(PHOTO_Front_Left > PHOTO_Front_Right) { 576 LIGHT_rate = PHOTO_Front_Left; 577 } 578 else if(PHOTO_Front_Right > PHOTO_Front_Left) { 579 LIGHT_rate = PHOTO_Front_Right; 580 } 581 else { 582 // choose to use one and comments the other of these variables bellow 583 // LIGHT_rate = PHOTO_Front_Left; 584 LIGHT_rate = PHOTO_Front_Right; 585 } 586 587 int LIGHT_compareRAW = PHOTO_Front_Left - PHOTO_Front_Right; 588 LIGHT_compareRAW = map(LIGHT_compareRAW, -LIGHT_rate, LIGHT_rate, -ANGLE_turnMAX, ANGLE_turnMAX);; 589 590 return LIGHT_compareRAW; 591} 592 593/*================================= END OF LIGHT DETECT =================================*/ 594 595 596/*====================================== BEHAVIOUR ======================================*/ 597 598void RETREAT_AVOID() { 599 counter_gait = 0; 600 while(counter_gait <= counter_gait_max) { 601 RETREAT(); 602 } 603} 604 605void ROTATE_LEFT_AVOID() { 606 counter_gait = 0; 607 rotate_random = 2; 608 while(counter_gait <= counter_gait_max) { 609 ROTATE_LEFT(); 610 } 611} 612 613void ROTATE_RIGHT_AVOID() { 614 counter_gait = 0; 615 rotate_random = 2; 616 while(counter_gait <= counter_gait_max) { 617 ROTATE_RIGHT(); 618 } 619} 620 621void ROTATE_RANDOM_AVOID() { 622 rotate_random = ROTATE_RANDOM(); 623 while(rotate_random == 0) { 624 ROTATE_LEFT_AVOID(); 625 } 626 while(rotate_random == 1) { 627 ROTATE_RIGHT_AVOID(); 628 } 629} 630 631void SIDE_AVOID() { 632 if(distance[LEFT] <= distTurn && distance[RIGHT] > distTurn) { 633 LIGHT_left = 0; 634 LIGHT_right = 0; 635 636 SONAR_left = 0; 637 SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnMAX, 0)); 638 } 639 else if(distance[RIGHT] <= distTurn && distance[LEFT] > distTurn) { 640 LIGHT_left = 0; 641 LIGHT_right = 0; 642 643 SONAR_right = 0; 644 SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnMAX, 0); 645 } 646 else if(distance[LEFT] <= distTurn && distance[RIGHT] <= distTurn) { 647 LIGHT_left = 0; 648 LIGHT_right = 0; 649 650 if(distance[LEFT] < distance[RIGHT]) { 651 SONAR_left = 0; 652 SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnNARROW, 0)); 653 } 654 else if(distance[RIGHT] < distance[LEFT]) { 655 SONAR_right = 0; 656 SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnNARROW, 0); 657 } 658 } 659 else { 660 SONAR_right = 0; 661 SONAR_left = 0; 662 } 663} 664 665/*================================== END OF BEHAVIOUR ==================================*/ 666
WALTER.ino
arduino
1/* 2WALTER - THE 4 LEGGED PHOTOVORE 3 4This Arduino sketch is my 5 attemp to build a 5 servo quadruped (4 legged) robot named "WALTER". 6 7In 8 order to use this sketch, you might need to change some values for your convenient 9 or to adjust to your own hardware set up. 10Find (Ctrl + F) these marks to easily 11 search which values might need to be changed: 12 - **** : These marks mean they 13 are servos' center potition and have to calibrated (your robot legs' potitioning 14 when it's idle). 15 - *** : These marks mean arduino pins asignment (sensors 16 & servos connection to arduino). Refer to this when you build the robot. 17 - 18 ** : These marks mean that the values optionally can be changed according to your 19 taste (legs' step width, how much to turn when sensing light/obstacle, etc). Just 20 leave it as is if you don't know what you were doing. 21 22You can use this sketch 23 at your own risk.. and, it's provided as is and.. uh.. 24What're those all about 25 copyrights stuff people use to write at published source code?? 26The point is 27 I don't want to be held responsible if something bad happened when you were using 28 this codes. 29Have fun! 30 31Yohanes Martedi - 2015 32*/ 33 34#include <Servo.h> 35 36// 37 ****Calibrate servos' center angle (in microseconds because we'll use "xx.writeMicroseconds();" 38 command). Start with 1500. 39const int ANGLE_mid_Shaft = 1520; 40const int ANGLE_mid_FLeft 41 = 1550; 42const int ANGLE_mid_FRight = 1570; 43const int ANGLE_mid_BLeft = 1450; 44const 45 int ANGLE_mid_BRight = 1450; 46 47const int ANGLE_sweep = 250; // **Set 48 this value (in microseconds) to determine how wide the servos will sweep (legs' 49 step width). Bigger value means wider sweep angle. 50const int ANGLE_res = 10; 51 // **Set the servo movement resolution (in microseconds) at least at 52 minimum servo's default dead band width (highest resolution) or more (less resolution). 53 Example: SG90 servo dead band width is 10 microseconds. 54int sweepSPEED; // 55 variable to determine how fast the servos will sweep. 56int sweepSPEED_Rand[3] 57 = {4, 6, 8}; // **Servo speed (gait speed) will change randomly in 3 modes. Set 58 the speed (in miliseconds) for each mode. Smaller value means faster. 59 60const 61 int ANGLE_turnMAX = ANGLE_sweep * 1.5; // **Set this value to determine how 62 much maximum the bot will turn toward light. Bigger value means bigger turn. 63const 64 int ANGLE_turnNARROW = ANGLE_sweep * 0.25; // **Set this value to determine how 65 much maximum the bot will turn avoiding objects at its sides in narrow space. Bigger 66 value means bigger turn. 67 68const int SONAR_sum = 3; // Amount of sonars used. 69const 70 int PHOTO_sum = 4; // Amount of photodiodes used. 71int PIN_trig[SONAR_sum] = {13, 72 11, 8}; // ***Set arduino pins connected to ultrasonic sensors' trigger pins; 73 {front, left, right}. 74int PIN_ec[SONAR_sum] = {12, 10, 7}; // ***Set arduino 75 pins connected to ultrasonic sensors' echo pins; {front, left, right}. 76int PIN_PHOTO[PHOTO_sum] 77 = {2, 3, 1, 0}; // ***Set arduino analog input pins connected to photodiodes; {front 78 left, front right, back left, back right}. 79 80const int distRotate = 25; // 81 **Configure minimum distance (in cm) between the robot and obstacle before the robot 82 avoid it by rotating. 83const int distRetreat = 10; // **Configure minimum 84 distance (in cm) between the robot and obstacle before the robot avoid it by retreating. 85const 86 int distTurn = 20; // **Configure minimum distance (in cm) between the robot 87 and obstacle before the robot avoid it by turning. 88const int counter_gait_max 89 = 8; // **Configure how many steps the robot will take to avoid obstacle (when 90 rotating or retreating). 91 92// **Configure how long the bot rest & run (in milliseconds). 93const 94 int RUN_time = 25000; 95const int REST_time = 3000; 96 97// ID's for sonars: 98int 99 SONAR_id; 100const int FRONT = 0; 101const int LEFT = 1; 102const int RIGHT = 2; 103 104// 105 ID's for photodiodes: 106const int FRONT_LEFT = 0; 107const int FRONT_RIGHT = 1; 108const 109 int BACK_LEFT = 2; 110const int BACK_RIGHT = 3; 111 112// Variables for photodiodes 113 reading: 114int PHOTO_Front_Left; 115int PHOTO_Front_Right; 116int PHOTO_Back_Left; 117int 118 PHOTO_Back_Right; 119 120const int SONAR_TrigSig = 10; // Duration (in 121 S) of trigger signal the sensors needed to produce ultrasonic sound (already specified 122 by the products, don't change this value). 123const unsigned long SONAR_MaxEc = 124 50000; // Maximum duration (in S) of the echo signal given by the sensors (already 125 specified by the products, don't change this value). 126const float SOUND_speed 127 = 0.034; // The speed of sound on air in S/cm (already specified by sciene, 128 avatar Aang is needed to do air bending if this value is wanted to be changed). 129int 130 distance[SONAR_sum]; // Results of the calculation of distance. 131 132// 133 Ddeclaration of servos: 134Servo SERVO_shaft; 135Servo SERVO_front_left; 136Servo 137 SERVO_front_right; 138Servo SERVO_back_left; 139Servo SERVO_back_right; 140 141// 142 Variables for each servos' angles: 143int ANGLE_shaft = ANGLE_mid_Shaft; 144int 145 ANGLE_front_left = ANGLE_mid_FLeft; 146int ANGLE_front_right = ANGLE_mid_FRight; 147int 148 ANGLE_back_left = ANGLE_mid_BLeft; 149int ANGLE_back_right = ANGLE_mid_BRight; 150 151// 152 Angle manipulation for middle servo (shaft). 153const int ANGLE_max_Shaft = ANGLE_mid_Shaft 154 + ANGLE_sweep; 155const int ANGLE_min_Shaft = ANGLE_mid_Shaft - ANGLE_sweep; 156int 157 ANGLE_sweep_val; 158 159// Variables for recording current angles of each servos: 160int 161 ANGLE_shaft_record; 162int ANGLE_front_left_record; 163int ANGLE_front_right_record; 164int 165 ANGLE_back_left_record; 166int ANGLE_back_right_record; 167 168// Variables for 169 servos angles correction according to light detection: 170int LIGHT_left; 171int 172 LIGHT_right; 173// Variables for servos angles correction according to sonar detection: 174int 175 SONAR_left; 176int SONAR_right; 177 178// That things such as flags, counters, records 179 that I'm always not sure how to explain. :( 180int ANGLE_prev; 181int flag_shaft_reverse; 182int 183 flag_transition_rotate; 184int flag_transition_start = 1; 185int flag_rest = 0; 186int 187 flag_RUN_time = 0; 188int rotate_random; 189int counter_gait; 190 191void setup() 192 { 193 // Serial.begin(9600); // Serial.. you know, checking & debugging.. 194 195 196 SERVO_shaft.attach(2); // ***Set up horizontal (shaft) servo's signal pin 197 on arduino. 198 SERVO_front_left.attach(4); // ***Set up front-left servo's signal 199 pin on arduino. 200 SERVO_front_right.attach(3); // ***Set up front-right servo's 201 signal pin on arduino. 202 SERVO_back_left.attach(6); // ***Set up back-left 203 servo's signal pin on arduino. 204 SERVO_back_right.attach(5); // ***Set up back-right 205 servo's signal pin on arduino. 206 207 // Get the servos ready at their middle 208 angles. 209 SERVO_shaft.writeMicroseconds(ANGLE_mid_Shaft); 210 SERVO_front_left.writeMicroseconds(ANGLE_mid_FLeft); 211 212 SERVO_front_right.writeMicroseconds(ANGLE_mid_FRight); 213 SERVO_back_left.writeMicroseconds(ANGLE_mid_BLeft); 214 215 SERVO_back_right.writeMicroseconds(ANGLE_mid_BRight); 216 217 // Setting pins 218 for sonars, both pinMode and value. 219 for(SONAR_id = 0; SONAR_id < SONAR_sum; 220 SONAR_id++) { 221 pinMode(PIN_trig[SONAR_id],OUTPUT); 222 pinMode(PIN_ec[SONAR_id],INPUT); 223 224 digitalWrite(PIN_trig[SONAR_id], LOW); 225 } 226 227 randomSeed(analogRead(5)); 228 // Using analog pin 5 to generate random values to be used later. 229 SONAR_READ_ALL(); 230 // Initiate first sonar reading before doing anything, 231 SONAR_READ_ALL(); 232 // and actually I forget why I have to call it twice for the first time. 233 But believe me, it's needed. 234 START(); // This function make 235 sure which legs are lifted randomly at the first step. 236 237 delay(3000); // 238 **dum.. daa.. dum.. 239} 240 241void loop() { 242 int state = random(0,2); 243 244 245 if(state == 0) { 246 REST(); 247 } 248 else { 249 int randomSPEED 250 = random(0,3); 251 sweepSPEED = sweepSPEED_Rand[randomSPEED]; 252 RUN(); 253 254 flag_rest = 0; 255 } 256} 257 258void REST() { 259 if(flag_rest == 0) { 260 261 TRANSITION_GAIT(); 262 flag_rest = 1; 263 } 264 delay(REST_time); 265} 266 267void 268 RUN() { 269 unsigned long TIMER_state = millis(); 270 while((millis() - TIMER_state) 271 <= RUN_time) { 272 if(distance[FRONT] > distRotate) { 273 flag_RUN_time 274 = 0; 275 while(flag_RUN_time == 0) { 276 FORWARD(); 277 } 278 } 279 280 281 while(distance[FRONT] > distRetreat && distance[FRONT] <= distRotate) 282 { 283 while(distance[LEFT] > distance[RIGHT]) { 284 ROTATE_LEFT_AVOID(); 285 286 break; 287 } 288 while(distance[LEFT] < distance[RIGHT]) { 289 290 ROTATE_RIGHT_AVOID(); 291 break; 292 } 293 while(distance[LEFT] 294 == distance[RIGHT]) { 295 ROTATE_RANDOM_AVOID(); 296 break; 297 } 298 299 } 300 301 while(distance[FRONT] <= distRetreat) { 302 RETREAT_AVOID(); 303 304 } 305 } 306} 307 308/*________________________________________________________________________________________ 309########################################## 310 GAIT ########################################*/ 311 312/*=================================== 313 SHAFT MOVEMENT ===================================*/ 314 315void SHAFT() { 316 unsigned 317 long TIMER_servo = millis(); 318 while((millis() - TIMER_servo) <= sweepSPEED) 319 { 320 while(ANGLE_shaft == ANGLE_mid_Shaft) { 321 counter_gait++; 322 SONAR_READ_ALL(); 323 324 LIGHT_COMPARE_EXECUTE(); 325 SIDE_AVOID(); 326 flag_RUN_time = 1; 327 328 break; 329 } 330 } 331 332 if(ANGLE_prev < ANGLE_shaft && ANGLE_shaft 333 < ANGLE_max_Shaft) { 334 ANGLE_prev = ANGLE_shaft; 335 ANGLE_shaft += ANGLE_res; 336 337 } 338 else if(ANGLE_shaft >= ANGLE_max_Shaft) { 339 ANGLE_prev = ANGLE_shaft; 340 341 ANGLE_shaft -= ANGLE_res; 342 } 343 else if(ANGLE_prev > ANGLE_shaft && ANGLE_shaft 344 > ANGLE_min_Shaft) { 345 ANGLE_prev = ANGLE_shaft; 346 ANGLE_shaft -= ANGLE_res; 347 348 } 349 else if(ANGLE_shaft <= ANGLE_min_Shaft) { 350 ANGLE_prev = ANGLE_shaft; 351 352 ANGLE_shaft += ANGLE_res; 353 } 354 SERVO_shaft.writeMicroseconds(ANGLE_shaft); 355} 356 357void 358 SHAFT_REVERSE() { 359 if(ANGLE_prev < ANGLE_shaft) { 360 ANGLE_prev = ANGLE_shaft 361 + 1; 362 } 363 else if(ANGLE_prev > ANGLE_shaft) { 364 ANGLE_prev = ANGLE_shaft 365 - 1; 366 } 367} 368 369/*================================ END OF SHAFT MOVEMENT 370 ================================*/ 371 372 373/*===================================== 374 TRANSITION =====================================*/ 375 376void TRANSITION_GAIT() 377 { 378 ANGLE_front_left_record = ANGLE_front_left; 379 ANGLE_front_right_record 380 = ANGLE_front_right; 381 ANGLE_back_left_record = ANGLE_back_left; 382 ANGLE_back_right_record 383 = ANGLE_back_right; 384 ANGLE_shaft_record = ANGLE_shaft; 385 386 int flag = 387 HIGH; 388 int counter = 0; 389 while(flag == HIGH) { 390 SHAFT(); 391 LIGHT_left 392 = 0; 393 LIGHT_right = 0; 394 395 counter++; 396 397 ANGLE_front_left 398 = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_left_record, ANGLE_mid_FLeft); 399 400 ANGLE_front_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_right_record, 401 ANGLE_mid_FRight); 402 ANGLE_back_left = map(counter, 1, ((ANGLE_sweep * 2) / 403 ANGLE_res), ANGLE_back_left_record, ANGLE_mid_BLeft); 404 ANGLE_back_right = 405 map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_right_record, ANGLE_mid_BRight); 406 407 408 SERVO_shaft.writeMicroseconds(ANGLE_shaft); 409 SERVO_front_left.writeMicroseconds(ANGLE_front_left); 410 411 SERVO_front_right.writeMicroseconds(ANGLE_front_right); 412 SERVO_back_left.writeMicroseconds(ANGLE_back_left); 413 414 SERVO_back_right.writeMicroseconds(ANGLE_back_right); 415 416 if(counter 417 == ((ANGLE_sweep * 2) / ANGLE_res)) { 418 flag = LOW; 419 START(); 420 421 flag_transition_rotate = 0; 422 } 423 } 424} 425 426void TRANSITION_START() 427 { 428 if(ANGLE_shaft == ANGLE_mid_Shaft || (ANGLE_shaft > ANGLE_mid_Shaft && ANGLE_shaft 429 > ANGLE_prev) || (ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_shaft < ANGLE_prev)) { 430 431 ANGLE_sweep_val = 0; 432 } 433 else { 434 flag_transition_start = 0; 435 436 } 437} 438 439void START() { 440 ANGLE_prev = random((ANGLE_shaft), (ANGLE_shaft 441 + 2)); 442 if(ANGLE_prev == ANGLE_shaft) { 443 ANGLE_prev -= 1; 444 } 445 flag_transition_start 446 = 1; 447} 448 449int ROTATE_RANDOM() { 450 return random(0, 2); 451} 452 453/*================================== 454 END OF TRANSITION ==================================*/ 455 456 457/*======================================== 458 WALK ========================================*/ 459 460void FORWARD() { 461 while(flag_transition_rotate 462 == 2) { 463 TRANSITION_GAIT(); 464 } 465 flag_transition_rotate = 1; 466 467 468 while(flag_shaft_reverse == 0) { 469 SHAFT_REVERSE(); 470 break; 471 } 472 473 flag_shaft_reverse = 1; 474 475 while(flag_transition_start == 1) { 476 SHAFT(); 477 478 TRANSITION_START(); 479 WALK(); 480 } 481 482 SHAFT(); 483 ANGLE_sweep_val 484 = ANGLE_sweep; 485 WALK(); 486} 487 488void RETREAT() { 489 while(flag_transition_rotate 490 == 2) { 491 TRANSITION_GAIT(); 492 } 493 flag_transition_rotate = 1; 494 495 496 while(flag_shaft_reverse == 1) { 497 SHAFT_REVERSE(); 498 break; 499 } 500 501 flag_shaft_reverse = 0; 502 503 while(flag_transition_start == 1) { 504 SHAFT(); 505 506 LIGHT_left = 0; 507 LIGHT_right = 0; 508 TRANSITION_START(); 509 WALK(); 510 511 } 512 513 SHAFT(); 514 LIGHT_left = 0; 515 LIGHT_right = 0; 516 ANGLE_sweep_val 517 = (ANGLE_sweep * -1); 518 WALK(); 519} 520 521void WALK() { 522 if(ANGLE_shaft 523 >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) { 524 ANGLE_front_left = map(ANGLE_shaft, 525 ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left 526 + SONAR_left), ANGLE_mid_FLeft); 527 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, 528 ANGLE_max_Shaft, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), 529 ANGLE_mid_FRight); 530 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, 531 ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_BLeft); 532 533 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BRight 534 + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_BRight); 535 } 536 else 537 if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 538 ANGLE_front_left 539 = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft 540 + ANGLE_sweep_val) - LIGHT_left - SONAR_left)); 541 ANGLE_front_right = map(ANGLE_shaft, 542 ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight + ANGLE_sweep_val) 543 + LIGHT_right + SONAR_right)); 544 ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, 545 ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left 546 + SONAR_left)); 547 ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, 548 ANGLE_mid_BRight, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right)); 549 550 } 551 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 552 553 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FLeft 554 + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_FLeft); 555 ANGLE_front_right 556 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FRight + ANGLE_sweep_val) 557 + LIGHT_right + SONAR_right), ANGLE_mid_FRight); 558 ANGLE_back_left = map(ANGLE_shaft, 559 ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left 560 + SONAR_left), ANGLE_mid_BLeft); 561 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, 562 ANGLE_min_Shaft, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), 563 ANGLE_mid_BRight); 564 } 565 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev 566 < ANGLE_shaft) { 567 ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, 568 ANGLE_mid_FLeft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left)); 569 570 ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, 571 ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right)); 572 ANGLE_back_left 573 = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft 574 + ANGLE_sweep_val) - LIGHT_left - SONAR_left)); 575 ANGLE_back_right = map(ANGLE_shaft, 576 ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight + ANGLE_sweep_val) 577 + LIGHT_right + SONAR_right)); 578 } 579 580 SERVO_front_left.writeMicroseconds(ANGLE_front_left); 581 582 SERVO_front_right.writeMicroseconds(ANGLE_front_right); 583 SERVO_back_left.writeMicroseconds(ANGLE_back_left); 584 585 SERVO_back_right.writeMicroseconds(ANGLE_back_right); 586} 587 588/*===================================== 589 END OF WALK =====================================*/ 590 591 592/*======================================= 593 ROTATE =======================================*/ 594 595void ROTATE_LEFT() { 596 597 while(flag_transition_rotate == 1) { 598 TRANSITION_GAIT(); 599 } 600 flag_transition_rotate 601 = 2; 602 603 while(flag_shaft_reverse == 2) { 604 SHAFT_REVERSE(); 605 break; 606 607 } 608 flag_shaft_reverse = 3; 609 610 while(flag_transition_start == 1) { 611 612 SHAFT(); 613 LIGHT_left = 0; 614 LIGHT_right = 0; 615 TRANSITION_START(); 616 617 ROTATE(); 618 } 619 620 SHAFT(); 621 LIGHT_left = 0; 622 LIGHT_right = 623 0; 624 ANGLE_sweep_val = ANGLE_sweep; 625 ROTATE(); 626} 627 628void ROTATE_RIGHT() 629 { 630 while(flag_transition_rotate == 1) { 631 TRANSITION_GAIT(); 632 } 633 634 flag_transition_rotate = 2; 635 636 while(flag_shaft_reverse == 3) { 637 SHAFT_REVERSE(); 638 639 break; 640 } 641 flag_shaft_reverse = 2; 642 643 while(flag_transition_start 644 == 1) { 645 SHAFT(); 646 LIGHT_left = 0; 647 LIGHT_right = 0; 648 TRANSITION_START(); 649 650 ROTATE(); 651 } 652 653 SHAFT(); 654 LIGHT_left = 0; 655 LIGHT_right = 656 0; 657 ANGLE_sweep_val = (ANGLE_sweep * -1); 658 ROTATE(); 659} 660 661void ROTATE() 662 { 663 if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) { 664 ANGLE_front_left 665 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FLeft + ANGLE_sweep_val), 666 ANGLE_mid_FLeft); 667 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, 668 (ANGLE_mid_FRight - ANGLE_sweep_val), ANGLE_mid_FRight); 669 ANGLE_back_left 670 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BLeft - ANGLE_sweep_val), 671 ANGLE_mid_BLeft); 672 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, 673 (ANGLE_mid_BRight + ANGLE_sweep_val), ANGLE_mid_BRight); 674 } 675 else if(ANGLE_shaft 676 >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) { 677 ANGLE_front_left = map(ANGLE_shaft, 678 ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft - ANGLE_sweep_val)); 679 680 ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, 681 (ANGLE_mid_FRight + ANGLE_sweep_val)); 682 ANGLE_back_left = map(ANGLE_shaft, 683 ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft + ANGLE_sweep_val)); 684 685 ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, 686 (ANGLE_mid_BRight - ANGLE_sweep_val)); 687 } 688 else if(ANGLE_shaft < ANGLE_mid_Shaft 689 && ANGLE_prev > ANGLE_shaft) { 690 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, 691 ANGLE_min_Shaft, (ANGLE_mid_FLeft - ANGLE_sweep_val), ANGLE_mid_FLeft); 692 ANGLE_front_right 693 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FRight + ANGLE_sweep_val), 694 ANGLE_mid_FRight); 695 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, 696 (ANGLE_mid_BLeft + ANGLE_sweep_val), ANGLE_mid_BLeft); 697 ANGLE_back_right = 698 map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BRight - ANGLE_sweep_val), 699 ANGLE_mid_BRight); 700 } 701 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev 702 < ANGLE_shaft) { 703 ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, 704 ANGLE_mid_FLeft, (ANGLE_mid_FLeft + ANGLE_sweep_val)); 705 ANGLE_front_right 706 = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight 707 - ANGLE_sweep_val)); 708 ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, 709 ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft - ANGLE_sweep_val)); 710 ANGLE_back_right 711 = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight 712 + ANGLE_sweep_val)); 713 } 714 715 SERVO_front_left.writeMicroseconds(ANGLE_front_left); 716 717 SERVO_front_right.writeMicroseconds(ANGLE_front_right); 718 SERVO_back_left.writeMicroseconds(ANGLE_back_left); 719 720 SERVO_back_right.writeMicroseconds(ANGLE_back_right); 721} 722 723/*==================================== 724 END OF ROTATE ====================================*/ 725 726/*_________________________________________________________________________________________ 727/*##################################### 728 END OF GAIT #####################################*/ 729 730 731/*================================= 732 ULTRASONIC READING =================================*/ 733 734void SONAR_READ_ALL() 735 { 736 for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) { 737 distance[SONAR_id] 738 = int (SONAR_READ(SONAR_id)); 739 } 740} 741 742float SONAR_READ(int index) { 743 744 float SONAR_distance; 745 746 digitalWrite(PIN_trig[index], HIGH); 747 delayMicroseconds(SONAR_TrigSig); 748 749 digitalWrite(PIN_trig[index], LOW); 750 751 float SONAR_EcInterval = float 752 (pulseIn(PIN_ec[index], HIGH, SONAR_MaxEc)); 753 754 while(SONAR_EcInterval > 755 0.0) { 756 SONAR_distance = SONAR_EcInterval * (SOUND_speed / 2.0); 757 break; 758 759 } 760 while(SONAR_EcInterval == 0.0) { 761 SONAR_distance = 501.0; 762 break; 763 764 } 765 return SONAR_distance; 766} 767 768/*============================= END 769 OF ULTRASONIC READING =============================*/ 770 771 772/*==================================== 773 LIGHT DETECT ====================================*/ 774 775void LIGHT_COMPARE_EXECUTE() 776 { 777 //PHOTO_FLeft_RAW = analogRead(PIN_PHOTO[FRONT_LEFT]); 778 //PHOTO_FRight_RAW 779 = analogRead(PIN_PHOTO[FRONT_RIGHT]); 780 PHOTO_Front_Left = analogRead(PIN_PHOTO[FRONT_LEFT]); 781 782 PHOTO_Front_Right = analogRead(PIN_PHOTO[FRONT_RIGHT]); 783 PHOTO_Back_Left = 784 analogRead(PIN_PHOTO[BACK_LEFT]); 785 PHOTO_Back_Right = analogRead(PIN_PHOTO[BACK_RIGHT]); 786 787 788 if((PHOTO_Front_Left + PHOTO_Front_Right) >= (PHOTO_Back_Left + PHOTO_Back_Right)) 789 { 790 int LIGHT_Sensitivity = 50; 791 if(LIGHT_COMPARE() > LIGHT_Sensitivity) 792 { 793 LIGHT_left = LIGHT_COMPARE(); 794 LIGHT_right = 0; 795 } 796 797 else if(LIGHT_COMPARE() < -LIGHT_Sensitivity) { 798 LIGHT_left = 0; 799 800 LIGHT_right = LIGHT_COMPARE(); 801 } 802 else { 803 LIGHT_left 804 = 0; 805 LIGHT_right = 0; 806 } 807 } 808 else { 809 if(PHOTO_Back_Left 810 > PHOTO_Back_Right) { 811 LIGHT_right = 0; 812 LIGHT_left = ANGLE_turnMAX; 813 814 } 815 else if(PHOTO_Back_Left < PHOTO_Back_Right) { 816 LIGHT_right 817 = -ANGLE_turnMAX; 818 LIGHT_left = 0; 819 } 820 else { 821 int 822 RANDOM_back_light = random(0,2); 823 824 if(RANDOM_back_light == 0) { 825 826 LIGHT_right = 0; 827 LIGHT_left = ANGLE_turnMAX; 828 } 829 830 else if(RANDOM_back_light == 1) { 831 LIGHT_right = -ANGLE_turnMAX; 832 833 LIGHT_left = 0; 834 } 835 } 836 } 837} 838 839int LIGHT_COMPARE() 840 { 841 int LIGHT_rate; 842 if(PHOTO_Front_Left > PHOTO_Front_Right) { 843 LIGHT_rate 844 = PHOTO_Front_Left; 845 } 846 else if(PHOTO_Front_Right > PHOTO_Front_Left) { 847 848 LIGHT_rate = PHOTO_Front_Right; 849 } 850 else { 851 // choose to use one 852 and comments the other of these variables bellow 853 // LIGHT_rate = PHOTO_Front_Left; 854 855 LIGHT_rate = PHOTO_Front_Right; 856 } 857 858 int LIGHT_compareRAW = PHOTO_Front_Left 859 - PHOTO_Front_Right; 860 LIGHT_compareRAW = map(LIGHT_compareRAW, -LIGHT_rate, 861 LIGHT_rate, -ANGLE_turnMAX, ANGLE_turnMAX);; 862 863 return LIGHT_compareRAW; 864} 865 866/*================================= 867 END OF LIGHT DETECT =================================*/ 868 869 870/*====================================== 871 BEHAVIOUR ======================================*/ 872 873void RETREAT_AVOID() { 874 875 counter_gait = 0; 876 while(counter_gait <= counter_gait_max) { 877 RETREAT(); 878 879 } 880} 881 882void ROTATE_LEFT_AVOID() { 883 counter_gait = 0; 884 rotate_random 885 = 2; 886 while(counter_gait <= counter_gait_max) { 887 ROTATE_LEFT(); 888 } 889} 890 891void 892 ROTATE_RIGHT_AVOID() { 893 counter_gait = 0; 894 rotate_random = 2; 895 while(counter_gait 896 <= counter_gait_max) { 897 ROTATE_RIGHT(); 898 } 899} 900 901void ROTATE_RANDOM_AVOID() 902 { 903 rotate_random = ROTATE_RANDOM(); 904 while(rotate_random == 0) { 905 ROTATE_LEFT_AVOID(); 906 907 } 908 while(rotate_random == 1) { 909 ROTATE_RIGHT_AVOID(); 910 } 911} 912 913void 914 SIDE_AVOID() { 915 if(distance[LEFT] <= distTurn && distance[RIGHT] > distTurn) 916 { 917 LIGHT_left = 0; 918 LIGHT_right = 0; 919 920 SONAR_left = 0; 921 922 SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnMAX, 0)); 923 } 924 925 else if(distance[RIGHT] <= distTurn && distance[LEFT] > distTurn) { 926 LIGHT_left 927 = 0; 928 LIGHT_right = 0; 929 930 SONAR_right = 0; 931 SONAR_left = 932 map(distance[RIGHT], 0, distTurn, ANGLE_turnMAX, 0); 933 } 934 else if(distance[LEFT] 935 <= distTurn && distance[RIGHT] <= distTurn) { 936 LIGHT_left = 0; 937 LIGHT_right 938 = 0; 939 940 if(distance[LEFT] < distance[RIGHT]) { 941 SONAR_left = 942 0; 943 SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnNARROW, 0)); 944 945 } 946 else if(distance[RIGHT] < distance[LEFT]) { 947 SONAR_right = 948 0; 949 SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnNARROW, 0); 950 951 } 952 } 953 else { 954 SONAR_right = 0; 955 SONAR_left = 0; 956 } 957} 958 959/*================================== 960 END OF BEHAVIOUR ==================================*/ 961
Downloadable files
WALTER's Circuit
WALTER's Circuit
Comments
Only logged in users can leave comments