Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@15:c4799ad02cdc, 2019-10-18 (annotated)

Committer:
MatthewMaat
Date:
Fri Oct 18 09:42:36 2019 +0000
Revision:
15:c4799ad02cdc
Parent:
14:dc89250ebc52
Child:
16:2d115aa2773e
FSM with EMG calibration

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