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:
- 29:d0dab8921e9d
- Parent:
- 28:c33a0658605e
- Child:
- 30:7a0a3c272308
diff -r c33a0658605e -r d0dab8921e9d main.cpp --- a/main.cpp Sat Nov 01 16:19:53 2014 +0000 +++ b/main.cpp Mon Nov 03 15:37:48 2014 +0000 @@ -6,12 +6,11 @@ #include "PwmOut.h" #define TSAMP 0.005 -#define K_P1 (3.5) //voor motor1 is het 3.5 -#define K_I1 (0.01 *TSAMP) //voor motor1 is het 0.01 -#define K_P2 (0.7) -#define K_I2 (0.01 *TSAMP) +#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 I_LIMIT 1. -//#define PI 3.14159265 #define l_arm 0.5 #define M1_PWM PTC8 @@ -34,7 +33,7 @@ AnalogIn emg1(PTC2); //Analog input HIDScope scope(4); -//motor 25D +//motor1 25D Encoder motor1(PTD3,PTD5); //wit, geel PwmOut pwm_motor1(M2_PWM); DigitalOut motordir1(M2_DIR); @@ -45,8 +44,8 @@ DigitalOut motordir2(M1_DIR); // Motor variabelen -float pwm1_percentage = 0; -float pwm2_percentage = 0; +float pwm_out1 = 0; +float pwm_out2 = 0; int cur_pos_motor1; int prev_pos_motor1 = 0; int cur_pos_motor2; @@ -87,7 +86,7 @@ float filtered_average_bi; float filtered_average_del; - +//gemiddelde EMG waarden over 250 sample stappen void average_biceps(float filtered_biceps,float *average) { static float total=0; @@ -184,14 +183,6 @@ myledred= 0; myledblue =0; ledticker.attach(&greenblink,.5); - /* myled1 = 1; - myled2 = 1; - myled3 = 1; - wait(0.1); - myled1 = 0; - myled2 = 1; - myled3 = 1; - wait(0.1);*/ } void stopblinkgreen() @@ -228,11 +219,9 @@ // Motor1 = batje // Motor2 = arm -void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op -// maar de locatie van de variabele. +void clamp(float* in, float min, float max) //Clamp geeft een maximum en minimum limiet aan een functie { -*in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c - // *in = het getal dat staat op locatie van in --> waarde van new_pwm +*in > min ? /*(*/*in < max? : *in = max : *in = min; } // PI-regelaar motor1: batje @@ -273,326 +262,321 @@ void batje_links () { speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht) - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden, nu 11.3. 2.3 is de verhouding van de tanden - 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; -} - -// Motor1 rechts draaien -void batje_rechts () -{ - speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden - setpoint1 = (11.3*2.3*2.0*PI/360); + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; //bepalen van de setpoint + if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //Het eerste getal geeft een aantal graden weer, dus alleen dit hoeft aangepast te worden/ + setpoint1 = (11.3*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek. } if(setpoint1 < -(11.3*2.3*2.0*PI/360)) { setpoint1 = -(11.3*2.3*2.0*PI/360); } - pwm_motor1.write(abs(pwm1_percentage)); - prev_setpoint1 = setpoint1; - 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; - } -} - -//Motor1 na links draaien weer terug laten draaien naar beginstand -void batje_begin_links () -{ - speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden - 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; //positief is CCW, negatief CW (boven aanzicht) - setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden - 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 () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN -{ - speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht) - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden - 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 in het midden slaan -void arm_mid () -{ - speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht) - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden - 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; + 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; + } + } + +//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; + } + +//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; + } + +// 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; + } + } + +// 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; + } + } // Motor2 balletje op het laagst slaan -void arm_laag () -{ - speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht) - setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (155*2.0*PI/360)) { //setpoint in graden - setpoint2 = (155*2.0*PI/360); + 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; + } } - 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); + 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(setpoint2 < -(0.0*2.0*PI/360)) { + setpoint2 = -(0.0*2.0*PI/360); + } + prev_setpoint2 = setpoint2; } - if(setpoint2 < -(0.0*2.0*PI/360)) { - setpoint2 = -(0.0*2.0*PI/360); - } - prev_setpoint2 = setpoint2; -} // MOTOR aansturing -void looper_motor() -{ - //MOTOR1 - pc.printf("%d \r\n", motor1.getPosition()); - cur_pos_motor1 = motor1.getPosition(); - pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128 - pwm1_percentage = pid1(setpoint1, pos_motor1_rad); - if (pwm1_percentage < -1.0) { - pwm1_percentage = -1.0; - } - if (pwm1_percentage > 1.0) { - pwm1_percentage = 1.0; - } - pwm_motor1.write(abs(pwm1_percentage)); - if(pwm1_percentage > 0) { - motordir1 = 0; - } else { - motordir1 = 1; - } + 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; - //MOTOR2 - cur_pos_motor2 = motor2.getPosition(); - pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); - pwm2_percentage = pid2(setpoint2, pos_motor2_rad); // - if (pwm2_percentage < -1.0) { - pwm2_percentage = -1.0; - } - if (pwm2_percentage > 1.0) { - pwm2_percentage = 1.0; - } - pwm_motor2.write(abs(pwm2_percentage)); - if(pwm2_percentage > 0) { - motordir2 = 0; - } else { - motordir2 = 1; + 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(); + } + } + } + } - //STATES - - if (batje_hoek == 1) { - if(staat1 == 0) { - batje_rechts(); - wait_iterator1 = 0; - } else if(staat1 ==1) { - wait_iterator1++; - if(wait_iterator1 > 400) { - 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 > 400) { - 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; +// Hoofdprogramma, hierin staat de aansturing vd LED + int main() { - 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; + 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); - arm_begin(); - } - } - } - -} - - -// Hoofdprogramma, hierin staat de aansturing vd LED -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); - - // Aanroepen van motoraansturing in motor ticker - Ticker looptimer; - looptimer.attach(looper_motor,TSAMP); - - while(1) { + // Aanroepen van motoraansturing in motor ticker + Ticker looptimer; + looptimer.attach(looper_motor,TSAMP); 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; + + 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; + } } - 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; + } } - } - 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