Fa2018-es200-3321-proj2-GoodTeam#2
/
peanut_butter_state_machine
DE refactored as state machine
main.cpp@0:d531bdb9c2c7, 2018-10-22 (annotated)
- Committer:
- evangeli
- Date:
- Mon Oct 22 14:34:09 2018 +0000
- Revision:
- 0:d531bdb9c2c7
DE helped refactor code as state machine
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
evangeli | 0:d531bdb9c2c7 | 1 | /* |
evangeli | 0:d531bdb9c2c7 | 2 | ES200 3321 Project 2 |
evangeli | 0:d531bdb9c2c7 | 3 | MIDN 3/C Dietrich, Schwind, Silver |
evangeli | 0:d531bdb9c2c7 | 4 | */ |
evangeli | 0:d531bdb9c2c7 | 5 | |
evangeli | 0:d531bdb9c2c7 | 6 | #include "mbed.h" |
evangeli | 0:d531bdb9c2c7 | 7 | #include "rtos.h" |
evangeli | 0:d531bdb9c2c7 | 8 | #include "stdlib.h" |
evangeli | 0:d531bdb9c2c7 | 9 | #include "Motor.h" |
evangeli | 0:d531bdb9c2c7 | 10 | #include "Servo.h" |
evangeli | 0:d531bdb9c2c7 | 11 | |
evangeli | 0:d531bdb9c2c7 | 12 | // global hardware objects |
evangeli | 0:d531bdb9c2c7 | 13 | Serial pc(USBTX,USBRX); |
evangeli | 0:d531bdb9c2c7 | 14 | |
evangeli | 0:d531bdb9c2c7 | 15 | Servo servo_1(p24);//rotates hand between palm facing up and palm facing in |
evangeli | 0:d531bdb9c2c7 | 16 | Servo servo_2(p21); //on jar to rotate it in hand |
evangeli | 0:d531bdb9c2c7 | 17 | Motor motor(p29, p30, p26); // drive arm from 90 degrees to forehead |
evangeli | 0:d531bdb9c2c7 | 18 | DigitalIn switch_1(p17); //on protoboard - hit to start |
evangeli | 0:d531bdb9c2c7 | 19 | DigitalIn switch_2(p18); //on "hand" - hits forehead to turn motor off |
evangeli | 0:d531bdb9c2c7 | 20 | DigitalIn switch_3(p12); //on protoboard - returns arm to start position |
evangeli | 0:d531bdb9c2c7 | 21 | DigitalIn switch_4(p10); //on "hand" - hits desk when going down to turn motor off |
evangeli | 0:d531bdb9c2c7 | 22 | |
evangeli | 0:d531bdb9c2c7 | 23 | |
evangeli | 0:d531bdb9c2c7 | 24 | |
evangeli | 0:d531bdb9c2c7 | 25 | int main() { |
evangeli | 0:d531bdb9c2c7 | 26 | // setup |
evangeli | 0:d531bdb9c2c7 | 27 | pc.printf("ES200 3321 Team Peanut Butter Beat Army\r\n"); |
evangeli | 0:d531bdb9c2c7 | 28 | servo_1.calibrate(0.0009, 45.0); //Calibrates the servo_1 timing by setting the pulse width and range of motion (45*2 = 90 degrees) |
evangeli | 0:d531bdb9c2c7 | 29 | servo_2.calibrate(0.0009, 90.0); //Calibrates the servo_2 timing by setting the pulse width and range of motion (45*2 = 90 degrees) |
evangeli | 0:d531bdb9c2c7 | 30 | |
evangeli | 0:d531bdb9c2c7 | 31 | // state machines start here |
evangeli | 0:d531bdb9c2c7 | 32 | while(1){ |
evangeli | 0:d531bdb9c2c7 | 33 | |
evangeli | 0:d531bdb9c2c7 | 34 | // idle state |
evangeli | 0:d531bdb9c2c7 | 35 | do { |
evangeli | 0:d531bdb9c2c7 | 36 | pc.printf("main() in idle state, awaiting sw1\r\n"); |
evangeli | 0:d531bdb9c2c7 | 37 | servo_1.write(0.25); //Sets the servo in the initial position - palm facing in |
evangeli | 0:d531bdb9c2c7 | 38 | servo_2.write(0.0); //Sets the servo in the initial position - Does not matter where |
evangeli | 0:d531bdb9c2c7 | 39 | ThisThread::sleep_for(200); |
evangeli | 0:d531bdb9c2c7 | 40 | } while (!switch_1.read()); |
evangeli | 0:d531bdb9c2c7 | 41 | |
evangeli | 0:d531bdb9c2c7 | 42 | // peanut butter rotation |
evangeli | 0:d531bdb9c2c7 | 43 | pc.printf("main() in peanut butter rotation state LATER\r\n"); |
evangeli | 0:d531bdb9c2c7 | 44 | |
evangeli | 0:d531bdb9c2c7 | 45 | |
evangeli | 0:d531bdb9c2c7 | 46 | // palm up state |
evangeli | 0:d531bdb9c2c7 | 47 | pc.printf("main() going to palm up state\r\n"); |
evangeli | 0:d531bdb9c2c7 | 48 | servo_1.write(.75); //turn the hand to the "palm up" position |
evangeli | 0:d531bdb9c2c7 | 49 | ThisThread::sleep_for(2000); //wait for hand to get into "palm up" position |
evangeli | 0:d531bdb9c2c7 | 50 | |
evangeli | 0:d531bdb9c2c7 | 51 | // smash state |
evangeli | 0:d531bdb9c2c7 | 52 | pc.printf("main() smash PB against head state\r\n"); |
evangeli | 0:d531bdb9c2c7 | 53 | motor.speed(0.7); //turn motor on to drive arm up |
evangeli | 0:d531bdb9c2c7 | 54 | while (!switch_2.read()){ |
evangeli | 0:d531bdb9c2c7 | 55 | pc.printf("main() awaiting palm switch 2 to finish PB head smash\r\n"); |
evangeli | 0:d531bdb9c2c7 | 56 | ThisThread::sleep_for(100); |
evangeli | 0:d531bdb9c2c7 | 57 | } |
evangeli | 0:d531bdb9c2c7 | 58 | motor.speed(0.0); |
evangeli | 0:d531bdb9c2c7 | 59 | |
evangeli | 0:d531bdb9c2c7 | 60 | // play sound |
evangeli | 0:d531bdb9c2c7 | 61 | pc.printf("main() in play sound state LATER\r\n"); |
evangeli | 0:d531bdb9c2c7 | 62 | while (!switch_3.read()){ |
evangeli | 0:d531bdb9c2c7 | 63 | pc.printf("main() awaiting switch_3 to recycle\r\n"); |
evangeli | 0:d531bdb9c2c7 | 64 | ThisThread::sleep_for(200); |
evangeli | 0:d531bdb9c2c7 | 65 | }; |
evangeli | 0:d531bdb9c2c7 | 66 | |
evangeli | 0:d531bdb9c2c7 | 67 | // lower arm |
evangeli | 0:d531bdb9c2c7 | 68 | pc.printf("main() lowering arm\r\n"); |
evangeli | 0:d531bdb9c2c7 | 69 | motor.speed(-0.5); //turns the motor on to return the arm to the original position |
evangeli | 0:d531bdb9c2c7 | 70 | while (!switch_4.read()) { |
evangeli | 0:d531bdb9c2c7 | 71 | pc.printf("main() awaiting switch_4 during recycle\r\n"); |
evangeli | 0:d531bdb9c2c7 | 72 | ThisThread::sleep_for(200); |
evangeli | 0:d531bdb9c2c7 | 73 | } |
evangeli | 0:d531bdb9c2c7 | 74 | motor.speed(0.0); // motor will turn off |
evangeli | 0:d531bdb9c2c7 | 75 | ThisThread::sleep_for(1000); //wait 1 second for arm to level out |
evangeli | 0:d531bdb9c2c7 | 76 | |
evangeli | 0:d531bdb9c2c7 | 77 | // palm to side |
evangeli | 0:d531bdb9c2c7 | 78 | pc.printf("main() palm to side position\r\n"); |
evangeli | 0:d531bdb9c2c7 | 79 | servo_1.write(0.25); //Servo 1 will turn on returning the hand to the original "palm in" position |
evangeli | 0:d531bdb9c2c7 | 80 | |
evangeli | 0:d531bdb9c2c7 | 81 | pc.printf("main() returning to idle state\r\n"); |
evangeli | 0:d531bdb9c2c7 | 82 | } // while(1) |
evangeli | 0:d531bdb9c2c7 | 83 | } // main() |
evangeli | 0:d531bdb9c2c7 | 84 | |
evangeli | 0:d531bdb9c2c7 | 85 | |
evangeli | 0:d531bdb9c2c7 | 86 | |
evangeli | 0:d531bdb9c2c7 | 87 | |
evangeli | 0:d531bdb9c2c7 | 88 | |
evangeli | 0:d531bdb9c2c7 | 89 | |
evangeli | 0:d531bdb9c2c7 | 90 | |
evangeli | 0:d531bdb9c2c7 | 91 | |
evangeli | 0:d531bdb9c2c7 | 92 | |
evangeli | 0:d531bdb9c2c7 | 93 | |
evangeli | 0:d531bdb9c2c7 | 94 | |
evangeli | 0:d531bdb9c2c7 | 95 | |
evangeli | 0:d531bdb9c2c7 | 96 | |
evangeli | 0:d531bdb9c2c7 | 97 | |
evangeli | 0:d531bdb9c2c7 | 98 | |
evangeli | 0:d531bdb9c2c7 | 99 | if (switch_1.read() == 1) //User hits switch 1 (button on protoboard) |
evangeli | 0:d531bdb9c2c7 | 100 | { |
evangeli | 0:d531bdb9c2c7 | 101 | |
evangeli | 0:d531bdb9c2c7 | 102 | |
evangeli | 0:d531bdb9c2c7 | 103 | |
evangeli | 0:d531bdb9c2c7 | 104 | /* |
evangeli | 0:d531bdb9c2c7 | 105 | {motor.speed(0); //turn motor off |
evangeli | 0:d531bdb9c2c7 | 106 | int a = rand()%1;} //Generate random integer 0 or 1 to determine if the jar will open or not - what sound will play and if jar opens |
evangeli | 0:d531bdb9c2c7 | 107 | |
evangeli | 0:d531bdb9c2c7 | 108 | if (a == 1) { |
evangeli | 0:d531bdb9c2c7 | 109 | servo_1.write(...); //Servo on wrist turns to allow pb to come out |
evangeli | 0:d531bdb9c2c7 | 110 | pc.printf("1\n"); // send signal to Matlab to play Beat Army sound |
evangeli | 0:d531bdb9c2c7 | 111 | wait(10.0); // wait for Matlab to finish playing the sound NATHAN'S CODE??? |
evangeli | 0:d531bdb9c2c7 | 112 | |
evangeli | 0:d531bdb9c2c7 | 113 | else if (a == 0) |
evangeli | 0:d531bdb9c2c7 | 114 | { |
evangeli | 0:d531bdb9c2c7 | 115 | pc.printf("2\n"); // send signal to Matlab to play boo sound |
evangeli | 0:d531bdb9c2c7 | 116 | wait(10.0); // wait for Matlab to finish playing the sound NATHAN'S CODE??? |
evangeli | 0:d531bdb9c2c7 | 117 | } |
evangeli | 0:d531bdb9c2c7 | 118 | */ |
evangeli | 0:d531bdb9c2c7 | 119 |