Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@16:2d115aa2773e, 2019-10-18 (annotated)

Committer:
MatthewMaat
Date:
Fri Oct 18 10:14:17 2019 +0000
Revision:
16:2d115aa2773e
Parent:
15:c4799ad02cdc
Child:
17:d1acb6888b82
FSM met EMG kalibratie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
MatthewMaat 13:ec4708dab45d 2 #include "HIDScope.h"
MatthewMaat 11:de4a85703169 3 //#include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
RobertoO 0:67c50348f842 5 //#include "BiQuad.h"
MatthewMaat 8:ec3c634390c7 6 #include "FastPWM.h"
MatthewMaat 2:626688c21b6f 7 #include <iostream>
MatthewMaat 5:cee5f898b350 8 MODSERIAL pc(USBTX, USBRX);
MatthewMaat 13:ec4708dab45d 9 AnalogIn ain2(A2);
MatthewMaat 13:ec4708dab45d 10 AnalogIn ain1(A3);
MatthewMaat 13:ec4708dab45d 11 DigitalOut dir2(D4);
MatthewMaat 13:ec4708dab45d 12 DigitalOut dir1(D7);
MatthewMaat 13:ec4708dab45d 13 //D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
MatthewMaat 13:ec4708dab45d 14 PwmOut motor2_pwm(D5);
MatthewMaat 13:ec4708dab45d 15 PwmOut motor1_pwm(D6);
MatthewMaat 13:ec4708dab45d 16 AnalogIn emg0( A0 );
MatthewMaat 13:ec4708dab45d 17 AnalogIn emg1( A1 );
MatthewMaat 13:ec4708dab45d 18
MatthewMaat 15:c4799ad02cdc 19 Ticker ticktick;
MatthewMaat 15:c4799ad02cdc 20 Timer state_time;
MatthewMaat 15:c4799ad02cdc 21 Timeout EMG_peak;
MatthewMaat 13:ec4708dab45d 22 Ticker sample_timer;
MatthewMaat 15:c4799ad02cdc 23 HIDScope scope( 3);
MatthewMaat 14:dc89250ebc52 24 DigitalOut ledred(LED_RED);
MatthewMaat 14:dc89250ebc52 25 DigitalOut ledblue(LED_BLUE);
MatthewMaat 14:dc89250ebc52 26 DigitalOut ledgreen(LED_GREEN);
MatthewMaat 15:c4799ad02cdc 27 InterruptIn err(SW2);
MatthewMaat 15:c4799ad02cdc 28 InterruptIn button(SW3);
MatthewMaat 15:c4799ad02cdc 29
MatthewMaat 15:c4799ad02cdc 30 volatile float P0;
MatthewMaat 15:c4799ad02cdc 31 volatile float P1;
MatthewMaat 15:c4799ad02cdc 32 volatile float EMG_min0=1;
MatthewMaat 15:c4799ad02cdc 33 volatile float EMG_max0=0;
MatthewMaat 15:c4799ad02cdc 34 volatile float EMG_min1=1;
MatthewMaat 15:c4799ad02cdc 35 volatile float EMG_max1=0;
MatthewMaat 15:c4799ad02cdc 36 volatile bool ignore_peaks=false;
MatthewMaat 16:2d115aa2773e 37 volatile bool ignore_turn=true;
MatthewMaat 14:dc89250ebc52 38 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
MatthewMaat 15:c4799ad02cdc 39 states currentState=Waiting;
MatthewMaat 10:e1eb73e19540 40
MatthewMaat 14:dc89250ebc52 41
MatthewMaat 14:dc89250ebc52 42 void read_emg()
MatthewMaat 13:ec4708dab45d 43 {
MatthewMaat 15:c4799ad02cdc 44 //EMG signal 0
MatthewMaat 15:c4799ad02cdc 45 static int count0=0;
MatthewMaat 15:c4799ad02cdc 46 static float RMS_value0=0;
MatthewMaat 15:c4799ad02cdc 47 static float HighPass_value0=0;
MatthewMaat 15:c4799ad02cdc 48 count0+=1;
MatthewMaat 15:c4799ad02cdc 49 static float RMS0[150];
MatthewMaat 15:c4799ad02cdc 50 static float HighPass0[30];
MatthewMaat 15:c4799ad02cdc 51 float I0;
MatthewMaat 15:c4799ad02cdc 52 float If0;
MatthewMaat 15:c4799ad02cdc 53 //signal 1
MatthewMaat 15:c4799ad02cdc 54 static int count1=0;
MatthewMaat 15:c4799ad02cdc 55 static float RMS_value1=0;
MatthewMaat 15:c4799ad02cdc 56 static float HighPass_value1=0;
MatthewMaat 15:c4799ad02cdc 57 count1+=1;
MatthewMaat 15:c4799ad02cdc 58 static float RMS1[150];
MatthewMaat 15:c4799ad02cdc 59 static float HighPass1[30];
MatthewMaat 13:ec4708dab45d 60 float I1;
MatthewMaat 15:c4799ad02cdc 61 float If1;
MatthewMaat 15:c4799ad02cdc 62 I0=emg0.read(); //read signal
MatthewMaat 15:c4799ad02cdc 63 HighPass_value0+=(I0-HighPass0[count0%30])/30.0;
MatthewMaat 15:c4799ad02cdc 64 HighPass0[count0%30]=I0;
MatthewMaat 15:c4799ad02cdc 65 If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 66 RMS_value0+=(If0-RMS0[count0%150])/150.0;
MatthewMaat 15:c4799ad02cdc 67 RMS0[count0%150]=If0;
MatthewMaat 13:ec4708dab45d 68 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 15:c4799ad02cdc 69 P0=sqrt(RMS_value0);
MatthewMaat 15:c4799ad02cdc 70 I1=emg1.read(); //read signal
MatthewMaat 15:c4799ad02cdc 71 HighPass_value1+=(I1-HighPass1[count1%30])/30.0;
MatthewMaat 15:c4799ad02cdc 72 HighPass1[count1%30]=I1;
MatthewMaat 15:c4799ad02cdc 73 If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 74 RMS_value1+=(If1-RMS1[count1%150])/150.0;
MatthewMaat 15:c4799ad02cdc 75 RMS1[count1%150]=If1;
MatthewMaat 15:c4799ad02cdc 76 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 15:c4799ad02cdc 77 P1=sqrt(RMS_value1);
MatthewMaat 15:c4799ad02cdc 78 scope.set(0, P0 ); // send root mean squared
MatthewMaat 15:c4799ad02cdc 79 scope.set(1, EMG_min0);
MatthewMaat 15:c4799ad02cdc 80 scope.set(2,EMG_max0);
MatthewMaat 13:ec4708dab45d 81 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MatthewMaat 13:ec4708dab45d 82 * Ensure that enough channels are available (HIDScope scope( 2 ))
MatthewMaat 13:ec4708dab45d 83 * Finally, send all channels to the PC at once */
MatthewMaat 13:ec4708dab45d 84 scope.send();
MatthewMaat 13:ec4708dab45d 85 /* To indicate that the function is working, the LED is toggled */
MatthewMaat 14:dc89250ebc52 86 ledred=1;
MatthewMaat 14:dc89250ebc52 87 ledgreen=0;
MatthewMaat 14:dc89250ebc52 88 ledblue=1;
MatthewMaat 13:ec4708dab45d 89 }
MatthewMaat 14:dc89250ebc52 90
MatthewMaat 15:c4799ad02cdc 91 void record_min_max(void)
MatthewMaat 15:c4799ad02cdc 92 {
MatthewMaat 15:c4799ad02cdc 93 float t=state_time.read();
MatthewMaat 15:c4799ad02cdc 94 if(t>0.4)
MatthewMaat 15:c4799ad02cdc 95 {
MatthewMaat 15:c4799ad02cdc 96 if(P0<EMG_min0)
MatthewMaat 15:c4799ad02cdc 97 {
MatthewMaat 15:c4799ad02cdc 98 EMG_min0=P0;
MatthewMaat 15:c4799ad02cdc 99 }
MatthewMaat 15:c4799ad02cdc 100 else if(P0>EMG_max0)
MatthewMaat 15:c4799ad02cdc 101 {
MatthewMaat 15:c4799ad02cdc 102 EMG_max0=P0;
MatthewMaat 15:c4799ad02cdc 103 }
MatthewMaat 15:c4799ad02cdc 104 if(P1<EMG_min1)
MatthewMaat 15:c4799ad02cdc 105 {
MatthewMaat 15:c4799ad02cdc 106 EMG_min1=P1;
MatthewMaat 15:c4799ad02cdc 107 }
MatthewMaat 15:c4799ad02cdc 108 else if(P1>EMG_max1)
MatthewMaat 15:c4799ad02cdc 109 {
MatthewMaat 15:c4799ad02cdc 110 EMG_max1=P1;
MatthewMaat 15:c4799ad02cdc 111 }
MatthewMaat 15:c4799ad02cdc 112 }
MatthewMaat 15:c4799ad02cdc 113 }
MatthewMaat 15:c4799ad02cdc 114
MatthewMaat 15:c4799ad02cdc 115 void unignore_peaks(void)
MatthewMaat 15:c4799ad02cdc 116 {
MatthewMaat 15:c4799ad02cdc 117 ignore_peaks=false;
MatthewMaat 15:c4799ad02cdc 118 }
MatthewMaat 16:2d115aa2773e 119 void start_ignore_turn(void)
MatthewMaat 16:2d115aa2773e 120 {
MatthewMaat 16:2d115aa2773e 121 ignore_turn=true;
MatthewMaat 16:2d115aa2773e 122 }
MatthewMaat 15:c4799ad02cdc 123
MatthewMaat 14:dc89250ebc52 124 void set_PWM(void)
MatthewMaat 11:de4a85703169 125 {
MatthewMaat 15:c4799ad02cdc 126 static bool motor_on=false;
MatthewMaat 15:c4799ad02cdc 127 float Q0;
MatthewMaat 15:c4799ad02cdc 128 Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
MatthewMaat 16:2d115aa2773e 129 if (Q0>0.3 && !ignore_peaks)
MatthewMaat 13:ec4708dab45d 130 {
MatthewMaat 15:c4799ad02cdc 131 if (!motor_on)
MatthewMaat 15:c4799ad02cdc 132 {
MatthewMaat 15:c4799ad02cdc 133 motor1_pwm.write(1.0f);
MatthewMaat 15:c4799ad02cdc 134 EMG_peak.attach(unignore_peaks,1);
MatthewMaat 15:c4799ad02cdc 135 ignore_peaks=true;
MatthewMaat 15:c4799ad02cdc 136 motor_on=true;
MatthewMaat 15:c4799ad02cdc 137 }
MatthewMaat 16:2d115aa2773e 138 else if(ignore_turn)
MatthewMaat 15:c4799ad02cdc 139 {
MatthewMaat 15:c4799ad02cdc 140 motor1_pwm.write(0.0f);
MatthewMaat 15:c4799ad02cdc 141 EMG_peak.attach(unignore_peaks,1);
MatthewMaat 16:2d115aa2773e 142 EMG_peak.attach(start_ignore_turn,1.5);
MatthewMaat 16:2d115aa2773e 143 ignore_turn=false;
MatthewMaat 15:c4799ad02cdc 144 ignore_peaks=true;
MatthewMaat 15:c4799ad02cdc 145 motor_on=false;
MatthewMaat 15:c4799ad02cdc 146 }
MatthewMaat 16:2d115aa2773e 147 else
MatthewMaat 16:2d115aa2773e 148 {
MatthewMaat 16:2d115aa2773e 149 motor1_pwm.write(1.0f);
MatthewMaat 16:2d115aa2773e 150 dir1=!dir1;
MatthewMaat 16:2d115aa2773e 151 EMG_peak.attach(unignore_peaks,1);
MatthewMaat 16:2d115aa2773e 152 ignore_peaks=true;
MatthewMaat 16:2d115aa2773e 153 motor_on=true;
MatthewMaat 16:2d115aa2773e 154 }
MatthewMaat 13:ec4708dab45d 155 }
MatthewMaat 13:ec4708dab45d 156 motor2_pwm.write(ain1.read());
MatthewMaat 11:de4a85703169 157 }
MatthewMaat 11:de4a85703169 158
MatthewMaat 14:dc89250ebc52 159 void sample()
MatthewMaat 14:dc89250ebc52 160 {
MatthewMaat 14:dc89250ebc52 161 switch(currentState)
MatthewMaat 14:dc89250ebc52 162 {
MatthewMaat 15:c4799ad02cdc 163 case Waiting:
MatthewMaat 15:c4799ad02cdc 164 ledred=0;
MatthewMaat 15:c4799ad02cdc 165 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 166 ledblue=1;
MatthewMaat 15:c4799ad02cdc 167 break;
MatthewMaat 15:c4799ad02cdc 168 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 169 ledred=1;
MatthewMaat 15:c4799ad02cdc 170 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 171 ledblue=0;
MatthewMaat 15:c4799ad02cdc 172 break;
MatthewMaat 15:c4799ad02cdc 173 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 174 ledred=1;
MatthewMaat 15:c4799ad02cdc 175 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 176 ledblue=0;
MatthewMaat 15:c4799ad02cdc 177 read_emg();
MatthewMaat 15:c4799ad02cdc 178 record_min_max();
MatthewMaat 15:c4799ad02cdc 179 break;
MatthewMaat 15:c4799ad02cdc 180 case Homing:
MatthewMaat 15:c4799ad02cdc 181 ledred=0;
MatthewMaat 15:c4799ad02cdc 182 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 183 ledblue=0;
MatthewMaat 15:c4799ad02cdc 184 break;
MatthewMaat 14:dc89250ebc52 185 case Operating:
MatthewMaat 14:dc89250ebc52 186 read_emg();
MatthewMaat 14:dc89250ebc52 187 set_PWM();
MatthewMaat 15:c4799ad02cdc 188 ledred=1;
MatthewMaat 15:c4799ad02cdc 189 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 190 ledblue=1;
MatthewMaat 15:c4799ad02cdc 191 break;
MatthewMaat 15:c4799ad02cdc 192 case Demo:
MatthewMaat 15:c4799ad02cdc 193 ledred=0;
MatthewMaat 15:c4799ad02cdc 194 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 195 ledblue=0;
MatthewMaat 14:dc89250ebc52 196 break;
MatthewMaat 14:dc89250ebc52 197 case Failure:
MatthewMaat 14:dc89250ebc52 198 ledred=0;
MatthewMaat 14:dc89250ebc52 199 ledgreen=1;
MatthewMaat 14:dc89250ebc52 200 ledblue=1;
MatthewMaat 15:c4799ad02cdc 201 motor1_pwm.write(0.0);
MatthewMaat 15:c4799ad02cdc 202 motor2_pwm.write(0.0);
MatthewMaat 14:dc89250ebc52 203 break;
MatthewMaat 14:dc89250ebc52 204 }
MatthewMaat 14:dc89250ebc52 205 }
MatthewMaat 14:dc89250ebc52 206
MatthewMaat 14:dc89250ebc52 207 void error_occur()
MatthewMaat 14:dc89250ebc52 208 {
MatthewMaat 14:dc89250ebc52 209 currentState=Failure;
MatthewMaat 14:dc89250ebc52 210 }
MatthewMaat 14:dc89250ebc52 211
MatthewMaat 15:c4799ad02cdc 212 void button_press()
MatthewMaat 15:c4799ad02cdc 213 //Press button to change state
MatthewMaat 15:c4799ad02cdc 214 {
MatthewMaat 15:c4799ad02cdc 215 state_time.start();
MatthewMaat 15:c4799ad02cdc 216 switch(currentState)
MatthewMaat 15:c4799ad02cdc 217 {
MatthewMaat 15:c4799ad02cdc 218 case Waiting:
MatthewMaat 15:c4799ad02cdc 219 currentState=Position_calibration;
MatthewMaat 15:c4799ad02cdc 220 wait(1);
MatthewMaat 15:c4799ad02cdc 221 break;
MatthewMaat 15:c4799ad02cdc 222 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 223 currentState=EMG_calibration;
MatthewMaat 15:c4799ad02cdc 224 wait(1);
MatthewMaat 15:c4799ad02cdc 225 break;
MatthewMaat 15:c4799ad02cdc 226 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 227 currentState=Homing;
MatthewMaat 15:c4799ad02cdc 228 wait(1);
MatthewMaat 15:c4799ad02cdc 229 break;
MatthewMaat 15:c4799ad02cdc 230 case Homing:
MatthewMaat 15:c4799ad02cdc 231 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 232 wait(1);
MatthewMaat 15:c4799ad02cdc 233 break;
MatthewMaat 15:c4799ad02cdc 234 case Operating:
MatthewMaat 15:c4799ad02cdc 235 currentState=Demo;
MatthewMaat 15:c4799ad02cdc 236 wait(1);
MatthewMaat 15:c4799ad02cdc 237 break;
MatthewMaat 15:c4799ad02cdc 238 case Demo:
MatthewMaat 15:c4799ad02cdc 239 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 240 wait(1);
MatthewMaat 15:c4799ad02cdc 241 break;
MatthewMaat 15:c4799ad02cdc 242 }
MatthewMaat 15:c4799ad02cdc 243 }
MatthewMaat 15:c4799ad02cdc 244
MatthewMaat 8:ec3c634390c7 245 int main()
MatthewMaat 4:f988679bf9a1 246 {
MatthewMaat 14:dc89250ebc52 247 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 248 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 249 ledred=0;
MatthewMaat 13:ec4708dab45d 250 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 251 err.fall(error_occur);
MatthewMaat 15:c4799ad02cdc 252 button.fall(button_press);
MatthewMaat 12:7f280a661e71 253 int frequency_pwm=10000;
MatthewMaat 12:7f280a661e71 254 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 255 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 11:de4a85703169 256
MatthewMaat 8:ec3c634390c7 257 while (true) {
MatthewMaat 12:7f280a661e71 258 wait(10);
MatthewMaat 4:f988679bf9a1 259 }
MatthewMaat 4:f988679bf9a1 260 }