Mart Wijntjes / Mbed 2 deprecated robot_states

sdf

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

Opzet_Eli.cpp@2:5730195cf595, 2019-10-14 (annotated)

Committer:
ehopman
Date:
Mon Oct 14 12:14:18 2019 +0000
Revision:
2:5730195cf595
Parent:
1:268bf7dbb15c
Child:
3:0c31a4a5d1fe
dok

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 2:5730195cf595 4 #include "QEI.h"
ehopman 0:50f25a675c72 5 #include "MODSERIAL.h"
ehopman 0:50f25a675c72 6
ehopman 0:50f25a675c72 7 // Define objects
ehopman 2:5730195cf595 8 MODSERIAL pc(USBTX,USBRX);
ehopman 2:5730195cf595 9 DigitalOut led_red(LED_RED);
ehopman 2:5730195cf595 10 DigitalOut led_blue(LED_BLUE);
ehopman 2:5730195cf595 11 DigitalOut led_green(LED_GREEN);
ehopman 2:5730195cf595 12 InterruptIn button_Mbed(PTC6); //Button 1 Mbed
ehopman 2:5730195cf595 13 InterruptIn button_1(PTB10); //Button 2 BRS
ehopman 2:5730195cf595 14 InterruptIn button_2(PTB11); // Button 3 BRS
ehopman 0:50f25a675c72 15
ehopman 0:50f25a675c72 16 // Functions
ehopman 0:50f25a675c72 17 enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
ehopman 0:50f25a675c72 18
ehopman 1:268bf7dbb15c 19 states CurrentState = START;
ehopman 1:268bf7dbb15c 20 bool StateChanged = true; // this is the initialization of the first state
ehopman 0:50f25a675c72 21
ehopman 2:5730195cf595 22 // Function START_TO_KAL_ME
ehopman 2:5730195cf595 23
ehopman 0:50f25a675c72 24 void StateMachine(void)
ehopman 0:50f25a675c72 25 {
ehopman 0:50f25a675c72 26 switch(CurrentState)
ehopman 0:50f25a675c72 27 {
ehopman 1:268bf7dbb15c 28 case START:
ehopman 1:268bf7dbb15c 29 if (StateChanged)
ehopman 0:50f25a675c72 30 {
ehopman 1:268bf7dbb15c 31 pc.printf("Start state, red led is on. If button 1 pressed, go to kal_me state");
ehopman 2:5730195cf595 32 led_red = 0; // Red led is on
ehopman 2:5730195cf595 33 led_blue = 1;
ehopman 2:5730195cf595 34 led_green = 1;
ehopman 1:268bf7dbb15c 35 StateChanged = false;
ehopman 0:50f25a675c72 36 }
ehopman 2:5730195cf595 37 if (button_Mbed.mode (PullDown)== false; ) // State switches when button pressed
ehopman 0:50f25a675c72 38 {
ehopman 0:50f25a675c72 39 CurrentState = KAL_ME;
ehopman 0:50f25a675c72 40 StateChanged = true;
ehopman 2:5730195cf595 41 wait(0.2f);
ehopman 2:5730195cf595 42
ehopman 0:50f25a675c72 43 }
ehopman 0:50f25a675c72 44 break; // end of state START
ehopman 0:50f25a675c72 45
ehopman 0:50f25a675c72 46 case KAL_ME:
ehopman 1:268bf7dbb15c 47 if (StateChanged)
ehopman 0:50f25a675c72 48 {
ehopman 2:5730195cf595 49 pc.printf("Calibration ME state, red ld flickers slow");
ehopman 2:5730195cf595 50 //FUNCTION Red led flickers slow
ehopman 1:268bf7dbb15c 51
ehopman 2:5730195cf595 52
ehopman 2:5730195cf595 53 // FUNCTION Move to mechanical stop, include v_motor, t_passed
ehopman 1:268bf7dbb15c 54 // FUNCTION Reset encoders
ehopman 1:268bf7dbb15c 55
ehopman 1:268bf7dbb15c 56 StateChanged = false;
ehopman 0:50f25a675c72 57 }
ehopman 1:268bf7dbb15c 58
ehopman 2:5730195cf595 59 if (v_motor == 0 && t_passed > 2) // FUNCTION t_passed, included in v_motor?
ehopman 0:50f25a675c72 60 {
ehopman 0:50f25a675c72 61 CurrentState = KAL_EMG;
ehopman 0:50f25a675c72 62 StateChanged = true;
ehopman 0:50f25a675c72 63 }
ehopman 0:50f25a675c72 64 break; // end of state KAL_ME
ehopman 0:50f25a675c72 65
ehopman 0:50f25a675c72 66 case KAL_EMG:
ehopman 1:268bf7dbb15c 67 if (StateChanged)
ehopman 0:50f25a675c72 68 {
ehopman 2:5730195cf595 69 // FUCNTION Red led flickers fast
ehopman 2:5730195cf595 70
ehopman 2:5730195cf595 71 //FUNCTION Measure EMG_max, EMG variable meten, t_passed
ehopman 1:268bf7dbb15c 72
ehopman 1:268bf7dbb15c 73 StateChanged = false;
ehopman 0:50f25a675c72 74 }
ehopman 0:50f25a675c72 75
ehopman 1:268bf7dbb15c 76 if (EMG < 0.1*EMG_max && t_passed > 2)
ehopman 0:50f25a675c72 77 {
ehopman 0:50f25a675c72 78 CurrentState = MOVE_START;
ehopman 0:50f25a675c72 79 StateChanged = true;
ehopman 0:50f25a675c72 80 }
ehopman 0:50f25a675c72 81 break; // end of state KAL_EMG
ehopman 0:50f25a675c72 82
ehopman 0:50f25a675c72 83 case MOVE_START:
ehopman 1:268bf7dbb15c 84 if(StateChanged)
ehopman 0:50f25a675c72 85 {
ehopman 2:5730195cf595 86 led_red = 1;
ehopman 2:5730195cf595 87 led_blue = 1;
ehopman 2:5730195cf595 88 led_green = 0; // Green led is on
ehopman 0:50f25a675c72 89
ehopman 2:5730195cf595 90 // FUNCTION move to start, t_passed
ehopman 2:5730195cf595 91 // Define current_position & start_position
ehopman 2:5730195cf595 92 StateChanged = false;
ehopman 2:5730195cf595 93 }
ehopman 0:50f25a675c72 94
ehopman 2:5730195cf595 95 if (current_position == start_position && t_passed > 2) // FUNCTIO t_passed
ehopman 0:50f25a675c72 96 {
ehopman 0:50f25a675c72 97 CurrentState = READY_START;
ehopman 0:50f25a675c72 98 StateChanged = true;
ehopman 0:50f25a675c72 99 }
ehopman 0:50f25a675c72 100 break; // end of state MOVE_START
ehopman 0:50f25a675c72 101
ehopman 0:50f25a675c72 102 case READY_START:
ehopman 2:5730195cf595 103 if (StateChanged)
ehopman 0:50f25a675c72 104 {
ehopman 2:5730195cf595 105 led_red = 1;
ehopman 2:5730195cf595 106 led_blue = 1;
ehopman 2:5730195cf595 107 led_green = 0; // Green led is on
ehopman 2:5730195cf595 108
ehopman 2:5730195cf595 109 StateChanged = false;
ehopman 0:50f25a675c72 110 }
ehopman 0:50f25a675c72 111
ehopman 2:5730195cf595 112 if (button_1.read() == false) // Button 1
ehopman 0:50f25a675c72 113 {
ehopman 0:50f25a675c72 114 CurrentState = DEMO;
ehopman 0:50f25a675c72 115 StateChanged = true;
ehopman 2:5730195cf595 116 wait(0.2f);
ehopman 0:50f25a675c72 117 }
ehopman 2:5730195cf595 118 else if (button_2.read() == false || EMG > 0.2*EMG_max) // Button 2 or 20% EMG_max
ehopman 0:50f25a675c72 119 {
ehopman 0:50f25a675c72 120 CurrentState = MOVE;
ehopman 0:50f25a675c72 121 StateChanged = true;
ehopman 2:5730195cf595 122 wait(0.2f);
ehopman 0:50f25a675c72 123 }
ehopman 0:50f25a675c72 124 break; // end of state READY_START
ehopman 0:50f25a675c72 125
ehopman 0:50f25a675c72 126 case DEMO:
ehopman 2:5730195cf595 127 if (StateChanged)
ehopman 2:5730195cf595 128 {
ehopman 2:5730195cf595 129 //FUNCTION Blue led blink fast
ehopman 2:5730195cf595 130
ehopman 2:5730195cf595 131 //FUNCTION perform straight movements for demo
ehopman 2:5730195cf595 132
ehopman 2:5730195cf595 133 StateChanged = false;
ehopman 2:5730195cf595 134 }
ehopman 2:5730195cf595 135
ehopman 2:5730195cf595 136 if (current_position == end_position && t_passed > 2)
ehopman 0:50f25a675c72 137 {
ehopman 0:50f25a675c72 138 CurrentState = MOVE_START;
ehopman 0:50f25a675c72 139 StateChanged = true;
ehopman 0:50f25a675c72 140 }
ehopman 0:50f25a675c72 141 break; // end of state DEMO
ehopman 0:50f25a675c72 142
ehopman 0:50f25a675c72 143 case MOVE:
ehopman 2:5730195cf595 144 if (StateChanged)
ehopman 2:5730195cf595 145 {
ehopman 2:5730195cf595 146 led_red = 1;
ehopman 2:5730195cf595 147 led_green = 1;
ehopman 2:5730195cf595 148 led_blue = 0; //Blue led is on
ehopman 2:5730195cf595 149
ehopman 2:5730195cf595 150 // FUNCTION Play the game with EMG signal
ehopman 2:5730195cf595 151
ehopman 2:5730195cf595 152 StateChanged = false;
ehopman 2:5730195cf595 153 }
ehopman 2:5730195cf595 154
ehopman 2:5730195cf595 155 if (button_Mbed.read() == false)
ehopman 2:5730195cf595 156 {
ehopman 2:5730195cf595 157 CurrentState = OFF;
ehopman 2:5730195cf595 158 StateChanged = true;
ehopman 2:5730195cf595 159 wait(0.2f);
ehopman 2:5730195cf595 160 }
ehopman 2:5730195cf595 161
ehopman 2:5730195cf595 162 else if (button_1.read() == false || EMG < 0.2*EMG_max)
ehopman 0:50f25a675c72 163 {
ehopman 0:50f25a675c72 164 CurrentState = WAIT;
ehopman 0:50f25a675c72 165 StateChanged = true;
ehopman 2:5730195cf595 166 wait(0.2f);
ehopman 0:50f25a675c72 167 }
ehopman 2:5730195cf595 168 break; // end of state MOVE
ehopman 0:50f25a675c72 169
ehopman 0:50f25a675c72 170 case WAIT:
ehopman 2:5730195cf595 171 if (StateChanged)
ehopman 2:5730195cf595 172 {
ehopman 2:5730195cf595 173 led_red = 0; // Pink led iS on
ehopman 2:5730195cf595 174 led_blue = 0;
ehopman 2:5730195cf595 175 led_green = 1
ehopman 2:5730195cf595 176
ehopman 2:5730195cf595 177 if (button_Mbed.read() == false) // For five second, too hard
ehopman 0:50f25a675c72 178 {
ehopman 2:5730195cf595 179 CurrentState = OFF;
ehopman 0:50f25a675c72 180 StateChanged = true;
ehopman 2:5730195cf595 181 wait(0.2f);
ehopman 0:50f25a675c72 182 )
ehopman 2:5730195cf595 183 else if (button_1.read() == false)
ehopman 2:5730195cf595 184 {
ehopman 2:5730195cf595 185 CurrentState = MOVE_START;
ehopman 2:5730195cf595 186 StateChanged = true;
ehopman 2:5730195cf595 187 wait(0.2f);
ehopman 2:5730195cf595 188 }
ehopman 2:5730195cf595 189 else if (button_2.read() == false || EMG > 0.2*EMG_max)
ehopman 0:50f25a675c72 190 {
ehopman 0:50f25a675c72 191 CurrentState = MOVE;
ehopman 0:50f25a675c72 192 StateChanged = true;
ehopman 2:5730195cf595 193 wait(0.2f);
ehopman 0:50f25a675c72 194 }
ehopman 2:5730195cf595 195
ehopman 0:50f25a675c72 196 break; // end of state WAIT
ehopman 0:50f25a675c72 197
ehopman 0:50f25a675c72 198 case OFF:
ehopman 2:5730195cf595 199 led_red = 0; // White led is on
ehopman 2:5730195cf595 200 led_blue = 0;
ehopman 2:5730195cf595 201 led_green = 0;
ehopman 2:5730195cf595 202
ehopman 0:50f25a675c72 203 break; // end of state OFF
ehopman 0:50f25a675c72 204
ehopman 0:50f25a675c72 205 default:
ehopman 2:5730195cf595 206 TurnMotorsoff(); //FUNCTION
ehopman 0:50f25a675c72 207 printf("Unknown state reached");
ehopman 0:50f25a675c72 208 } // End of the switch, all states are prescribed
ehopman 0:50f25a675c72 209 }
ehopman 0:50f25a675c72 210
ehopman 0:50f25a675c72 211 int main(void) // wat hier in moet snap ik nog niet
ehopman 0:50f25a675c72 212 {
ehopman 0:50f25a675c72 213 // hier moeten dingen komen
ehopman 0:50f25a675c72 214 while (true)
ehopman 0:50f25a675c72 215 {
ehopman 0:50f25a675c72 216 CheckForCommandFromTerminal();
ehopman 0:50f25a675c72 217 StateMachine();
ehopman 0:50f25a675c72 218 }
ehopman 0:50f25a675c72 219 }
ehopman 0:50f25a675c72 220
ehopman 0:50f25a675c72 221
ehopman 0:50f25a675c72 222
ehopman 0:50f25a675c72 223