Mart Wijntjes / Mbed 2 deprecated robot_states

sdf

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

Opzet_Eli.cpp@0:50f25a675c72, 2019-10-07 (annotated)

Committer:
ehopman
Date:
Mon Oct 07 07:51:23 2019 +0000
Revision:
0:50f25a675c72
Child:
1:268bf7dbb15c
7okt 0951

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ehopman 0:50f25a675c72 1 // Robot states
ehopman 0:50f25a675c72 2 #include "mbed.h"
ehopman 0:50f25a675c72 3 #include "MODSERIAL.h"
ehopman 0:50f25a675c72 4
ehopman 0:50f25a675c72 5 // Define objects
ehopman 0:50f25a675c72 6 DigitalOut led1(LED_RED);
ehopman 0:50f25a675c72 7 DigitalOut led2(LED_BLUE);
ehopman 0:50f25a675c72 8 DigitalOut led3(LED_GREEN);
ehopman 0:50f25a675c72 9 DigitalIn ptc1(PTC6); //Button 1
ehopman 0:50f25a675c72 10 DigitalIn ptc2(PTA4); //Button 2
ehopman 0:50f25a675c72 11
ehopman 0:50f25a675c72 12 //v_motor, t_passed, EMG zijn allemaal inputs die we nog even moeten fiksen ik weet niet hoe
ehopman 0:50f25a675c72 13 // Functions
ehopman 0:50f25a675c72 14 enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
ehopman 0:50f25a675c72 15
ehopman 0:50f25a675c72 16 states CurrentState(START);
ehopman 0:50f25a675c72 17 bool StateChanged true; // this is the initialization of the first state
ehopman 0:50f25a675c72 18
ehopman 0:50f25a675c72 19 void StateMachine(void)
ehopman 0:50f25a675c72 20 {
ehopman 0:50f25a675c72 21 switch(CurrentState)
ehopman 0:50f25a675c72 22 {
ehopman 0:50f25a675c72 23 case START:
ehopman 0:50f25a675c72 24 while(true) //RED LED ON
ehopman 0:50f25a675c72 25 {
ehopman 0:50f25a675c72 26 led2 = 1;
ehopman 0:50f25a675c72 27 led3 = 1;
ehopman 0:50f25a675c72 28 led1 = !led1;
ehopman 0:50f25a675c72 29 }
ehopman 0:50f25a675c72 30
ehopman 0:50f25a675c72 31 if (ptc1 == 0) // State switches when button pressed
ehopman 0:50f25a675c72 32 {
ehopman 0:50f25a675c72 33 CurrentState = KAL_ME;
ehopman 0:50f25a675c72 34 StateChanged = true;
ehopman 0:50f25a675c72 35 }
ehopman 0:50f25a675c72 36 break; // end of state START
ehopman 0:50f25a675c72 37
ehopman 0:50f25a675c72 38 case KAL_ME:
ehopman 0:50f25a675c72 39 while(true)
ehopman 0:50f25a675c72 40 {
ehopman 0:50f25a675c72 41 led2 = 1;
ehopman 0:50f25a675c72 42 led3 = 1;
ehopman 0:50f25a675c72 43 led1 = !led1;
ehopman 0:50f25a675c72 44 wait(0.7);
ehopman 0:50f25a675c72 45 }
ehopman 0:50f25a675c72 46 if (v_motor == 0 && t_passed = 2) //sec
ehopman 0:50f25a675c72 47 {
ehopman 0:50f25a675c72 48 CurrentState = KAL_EMG;
ehopman 0:50f25a675c72 49 StateChanged = true;
ehopman 0:50f25a675c72 50 }
ehopman 0:50f25a675c72 51 break; // end of state KAL_ME
ehopman 0:50f25a675c72 52
ehopman 0:50f25a675c72 53 case KAL_EMG:
ehopman 0:50f25a675c72 54 while(true)
ehopman 0:50f25a675c72 55 {
ehopman 0:50f25a675c72 56 led2 = 1;
ehopman 0:50f25a675c72 57 led3 = 1;
ehopman 0:50f25a675c72 58 led1 = !led1;
ehopman 0:50f25a675c72 59 wait(0.2);
ehopman 0:50f25a675c72 60 }
ehopman 0:50f25a675c72 61
ehopman 0:50f25a675c72 62 while(t_passed < 5) //sec
ehopman 0:50f25a675c72 63 {
ehopman 0:50f25a675c72 64 //EMG_max = max EMG measured in these 5 sec
ehopman 0:50f25a675c72 65 }
ehopman 0:50f25a675c72 66
ehopman 0:50f25a675c72 67 if (EMG <= 0.1*EMG_max && t_passed == 2) //sec
ehopman 0:50f25a675c72 68 {
ehopman 0:50f25a675c72 69 CurrentState = MOVE_START;
ehopman 0:50f25a675c72 70 StateChanged = true;
ehopman 0:50f25a675c72 71 }
ehopman 0:50f25a675c72 72 break; // end of state KAL_EMG
ehopman 0:50f25a675c72 73
ehopman 0:50f25a675c72 74 case MOVE_START:
ehopman 0:50f25a675c72 75 while(true)
ehopman 0:50f25a675c72 76 {
ehopman 0:50f25a675c72 77 led1 = 1;
ehopman 0:50f25a675c72 78 led2 = 1;
ehopman 0:50f25a675c72 79 led3 = !led3;
ehopman 0:50f25a675c72 80 wait(0.2);
ehopman 0:50f25a675c72 81 }
ehopman 0:50f25a675c72 82
ehopman 0:50f25a675c72 83 // define start position
ehopman 0:50f25a675c72 84
ehopman 0:50f25a675c72 85 if (t_passed == 2 && //position realised and tmie passed
ehopman 0:50f25a675c72 86 {
ehopman 0:50f25a675c72 87 // Stop moving
ehopman 0:50f25a675c72 88 CurrentState = READY_START;
ehopman 0:50f25a675c72 89 StateChanged = true;
ehopman 0:50f25a675c72 90 }
ehopman 0:50f25a675c72 91 break; // end of state MOVE_START
ehopman 0:50f25a675c72 92
ehopman 0:50f25a675c72 93 case READY_START:
ehopman 0:50f25a675c72 94 while(true)
ehopman 0:50f25a675c72 95 {
ehopman 0:50f25a675c72 96 led1 = 1;
ehopman 0:50f25a675c72 97 led2 = 1;
ehopman 0:50f25a675c72 98 led3 = 0;
ehopman 0:50f25a675c72 99 }
ehopman 0:50f25a675c72 100
ehopman 0:50f25a675c72 101 if (ptc1 == 0)
ehopman 0:50f25a675c72 102 {
ehopman 0:50f25a675c72 103 CurrentState = DEMO;
ehopman 0:50f25a675c72 104 StateChanged = true;
ehopman 0:50f25a675c72 105 }
ehopman 0:50f25a675c72 106 else if (ptc2 == 0) // or EMG signal >= 20% max
ehopman 0:50f25a675c72 107 {
ehopman 0:50f25a675c72 108 CurrentState = MOVE;
ehopman 0:50f25a675c72 109 StateChanged = true;
ehopman 0:50f25a675c72 110 }
ehopman 0:50f25a675c72 111 break; // end of state READY_START
ehopman 0:50f25a675c72 112
ehopman 0:50f25a675c72 113 case DEMO:
ehopman 0:50f25a675c72 114 //perform straight movements in some way, dunno how to make that here yet
ehopman 0:50f25a675c72 115 if //position = end and time
ehopman 0:50f25a675c72 116 {
ehopman 0:50f25a675c72 117 CurrentState = MOVE_START;
ehopman 0:50f25a675c72 118 StateChanged = true;
ehopman 0:50f25a675c72 119 }
ehopman 0:50f25a675c72 120 break; // end of state DEMO
ehopman 0:50f25a675c72 121
ehopman 0:50f25a675c72 122 case MOVE:
ehopman 0:50f25a675c72 123 if // no movement for 2 sec after input
ehopman 0:50f25a675c72 124 {
ehopman 0:50f25a675c72 125 CurrentState = WAIT;
ehopman 0:50f25a675c72 126 StateChanged = true;
ehopman 0:50f25a675c72 127 }
ehopman 0:50f25a675c72 128 else if // input signal
ehopman 0:50f25a675c72 129 {
ehopman 0:50f25a675c72 130 // move in direction linked to EMG
ehopman 0:50f25a675c72 131 }
ehopman 0:50f25a675c72 132 // Iets met dat oranje lichtje maar snap die stap niet helemaal
ehopman 0:50f25a675c72 133 break; // end of state MOVE
ehopman 0:50f25a675c72 134
ehopman 0:50f25a675c72 135 case WAIT:
ehopman 0:50f25a675c72 136 if (ptc1 == 0)
ehopman 0:50f25a675c72 137 {
ehopman 0:50f25a675c72 138 CurrentState = READY_START;
ehopman 0:50f25a675c72 139 StateChanged = true;
ehopman 0:50f25a675c72 140 )
ehopman 0:50f25a675c72 141 else if (ptc2 == 0) // or EMG>= 20%max
ehopman 0:50f25a675c72 142 {
ehopman 0:50f25a675c72 143 CurrentState = MOVE;
ehopman 0:50f25a675c72 144 StateChanged = true;
ehopman 0:50f25a675c72 145 }
ehopman 0:50f25a675c72 146 else if (ptc1 == 0) //maar dan heel lang achter elkaar
ehopman 0:50f25a675c72 147 break; // end of state WAIT
ehopman 0:50f25a675c72 148
ehopman 0:50f25a675c72 149 case OFF:
ehopman 0:50f25a675c72 150 // turns all of
ehopman 0:50f25a675c72 151 break; // end of state OFF
ehopman 0:50f25a675c72 152
ehopman 0:50f25a675c72 153 default:
ehopman 0:50f25a675c72 154 TurnMotorsoff();
ehopman 0:50f25a675c72 155 printf("Unknown state reached");
ehopman 0:50f25a675c72 156 } // End of the switch, all states are prescribed
ehopman 0:50f25a675c72 157 }
ehopman 0:50f25a675c72 158
ehopman 0:50f25a675c72 159 int main(void) // wat hier in moet snap ik nog niet
ehopman 0:50f25a675c72 160 {
ehopman 0:50f25a675c72 161 // hier moeten dingen komen
ehopman 0:50f25a675c72 162 while (true)
ehopman 0:50f25a675c72 163 {
ehopman 0:50f25a675c72 164 CheckForCommandFromTerminal();
ehopman 0:50f25a675c72 165 StateMachine();
ehopman 0:50f25a675c72 166 }
ehopman 0:50f25a675c72 167 }
ehopman 0:50f25a675c72 168
ehopman 0:50f25a675c72 169
ehopman 0:50f25a675c72 170
ehopman 0:50f25a675c72 171