Mart Wijntjes / Mbed 2 deprecated robot_states

sdf

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

Opzet_Eli.cpp@3:0c31a4a5d1fe, 2019-10-18 (annotated)

Committer:
Mortimerz
Date:
Fri Oct 18 12:06:15 2019 +0000
Revision:
3:0c31a4a5d1fe
Parent:
2:5730195cf595 
meerdere functies in state machine verwerkt;

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"
Mortimerz 3:0c31a4a5d1fe 6 #include "biquadFilter.h"
Mortimerz 3:0c31a4a5d1fe 7 #include "math.h"
Mortimerz 3:0c31a4a5d1fe 8 #include "FastPWM.h"
ehopman 0:50f25a675c72 9
ehopman 0:50f25a675c72 10 // Define objects
Mortimerz 3:0c31a4a5d1fe 11 AnalogIn emg1(A0);
Mortimerz 3:0c31a4a5d1fe 12 AnalogIn emg2(A1);
Mortimerz 3:0c31a4a5d1fe 13 AnalogIn emg3(A2);
Mortimerz 3:0c31a4a5d1fe 14 AnalogIn emg4(A3);
ehopman 2:5730195cf595 15 MODSERIAL pc(USBTX,USBRX);
ehopman 2:5730195cf595 16 DigitalOut led_red(LED_RED);
ehopman 2:5730195cf595 17 DigitalOut led_blue(LED_BLUE);
ehopman 2:5730195cf595 18 DigitalOut led_green(LED_GREEN);
ehopman 2:5730195cf595 19 InterruptIn button_Mbed(PTC6); //Button 1 Mbed
Mortimerz 3:0c31a4a5d1fe 20 InterruptIn button_1(); //Button 2 BRS
Mortimerz 3:0c31a4a5d1fe 21 InterruptIn button_2(); // Button 3 BRS
Mortimerz 3:0c31a4a5d1fe 22
Mortimerz 3:0c31a4a5d1fe 23 FastPWM motor1(D6);
Mortimerz 3:0c31a4a5d1fe 24 DigitalOut motor1_dir(D7);
Mortimerz 3:0c31a4a5d1fe 25 FastPWM motor2(D5);
Mortimerz 3:0c31a4a5d1fe 26 DigitalOut motor2_dir(D4);
Mortimerz 3:0c31a4a5d1fe 27
Mortimerz 3:0c31a4a5d1fe 28
Mortimerz 3:0c31a4a5d1fe 29 //ticker setup
Mortimerz 3:0c31a4a5d1fe 30 Ticker states_machine;
Mortimerz 3:0c31a4a5d1fe 31
Mortimerz 3:0c31a4a5d1fe 32
Mortimerz 3:0c31a4a5d1fe 33 //encoder setup
Mortimerz 3:0c31a4a5d1fe 34 QEI encoder_1(D13,D12,NC,32,QEI::X4_ENCODING);
Mortimerz 3:0c31a4a5d1fe 35 QEI encoder_2(D11,D10,NC,32,QEI::X4_ENCODING);
Mortimerz 3:0c31a4a5d1fe 36
Mortimerz 3:0c31a4a5d1fe 37 //motor setup
Mortimerz 3:0c31a4a5d1fe 38 int motordir1 = 1;
Mortimerz 3:0c31a4a5d1fe 39 int motordir2 = 1;
Mortimerz 3:0c31a4a5d1fe 40 double Kp = 17.5;
Mortimerz 3:0c31a4a5d1fe 41
Mortimerz 3:0c31a4a5d1fe 42 //Hidscope setup
Mortimerz 3:0c31a4a5d1fe 43 HIDScope scope(4);
Mortimerz 3:0c31a4a5d1fe 44
Mortimerz 3:0c31a4a5d1fe 45 // EMG setup
Mortimerz 3:0c31a4a5d1fe 46 biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
Mortimerz 3:0c31a4a5d1fe 47 biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
Mortimerz 3:0c31a4a5d1fe 48 biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
Mortimerz 3:0c31a4a5d1fe 49 biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
Mortimerz 3:0c31a4a5d1fe 50 double emg_value_1;
Mortimerz 3:0c31a4a5d1fe 51 double signalpart1_1;
Mortimerz 3:0c31a4a5d1fe 52 double signalpart2_1;
Mortimerz 3:0c31a4a5d1fe 53 double signalpart3_1;
Mortimerz 3:0c31a4a5d1fe 54 double signalpart4_1;
Mortimerz 3:0c31a4a5d1fe 55 double signalfinal_1;
Mortimerz 3:0c31a4a5d1fe 56 double emgsignal_1;
Mortimerz 3:0c31a4a5d1fe 57 double onoffsignal_1;
Mortimerz 3:0c31a4a5d1fe 58 double maxcal_1=0;
ehopman 0:50f25a675c72 59
Mortimerz 3:0c31a4a5d1fe 60 double emg_value_2;
Mortimerz 3:0c31a4a5d1fe 61 double signalpart1_2;
Mortimerz 3:0c31a4a5d1fe 62 double signalpart2_2;
Mortimerz 3:0c31a4a5d1fe 63 double signalpart3_2;
Mortimerz 3:0c31a4a5d1fe 64 double signalpart4_2;
Mortimerz 3:0c31a4a5d1fe 65 double signalfinal_2;
Mortimerz 3:0c31a4a5d1fe 66 double emgsignal_2;
Mortimerz 3:0c31a4a5d1fe 67 double onoffsignal_2;
Mortimerz 3:0c31a4a5d1fe 68 double maxcal_2=0;
Mortimerz 3:0c31a4a5d1fe 69
Mortimerz 3:0c31a4a5d1fe 70 double emg_value_3;
Mortimerz 3:0c31a4a5d1fe 71 double signalpart1_3;
Mortimerz 3:0c31a4a5d1fe 72 double signalpart2_3;
Mortimerz 3:0c31a4a5d1fe 73 double signalpart3_3;
Mortimerz 3:0c31a4a5d1fe 74 double signalpart4_3;
Mortimerz 3:0c31a4a5d1fe 75 double signalfinal_3;
Mortimerz 3:0c31a4a5d1fe 76 double emgsignal_3;
Mortimerz 3:0c31a4a5d1fe 77 double onoffsignal_3;
Mortimerz 3:0c31a4a5d1fe 78 double maxcal_3=0;
Mortimerz 3:0c31a4a5d1fe 79
Mortimerz 3:0c31a4a5d1fe 80 double emg_value_4;
Mortimerz 3:0c31a4a5d1fe 81 double signalpart1_4;
Mortimerz 3:0c31a4a5d1fe 82 double signalpart2_4;
Mortimerz 3:0c31a4a5d1fe 83 double signalpart3_4;
Mortimerz 3:0c31a4a5d1fe 84 double signalpart4_4;
Mortimerz 3:0c31a4a5d1fe 85 double signalfinal_4;
Mortimerz 3:0c31a4a5d1fe 86 double emgsignal_4;
Mortimerz 3:0c31a4a5d1fe 87 double onoffsignal_4;
Mortimerz 3:0c31a4a5d1fe 88 double maxcal_4=0;
Mortimerz 3:0c31a4a5d1fe 89
Mortimerz 3:0c31a4a5d1fe 90 double emgx;
Mortimerz 3:0c31a4a5d1fe 91 double emgy;
Mortimerz 3:0c31a4a5d1fe 92 double kemg = 0.1;
Mortimerz 3:0c31a4a5d1fe 93
Mortimerz 3:0c31a4a5d1fe 94 //kinematics setup
Mortimerz 3:0c31a4a5d1fe 95 double xgoal;
Mortimerz 3:0c31a4a5d1fe 96 double ygoal;
Mortimerz 3:0c31a4a5d1fe 97 double theta1;
Mortimerz 3:0c31a4a5d1fe 98 double theta2;
Mortimerz 3:0c31a4a5d1fe 99
Mortimerz 3:0c31a4a5d1fe 100
Mortimerz 3:0c31a4a5d1fe 101 // State machine
ehopman 0:50f25a675c72 102 enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
ehopman 1:268bf7dbb15c 103 states CurrentState = START;
ehopman 1:268bf7dbb15c 104 bool StateChanged = true; // this is the initialization of the first state
ehopman 0:50f25a675c72 105
Mortimerz 3:0c31a4a5d1fe 106 // button functions
Mortimerz 3:0c31a4a5d1fe 107
Mortimerz 3:0c31a4a5d1fe 108 void emgread(){
Mortimerz 3:0c31a4a5d1fe 109 emg_value_1 = emg1.read();//read the emg value from the elektrodes
Mortimerz 3:0c31a4a5d1fe 110 signalpart1_1 = notch.step(emg_value_1);//Highpass filter for removing offset and artifacts
Mortimerz 3:0c31a4a5d1fe 111 signalpart2_1 = filterhigh1.step(signalpart1_1);//rectify the filtered signal
Mortimerz 3:0c31a4a5d1fe 112 signalpart3_1 = abs(signalpart2_1);//low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 113 signalpart4_1 = filterlow1.step(signalpart3_1);//notch filter to remove 50Hz signal
Mortimerz 3:0c31a4a5d1fe 114 emgsignal_1 = filterlow2.step(signalpart4_1);//2nd low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 115 emg_value_2 = emg2.read();//read the emg value from the elektrodes
Mortimerz 3:0c31a4a5d1fe 116 signalpart1_2 = notch.step(emg_value_2);//Highpass filter for removing offset and artifacts
Mortimerz 3:0c31a4a5d1fe 117 signalpart2_2 = filterhigh1.step(signalpart1_2);//rectify the filtered signal
Mortimerz 3:0c31a4a5d1fe 118 signalpart3_2 = abs(signalpart2_2);//low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 119 signalpart4_2 = filterlow1.step(signalpart3_2);//notch filter to remove 50Hz signal
Mortimerz 3:0c31a4a5d1fe 120 emgsignal_2 = filterlow2.step(signalpart4_2);//2nd low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 121 emg_value_3 = emg3.read();//read the emg value from the elektrodes
Mortimerz 3:0c31a4a5d1fe 122 signalpart1_3 = notch.step(emg_value_3);//Highpass filter for removing offset and artifacts
Mortimerz 3:0c31a4a5d1fe 123 signalpart2_3 = filterhigh1.step(signalpart1_3);//rectify the filtered signal
Mortimerz 3:0c31a4a5d1fe 124 signalpart3_3 = abs(signalpart2_3);//low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 125 signalpart4_3 = filterlow1.step(signalpart3_3);//notch filter to remove 50Hz signal
Mortimerz 3:0c31a4a5d1fe 126 emgsignal_3 = filterlow2.step(signalpart4_3);//2nd low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 127 emg_value_4 = emg4.read();//read the emg value from the elektrodes
Mortimerz 3:0c31a4a5d1fe 128 signalpart1_4 = notch.step(emg_value_4);//Highpass filter for removing offset and artifacts
Mortimerz 3:0c31a4a5d1fe 129 signalpart2_4 = filterhigh1.step(signalpart1_4);//rectify the filtered signal
Mortimerz 3:0c31a4a5d1fe 130 signalpart3_4 = abs(signalpart2_4);//low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 131 signalpart4_4 = filterlow1.step(signalpart3_4);//notch filter to remove 50Hz signal
Mortimerz 3:0c31a4a5d1fe 132 emgsignal_4 = filterlow2.step(signalpart4_4);//2nd low pass filter to envelope the emg
Mortimerz 3:0c31a4a5d1fe 133 }
Mortimerz 3:0c31a4a5d1fe 134 void emgcal(){
Mortimerz 3:0c31a4a5d1fe 135 emgread();
Mortimerz 3:0c31a4a5d1fe 136 double signalmeasure_1 = emgsignal_1;
Mortimerz 3:0c31a4a5d1fe 137 if (signalmeasure_1 > maxcal_1){//determine what the highest reachable emg signal is
Mortimerz 3:0c31a4a5d1fe 138 maxcal_1 = signalmeasure_1;}
Mortimerz 3:0c31a4a5d1fe 139 double signalmeasure_2 = emgsignal_2;
Mortimerz 3:0c31a4a5d1fe 140 if (signalmeasure_2 > maxcal_2){//determine what the highest reachable emg signal is
Mortimerz 3:0c31a4a5d1fe 141 maxcal_2 = signalmeasure_2;}
Mortimerz 3:0c31a4a5d1fe 142 double signalmeasure_3 = emgsignal_3;
Mortimerz 3:0c31a4a5d1fe 143 if (signalmeasure_3 > maxcal_3){//determine what the highest reachable emg signal is
Mortimerz 3:0c31a4a5d1fe 144 maxcal_3 = signalmeasure_3;}
Mortimerz 3:0c31a4a5d1fe 145 double signalmeasure_4 = emgsignal_4;
Mortimerz 3:0c31a4a5d1fe 146 if (signalmeasure_4 > maxcal_4){//determine what the highest reachable emg signal is
Mortimerz 3:0c31a4a5d1fe 147 maxcal_4 = signalmeasure_4;}
Mortimerz 3:0c31a4a5d1fe 148 scope.set(0,signalmeasure_1);//set emg signal to scope in channel 1
Mortimerz 3:0c31a4a5d1fe 149 scope.set(1,signalmeasure_2);//set filtered signal to scope in channel 2
Mortimerz 3:0c31a4a5d1fe 150 scope.set(2,signalmeasure_3);//set filtered signal to scope in channel 3
Mortimerz 3:0c31a4a5d1fe 151 scope.set(3,signalmeasure_4);//set filtered signal to scope in channel 4
Mortimerz 3:0c31a4a5d1fe 152 }
Mortimerz 3:0c31a4a5d1fe 153
Mortimerz 3:0c31a4a5d1fe 154
Mortimerz 3:0c31a4a5d1fe 155 void emgshow(){
Mortimerz 3:0c31a4a5d1fe 156 emgread();
Mortimerz 3:0c31a4a5d1fe 157 onoffsignal_1=emgsignal_1/maxcal_1;// emg positive x
Mortimerz 3:0c31a4a5d1fe 158 onoffsignal_2=emgsignal_2/maxcal_2;// emg negative x
Mortimerz 3:0c31a4a5d1fe 159 onoffsignal_3=emgsignal_3/maxcal_3;// emg positive y
Mortimerz 3:0c31a4a5d1fe 160 onoffsignal_4=emgsignal_4/maxcal_4;// emg negative y
Mortimerz 3:0c31a4a5d1fe 161 scope.set(0,onoffsignal_1);//set emg signal to scope in channel 1
Mortimerz 3:0c31a4a5d1fe 162 scope.set(1,onoffsignal_2);//set filtered signal to scope in channel 2
Mortimerz 3:0c31a4a5d1fe 163 scope.set(2,onoffsignal_3);//set filtered signal to scope in channel 3
Mortimerz 3:0c31a4a5d1fe 164 scope.set(3,onoffsignal_4);//set filtered signal to scope in channel 4
Mortimerz 3:0c31a4a5d1fe 165 emgx = onoffsignal_1- onoffsignal_2;
Mortimerz 3:0c31a4a5d1fe 166 emgy = onoffsignal_3- onoffsignal_4;
Mortimerz 3:0c31a4a5d1fe 167 scope.send();
Mortimerz 3:0c31a4a5d1fe 168 }
Mortimerz 3:0c31a4a5d1fe 169
Mortimerz 3:0c31a4a5d1fe 170 void forwardkin(){
Mortimerz 3:0c31a4a5d1fe 171 double qref2 = encoder_2.getPulses();
Mortimerz 3:0c31a4a5d1fe 172 double q2_correct = (qref2*2*3.14)/8400.0;
Mortimerz 3:0c31a4a5d1fe 173 double qref1 = encoder_1.getPulses();
Mortimerz 3:0c31a4a5d1fe 174 double q1_correct = (qref1*2*3.14)/8400.0;
Mortimerz 3:0c31a4a5d1fe 175 double x = 0.3*cos(q1_correct)+0.3*cos(q1_correct+q2_correct);
Mortimerz 3:0c31a4a5d1fe 176 double y = 0.3*sin(q1_correct)+0.3*cos(q1_correct+q2_correct);
Mortimerz 3:0c31a4a5d1fe 177 emgshow();
Mortimerz 3:0c31a4a5d1fe 178 xgoal = x + emgx* kemg;
Mortimerz 3:0c31a4a5d1fe 179 ygoal = y + emgy* kemg;
Mortimerz 3:0c31a4a5d1fe 180 }
Mortimerz 3:0c31a4a5d1fe 181
Mortimerz 3:0c31a4a5d1fe 182 void reversekin(){
Mortimerz 3:0c31a4a5d1fe 183 forwardkin();
Mortimerz 3:0c31a4a5d1fe 184 theta2= acos(((xgoal*xgoal)+(ygoal*ygoal)-(0.3*0.3)-(0.3*0.3))/(2*0.3*0.3));
Mortimerz 3:0c31a4a5d1fe 185 theta1= atan(ygoal/xgoal)-atan((0.3*sin(theta2))/(0.3+0.3*cos(theta2)));
Mortimerz 3:0c31a4a5d1fe 186 }
Mortimerz 3:0c31a4a5d1fe 187
Mortimerz 3:0c31a4a5d1fe 188 void motor_position()
Mortimerz 3:0c31a4a5d1fe 189 {
Mortimerz 3:0c31a4a5d1fe 190 reversekin();
Mortimerz 3:0c31a4a5d1fe 191 double pos_1 = encoder_1.getPulses();
Mortimerz 3:0c31a4a5d1fe 192 double poscorrect_1 = (pos_1*3.14*2)/8400.0;
Mortimerz 3:0c31a4a5d1fe 193 double error1 = theta1-poscorrect_1;
Mortimerz 3:0c31a4a5d1fe 194 if (error1 >=0) motor1_dir=1;
Mortimerz 3:0c31a4a5d1fe 195 else motor1_dir=0;
Mortimerz 3:0c31a4a5d1fe 196 if (fabs(error1)>1) motor1 = 1;
Mortimerz 3:0c31a4a5d1fe 197 else motor1 = fabs(error1);
Mortimerz 3:0c31a4a5d1fe 198
Mortimerz 3:0c31a4a5d1fe 199 double pos_2 = encoder_2.getPulses();
Mortimerz 3:0c31a4a5d1fe 200 double poscorrect_2 = (pos_2*3.14*2)/8400.0;
Mortimerz 3:0c31a4a5d1fe 201 double error2 = theta2-poscorrect_2;
Mortimerz 3:0c31a4a5d1fe 202 if (error2 >=0) motor2_dir=1;
Mortimerz 3:0c31a4a5d1fe 203 else motor1_dir=0;
Mortimerz 3:0c31a4a5d1fe 204 if (fabs(error2)>1) motor2 = 1;
Mortimerz 3:0c31a4a5d1fe 205 else motor1 = fabs(error2);
Mortimerz 3:0c31a4a5d1fe 206 }
Mortimerz 3:0c31a4a5d1fe 207
Mortimerz 3:0c31a4a5d1fe 208 void kalmot(){
Mortimerz 3:0c31a4a5d1fe 209 CurrentState = KAL_ME;
Mortimerz 3:0c31a4a5d1fe 210 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 211 }
Mortimerz 3:0c31a4a5d1fe 212
Mortimerz 3:0c31a4a5d1fe 213 void kalemg(){
Mortimerz 3:0c31a4a5d1fe 214 CurrentState = KAL_EMG;
Mortimerz 3:0c31a4a5d1fe 215 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 216 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 217 }
Mortimerz 3:0c31a4a5d1fe 218 void movestart(){
Mortimerz 3:0c31a4a5d1fe 219 CurrentState = MOVE_START;
Mortimerz 3:0c31a4a5d1fe 220 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 221 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 222 }
Mortimerz 3:0c31a4a5d1fe 223
Mortimerz 3:0c31a4a5d1fe 224 void readystart(){
Mortimerz 3:0c31a4a5d1fe 225 CurrentState = READY_START;
Mortimerz 3:0c31a4a5d1fe 226 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 227 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 228 }
Mortimerz 3:0c31a4a5d1fe 229 void demo(){
Mortimerz 3:0c31a4a5d1fe 230 CurrentState = DEMO;
Mortimerz 3:0c31a4a5d1fe 231 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 232 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 233 }
Mortimerz 3:0c31a4a5d1fe 234 void move(){
Mortimerz 3:0c31a4a5d1fe 235 CurrentState = MOVE;
Mortimerz 3:0c31a4a5d1fe 236 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 237 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 238 }
Mortimerz 3:0c31a4a5d1fe 239 void wait(){
Mortimerz 3:0c31a4a5d1fe 240 CurrentState = WAIT;
Mortimerz 3:0c31a4a5d1fe 241 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 242 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 243 }
Mortimerz 3:0c31a4a5d1fe 244 void off(){
Mortimerz 3:0c31a4a5d1fe 245 CurrentState = OFF;
Mortimerz 3:0c31a4a5d1fe 246 StateChanged = true;
Mortimerz 3:0c31a4a5d1fe 247 wait(0.2f);
Mortimerz 3:0c31a4a5d1fe 248 }
Mortimerz 3:0c31a4a5d1fe 249
Mortimerz 3:0c31a4a5d1fe 250 //led functions
Mortimerz 3:0c31a4a5d1fe 251 void flashred(){
Mortimerz 3:0c31a4a5d1fe 252 led_red = !led_red;
Mortimerz 3:0c31a4a5d1fe 253 wait(0.4f);
Mortimerz 3:0c31a4a5d1fe 254 }
Mortimerz 3:0c31a4a5d1fe 255 void flashgreen(){
Mortimerz 3:0c31a4a5d1fe 256 led_green = !led_green;
Mortimerz 3:0c31a4a5d1fe 257 wait(0.4f);
Mortimerz 3:0c31a4a5d1fe 258 }
Mortimerz 3:0c31a4a5d1fe 259 void flashblue(){
Mortimerz 3:0c31a4a5d1fe 260 led_blue = !led_blue;
Mortimerz 3:0c31a4a5d1fe 261 wait(0.4f);
Mortimerz 3:0c31a4a5d1fe 262 }
ehopman 2:5730195cf595 263 // Function START_TO_KAL_ME
ehopman 2:5730195cf595 264
ehopman 0:50f25a675c72 265 void StateMachine(void)
ehopman 0:50f25a675c72 266 {
ehopman 0:50f25a675c72 267 switch(CurrentState)
ehopman 0:50f25a675c72 268 {
ehopman 1:268bf7dbb15c 269 case START:
ehopman 1:268bf7dbb15c 270 if (StateChanged)
ehopman 0:50f25a675c72 271 {
ehopman 1:268bf7dbb15c 272 pc.printf("Start state, red led is on. If button 1 pressed, go to kal_me state");
ehopman 2:5730195cf595 273 led_red = 0; // Red led is on
ehopman 2:5730195cf595 274 led_blue = 1;
ehopman 2:5730195cf595 275 led_green = 1;
ehopman 1:268bf7dbb15c 276 StateChanged = false;
ehopman 0:50f25a675c72 277 }
Mortimerz 3:0c31a4a5d1fe 278 button_Mbed.rise(&kalmot); // State switches when button pressed
Mortimerz 3:0c31a4a5d1fe 279
ehopman 0:50f25a675c72 280 break; // end of state START
ehopman 0:50f25a675c72 281
ehopman 0:50f25a675c72 282 case KAL_ME:
ehopman 1:268bf7dbb15c 283 if (StateChanged)
ehopman 0:50f25a675c72 284 {
ehopman 2:5730195cf595 285 pc.printf("Calibration ME state, red ld flickers slow");
Mortimerz 3:0c31a4a5d1fe 286 flashred();
Mortimerz 3:0c31a4a5d1fe 287
ehopman 2:5730195cf595 288
ehopman 2:5730195cf595 289 // FUNCTION Move to mechanical stop, include v_motor, t_passed
ehopman 1:268bf7dbb15c 290 // FUNCTION Reset encoders
ehopman 1:268bf7dbb15c 291
ehopman 1:268bf7dbb15c 292 StateChanged = false;
ehopman 0:50f25a675c72 293 }
ehopman 1:268bf7dbb15c 294
Mortimerz 3:0c31a4a5d1fe 295 button_Mbed.rise(&kalemg);
ehopman 0:50f25a675c72 296 break; // end of state KAL_ME
ehopman 0:50f25a675c72 297
ehopman 0:50f25a675c72 298 case KAL_EMG:
ehopman 1:268bf7dbb15c 299 if (StateChanged)
ehopman 0:50f25a675c72 300 {
Mortimerz 3:0c31a4a5d1fe 301 flashgreen();
Mortimerz 3:0c31a4a5d1fe 302 emgcal();
ehopman 1:268bf7dbb15c 303 StateChanged = false;
ehopman 0:50f25a675c72 304 }
ehopman 0:50f25a675c72 305
Mortimerz 3:0c31a4a5d1fe 306 button_Mbed.rise(&readystart);
Mortimerz 3:0c31a4a5d1fe 307
ehopman 0:50f25a675c72 308 break; // end of state KAL_EMG
ehopman 0:50f25a675c72 309
ehopman 0:50f25a675c72 310 case MOVE_START:
ehopman 1:268bf7dbb15c 311 if(StateChanged)
ehopman 0:50f25a675c72 312 {
ehopman 2:5730195cf595 313 led_red = 1;
ehopman 2:5730195cf595 314 led_blue = 1;
ehopman 2:5730195cf595 315 led_green = 0; // Green led is on
ehopman 0:50f25a675c72 316
ehopman 2:5730195cf595 317 // FUNCTION move to start, t_passed
ehopman 2:5730195cf595 318 // Define current_position & start_position
ehopman 2:5730195cf595 319 StateChanged = false;
ehopman 2:5730195cf595 320 }
ehopman 0:50f25a675c72 321
Mortimerz 3:0c31a4a5d1fe 322 button_Mbed.rise(&readystart);
Mortimerz 3:0c31a4a5d1fe 323
ehopman 0:50f25a675c72 324 break; // end of state MOVE_START
ehopman 0:50f25a675c72 325
ehopman 0:50f25a675c72 326 case READY_START:
ehopman 2:5730195cf595 327 if (StateChanged)
ehopman 0:50f25a675c72 328 {
ehopman 2:5730195cf595 329 led_red = 1;
ehopman 2:5730195cf595 330 led_blue = 1;
ehopman 2:5730195cf595 331 led_green = 0; // Green led is on
ehopman 2:5730195cf595 332
ehopman 2:5730195cf595 333 StateChanged = false;
ehopman 0:50f25a675c72 334 }
ehopman 0:50f25a675c72 335
Mortimerz 3:0c31a4a5d1fe 336 button_Mbed.rise(&demo);
Mortimerz 3:0c31a4a5d1fe 337 //button_1.rise(&move);
Mortimerz 3:0c31a4a5d1fe 338
ehopman 0:50f25a675c72 339 break; // end of state READY_START
ehopman 0:50f25a675c72 340
ehopman 0:50f25a675c72 341 case DEMO:
ehopman 2:5730195cf595 342 if (StateChanged)
ehopman 2:5730195cf595 343 {
ehopman 2:5730195cf595 344 //FUNCTION Blue led blink fast
ehopman 2:5730195cf595 345
ehopman 2:5730195cf595 346 //FUNCTION perform straight movements for demo
ehopman 2:5730195cf595 347
ehopman 2:5730195cf595 348 StateChanged = false;
ehopman 2:5730195cf595 349 }
ehopman 2:5730195cf595 350
Mortimerz 3:0c31a4a5d1fe 351 button_Mbed.rise(&movestart);
ehopman 0:50f25a675c72 352 break; // end of state DEMO
ehopman 0:50f25a675c72 353
ehopman 0:50f25a675c72 354 case MOVE:
ehopman 2:5730195cf595 355 if (StateChanged)
ehopman 2:5730195cf595 356 {
ehopman 2:5730195cf595 357 led_red = 1;
ehopman 2:5730195cf595 358 led_green = 1;
ehopman 2:5730195cf595 359 led_blue = 0; //Blue led is on
ehopman 2:5730195cf595 360
ehopman 2:5730195cf595 361 // FUNCTION Play the game with EMG signal
ehopman 2:5730195cf595 362
ehopman 2:5730195cf595 363 StateChanged = false;
ehopman 2:5730195cf595 364 }
ehopman 2:5730195cf595 365
Mortimerz 3:0c31a4a5d1fe 366 button_Mbed.rise(&off);
Mortimerz 3:0c31a4a5d1fe 367 button1.rise(&wait);
ehopman 2:5730195cf595 368
ehopman 2:5730195cf595 369 break; // end of state MOVE
ehopman 0:50f25a675c72 370
ehopman 0:50f25a675c72 371 case WAIT:
ehopman 2:5730195cf595 372 if (StateChanged)
ehopman 2:5730195cf595 373 {
ehopman 2:5730195cf595 374 led_red = 0; // Pink led iS on
ehopman 2:5730195cf595 375 led_blue = 0;
ehopman 2:5730195cf595 376 led_green = 1
ehopman 2:5730195cf595 377
Mortimerz 3:0c31a4a5d1fe 378 button_Mbed.rise(&off);
Mortimerz 3:0c31a4a5d1fe 379 button1.rise(&movestart);
Mortimerz 3:0c31a4a5d1fe 380 button2.rise(&move);
Mortimerz 3:0c31a4a5d1fe 381
ehopman 2:5730195cf595 382
ehopman 0:50f25a675c72 383 break; // end of state WAIT
ehopman 0:50f25a675c72 384
ehopman 0:50f25a675c72 385 case OFF:
ehopman 2:5730195cf595 386 led_red = 0; // White led is on
ehopman 2:5730195cf595 387 led_blue = 0;
ehopman 2:5730195cf595 388 led_green = 0;
ehopman 2:5730195cf595 389
ehopman 0:50f25a675c72 390 break; // end of state OFF
ehopman 0:50f25a675c72 391
ehopman 0:50f25a675c72 392 default:
ehopman 2:5730195cf595 393 TurnMotorsoff(); //FUNCTION
ehopman 0:50f25a675c72 394 printf("Unknown state reached");
ehopman 0:50f25a675c72 395 } // End of the switch, all states are prescribed
ehopman 0:50f25a675c72 396 }
ehopman 0:50f25a675c72 397
ehopman 0:50f25a675c72 398 int main(void) // wat hier in moet snap ik nog niet
ehopman 0:50f25a675c72 399 {
Mortimerz 3:0c31a4a5d1fe 400 states_machine.attach(&StateMachine, 0.002)
ehopman 0:50f25a675c72 401 // hier moeten dingen komen
ehopman 0:50f25a675c72 402 while (true)
ehopman 0:50f25a675c72 403 {
ehopman 0:50f25a675c72 404 CheckForCommandFromTerminal();
ehopman 0:50f25a675c72 405 }
ehopman 0:50f25a675c72 406 }
ehopman 0:50f25a675c72 407
ehopman 0:50f25a675c72 408
ehopman 0:50f25a675c72 409
ehopman 0:50f25a675c72 410