2 losse EMG signalen van de biceps en deltoid
Dependencies: HIDScope MODSERIAL mbed-dsp mbed Encoder
Fork of Lampje_EMG_Gr6 by
Diff: main.cpp
- Revision:
- 33:381bc6635528
- Parent:
- 32:e35bedc7cefd
- Child:
- 34:7e0bf3e9f135
diff -r e35bedc7cefd -r 381bc6635528 main.cpp --- a/main.cpp Mon Nov 03 20:45:47 2014 +0000 +++ b/main.cpp Mon Nov 03 22:10:00 2014 +0000 @@ -5,12 +5,11 @@ #include "encoder.h" #include "PwmOut.h" -// define #define TSAMP 0.005 -#define K_P1 (3.5) //Kp waarde voor motor1, van het batje // 7.0 -#define K_I1 (0.01 *TSAMP) //0.1 -#define K_P2 (0.7) //Kp waarde voor motor2, de arm //10.0 -#define K_I2 (0.01 *TSAMP) //3.0 +#define K_P1 (3.5) +#define K_I1 (0.01) +#define K_P2 (3.5) +#define K_I2 (0.01) #define I_LIMIT 1. #define l_arm 0.5 @@ -32,7 +31,7 @@ //EMG AnalogIn emg0(PTB0); //Analog input AnalogIn emg1(PTC2); //Analog input -HIDScope scope(4); +HIDScope scope(3); //motor1 25D Encoder motor1(PTD3,PTD5); //wit, geel @@ -142,12 +141,12 @@ average_deltoid(filtered_deltoid, &filtered_average_del); /*send value to PC. */ - //scope.set(0,emg_value1); //Raw EMG signal biceps + scope.set(0,emg_value1); //Raw EMG signal biceps //scope.set(1,emg_value2); //Raw EMG signal Deltoid - scope.set(0,filtered_biceps); //processed float biceps - scope.set(1,filtered_average_bi); //processed float deltoid - scope.set(2,filtered_deltoid); //processed float biceps - scope.set(3,filtered_average_del); //processed float deltoid + scope.set(1,filtered_biceps); //processed float biceps + scope.set(2,filtered_average_bi); //processed float deltoid + //scope.set(2,filtered_deltoid); //processed float biceps + //scope.set(3,filtered_average_del); //processed float deltoid scope.send(); } @@ -270,314 +269,324 @@ if(setpoint1 < -(11.3*2.3*2.0*PI/360)) { setpoint1 = -(11.3*2.3*2.0*PI/360); } - if(setpoint1 <= -(11.3*2.3*2.0*PI/360)-0.1) { + if(setpoint1 <= -((11.3*2.3*2.0*PI/360)-0.1)) { staat1 = 1; prev_setpoint1 = setpoint1; } } // Motor1 rechts draaien - void batje_rechts () { - speed1_rad = 1.0; - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (11.3*2.3*2.0*PI/360)) { - setpoint1 = (11.3*2.3*2.0*PI/360); - } - if(setpoint1 < -(11.3*2.3*2.0*PI/360)) { - setpoint1 = -(11.3*2.3*2.0*PI/360); - } - prev_setpoint1 = setpoint1; - if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) { - staat1 = 1; - } +void batje_rechts () +{ + speed1_rad = 1.0; + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; + if(setpoint1 > (11.3*2.3*2.0*PI/360)) { + setpoint1 = (11.3*2.3*2.0*PI/360); } + if(setpoint1 < -(11.3*2.3*2.0*PI/360)) { + setpoint1 = -(11.3*2.3*2.0*PI/360); + } + prev_setpoint1 = setpoint1; + if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) { + staat1 = 1; + } +} //Motor1 na links draaien weer terug laten draaien naar beginstand - void batje_begin_links () { - speed1_rad = 1.0; - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (0*2.3*2.0*PI/360)) { - setpoint1 = (0*2.3*2.0*PI/360); - } - if(setpoint1 < -(0*2.3*2.0*PI/360)) { - setpoint1 = -(0*2.3*2.0*PI/360); - } - prev_setpoint1 = setpoint1; +void batje_begin_links () +{ + speed1_rad = 1.0; + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; + if(setpoint1 > (0*2.3*2.0*PI/360)) { + setpoint1 = (0*2.3*2.0*PI/360); } + //if(setpoint1 < -(0*2.3*2.0*PI/360)) { + // setpoint1 = -(0*2.3*2.0*PI/360); + //} + prev_setpoint1 = setpoint1; +} //Motor1 na links draaien weer terug laten draaien naar beginstand - void batje_begin_rechts () { - speed1_rad = -1.0; - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (0*2.3*2.0*PI/360)) { - setpoint1 = (0*2.3*2.0*PI/360); - } - if(setpoint1 < -(0.0*2.3*2.0*PI/360)) { - setpoint1 = -(0.0*2.3*2.0*PI/360); - } - prev_setpoint1 = setpoint1; +void batje_begin_rechts () +{ + speed1_rad = -1.0; + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; + //if(setpoint1 > (0*2.3*2.0*PI/360)) { + // setpoint1 = (0*2.3*2.0*PI/360); + //} + if(setpoint1 < -(0.0*2.3*2.0*PI/360)) { + setpoint1 = -(0.0*2.3*2.0*PI/360); } + prev_setpoint1 = setpoint1; +} // Motor2 balletje op zn hoogst slaan - void arm_hoog () { - speed2_rad = 6.0; - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (155.0*2.0*PI/360)) { - setpoint2 = (155.0*2.0*PI/360); - } - if(setpoint2 < -(155.0*2.0*PI/360)) { - setpoint2 = -(155.0*2.0*PI/360); - } - prev_setpoint2 = setpoint2; - if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { - staat2 = 1; - } +void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN +{ + speed2_rad = -6.0; //positief is CCW, negatief CW (boven aanzicht) + setpoint2 = -(150*2.0*PI/360); + /*setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; + if(setpoint2 > (160*2.0*PI/360)) { //setpoint in graden + setpoint2 = (160.0*2.0*PI/360); } + if(setpoint2 < -(250.0*2.0*PI/360)) { + setpoint2 = -(250.0*2.0*PI/360); + } + */ + prev_setpoint2 = setpoint2; + if(setpoint2 <= -((150.0*2.0*PI/360)-0.1)) { + staat2 = 1; + } +} // Motor2 balletje in het midden slaan - void arm_mid () { - speed2_rad = 4.0; - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (155.0*2.0*PI/360)) { - setpoint2 = (155.0*2.0*PI/360); - } - if(setpoint2 < -(155.0*2.0*PI/360)) { - setpoint2 = -(155.0*2.0*PI/360); - } - prev_setpoint2 = setpoint2; - if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { - staat2 = 1; +void arm_mid () +{ + speed2_rad = -4.0; + setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; + if(setpoint2 > (155.0*2.0*PI/360)) { + setpoint2 = (155.0*2.0*PI/360); + } + if(setpoint2 < -(155.0*2.0*PI/360)) { + setpoint2 = -(155.0*2.0*PI/360); + } + prev_setpoint2 = setpoint2; + if(setpoint2 <= -((155.0*2.0*PI/360)-0.1)) { + staat2 = 1; + } +} + +// Motor2 balletje op het laagst slaan +void arm_laag () +{ + speed2_rad = -2.0; + setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; + if(setpoint2 > (155*2.0*PI/360)) { + setpoint2 = (155*2.0*PI/360); + } + if(setpoint2 < -(155.0*2.0*PI/360)) { + setpoint2 = -(155.0*2.0*PI/360); + } + prev_setpoint2 = setpoint2; + if(setpoint2 <= ((155.0*2.0*PI/360)-0.1)) { + staat2 = 1; + } +} + +// Motor2 arm terug zetten in beginstand +void arm_begin () +{ + speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) + setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; + if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden + setpoint2 = (0.0*2.0*PI/360); + } + //if(setpoint2 < -(0.0*2.0*PI/360)) { + // setpoint2 = -(0.0*2.0*PI/360); + //} + prev_setpoint2 = setpoint2; +} + +// MOTOR aansturing +void looper_motor() +{ + pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition()); //Geeft de posities weer van beide motoren met een sample frequentie van 0.005 + + //MOTOR1 + cur_pos_motor1 = motor1.getPosition(); + pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //voor 1 rotatie van de motoras geldt 24(aantal cpr vd encoder)*172(gearbox ratio)=4128 counts. + pwm_out1 = pid1(setpoint1, pos_motor1_rad); + if (pwm_out1 < -1.0) { //Hier wordt de grens voor de pwm waarde ingesteld. + pwm_out1 = -1.0; + } + if (pwm_out1 > 1.0) { + pwm_out1 = 1.0; + } + pwm_motor1.write(abs(pwm_out1)); + if(pwm_out1 > 0) { + motordir1 = 0; + } else { + motordir1 = 1; + } + + //MOTOR2 + cur_pos_motor2 = motor2.getPosition(); + pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); + pwm_out2 = pid2(setpoint2, pos_motor2_rad); // + if (pwm_out2 < -1.0) { + pwm_out2 = -1.0; + } + if (pwm_out2 > 1.0) { + pwm_out2 = 1.0; + } + pwm_motor2.write(abs(pwm_out2)); + if(pwm_out2 > 0) { + motordir2 = 0; + } else { + motordir2 = 1; + } + + + //STATES + + //Het batje draait naar opgegeven positie, doet dan een bepaalde tijd niks (wait_iterator), en draait daarna weer terug + if (batje_hoek == 1) { + if(staat1 == 0) { + batje_rechts(); + wait_iterator1 = 0; + } else if(staat1 ==1) { + wait_iterator1++; + if(wait_iterator1 > 1200) { + staat1 = 2; + + batje_begin_rechts(); + } } } + if (batje_hoek == 2) { + if(staat1 == 0) { + batje_links(); + wait_iterator1 = 0; + } else if(staat1 ==1) { + wait_iterator1++; + if(wait_iterator1 > 1200) { + staat1 = 2; -// Motor2 balletje op het laagst slaan - void arm_laag () { - speed2_rad = 2.0; - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (155*2.0*PI/360)) { - setpoint2 = (155*2.0*PI/360); - } - if(setpoint2 < -(155.0*2.0*PI/360)) { - setpoint2 = -(155.0*2.0*PI/360); - } - prev_setpoint2 = setpoint2; - if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { - staat2 = 1; + batje_begin_links (); + } } } -// Motor2 arm terug zetten in beginstand - void arm_begin () { - speed2_rad = 1.0; - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (0.0*2.0*PI/360)) { - setpoint2 = (0.0*2.0*PI/360); + if(arm_hoogte == 1) { + if(staat2 == 0) { + arm_laag(); + wait_iterator2 = 0; + } else if(staat2 == 1) { + wait_iterator2++; + if(wait_iterator2 > 400) { + staat2 = 2; + + arm_begin(); + } } - if(setpoint2 < -(0.0*2.0*PI/360)) { - setpoint2 = -(0.0*2.0*PI/360); + } + if(arm_hoogte == 2) { + if(staat2 == 0) { + arm_mid(); + wait_iterator2 = 0; + } else if(staat2 == 1) { + wait_iterator2++; + if(wait_iterator2 > 400) { + staat2 = 2; + + arm_begin(); + } } - prev_setpoint2 = setpoint2; + } + if(arm_hoogte == 3) { + if(staat2 == 0) { + arm_hoog(); + wait_iterator2 = 0; + } else if(staat2 == 1) { + wait_iterator2++; + if(wait_iterator2 > 400) { + staat2 = 2; + + arm_begin(); + } + } } -// MOTOR aansturing - void looper_motor() { - pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition()); //Geeft de posities weer van beide motoren met een sample frequentie van 0.005 - - //MOTOR1 - \ - cur_pos_motor1 = motor1.getPosition(); - pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //voor 1 rotatie van de motoras geldt 24(aantal cpr vd encoder)*172(gearbox ratio)=4128 counts. - pwm_out1 = pid1(setpoint1, pos_motor1_rad); - if (pwm_out1 < -1.0) { //Hier wordt de grens voor de pwm waarde ingesteld. - pwm_out1 = -1.0; - } - if (pwm_out1 > 1.0) { - pwm_out1 = 1.0; - } - pwm_motor1.write(abs(pwm_out1)); - if(pwm_out1 > 0) { - motordir1 = 0; - } else { - motordir1 = 1; - } - - //MOTOR2 - cur_pos_motor2 = motor2.getPosition(); - pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); - pwm_out2 = pid2(setpoint2, pos_motor2_rad); // - if (pwm_out2 < -1.0) { - pwm_out2 = -1.0; - } - if (pwm_out2 > 1.0) { - pwm_out2 = 1.0; - } - pwm_motor2.write(abs(pwm_out2)); - if(pwm_out2 > 0) { - motordir2 = 0; - } else { - motordir2 = 1; - } - - - //STATES - - //Het batje draait naar opgegeven positie, doet dan een bepaalde tijd niks (wait_iterator), en draait daarna weer terug - if (batje_hoek == 1) { - if(staat1 == 0) { - batje_rechts(); - wait_iterator1 = 0; - } else if(staat1 ==1) { - wait_iterator1++; - if(wait_iterator1 > 1200) { - staat1 = 2; - - batje_begin_rechts(); - } - } - } - if (batje_hoek == 2) { - if(staat1 == 0) { - batje_links(); - wait_iterator1 = 0; - } else if(staat1 ==1) { - wait_iterator1++; - if(wait_iterator1 > 1200) { - staat1 = 2; - - batje_begin_links (); - } - } - } - - if(arm_hoogte == 1) { - if(staat2 == 0) { - arm_laag(); - wait_iterator2 = 0; - } else if(staat2 == 1) { - wait_iterator2++; - if(wait_iterator2 > 400) { - staat2 = 2; - - arm_begin(); - } - } - } - if(arm_hoogte == 2) { - if(staat2 == 0) { - arm_mid(); - wait_iterator2 = 0; - } else if(staat2 == 1) { - wait_iterator2++; - if(wait_iterator2 > 400) { - staat2 = 2; - - arm_begin(); - } - } - } - if(arm_hoogte == 3) { - if(staat2 == 0) { - arm_hoog(); - wait_iterator2 = 0; - } else if(staat2 == 1) { - wait_iterator2++; - if(wait_iterator2 > 400) { - staat2 = 2; - - arm_begin(); - } - } - } - - } +} // Hoofdprogramma, hierin staat de aansturing vd LED - int main() { +int main() +{ - pwm_motor1.period_us(100); - motor1.setPosition(0); - pwm_motor2.period_us(100); - motor2.setPosition(0); - pc.baud(115200); - // Ticker EMG signaal meten - Ticker log_timer; - //set up filters. Use external array for constants - arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states); - arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states); - arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states); - arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states); - // Uitvoeren van ticker EMG, sample frequentie 500Hz - log_timer.attach(looper, 0.002); + pwm_motor1.period_us(100); + motor1.setPosition(0); + pwm_motor2.period_us(100); + motor2.setPosition(0); + pc.baud(115200); + // Ticker EMG signaal meten + Ticker log_timer; + //set up filters. Use external array for constants + arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states); + arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states); + arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states); + arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states); + // Uitvoeren van ticker EMG, sample frequentie 500Hz + log_timer.attach(looper, 0.002); - // Aanroepen van motoraansturing in motor ticker - Ticker looptimer; - looptimer.attach(looper_motor,TSAMP); + // Aanroepen van motoraansturing in motor ticker + Ticker looptimer; + looptimer.attach(looper_motor,TSAMP); + + while(1) { while(1) { - - while(1) { - pc.printf("Span de biceps aan om het instellen te starten.\n"); - do { - ShineRed(); - } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting - if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen - BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking - BlinkGreen(); // groen knipperen, meten van spieraanspanning - while (1) { // eerste loop, keuze voor de positie van het batje - pc.printf("In de loop.\n"); - if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden - stopblinkgreen(); - pc.printf("ShineGreen.\n"); - ShineGreen(); - wait (4); - break; - } - if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links - stopblinkgreen(); - pc.printf("ShineBlue.\n"); - ShineBlue(); - batje_hoek = 2; - wait(4); - break; - } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts - stopblinkgreen(); - pc.printf("ShineRed.\n"); - ShineRed(); - batje_hoek = 1; - wait (4); - break; - } + pc.printf("Span de biceps aan om het instellen te starten.\n"); + do { + ShineRed(); + } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting + if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen + BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking + BlinkGreen(); // groen knipperen, meten van spieraanspanning + while (1) { // eerste loop, keuze voor de positie van het batje + pc.printf("In de loop.\n"); + if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden + stopblinkgreen(); + pc.printf("ShineGreen.\n"); + ShineGreen(); + wait (4); + break; } - BlinkGreen(); - while (1) { // loop voor het instellen van de kracht - pc.printf("In de loop.\n"); - if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan - stopblinkgreen(); - pc.printf("ShineGreen.\n"); - ShineGreen(); - arm_hoogte = 3; - wait (4); - break; - } - if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan - stopblinkgreen(); - pc.printf("ShineBlue.\n"); - ShineBlue(); - arm_hoogte = 1; - wait(4); - break; - } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan - stopblinkgreen(); - pc.printf("ShineRed.\n"); - ShineRed(); - arm_hoogte = 2; - wait (4); - break; - } + if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links + stopblinkgreen(); + pc.printf("ShineBlue.\n"); + ShineBlue(); + batje_hoek = 2; + wait(4); + break; + } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts + stopblinkgreen(); + pc.printf("ShineRed.\n"); + ShineRed(); + batje_hoek = 1; + wait (4); + break; } - + } + BlinkGreen(); + while (1) { // loop voor het instellen van de kracht + pc.printf("In de loop.\n"); + if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan + stopblinkgreen(); + pc.printf("ShineGreen.\n"); + ShineGreen(); + arm_hoogte = 3; + wait (4); + break; + } + if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan + stopblinkgreen(); + pc.printf("ShineBlue.\n"); + ShineBlue(); + arm_hoogte = 1; + wait(4); + break; + } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan + stopblinkgreen(); + pc.printf("ShineRed.\n"); + ShineRed(); + arm_hoogte = 2; + wait (4); + break; + } } } + } - } \ No newline at end of file + } +} \ No newline at end of file