Matthew Maat / Mbed 2 deprecated template_biorobotics

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@17:d1acb6888b82, 2019-10-18 (annotated)

Committer:
MatthewMaat
Date:
Fri Oct 18 13:05:44 2019 +0000
Revision:
17:d1acb6888b82
Parent:
16:2d115aa2773e
Child:
18:8002c75b8e20
Child:
20:0f6a88f29a71
FSM met goed afgestelde EMG

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