Components and supplies
Arduino Nano R3
Resistor 1k ohm
Button
Ultrasonic Sensor - HC-SR04 (Generic)
6 DOF Robot Arm
DC Power Supply 15A
Servo MG995 or MG998
Project description
Code
FSM_robot_arm.ino
c_cpp
1#include <Servo.h> 2 3//Ultrasonic sensor without pulsein 4unsigned long Ttrig; //Time of trigger 5unsigned long Techo; //Time of echo 6int trigger_pin = 12; 7int echo_pin = 3; 8int FSM_ultrasonic_state = 0; 9byte echoRead = 0; 10float distance; 11unsigned long duration = 0; 12int reset_echoRead = 0; 13//Ultrasonic sensor without pulsein 14 15//SERVO pin Config 16int s0 = 4; 17int s1 = 5; 18int s2 = 6; 19int s3 = 7; 20int s4 = 8; 21 22 23//SERVO START POSITION 24int servo0_start = 90; //90 25int servo1_start = 105; 26int servo2_start = 85; 27int servo3_start = 60; 28int servo4_start = 100; 29 30 31const byte interruptPin = 2; 32const byte startButton = 13; 33byte FSM_global_is_actual = 0; 34byte FSM_global_is_before = 0; 35int stimulus_interrupt = 1; 36 37//FSM Obstacle 38byte FSM_Obstacle_state = 0; 39 40//NUMBER OF STEPS in each loop CYCLE 41byte stepNumber = 1; 42 43int servo_delay = 30; 44 45//FSM GLOBAL VALs 46int FSM_GLOBAL_servo0_VAL; 47int FSM_GLOBAL_servo1_VAL; 48int FSM_GLOBAL_servo2_VAL; 49int FSM_GLOBAL_servo3_VAL; 50int FSM_GLOBAL_servo4_VAL; 51 52//FSM Servo 0 53Servo servo0; //servo 0 definition 54byte FSM_servo0_state = 0; 55int FSM_servo0_AP = servo0_start;//to avoid erratic movement 56int FSM_servo0_FP = servo0_start; 57int FSM_servo0_VAL = 0; 58unsigned long ts_servo0; 59 60//FSM Servo 1 61Servo servo1; //servo 1 definition 62byte FSM_servo1_state = 0; 63int FSM_servo1_AP = servo1_start;//to avoid erratic movement 64int FSM_servo1_FP = servo1_start; 65int FSM_servo1_VAL = 0; 66unsigned long ts_servo1; 67 68//FSM Servo 2 69Servo servo2; //servo 2 definition 70byte FSM_servo2_state = 0; 71int FSM_servo2_AP = servo2_start;//to avoid erratic movement 72int FSM_servo2_FP = servo2_start; 73int FSM_servo2_VAL = 0; 74unsigned long ts_servo2; 75 76//FSM Servo 3 77Servo servo3; //servo 3 definition 78byte FSM_servo3_state = 0; 79int FSM_servo3_AP = servo3_start;//to avoid erratic movement 80int FSM_servo3_FP = servo3_start; 81int FSM_servo3_VAL = 0; 82unsigned long ts_servo3; 83 84//FSM Servo 4 85Servo servo4; //servo 4 definition 86byte FSM_servo4_state = 0; 87int FSM_servo4_AP = servo4_start;//to avoid erratic movement 88int FSM_servo4_FP = servo4_start; 89int FSM_servo4_VAL = 0; 90unsigned long ts_servo4; 91 92// FSM GLOBAL 93byte FSM_global_state = 0; 94 95 96////Setup 97void setup() { 98 Serial.begin(9600); 99 pinMode(startButton, INPUT); 100 pinMode(interruptPin, OUTPUT); //Interrupt associated with the Obstacle FSM 101 102 servo0.attach(s0); // attach Servo 0 to pin 103 servo1.attach(s1); // attach Servo 1 to pin 104 servo2.attach(s2); // attach Servo 2 to pin 105 servo3.attach(s3); // attach Servo 3 to pin 106 servo4.attach(s4); // attach Servo 4 to pin 107 servo0.write(servo0_start); //inicializing positions equal to first position in Global FSM. 108 servo1.write(servo1_start); //This is to prevent erratic movement of the servos 109 servo2.write(servo2_start); 110 servo3.write(servo3_start); 111 servo4.write(servo4_start); 112 113 digitalWrite(interruptPin, LOW); // RESET Interrupt 114 attachInterrupt(digitalPinToInterrupt(interruptPin), call_interrupt, CHANGE); 115 116 //Sensor ultrasonico no pulsein 117 pinMode(trigger_pin, OUTPUT); 118 119 attachInterrupt(digitalPinToInterrupt(echo_pin), getTime, FALLING); 120 //Sensor ultrasonico no pulsein 121 122} 123////END setup 124 125////loop 126void loop() { 127 128 duration = Techo - Ttrig; 129 130 if(reset_echoRead && echoRead){ 131 echoRead = 0; 132 } 133 134 int stimulus = 0; 135 FSM_ultrasonic(echoRead, reset_echoRead); //FSM_ultrasonic; measures distance 136 if (distance<20 && distance>2){ //checks if distance is within the interval 137 stimulus = 1; 138 } 139 FSM_Obstacle(stimulus); //FSM_Obstacle 140 141 //Stimulus for FSM global 142 if (FSM_global_is_actual == 1 && FSM_global_is_before == 0){ 143 stimulus_interrupt = 0; 144 } else if (FSM_global_is_actual == 0 && FSM_global_is_before == 1){ 145 stimulus_interrupt = 1; 146 } 147 148 FSM_global(digitalRead(startButton), stimulus_interrupt); //FSM global 149 150 FSM_servo0(FSM_servo0_FP, FSM_GLOBAL_servo0_VAL); //FSM_servo0 151 FSM_servo1(FSM_servo1_FP, FSM_GLOBAL_servo1_VAL); //FSM_servo1 152 FSM_servo2(FSM_servo2_FP, FSM_GLOBAL_servo2_VAL); //FSM_servo2 153 FSM_servo3(FSM_servo3_FP, FSM_GLOBAL_servo3_VAL); //FSM_servo3 154 FSM_servo4(FSM_servo4_FP, FSM_GLOBAL_servo4_VAL); //FSM_servo4 155} 156////END loop 157 158 159void call_interrupt() { //Interrupt activated by FSM_obstacle 160 if(FSM_global_is_actual){ 161 FSM_global_is_actual = 0; 162 FSM_global_is_before = 1; 163 } else { 164 FSM_global_is_actual = 1; 165 FSM_global_is_before = 0; 166 } 167} 168 169 170///// FSM GLOBAL - Controls all servos 171void FSM_global(int button, int interrupt){ 172 FSM_global_nextstate(button,interrupt); 173 FSM_global_output(); 174} 175 176void FSM_global_nextstate(int button, int interrupt){ 177 switch(FSM_global_state){ 178 case 0: 179 if(button){ 180 FSM_global_state = 1; 181 } 182 break; 183 case 1: 184 //POS 1 185 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){ 186 FSM_global_state = 2; 187 } 188 break; 189 case 2: 190 FSM_global_state = 3; 191 break; 192 case 3: 193 //POS 2 194 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){ 195 FSM_global_state = 4; 196 } 197 break; 198 case 4: 199 FSM_global_state = 5; 200 break; 201 case 5: 202 //POS 3 Hand opens 203 if(FSM_servo4_VAL){ 204 FSM_global_state = 6; 205 } 206 break; 207 case 6: 208 FSM_global_state = 7; 209 break; 210 case 7: 211 //POS 4 Hand closes 212 if(FSM_servo4_VAL){ 213 FSM_global_state = 8; 214 } 215 break; 216 case 8: 217 FSM_global_state = 9; 218 break; 219 case 9: 220 //POS 5 = POS 1 => POS A 221 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){ 222 FSM_global_state = 10; 223 } 224 break; 225 case 10: 226 FSM_global_state = 11; 227 break; 228 case 11: 229 //POS 6 = POS 1 with servo0 different => POS B 230 if(interrupt) FSM_global_state = 20; // POS 6 231 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){ 232 FSM_global_state = 12; 233 } 234 break; 235 case 12: 236 FSM_global_state = 13; 237 break; 238 case 13: 239 //POS 7 = POS 2 with servo0 different 240 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){ 241 FSM_global_state = 14; 242 } 243 break; 244 case 14: 245 FSM_global_state = 15; 246 break; 247 case 15: 248 //POS 8 Hand opens 249 if(FSM_servo4_VAL){ 250 FSM_global_state = 16; 251 } 252 break; 253 case 16: 254 FSM_global_state = 0; 255 break; 256 case 20: //state for search alternative Z position upward 257 if (!interrupt) FSM_global_state = 10; 258 if(FSM_servo2_VAL){ 259 FSM_global_state = 21; 260 } 261 break; 262 case 21: 263 FSM_global_state = 22; 264 break; 265 case 22: //state for search alternative Z position downward 266 if (!interrupt) FSM_global_state = 10; 267 if(FSM_servo2_VAL){ 268 FSM_global_state = 23; 269 } 270 break; 271 case 23: 272 FSM_global_state = 20; 273 break; 274 275 } 276} 277 278void FSM_global_output(){ 279 switch(FSM_global_state){ 280 case 0: 281 //waiting to start 282 FSM_servo0_FP = servo0_start; 283 FSM_servo1_FP = servo1_start; 284 FSM_servo2_FP = servo2_start; 285 FSM_servo3_FP = servo3_start; 286 FSM_servo4_FP = servo4_start; 287 FSM_GLOBAL_servo0_VAL = 1; 288 FSM_GLOBAL_servo1_VAL = 1; 289 FSM_GLOBAL_servo2_VAL = 1; 290 FSM_GLOBAL_servo3_VAL = 1; 291 FSM_GLOBAL_servo4_VAL = 1; 292 break; 293 case 1: 294 //POS 1 295 FSM_servo0_FP = 170; 296 FSM_servo1_FP = 130; 297 FSM_servo2_FP = 30; 298 FSM_servo3_FP = 25; 299 FSM_GLOBAL_servo0_VAL = 0; 300 FSM_GLOBAL_servo1_VAL = 0; 301 FSM_GLOBAL_servo2_VAL = 0; 302 FSM_GLOBAL_servo3_VAL = 0; 303 FSM_GLOBAL_servo4_VAL = 0; 304 break; 305 case 2: 306 FSM_GLOBAL_servo0_VAL = 1; 307 FSM_GLOBAL_servo1_VAL = 1; 308 FSM_GLOBAL_servo2_VAL = 1; 309 FSM_GLOBAL_servo3_VAL = 1; 310 break; 311 case 3: 312 //POS 2 313 FSM_servo0_FP = 170; 314 FSM_servo1_FP = 65; 315 FSM_servo2_FP = 5; 316 FSM_servo3_FP = 140; 317 FSM_GLOBAL_servo0_VAL = 0; 318 FSM_GLOBAL_servo1_VAL = 0; 319 FSM_GLOBAL_servo2_VAL = 0; 320 FSM_GLOBAL_servo3_VAL = 0; 321 break; 322 case 4: 323 FSM_GLOBAL_servo0_VAL = 1; 324 FSM_GLOBAL_servo1_VAL = 1; 325 FSM_GLOBAL_servo2_VAL = 1; 326 FSM_GLOBAL_servo3_VAL = 1; 327 FSM_GLOBAL_servo4_VAL = 1; 328 break; 329 case 5: 330 //POS 3 Hand opens 331 FSM_servo4_FP = 15; 332 FSM_GLOBAL_servo4_VAL = 0; 333 break; 334 case 6: 335 FSM_GLOBAL_servo4_VAL = 1; 336 break; 337 case 7: 338 //POS 4 Hand closes 339 FSM_servo4_FP = 100; 340 FSM_GLOBAL_servo4_VAL = 0; 341 break; 342 case 8: 343 FSM_GLOBAL_servo0_VAL = 1; 344 FSM_GLOBAL_servo1_VAL = 1; 345 FSM_GLOBAL_servo2_VAL = 1; 346 FSM_GLOBAL_servo3_VAL = 1; 347 FSM_GLOBAL_servo4_VAL = 1; 348 break; 349 case 9: 350 //POS 5 = POS 1 => POS A 351 FSM_servo0_FP = 170; 352 FSM_servo1_FP = 130; 353 FSM_servo2_FP = 30; 354 FSM_servo3_FP = 25; 355 FSM_GLOBAL_servo0_VAL = 0; 356 FSM_GLOBAL_servo1_VAL = 0; 357 FSM_GLOBAL_servo2_VAL = 0; 358 FSM_GLOBAL_servo3_VAL = 0; 359 break; 360 case 10: 361 FSM_GLOBAL_servo0_VAL = 1; 362 FSM_GLOBAL_servo1_VAL = 1; 363 FSM_GLOBAL_servo2_VAL = 1; 364 FSM_GLOBAL_servo3_VAL = 1; 365 FSM_servo2_FP = FSM_servo2_AP;//keeps z position after obstacle avoidance 366 break; 367 case 11: 368 //POS 6 = POS 1 with servo0 different => POS B 369 FSM_servo0_FP = 30; 370 FSM_servo1_FP = 130; 371 FSM_servo3_FP = 25; 372 FSM_GLOBAL_servo0_VAL = 0; 373 FSM_GLOBAL_servo1_VAL = 0; 374 FSM_GLOBAL_servo2_VAL = 0; 375 FSM_GLOBAL_servo3_VAL = 0; 376 break; 377 case 12: 378 FSM_GLOBAL_servo0_VAL = 1; 379 FSM_GLOBAL_servo1_VAL = 1; 380 FSM_GLOBAL_servo2_VAL = 1; 381 FSM_GLOBAL_servo3_VAL = 1; 382 break; 383 case 13: 384 //POS 7 = POS 2 with servo0 different 385 FSM_servo0_FP = 30; 386 FSM_servo1_FP = 65; 387 FSM_servo2_FP = 5; 388 FSM_servo3_FP = 140; 389 FSM_GLOBAL_servo0_VAL = 0; 390 FSM_GLOBAL_servo1_VAL = 0; 391 FSM_GLOBAL_servo2_VAL = 0; 392 FSM_GLOBAL_servo3_VAL = 0; 393 break; 394 case 14: 395 FSM_GLOBAL_servo0_VAL = 1; 396 FSM_GLOBAL_servo1_VAL = 1; 397 FSM_GLOBAL_servo2_VAL = 1; 398 FSM_GLOBAL_servo3_VAL = 1; 399 break; 400 case 15: 401 //POS 8 Hand opens 402 FSM_servo4_FP = 15; 403 FSM_GLOBAL_servo4_VAL = 0; 404 break; 405 case 16: 406 FSM_GLOBAL_servo4_VAL = 1; 407 break; 408 case 20: //state for search alternative Z position upward 409 FSM_servo2_FP = 60; 410 FSM_servo0_FP = FSM_servo0_AP; 411 FSM_servo1_FP = FSM_servo1_AP; 412 FSM_servo3_FP = FSM_servo3_AP; 413 FSM_GLOBAL_servo2_VAL = 0; 414 break; 415 case 21: 416 FSM_GLOBAL_servo2_VAL = 1; 417 break; 418 case 22: //state for search alternative Z position downward 419 FSM_servo2_FP = 5; 420 FSM_GLOBAL_servo2_VAL = 0; 421 break; 422 case 23: 423 FSM_GLOBAL_servo2_VAL = 1; 424 break; 425 } 426 427} 428// END FSM GLOBAL 429 430///// FSM SERVO 0 431void FSM_servo0(int FSM_servo0_FP, int FSM_GLOBAL_servo0_VAL){ 432 int ts_stimulus; 433 if((millis() - ts_servo0) > servo_delay){ //Checks Delay 434 ts_stimulus = 1; 435 }else{ 436 ts_stimulus = 0; 437 } 438 439 440 FSM_servo0_nextstate((FSM_servo0_AP < FSM_servo0_FP),(FSM_servo0_AP > FSM_servo0_FP),(FSM_servo0_AP == FSM_servo0_FP),FSM_GLOBAL_servo0_VAL, ts_stimulus); 441 FSM_servo0_output(); 442} 443 444void FSM_servo0_nextstate(int m, int M, int I, int val, int ts_stimulus){ 445 switch(FSM_servo0_state){ 446 case 0: 447 if(I && !val){ //IF AP == FP 448 FSM_servo0_state = 3; 449 } else if(m){ //IF AP is smaller 450 FSM_servo0_state = 2; 451 }else if(M){ //IF AP is bigger 452 FSM_servo0_state = 1; 453 } 454 break; 455 case 1: 456 FSM_servo0_state = 4; 457 break; 458 case 2: 459 FSM_servo0_state = 4; 460 break; 461 case 3: 462 if (val == 1) FSM_servo0_state = 0; 463 break; 464 case 4: 465 if(ts_stimulus){ 466 FSM_servo0_state = 0; 467 } 468 break; 469 } 470} 471 472void FSM_servo0_output(){ 473 switch(FSM_servo0_state){ 474 case 0: 475 FSM_servo0_VAL = 0; 476 break; 477 case 1: 478 FSM_servo0_AP = FSM_servo0_AP - stepNumber; 479 servo0.write(FSM_servo0_AP); 480 ts_servo0 = millis(); 481 break; 482 case 2: 483 FSM_servo0_AP = FSM_servo0_AP + stepNumber; 484 servo0.write(FSM_servo0_AP); 485 ts_servo0 = millis(); 486 break; 487 case 3: 488 FSM_servo0_VAL = 1; 489 break; 490 case 4: 491 break; 492 } 493} 494// END FSM SERVO 0 495 496///// FSM SERVO 1 497void FSM_servo1(int FSM_servo1_FP, int FSM_GLOBAL_servo1_VAL){ 498 int ts_stimulus; 499 if((millis() - ts_servo1) > servo_delay){ //Checks Delay 500 ts_stimulus = 1; 501 }else{ 502 ts_stimulus = 0; 503 } 504 505 FSM_servo1_nextstate((FSM_servo1_AP < FSM_servo1_FP),(FSM_servo1_AP > FSM_servo1_FP),(FSM_servo1_AP == FSM_servo1_FP), FSM_GLOBAL_servo1_VAL, ts_stimulus); 506 FSM_servo1_output(); 507} 508 509void FSM_servo1_nextstate(int m, int M, int I, int val, int ts_stimulus){ 510 switch(FSM_servo1_state){ 511 case 0: 512 if(I && !val){ //IF AP == FP 513 FSM_servo1_state = 3; 514 } else if(m){ //IF AP is smaller 515 FSM_servo1_state = 2; 516 }else if(M){ //IF AP is bigger 517 FSM_servo1_state = 1; 518 } 519 break; 520 case 1: 521 FSM_servo1_state = 4; 522 break; 523 case 2: 524 FSM_servo1_state = 4; 525 break; 526 case 3: 527 if (val == 1) FSM_servo1_state = 0; 528 break; 529 case 4: 530 if(ts_stimulus){ 531 FSM_servo1_state = 0; 532 } 533 break; 534 } 535} 536 537void FSM_servo1_output(){ 538 switch(FSM_servo1_state){ 539 case 0: 540 FSM_servo1_VAL = 0; 541 break; 542 case 1: 543 FSM_servo1_AP = FSM_servo1_AP - stepNumber; 544 servo1.write(FSM_servo1_AP); 545 ts_servo1 = millis(); 546 break; 547 case 2: 548 FSM_servo1_AP = FSM_servo1_AP + stepNumber; 549 servo1.write(FSM_servo1_AP); 550 ts_servo1 = millis(); 551 break; 552 case 3: 553 FSM_servo1_VAL = 1; 554 break; 555 case 4: 556 break; 557 } 558} 559// END FSM SERVO 1 560 561///// FSM SERVO 2 562void FSM_servo2(int FSM_servo2_FP, int FSM_GLOBAL_servo2_VAL){ 563 int ts_stimulus; 564 if((millis() - ts_servo2) > servo_delay){ //Checks Delay 565 ts_stimulus = 1; 566 }else{ 567 ts_stimulus = 0; 568 } 569 570 571 FSM_servo2_nextstate((FSM_servo2_AP < FSM_servo2_FP),(FSM_servo2_AP > FSM_servo2_FP),(FSM_servo2_AP == FSM_servo2_FP), FSM_GLOBAL_servo2_VAL, ts_stimulus); 572 FSM_servo2_output(); 573} 574 575void FSM_servo2_nextstate(int m, int M, int I, int val, int ts_stimulus){ 576 switch(FSM_servo2_state){ 577 case 0: 578 if(I && !val){ //IF AP == FP 579 FSM_servo2_state = 3; 580 } else if(m){ //IF AP is smaller 581 FSM_servo2_state = 2; 582 }else if(M){ //IF AP is bigger 583 FSM_servo2_state = 1; 584 } 585 break; 586 case 1: 587 FSM_servo2_state = 4; 588 break; 589 case 2: 590 FSM_servo2_state = 4; 591 break; 592 case 3: 593 if (val == 1) FSM_servo2_state = 0; 594 break; 595 case 4: 596 if(ts_stimulus){ 597 FSM_servo2_state = 0; 598 } 599 break; 600 } 601} 602 603void FSM_servo2_output(){ 604 switch(FSM_servo2_state){ 605 case 0: 606 FSM_servo2_VAL = 0; 607 break; 608 case 1: 609 FSM_servo2_AP = FSM_servo2_AP - stepNumber; 610 servo2.write(FSM_servo2_AP); 611 ts_servo2 = millis(); 612 break; 613 case 2: 614 FSM_servo2_AP = FSM_servo2_AP + stepNumber; 615 servo2.write(FSM_servo2_AP); 616 ts_servo2 = millis(); 617 break; 618 case 3: 619 FSM_servo2_VAL = 1; 620 break; 621 case 4: 622 break; 623 } 624} 625// END FSM SERVO 2 626 627 628///// FSM SERVO 3 629void FSM_servo3(int FSM_servo3_FP, int FSM_GLOBAL_servo3_VAL){ 630 int ts_stimulus; 631 if((millis() - ts_servo3) > servo_delay){ //Checks Delay 632 ts_stimulus = 1; 633 }else{ 634 ts_stimulus = 0; 635 } 636 637 638 FSM_servo3_nextstate((FSM_servo3_AP < FSM_servo3_FP),(FSM_servo3_AP > FSM_servo3_FP),(FSM_servo3_AP == FSM_servo3_FP), FSM_GLOBAL_servo3_VAL, ts_stimulus); 639 FSM_servo3_output(); 640} 641 642void FSM_servo3_nextstate(int m, int M, int I, int val, int ts_stimulus){ 643 switch(FSM_servo3_state){ 644 case 0: 645 if(I && !val){ //IF AP == FP 646 FSM_servo3_state = 3; 647 } else if(m){ //IF AP is smaller 648 FSM_servo3_state = 2; 649 }else if(M){ //IF AP is bigger 650 FSM_servo3_state = 1; 651 } 652 break; 653 case 1: 654 FSM_servo3_state = 4; 655 break; 656 case 2: 657 FSM_servo3_state = 4; 658 break; 659 case 3: 660 if (val == 1) FSM_servo3_state = 0; 661 break; 662 case 4: 663 if(ts_stimulus){ 664 FSM_servo3_state = 0; 665 } 666 break; 667 } 668} 669 670void FSM_servo3_output(){ 671 switch(FSM_servo3_state){ 672 case 0: 673 FSM_servo3_VAL = 0; 674 break; 675 case 1: 676 FSM_servo3_AP = FSM_servo3_AP - stepNumber; 677 servo3.write(FSM_servo3_AP); 678 ts_servo3 = millis(); 679 break; 680 case 2: 681 FSM_servo3_AP = FSM_servo3_AP + stepNumber; 682 servo3.write(FSM_servo3_AP); 683 ts_servo3 = millis(); 684 break; 685 case 3: 686 FSM_servo3_VAL = 1; 687 break; 688 case 4: 689 break; 690 } 691} 692// END FSM SERVO 3 693 694 695///// FSM SERVO 4 696void FSM_servo4(int FSM_servo4_FP, int FSM_GLOBAL_servo4_VAL){ 697 int ts_stimulus; 698 if((millis() - ts_servo4) > servo_delay){ //Checks Delay 699 ts_stimulus = 1; 700 }else{ 701 ts_stimulus = 0; 702 } 703 704 705 FSM_servo4_nextstate((FSM_servo4_AP < FSM_servo4_FP),(FSM_servo4_AP > FSM_servo4_FP),(FSM_servo4_AP == FSM_servo4_FP),FSM_GLOBAL_servo4_VAL, ts_stimulus); 706 FSM_servo4_output(); 707} 708 709void FSM_servo4_nextstate(int m, int M, int I, int val, int ts_stimulus){ 710 switch(FSM_servo4_state){ 711 case 0: 712 if(I && !val){ //IF AP == FP 713 FSM_servo4_state = 3; 714 } else if(m){ //IF AP is smaller 715 FSM_servo4_state = 2; 716 }else if(M){ //IF AP is bigger 717 FSM_servo4_state = 1; 718 } 719 break; 720 case 1: 721 FSM_servo4_state = 4; 722 break; 723 case 2: 724 FSM_servo4_state = 4; 725 break; 726 case 3: 727 if (val == 1) FSM_servo4_state = 0; 728 break; 729 case 4: 730 if(ts_stimulus){ 731 FSM_servo4_state = 0; 732 } 733 break; 734 } 735} 736 737void FSM_servo4_output(){ 738 switch(FSM_servo4_state){ 739 case 0: 740 FSM_servo4_VAL = 0; 741 break; 742 case 1: 743 FSM_servo4_AP = FSM_servo4_AP - stepNumber; 744 servo4.write(FSM_servo4_AP); 745 ts_servo4 = millis(); 746 break; 747 case 2: 748 FSM_servo4_AP = FSM_servo4_AP + stepNumber; 749 servo4.write(FSM_servo4_AP); 750 ts_servo4 = millis(); 751 break; 752 case 3: 753 FSM_servo4_VAL = 1; 754 break; 755 case 4: 756 break; 757 } 758} 759// END FSM SERVO 4 760 761 762///// FSM Obstacle 763void FSM_Obstacle(int stimulus){ 764 765 FSM_Obstacle_nextstate(stimulus); 766 FSM_Obstacle_output(); 767} 768 769void FSM_Obstacle_nextstate(int stimulus){ 770 switch(FSM_Obstacle_state){ 771 case 0: 772 if(stimulus){ 773 FSM_Obstacle_state = 1; 774 } 775 break; 776 case 1: 777 if(!stimulus){ 778 FSM_Obstacle_state = 0; 779 } 780 break; 781 } 782} 783 784void FSM_Obstacle_output(){ 785 switch(FSM_Obstacle_state){ 786 case 0: //Activates interrupt 787 digitalWrite(interruptPin, LOW); 788 break; 789 case 1: //Activates interrupt 790 digitalWrite(interruptPin, HIGH); 791 break; 792 } 793} 794// END FSM Obstacle 795 796 797 //Ultrasonic sensor without pulsein 798 void getTime(){ //Interrupt Function connected with the echo pin of the Ultrasonic sensor 799 Techo = micros(); 800 echoRead = 1; 801} 802 803//Ultrasonic FSM without pulseIn Function 804void FSM_ultrasonic(byte echoRead, int reset_echoRead){ 805 806 FSM_ultrasonic_nextstate(echoRead); 807 FSM_ultrasonic_output(reset_echoRead); 808} 809 810void FSM_ultrasonic_nextstate(byte echoRead){ 811 switch(FSM_ultrasonic_state){ 812 case 0: 813 FSM_ultrasonic_state = 1; 814 break; 815 case 1: 816 if(echoRead){ 817 FSM_ultrasonic_state = 2; 818 } 819 if((micros() - Ttrig) > 30000){ //Timeout 820 FSM_ultrasonic_state = 0; 821 } 822 823 break; 824 case 2: 825 FSM_ultrasonic_state = 0; 826 break; 827 } 828} 829 830void FSM_ultrasonic_output(int reset_echoRead_local){ //////////////////No se est a utilizar o reset_echoRead_local///////////////////////////////////// 831 switch(FSM_ultrasonic_state){ 832 case 0: //Sends trigger 833 // Clears the trigPin 834 digitalWrite(trigger_pin, LOW); 835 delayMicroseconds(1); 836 // Sets the trigPin on HIGH state for 10 micro seconds 837 digitalWrite(trigger_pin, HIGH); 838 delayMicroseconds(10); 839 digitalWrite(trigger_pin, LOW); 840 841 Ttrig = micros(); 842 break; 843 case 1: 844 reset_echoRead = 1; ///////////////////////////////////////Porqu aqui?????//////////////////////////////////////////////// 845 break; 846 case 2: //Receives echo 847 if(echoRead){ 848 if(!duration){ //If duration is zero, avoids negative distance 849 distance = 0; 850 }else{ 851 distance = duration*0.034/2 - 7.28; 852 } 853 reset_echoRead = 1; 854 } 855 break; 856 857 858 } 859} 860//END Ultrasonic FSM without pulseIn Function 861
FSM_robot_arm.ino
c_cpp
1#include <Servo.h> 2 3//Ultrasonic sensor without pulsein 4unsigned 5 long Ttrig; //Time of trigger 6unsigned long Techo; //Time of echo 7int trigger_pin 8 = 12; 9int echo_pin = 3; 10int FSM_ultrasonic_state = 0; 11byte echoRead = 0; 12float 13 distance; 14unsigned long duration = 0; 15int reset_echoRead = 0; 16//Ultrasonic 17 sensor without pulsein 18 19//SERVO pin Config 20int s0 = 4; 21int s1 = 5; 22int 23 s2 = 6; 24int s3 = 7; 25int s4 = 8; 26 27 28//SERVO START POSITION 29int servo0_start 30 = 90; //90 31int servo1_start = 105; 32int servo2_start = 85; 33int servo3_start 34 = 60; 35int servo4_start = 100; 36 37 38const byte interruptPin = 2; 39const 40 byte startButton = 13; 41byte FSM_global_is_actual = 0; 42byte FSM_global_is_before 43 = 0; 44int stimulus_interrupt = 1; 45 46//FSM Obstacle 47byte FSM_Obstacle_state 48 = 0; 49 50//NUMBER OF STEPS in each loop CYCLE 51byte stepNumber = 1; 52 53int 54 servo_delay = 30; 55 56//FSM GLOBAL VALs 57int FSM_GLOBAL_servo0_VAL; 58int 59 FSM_GLOBAL_servo1_VAL; 60int FSM_GLOBAL_servo2_VAL; 61int FSM_GLOBAL_servo3_VAL; 62int 63 FSM_GLOBAL_servo4_VAL; 64 65//FSM Servo 0 66Servo servo0; //servo 0 definition 67byte 68 FSM_servo0_state = 0; 69int FSM_servo0_AP = servo0_start;//to avoid erratic movement 70int 71 FSM_servo0_FP = servo0_start; 72int FSM_servo0_VAL = 0; 73unsigned long ts_servo0; 74 75//FSM 76 Servo 1 77Servo servo1; //servo 1 definition 78byte FSM_servo1_state = 0; 79int 80 FSM_servo1_AP = servo1_start;//to avoid erratic movement 81int FSM_servo1_FP = 82 servo1_start; 83int FSM_servo1_VAL = 0; 84unsigned long ts_servo1; 85 86//FSM 87 Servo 2 88Servo servo2; //servo 2 definition 89byte FSM_servo2_state = 0; 90int 91 FSM_servo2_AP = servo2_start;//to avoid erratic movement 92int FSM_servo2_FP = 93 servo2_start; 94int FSM_servo2_VAL = 0; 95unsigned long ts_servo2; 96 97//FSM 98 Servo 3 99Servo servo3; //servo 3 definition 100byte FSM_servo3_state = 0; 101int 102 FSM_servo3_AP = servo3_start;//to avoid erratic movement 103int FSM_servo3_FP = 104 servo3_start; 105int FSM_servo3_VAL = 0; 106unsigned long ts_servo3; 107 108//FSM 109 Servo 4 110Servo servo4; //servo 4 definition 111byte FSM_servo4_state = 0; 112int 113 FSM_servo4_AP = servo4_start;//to avoid erratic movement 114int FSM_servo4_FP = 115 servo4_start; 116int FSM_servo4_VAL = 0; 117unsigned long ts_servo4; 118 119// FSM 120 GLOBAL 121byte FSM_global_state = 0; 122 123 124////Setup 125void setup() { 126 127 Serial.begin(9600); 128 pinMode(startButton, INPUT); 129 pinMode(interruptPin, 130 OUTPUT); //Interrupt associated with the Obstacle FSM 131 132 servo0.attach(s0); 133 // attach Servo 0 to pin 134 servo1.attach(s1); // attach Servo 1 to pin 135 servo2.attach(s2); 136 // attach Servo 2 to pin 137 servo3.attach(s3); // attach Servo 3 to pin 138 servo4.attach(s4); 139 // attach Servo 4 to pin 140 servo0.write(servo0_start); //inicializing positions 141 equal to first position in Global FSM. 142 servo1.write(servo1_start); //This is 143 to prevent erratic movement of the servos 144 servo2.write(servo2_start); 145 servo3.write(servo3_start); 146 147 servo4.write(servo4_start); 148 149 digitalWrite(interruptPin, 150 LOW); // RESET Interrupt 151 attachInterrupt(digitalPinToInterrupt(interruptPin), 152 call_interrupt, CHANGE); 153 154 //Sensor ultrasonico no pulsein 155 pinMode(trigger_pin, 156 OUTPUT); 157 158 attachInterrupt(digitalPinToInterrupt(echo_pin), getTime, FALLING); 159 160 //Sensor ultrasonico no pulsein 161 162} 163////END setup 164 165////loop 166void 167 loop() { 168 169 duration = Techo - Ttrig; 170 171 if(reset_echoRead && 172 echoRead){ 173 echoRead = 0; 174 } 175 176 int stimulus = 0; 177 FSM_ultrasonic(echoRead, 178 reset_echoRead); //FSM_ultrasonic; measures distance 179 if (distance<20 && distance>2){ 180 //checks if distance is within the interval 181 stimulus = 1; 182 } 183 FSM_Obstacle(stimulus); 184 //FSM_Obstacle 185 186 //Stimulus for FSM global 187 if (FSM_global_is_actual 188 == 1 && FSM_global_is_before == 0){ 189 stimulus_interrupt = 0; 190 } else 191 if (FSM_global_is_actual == 0 && FSM_global_is_before == 1){ 192 stimulus_interrupt 193 = 1; 194 } 195 196 FSM_global(digitalRead(startButton), stimulus_interrupt); 197 //FSM global 198 199 FSM_servo0(FSM_servo0_FP, FSM_GLOBAL_servo0_VAL); //FSM_servo0 200 201 FSM_servo1(FSM_servo1_FP, FSM_GLOBAL_servo1_VAL); //FSM_servo1 202 FSM_servo2(FSM_servo2_FP, 203 FSM_GLOBAL_servo2_VAL); //FSM_servo2 204 FSM_servo3(FSM_servo3_FP, FSM_GLOBAL_servo3_VAL); 205 //FSM_servo3 206 FSM_servo4(FSM_servo4_FP, FSM_GLOBAL_servo4_VAL); //FSM_servo4 207} 208////END 209 loop 210 211 212void call_interrupt() { //Interrupt activated by FSM_obstacle 213 214 if(FSM_global_is_actual){ 215 FSM_global_is_actual = 0; 216 FSM_global_is_before 217 = 1; 218 } else { 219 FSM_global_is_actual = 1; 220 FSM_global_is_before 221 = 0; 222 } 223} 224 225 226///// FSM GLOBAL - Controls all servos 227void FSM_global(int 228 button, int interrupt){ 229 FSM_global_nextstate(button,interrupt); 230 FSM_global_output(); 231} 232 233void 234 FSM_global_nextstate(int button, int interrupt){ 235 switch(FSM_global_state){ 236 237 case 0: 238 if(button){ 239 FSM_global_state = 1; 240 } 241 242 break; 243 case 1: 244 //POS 1 245 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){ 246 247 FSM_global_state = 2; 248 } 249 break; 250 case 2: 251 FSM_global_state 252 = 3; 253 break; 254 case 3: 255 //POS 2 256 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){ 257 258 FSM_global_state = 4; 259 } 260 break; 261 case 4: 262 FSM_global_state 263 = 5; 264 break; 265 case 5: 266 //POS 3 Hand opens 267 if(FSM_servo4_VAL){ 268 269 FSM_global_state = 6; 270 } 271 break; 272 case 6: 273 FSM_global_state 274 = 7; 275 break; 276 case 7: 277 //POS 4 Hand closes 278 if(FSM_servo4_VAL){ 279 280 FSM_global_state = 8; 281 } 282 break; 283 case 8: 284 FSM_global_state 285 = 9; 286 break; 287 case 9: 288 //POS 5 = POS 1 => POS A 289 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){ 290 291 FSM_global_state = 10; 292 } 293 break; 294 case 10: 295 FSM_global_state 296 = 11; 297 break; 298 case 11: 299 //POS 6 = POS 1 with servo0 different 300 => POS B 301 if(interrupt) FSM_global_state = 20; // POS 6 302 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){ 303 304 FSM_global_state = 12; 305 } 306 break; 307 case 12: 308 FSM_global_state 309 = 13; 310 break; 311 case 13: 312 //POS 7 = POS 2 with servo0 different 313 314 if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){ 315 FSM_global_state 316 = 14; 317 } 318 break; 319 case 14: 320 FSM_global_state = 15; 321 322 break; 323 case 15: 324 //POS 8 Hand opens 325 if(FSM_servo4_VAL){ 326 327 FSM_global_state = 16; 328 } 329 break; 330 case 16: 331 FSM_global_state 332 = 0; 333 break; 334 case 20: //state for search alternative Z position upward 335 336 if (!interrupt) FSM_global_state = 10; 337 if(FSM_servo2_VAL){ 338 FSM_global_state 339 = 21; 340 } 341 break; 342 case 21: 343 FSM_global_state = 22; 344 345 break; 346 case 22: //state for search alternative Z position downward 347 348 if (!interrupt) FSM_global_state = 10; 349 if(FSM_servo2_VAL){ 350 FSM_global_state 351 = 23; 352 } 353 break; 354 case 23: 355 FSM_global_state = 20; 356 357 break; 358 359 } 360} 361 362void FSM_global_output(){ 363 switch(FSM_global_state){ 364 365 case 0: 366 //waiting to start 367 FSM_servo0_FP = servo0_start; 368 369 FSM_servo1_FP = servo1_start; 370 FSM_servo2_FP = servo2_start; 371 FSM_servo3_FP 372 = servo3_start; 373 FSM_servo4_FP = servo4_start; 374 FSM_GLOBAL_servo0_VAL 375 = 1; 376 FSM_GLOBAL_servo1_VAL = 1; 377 FSM_GLOBAL_servo2_VAL = 1; 378 379 FSM_GLOBAL_servo3_VAL = 1; 380 FSM_GLOBAL_servo4_VAL = 1; 381 break; 382 383 case 1: 384 //POS 1 385 FSM_servo0_FP = 170; 386 FSM_servo1_FP 387 = 130; 388 FSM_servo2_FP = 30; 389 FSM_servo3_FP = 25; 390 FSM_GLOBAL_servo0_VAL 391 = 0; 392 FSM_GLOBAL_servo1_VAL = 0; 393 FSM_GLOBAL_servo2_VAL = 0; 394 395 FSM_GLOBAL_servo3_VAL = 0; 396 FSM_GLOBAL_servo4_VAL = 0; 397 break; 398 399 case 2: 400 FSM_GLOBAL_servo0_VAL = 1; 401 FSM_GLOBAL_servo1_VAL = 402 1; 403 FSM_GLOBAL_servo2_VAL = 1; 404 FSM_GLOBAL_servo3_VAL = 1; 405 break; 406 407 case 3: 408 //POS 2 409 FSM_servo0_FP = 170; 410 FSM_servo1_FP 411 = 65; 412 FSM_servo2_FP = 5; 413 FSM_servo3_FP = 140; 414 FSM_GLOBAL_servo0_VAL 415 = 0; 416 FSM_GLOBAL_servo1_VAL = 0; 417 FSM_GLOBAL_servo2_VAL = 0; 418 419 FSM_GLOBAL_servo3_VAL = 0; 420 break; 421 case 4: 422 FSM_GLOBAL_servo0_VAL 423 = 1; 424 FSM_GLOBAL_servo1_VAL = 1; 425 FSM_GLOBAL_servo2_VAL = 1; 426 427 FSM_GLOBAL_servo3_VAL = 1; 428 FSM_GLOBAL_servo4_VAL = 1; 429 break; 430 431 case 5: 432 //POS 3 Hand opens 433 FSM_servo4_FP = 15; 434 FSM_GLOBAL_servo4_VAL 435 = 0; 436 break; 437 case 6: 438 FSM_GLOBAL_servo4_VAL = 1; 439 break; 440 441 case 7: 442 //POS 4 Hand closes 443 FSM_servo4_FP = 100; 444 FSM_GLOBAL_servo4_VAL 445 = 0; 446 break; 447 case 8: 448 FSM_GLOBAL_servo0_VAL = 1; 449 FSM_GLOBAL_servo1_VAL 450 = 1; 451 FSM_GLOBAL_servo2_VAL = 1; 452 FSM_GLOBAL_servo3_VAL = 1; 453 454 FSM_GLOBAL_servo4_VAL = 1; 455 break; 456 case 9: 457 //POS 5 = POS 458 1 => POS A 459 FSM_servo0_FP = 170; 460 FSM_servo1_FP = 130; 461 FSM_servo2_FP 462 = 30; 463 FSM_servo3_FP = 25; 464 FSM_GLOBAL_servo0_VAL = 0; 465 FSM_GLOBAL_servo1_VAL 466 = 0; 467 FSM_GLOBAL_servo2_VAL = 0; 468 FSM_GLOBAL_servo3_VAL = 0; 469 470 break; 471 case 10: 472 FSM_GLOBAL_servo0_VAL = 1; 473 FSM_GLOBAL_servo1_VAL 474 = 1; 475 FSM_GLOBAL_servo2_VAL = 1; 476 FSM_GLOBAL_servo3_VAL = 1; 477 478 FSM_servo2_FP = FSM_servo2_AP;//keeps z position after obstacle avoidance 479 480 break; 481 case 11: 482 //POS 6 = POS 1 with servo0 different => POS B 483 484 FSM_servo0_FP = 30; 485 FSM_servo1_FP = 130; 486 FSM_servo3_FP = 487 25; 488 FSM_GLOBAL_servo0_VAL = 0; 489 FSM_GLOBAL_servo1_VAL = 0; 490 491 FSM_GLOBAL_servo2_VAL = 0; 492 FSM_GLOBAL_servo3_VAL = 0; 493 break; 494 495 case 12: 496 FSM_GLOBAL_servo0_VAL = 1; 497 FSM_GLOBAL_servo1_VAL 498 = 1; 499 FSM_GLOBAL_servo2_VAL = 1; 500 FSM_GLOBAL_servo3_VAL = 1; 501 502 break; 503 case 13: 504 //POS 7 = POS 2 with servo0 different 505 FSM_servo0_FP 506 = 30; 507 FSM_servo1_FP = 65; 508 FSM_servo2_FP = 5; 509 FSM_servo3_FP 510 = 140; 511 FSM_GLOBAL_servo0_VAL = 0; 512 FSM_GLOBAL_servo1_VAL = 0; 513 514 FSM_GLOBAL_servo2_VAL = 0; 515 FSM_GLOBAL_servo3_VAL = 0; 516 break; 517 518 case 14: 519 FSM_GLOBAL_servo0_VAL = 1; 520 FSM_GLOBAL_servo1_VAL 521 = 1; 522 FSM_GLOBAL_servo2_VAL = 1; 523 FSM_GLOBAL_servo3_VAL = 1; 524 525 break; 526 case 15: 527 //POS 8 Hand opens 528 FSM_servo4_FP = 15; 529 530 FSM_GLOBAL_servo4_VAL = 0; 531 break; 532 case 16: 533 FSM_GLOBAL_servo4_VAL 534 = 1; 535 break; 536 case 20: //state for search alternative Z position upward 537 538 FSM_servo2_FP = 60; 539 FSM_servo0_FP = FSM_servo0_AP; 540 FSM_servo1_FP 541 = FSM_servo1_AP; 542 FSM_servo3_FP = FSM_servo3_AP; 543 FSM_GLOBAL_servo2_VAL 544 = 0; 545 break; 546 case 21: 547 FSM_GLOBAL_servo2_VAL = 1; 548 break; 549 550 case 22: //state for search alternative Z position downward 551 FSM_servo2_FP 552 = 5; 553 FSM_GLOBAL_servo2_VAL = 0; 554 break; 555 case 23: 556 FSM_GLOBAL_servo2_VAL 557 = 1; 558 break; 559 } 560 561} 562// END FSM GLOBAL 563 564///// FSM SERVO 0 565void 566 FSM_servo0(int FSM_servo0_FP, int FSM_GLOBAL_servo0_VAL){ 567 int ts_stimulus; 568 569 if((millis() - ts_servo0) > servo_delay){ //Checks Delay 570 ts_stimulus = 571 1; 572 }else{ 573 ts_stimulus = 0; 574 } 575 576 577 FSM_servo0_nextstate((FSM_servo0_AP 578 < FSM_servo0_FP),(FSM_servo0_AP > FSM_servo0_FP),(FSM_servo0_AP == FSM_servo0_FP),FSM_GLOBAL_servo0_VAL, 579 ts_stimulus); 580 FSM_servo0_output(); 581} 582 583void FSM_servo0_nextstate(int 584 m, int M, int I, int val, int ts_stimulus){ 585 switch(FSM_servo0_state){ 586 case 587 0: 588 if(I && !val){ //IF AP == FP 589 FSM_servo0_state = 3; 590 } else 591 if(m){ //IF AP is smaller 592 FSM_servo0_state = 2; 593 }else if(M){ //IF 594 AP is bigger 595 FSM_servo0_state = 1; 596 } 597 break; 598 case 1: 599 600 FSM_servo0_state = 4; 601 break; 602 case 2: 603 FSM_servo0_state 604 = 4; 605 break; 606 case 3: 607 if (val == 1) FSM_servo0_state = 0; 608 609 break; 610 case 4: 611 if(ts_stimulus){ 612 FSM_servo0_state = 0; 613 614 } 615 break; 616 } 617} 618 619void FSM_servo0_output(){ 620 switch(FSM_servo0_state){ 621 622 case 0: 623 FSM_servo0_VAL = 0; 624 break; 625 case 1: 626 FSM_servo0_AP 627 = FSM_servo0_AP - stepNumber; 628 servo0.write(FSM_servo0_AP); 629 ts_servo0 630 = millis(); 631 break; 632 case 2: 633 FSM_servo0_AP = FSM_servo0_AP 634 + stepNumber; 635 servo0.write(FSM_servo0_AP); 636 ts_servo0 = millis(); 637 638 break; 639 case 3: 640 FSM_servo0_VAL = 1; 641 break; 642 case 643 4: 644 break; 645 } 646} 647// END FSM SERVO 0 648 649///// FSM SERVO 1 650void 651 FSM_servo1(int FSM_servo1_FP, int FSM_GLOBAL_servo1_VAL){ 652 int ts_stimulus; 653 654 if((millis() - ts_servo1) > servo_delay){ //Checks Delay 655 ts_stimulus = 656 1; 657 }else{ 658 ts_stimulus = 0; 659 } 660 661 FSM_servo1_nextstate((FSM_servo1_AP 662 < FSM_servo1_FP),(FSM_servo1_AP > FSM_servo1_FP),(FSM_servo1_AP == FSM_servo1_FP), 663 FSM_GLOBAL_servo1_VAL, ts_stimulus); 664 FSM_servo1_output(); 665} 666 667void 668 FSM_servo1_nextstate(int m, int M, int I, int val, int ts_stimulus){ 669 switch(FSM_servo1_state){ 670 671 case 0: 672 if(I && !val){ //IF AP == FP 673 FSM_servo1_state = 3; 674 675 } else if(m){ //IF AP is smaller 676 FSM_servo1_state = 2; 677 }else 678 if(M){ //IF AP is bigger 679 FSM_servo1_state = 1; 680 } 681 break; 682 683 case 1: 684 FSM_servo1_state = 4; 685 break; 686 case 2: 687 FSM_servo1_state 688 = 4; 689 break; 690 case 3: 691 if (val == 1) FSM_servo1_state = 0; 692 693 break; 694 case 4: 695 if(ts_stimulus){ 696 FSM_servo1_state = 0; 697 698 } 699 break; 700 } 701} 702 703void FSM_servo1_output(){ 704 switch(FSM_servo1_state){ 705 706 case 0: 707 FSM_servo1_VAL = 0; 708 break; 709 case 1: 710 FSM_servo1_AP 711 = FSM_servo1_AP - stepNumber; 712 servo1.write(FSM_servo1_AP); 713 ts_servo1 714 = millis(); 715 break; 716 case 2: 717 FSM_servo1_AP = FSM_servo1_AP 718 + stepNumber; 719 servo1.write(FSM_servo1_AP); 720 ts_servo1 = millis(); 721 722 break; 723 case 3: 724 FSM_servo1_VAL = 1; 725 break; 726 case 727 4: 728 break; 729 } 730} 731// END FSM SERVO 1 732 733///// FSM SERVO 2 734void 735 FSM_servo2(int FSM_servo2_FP, int FSM_GLOBAL_servo2_VAL){ 736 int ts_stimulus; 737 738 if((millis() - ts_servo2) > servo_delay){ //Checks Delay 739 ts_stimulus = 740 1; 741 }else{ 742 ts_stimulus = 0; 743 } 744 745 746 FSM_servo2_nextstate((FSM_servo2_AP 747 < FSM_servo2_FP),(FSM_servo2_AP > FSM_servo2_FP),(FSM_servo2_AP == FSM_servo2_FP), 748 FSM_GLOBAL_servo2_VAL, ts_stimulus); 749 FSM_servo2_output(); 750} 751 752void 753 FSM_servo2_nextstate(int m, int M, int I, int val, int ts_stimulus){ 754 switch(FSM_servo2_state){ 755 756 case 0: 757 if(I && !val){ //IF AP == FP 758 FSM_servo2_state = 3; 759 760 } else if(m){ //IF AP is smaller 761 FSM_servo2_state = 2; 762 }else 763 if(M){ //IF AP is bigger 764 FSM_servo2_state = 1; 765 } 766 break; 767 768 case 1: 769 FSM_servo2_state = 4; 770 break; 771 case 2: 772 FSM_servo2_state 773 = 4; 774 break; 775 case 3: 776 if (val == 1) FSM_servo2_state = 0; 777 778 break; 779 case 4: 780 if(ts_stimulus){ 781 FSM_servo2_state = 0; 782 783 } 784 break; 785 } 786} 787 788void FSM_servo2_output(){ 789 switch(FSM_servo2_state){ 790 791 case 0: 792 FSM_servo2_VAL = 0; 793 break; 794 case 1: 795 FSM_servo2_AP 796 = FSM_servo2_AP - stepNumber; 797 servo2.write(FSM_servo2_AP); 798 ts_servo2 799 = millis(); 800 break; 801 case 2: 802 FSM_servo2_AP = FSM_servo2_AP 803 + stepNumber; 804 servo2.write(FSM_servo2_AP); 805 ts_servo2 = millis(); 806 807 break; 808 case 3: 809 FSM_servo2_VAL = 1; 810 break; 811 case 812 4: 813 break; 814 } 815} 816// END FSM SERVO 2 817 818 819///// FSM SERVO 3 820void 821 FSM_servo3(int FSM_servo3_FP, int FSM_GLOBAL_servo3_VAL){ 822 int ts_stimulus; 823 824 if((millis() - ts_servo3) > servo_delay){ //Checks Delay 825 ts_stimulus = 826 1; 827 }else{ 828 ts_stimulus = 0; 829 } 830 831 832 FSM_servo3_nextstate((FSM_servo3_AP 833 < FSM_servo3_FP),(FSM_servo3_AP > FSM_servo3_FP),(FSM_servo3_AP == FSM_servo3_FP), 834 FSM_GLOBAL_servo3_VAL, ts_stimulus); 835 FSM_servo3_output(); 836} 837 838void 839 FSM_servo3_nextstate(int m, int M, int I, int val, int ts_stimulus){ 840 switch(FSM_servo3_state){ 841 842 case 0: 843 if(I && !val){ //IF AP == FP 844 FSM_servo3_state = 3; 845 846 } else if(m){ //IF AP is smaller 847 FSM_servo3_state = 2; 848 }else 849 if(M){ //IF AP is bigger 850 FSM_servo3_state = 1; 851 } 852 break; 853 854 case 1: 855 FSM_servo3_state = 4; 856 break; 857 case 2: 858 FSM_servo3_state 859 = 4; 860 break; 861 case 3: 862 if (val == 1) FSM_servo3_state = 0; 863 864 break; 865 case 4: 866 if(ts_stimulus){ 867 FSM_servo3_state = 0; 868 869 } 870 break; 871 } 872} 873 874void FSM_servo3_output(){ 875 switch(FSM_servo3_state){ 876 877 case 0: 878 FSM_servo3_VAL = 0; 879 break; 880 case 1: 881 FSM_servo3_AP 882 = FSM_servo3_AP - stepNumber; 883 servo3.write(FSM_servo3_AP); 884 ts_servo3 885 = millis(); 886 break; 887 case 2: 888 FSM_servo3_AP = FSM_servo3_AP 889 + stepNumber; 890 servo3.write(FSM_servo3_AP); 891 ts_servo3 = millis(); 892 893 break; 894 case 3: 895 FSM_servo3_VAL = 1; 896 break; 897 case 898 4: 899 break; 900 } 901} 902// END FSM SERVO 3 903 904 905///// FSM SERVO 4 906void 907 FSM_servo4(int FSM_servo4_FP, int FSM_GLOBAL_servo4_VAL){ 908 int ts_stimulus; 909 910 if((millis() - ts_servo4) > servo_delay){ //Checks Delay 911 ts_stimulus = 912 1; 913 }else{ 914 ts_stimulus = 0; 915 } 916 917 918 FSM_servo4_nextstate((FSM_servo4_AP 919 < FSM_servo4_FP),(FSM_servo4_AP > FSM_servo4_FP),(FSM_servo4_AP == FSM_servo4_FP),FSM_GLOBAL_servo4_VAL, 920 ts_stimulus); 921 FSM_servo4_output(); 922} 923 924void FSM_servo4_nextstate(int 925 m, int M, int I, int val, int ts_stimulus){ 926 switch(FSM_servo4_state){ 927 case 928 0: 929 if(I && !val){ //IF AP == FP 930 FSM_servo4_state = 3; 931 } else 932 if(m){ //IF AP is smaller 933 FSM_servo4_state = 2; 934 }else if(M){ //IF 935 AP is bigger 936 FSM_servo4_state = 1; 937 } 938 break; 939 case 1: 940 941 FSM_servo4_state = 4; 942 break; 943 case 2: 944 FSM_servo4_state 945 = 4; 946 break; 947 case 3: 948 if (val == 1) FSM_servo4_state = 0; 949 950 break; 951 case 4: 952 if(ts_stimulus){ 953 FSM_servo4_state = 0; 954 955 } 956 break; 957 } 958} 959 960void FSM_servo4_output(){ 961 switch(FSM_servo4_state){ 962 963 case 0: 964 FSM_servo4_VAL = 0; 965 break; 966 case 1: 967 FSM_servo4_AP 968 = FSM_servo4_AP - stepNumber; 969 servo4.write(FSM_servo4_AP); 970 ts_servo4 971 = millis(); 972 break; 973 case 2: 974 FSM_servo4_AP = FSM_servo4_AP 975 + stepNumber; 976 servo4.write(FSM_servo4_AP); 977 ts_servo4 = millis(); 978 979 break; 980 case 3: 981 FSM_servo4_VAL = 1; 982 break; 983 case 984 4: 985 break; 986 } 987} 988// END FSM SERVO 4 989 990 991///// FSM Obstacle 992void 993 FSM_Obstacle(int stimulus){ 994 995 FSM_Obstacle_nextstate(stimulus); 996 FSM_Obstacle_output(); 997} 998 999void 1000 FSM_Obstacle_nextstate(int stimulus){ 1001 switch(FSM_Obstacle_state){ 1002 case 1003 0: 1004 if(stimulus){ 1005 FSM_Obstacle_state = 1; 1006 } 1007 break; 1008 1009 case 1: 1010 if(!stimulus){ 1011 FSM_Obstacle_state = 0; 1012 } 1013 1014 break; 1015 } 1016} 1017 1018void FSM_Obstacle_output(){ 1019 switch(FSM_Obstacle_state){ 1020 1021 case 0: //Activates interrupt 1022 digitalWrite(interruptPin, LOW); 1023 1024 break; 1025 case 1: //Activates interrupt 1026 digitalWrite(interruptPin, 1027 HIGH); 1028 break; 1029 } 1030} 1031// END FSM Obstacle 1032 1033 1034 //Ultrasonic 1035 sensor without pulsein 1036 void getTime(){ //Interrupt Function connected with 1037 the echo pin of the Ultrasonic sensor 1038 Techo = micros(); 1039 echoRead = 1; 1040} 1041 1042//Ultrasonic 1043 FSM without pulseIn Function 1044void FSM_ultrasonic(byte echoRead, int reset_echoRead){ 1045 1046 1047 FSM_ultrasonic_nextstate(echoRead); 1048 FSM_ultrasonic_output(reset_echoRead); 1049} 1050 1051void 1052 FSM_ultrasonic_nextstate(byte echoRead){ 1053 switch(FSM_ultrasonic_state){ 1054 1055 case 0: 1056 FSM_ultrasonic_state = 1; 1057 break; 1058 case 1: 1059 1060 if(echoRead){ 1061 FSM_ultrasonic_state = 2; 1062 } 1063 if((micros() 1064 - Ttrig) > 30000){ //Timeout 1065 FSM_ultrasonic_state = 0; 1066 } 1067 1068 1069 break; 1070 case 2: 1071 FSM_ultrasonic_state = 0; 1072 break; 1073 1074 } 1075} 1076 1077void FSM_ultrasonic_output(int reset_echoRead_local){ //////////////////No 1078 se est a utilizar o reset_echoRead_local///////////////////////////////////// 1079 1080 switch(FSM_ultrasonic_state){ 1081 case 0: //Sends trigger 1082 // Clears 1083 the trigPin 1084 digitalWrite(trigger_pin, LOW); 1085 delayMicroseconds(1); 1086 1087 // Sets the trigPin on HIGH state for 10 micro seconds 1088 digitalWrite(trigger_pin, 1089 HIGH); 1090 delayMicroseconds(10); 1091 digitalWrite(trigger_pin, LOW); 1092 1093 1094 Ttrig = micros(); 1095 break; 1096 case 1: 1097 reset_echoRead = 1; 1098 ///////////////////////////////////////Porqu aqui?????//////////////////////////////////////////////// 1099 1100 break; 1101 case 2: //Receives echo 1102 if(echoRead){ 1103 if(!duration){ 1104 //If duration is zero, avoids negative distance 1105 distance = 0; 1106 }else{ 1107 1108 distance = duration*0.034/2 - 7.28; 1109 } 1110 reset_echoRead 1111 = 1; 1112 } 1113 break; 1114 1115 1116 } 1117} 1118//END Ultrasonic FSM 1119 without pulseIn Function 1120
Downloadable files
Schematic
Schematic
Comments
Only logged in users can leave comments
2 Team members on this project
2
0