Fa2018-es200-3321-proj2-GoodTeam#2 / Mbed OS peanut_butter_state_machine

DE refactored as state machine

Dependencies:   Motor Servo

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?

UserRevisionLine numberNew 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