Elianne Hopman / Mbed 2 deprecated robot_states

fsm

Dependencies:   mbed QEI HIDScope MODSERIAL FXOS8700Q

Opzet_Eli.cpp@1:268bf7dbb15c, 2019-10-07 (annotated)

Committer:
ehopman
Date:
Mon Oct 07 08:26:51 2019 +0000
Revision:
1:268bf7dbb15c
Parent:
0:50f25a675c72
Child:
2:5730195cf595
07okt 1026;

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