2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 25:49ccdc98639a
- Parent:
- 24:56db31267f10
- Child:
- 26:fe3a5469dd6b
--- a/main.cpp Wed Oct 14 13:04:54 2015 +0000 +++ b/main.cpp Fri Oct 16 13:46:39 2015 +0000 @@ -24,6 +24,11 @@ DigitalIn button(PTA1); //buttons DigitalIn button1(PTB9); // +//Debug legs +DigitalOut red(LED_RED); +DigitalOut green(LED_GREEN); +DigitalOut blue(LED_BLUE); + //EMG shields AnalogIn emg1(A0); //Analog input - Biceps EMG AnalogIn emg2(A1); //Analog input - Triceps EMG @@ -48,8 +53,8 @@ DigitalOut dir_motor2(D4); //Direction motor 2 //Limit Switches -DigitalIn shoulder_limit(PTA4); //using FRDM buttons for now -DigitalIn elbow_limit(PTC6); //using FRDM buttons for now +DigitalIn shoulder_limit(PTA4); //using FRDM buttons +DigitalIn elbow_limit(PTC6); //using FRDM buttons /*-------------------------------------------------------------------------------------------------------------------- @@ -62,6 +67,9 @@ double emg_flexor; double flexor_power; double flexorMVC = 0; double emg_extens; double extens_power; double extensMVC = 0; +int muscle; +double calibrate_time; + //PID variables double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2) double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error @@ -130,6 +138,22 @@ biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope + +biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts +biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope + +biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts +biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope + +biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts +biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope + biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter /*-------------------------------------------------------------------------------------------------------------------- @@ -138,7 +162,8 @@ void keep_in_range(double * in, double min, double max); void sample_filter(void); void control(); -void calibrate_emg(int muscle); +void calibrate_emg(); +void emg_mvc(); void calibrate_arm(void); void start_sampling(void); void stop_sampling(void); @@ -156,24 +181,22 @@ int main() { pc.baud(115200); + red=1; green=1; blue=1; // make a menu, user has to initiate next step - mainMenu(); - caliMenu(); // Menu function - calibrate_arm(); //Start Calibration - start_sampling(); //500 Hz EMG - calibrate_emg(1); //calibrate muscle 1 - start_control(); //100 Hz control + //mainMenu(); + //caliMenu(); // Menu function + //calibrate_arm(); //Start Calibration + + calibrate_emg(); + + //start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); //stop_sampling(); while(1) { - scope.set(0,emg_biceps); - scope.set(1,emg_triceps); - scope.set(2,emg_flexor); - scope.set(3,emg_extens); - scope.send(); + } //end of while loop } @@ -193,11 +216,12 @@ //Filter: high-pass -> notch -> rectify -> lowpass or moving average // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad? - biceps_power = highpass.step(emg_biceps); triceps_power = highpass.step(emg_triceps); flexor_power = highpass.step(emg_flexor); extens_power = highpass.step(emg_extens); - biceps_power = notch1.step(biceps_power); triceps_power = notch1.step(triceps_power); flexor_power = notch1.step(flexor_power); extens_power = notch1.step(extens_power); - biceps_power = notch2.step(biceps_power); triceps_power = notch2.step(triceps_power); flexor_power = notch2.step(flexor_power); extens_power = notch2.step(extens_power); + biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens); + biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power); + biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power); biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power); - biceps_power = lowpass.step(biceps_power); triceps_power = lowpass.step(triceps_power); flexor_power = lowpass.step(flexor_power); extens_power = lowpass.step(extens_power); + biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power); + /* alternative for lowpass filter: moving average window=30; //30 samples @@ -220,10 +244,22 @@ //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { - bool hit = true; + //const double hit = 0.5; dir_motor1=1; //ccw dir_motor2=1; //ccw - while(shoulder_limit != hit){ + pwm_motor1.write(0.2f); //move upper arm slowly ccw + pwm_motor2.write(0.2f); //move forearm slowly ccw + + if(shoulder_limit.read() < 0.5){ //when limit switches are hit, stop motor and reset encoder + pwm_motor1.write(0); + Encoder1.reset(); + } + if(elbow_limit.read() < 0.5){ + pwm_motor2.write(0); + Encoder2.reset(); + } + + /* while(shoulder_limit != hit){ pwm_motor1.write(0.4); } Encoder1.reset(); @@ -232,24 +268,27 @@ pwm_motor2.write(0.4); } Encoder2.reset(); + */ } //EMG Maximum Voluntary Contraction measurement -void calibrate_emg(int muscle) -{ - start_sampling(); - +void emg_mvc() +{ //double sampletime=0; //sampletime=+SAMPLE_RATE; // // if(sampletime<5) - - for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples + //int muscle=1; + //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples + if(muscle==1){ if(biceps_power>bicepsMVC){ + printf("+ "); bicepsMVC=biceps_power; } + else + printf("- "); } if(muscle==2){ @@ -272,8 +311,69 @@ extensMVC=extens_power; } } - } - stop_sampling(); + + //} + calibrate_time = calibrate_time + 0.002; + + + +} + +//EMG calibration +void calibrate_emg() +{ + Ticker timer; + + pc.printf("|-- Robot Started --| \r\n"); + pc.printf("Testcode calibration \r\n"); + wait(1); + pc.printf("+ means current sample is higher than stored MVC\r\n"); + pc.printf("- means current sample is lower than stored MVC\r\n"); + wait(3); + pc.printf(" Biceps is first. "); wait(1); + pc.printf(" Press any key to begin... "); wait(1); + char input; + input=pc.getc(); + pc.putc(input); + pc.printf(" \r\n Starting in 3... \r\n"); wait(1); + pc.printf(" \r\n Starting in 2... \r\n"); wait(1); + pc.printf(" \r\n Starting in 1... \r\n"); wait(1); + + start_sampling(); + muscle=1; + timer.attach(&emg_mvc,0.002); + wait(5); + timer.detach(); + pc.printf("\r\n MVC = %f \r\n",bicepsMVC); + + pc.printf("Calibrate_emg() exited \r\n"); + pc.printf("Measured time: %f seconds \r\n",calibrate_time); + calibrate_time=0; + + // Triceps: + pc.printf(" Triceps is next "); wait(1); + pc.printf(" Press any key to begin... "); wait(1); + input=pc.getc(); + pc.putc(input); + pc.printf(" \r\n Starting in 3... \r\n"); wait(1); + pc.printf(" \r\n Starting in 2... \r\n"); wait(1); + pc.printf(" \r\n Starting in 1... \r\n"); wait(1); + start_sampling(); + muscle=1; + timer.attach(&emg_mvc,0.002); + wait(5); + timer.detach(); + pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC); + + pc.printf("Calibrate_emg() exited \r\n"); + pc.printf("Measured time: %f seconds \r\n",calibrate_time); + calibrate_time=0; + + //Flexor: + + //Extensor: + + stop_sampling(); } @@ -316,8 +416,8 @@ pwm_motor1.write(abs(u1)); //send PWM and DIR to motor 2 - dir_motor1 = u2>0 ? 1 : 0; //conditional statement, same as if else below - pwm_motor1.write(abs(u2)); + dir_motor2 = u2>0 ? 1 : 0; //conditional statement, same as if else below + pwm_motor2.write(abs(u2)); /*if(u1 > 0) { @@ -347,18 +447,22 @@ void start_sampling(void) { sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG + blue=0; green=0; + pc.printf("||- started sampling -|| \r\n"); } //stop sampling void stop_sampling(void) { sample_timer.detach(); + blue=1; green=1; + pc.printf("||- stopped sampling -|| \r\n"); } //Start control void start_control(void) { - control_timer.attach(&control,CONTROL_RATE); //100 Hz control + control_timer.attach(&control,SAMPLE_RATE); //100 Hz control } //stop control