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:
- 21:674fafb6301d
- Parent:
- 20:d40b6cba4280
- Child:
- 22:14f5161d7d7b
diff -r d40b6cba4280 -r 674fafb6301d main.cpp --- a/main.cpp Wed Oct 29 15:08:30 2014 +0000 +++ b/main.cpp Fri Oct 31 16:01:22 2014 +0000 @@ -3,12 +3,16 @@ #include "arm_math.h" #include "MODSERIAL.h" #include "encoder.h" +#include "PwmOut.h" -#define TSAMP 0.01 -#define K_P (0.1) -#define K_I (0.03 *TSAMP) -#define K_D (0.001 /TSAMP) +#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 (3.5) +#define K_I2 (0.01 *TSAMP) #define I_LIMIT 1. +//#define PI 3.14159265 +#define l_arm 0.5 #define M1_PWM PTC8 #define M1_DIR PTC9 @@ -19,36 +23,41 @@ Serial pc(USBTX, USBRX); -DigitalOut myled1(LED_RED); -DigitalOut myled2(LED_GREEN); -DigitalOut myled3(LED_BLUE); +DigitalOut myledred(PTB3); +DigitalOut myledgreen(PTB1); +DigitalOut myledblue(PTB2); //Define objects -AnalogIn emg0(PTB1); //Analog input -AnalogIn emg1(PTB2); //Analog input +AnalogIn emg0(PTB0); //Analog input +AnalogIn emg1(PTC2); //Analog input HIDScope scope(2); //motor 25D -Encoder motor1(PTD3,PTD5,true); //wit, geel +Encoder motor1(PTD3,PTD5); //wit, geel PwmOut pwm_motor1(M2_PWM); DigitalOut motordir1(M2_DIR); //motor2 37D -Encoder motor2(PTD2, PTD0,true); //wit, geel +Encoder motor2(PTD2, PTD0); //wit, geel PwmOut pwm_motor2(M1_PWM); DigitalOut motordir2(M1_DIR); -float speed1; -float hoek1; -float speed2; -float hoek2; - -bool flip=false; - -void attime() -{ - flip = !flip; -} +float pwm1_percentage = 0; +float pwm2_percentage = 0; +int cur_pos_motor1; +int prev_pos_motor1 = 0; +int cur_pos_motor2; +int prev_pos_motor2 = 0; +float speed1_rad; +float speed2_rad; +float pos_motor1_rad; +float pos_motor2_rad; +int staat1 = 0; +int staat2 = 0; +float arm_hoogte; +float batje_hoek; +int wait_iterator1 = 0; +int wait_iterator2 = 0; // EMG @@ -63,7 +72,7 @@ arm_biquad_casd_df1_inst_f32 highnotch_biceps; arm_biquad_casd_df1_inst_f32 highnotch_deltoid; //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB -float highnotch_const[] = {0.956542835577484, -1.913085671154968, 0.956542835577484, 1.911196288237583, -0.914975054072353,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533}; +float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533}; //state values float highnotch_biceps_states[8]; float highnotch_deltoid_states[8]; @@ -161,29 +170,29 @@ void BlinkRed(int n) { for (int i=0; i<n; i++) { - myled1 = 1; - myled2 = 1; - myled3 = 1; + myledred = 0; + myledgreen = 0; + myledblue = 0; wait(0.1); - myled1 = 0; - myled2 = 1; - myled3 = 1; + myledred = 1; + myledgreen = 0; + myledblue = 0; wait(0.1); } } void greenblink() { - if(myled2.read()) - myled2 = 0; + if(myledgreen.read()) + myledgreen = 0; else - myled2 = 1; + myledgreen = 1; } void BlinkGreen() { - myled1= 1; - myled3 =1; + myledred= 0; + myledblue =0; ledticker.attach(&greenblink,.5); /* myled1 = 1; myled2 = 1; @@ -204,13 +213,13 @@ void BlinkGreen1 () { - myled1 = 1; - myled2 = 1; - myled3 = 1; + myledred = 0; + myledgreen = 0; + myledblue = 0; wait(0.1); - myled1 = 1; - myled2 = 0; - myled3 = 1; + myledred = 0; + myledgreen = 1; + myledblue = 0; wait(0.1); } @@ -218,125 +227,305 @@ void BlinkBlue(int n) { for (int i=0; i<n; i++) { - myled1 = 1; - myled2 = 1; - myled3 = 1; + myledred = 0; + myledgreen = 0; + myledblue = 0; wait(0.1); - myled1 = 1; - myled2 = 1; - myled3 = 0; + myledred = 0; + myledgreen = 0; + myledblue = 1; wait(0.1); } } void ShineGreen () { - myled1 = 1; - myled2 = 0; - myled3 = 1; + myledred = 0; + myledgreen = 1; + myledblue = 0; } void ShineBlue () { - myled1 = 1; - myled2 = 1; - myled3 = 0; + myledred = 0; + myledgreen = 0; + myledblue = 1; } void ShineRed () { - myled1 = 0; - myled2 = 1; - myled3 = 1; + myledred = 1; + myledgreen = 0; + myledblue = 0; } // MOTORFUNCTIES -void motor2_speed_low () +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. +{ +*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 +} + +float pid1(float setpoint1, float measurement1) { - wait(1); - speed2 = 1; - motordir2=1; - pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn. - wait(0.3); //naar 140 graden - pwm_motor2.write(0); //CCW - wait(1); - motordir2=0; - pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan. - wait(0.20); //balletje slaan, 160 graden - pwm_motor2.write(0); - wait(1); - motordir2=1; //CW - pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan. - wait(1); //terug naar begin positie, 20 graden - pwm_motor2.write(0); + float error1; + float out_p1 = 0; + static float out_i1 = 0; + error1 = (setpoint1 - measurement1); + out_p1 = error1*K_P1; + out_i1 += error1*K_I1; + clamp(&out_i1,-I_LIMIT,I_LIMIT); + return out_p1 + out_i1; } -void motor2_speed_mid () +float pid2(float setpoint2, float measurement2) { - wait(1); - speed2 = 1; - motordir2=1; - pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn. - wait(0.3); //naar 140 graden - pwm_motor2.write(0); //CCW - wait(1); - motordir2=0; - pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan. - wait(0.20); //balletje slaan, 160 graden - pwm_motor2.write(0); - wait(1); - motordir2=1; //CW - pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan. - wait(1); //terug naar begin positie, 20 graden - pwm_motor2.write(0); + float error2; + float out_p2 = 0; + static float out_i2 = 0; + error2 = (setpoint2 - measurement2); + out_p2 = error2*K_P2; + out_i2 += error2*K_I2; + clamp(&out_i2,-I_LIMIT,I_LIMIT); + return out_p2 + out_i2; +} +float prev_setpoint1 = 0; +float setpoint1 = 0; +float prev_setpoint2 = 0; +float setpoint2 = 0; + +void batje_links () +{ + speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht) + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; + if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden + setpoint1 = (180*2.3*2.0*PI/360); + } + if(setpoint1 < -(180*2.3*2.0*PI/360)) { + setpoint1 = -(180*2.3*2.0*PI/360); + } + prev_setpoint1 = setpoint1; } -void motor2_speed_high () +void batje_rechts () { - wait(1); - speed2 = 1; - motordir2=1; - pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn. - wait(0.3); //naar 140 graden - pwm_motor2.write(0); //CCW - wait(1); - motordir2=0; - pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan. - wait(0.20); //balletje slaan, 160 graden - pwm_motor2.write(0); - wait(1); - motordir2=1; //CW - pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan. - wait(1); //terug naar begin positie, 20 graden - pwm_motor2.write(0); -} - -void motor1_links() -{ - speed1 = 0.7; - hoek1 = 0.09; //in seconde - wait(1); - motordir1=0; //aangeven van directie (0 = CCW) - pwm_motor1.write(speed1); //snelheid van de motor - wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait - pwm_motor1.write(0); + speed1_rad = 3.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); + } + if(setpoint1 < -(360*2.3*2.0*PI/360)) { + setpoint1 = -(360*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) { + staat1 = 1; + } } -void motor1_rechts() +void batje_begin_links () +{ + speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; + if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden + setpoint1 = (180*2.3*2.0*PI/360); + } + if(setpoint1 < -(180*2.3*2.0*PI/360)) { + setpoint1 = -(180*2.3*2.0*PI/360); + } + prev_setpoint1 = setpoint1; +} + +void batje_begin_rechts () +{ + speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht) + setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; + if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden + setpoint1 = (180*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 arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN +{ + speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht) + setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; + if(setpoint2 > (360.0*2.0*PI/360)) { //setpoint in graden + setpoint2 = (360.0*2.0*PI/360); + } + if(setpoint2 < -(360.0*2.0*PI/360)) { + setpoint2 = -(360.0*2.0*PI/360); + } + prev_setpoint2 = setpoint2; + if(setpoint2 >= (360.0*2.0*PI/360)-0.1) { + staat2 = 1; + } +} + +void arm_mid () +{ + 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; + } +} + +void arm_laag () +{ + 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; + } +} + +void arm_begin () +{ + speed2_rad = -2.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; +} + +void looper_motor() { - speed1 = 0.7; - hoek1 = 0.09; //in seconde - wait(1); - motordir1=1; //aangeven van directie (1 = CW) - pwm_motor1.write(speed1); //snelheid van de motor - wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait - pwm_motor1.write(0); + //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; + } + + //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; + } + + + //STATES + + if (batje_hoek == 0) { + if(staat1 == 0) { + batje_rechts(); + wait_iterator1 = 0; + } else if(staat1 ==1) { + wait_iterator1++; + if(wait_iterator1 > 600) + staat1 = 2; + } else { + batje_begin_rechts(); + } + } + + if (batje_hoek == 1) { + if(staat1 == 0) { + batje_links(); + wait_iterator1 = 0; + } else if(staat1 ==1) { + wait_iterator1++; + if(wait_iterator1 > 600) + staat1 = 2; + } else { + batje_begin_links (); + } + } + + + if(arm_hoogte == 0) { + if(staat2 == 0) { + arm_laag(); + wait_iterator2 = 0; + } else if(staat2 == 1) { + wait_iterator2++; + if(wait_iterator2 > 200) + staat2 = 2; + } else { + arm_begin(); + } + } + if(arm_hoogte == 1) { + if(staat2 == 0) { + arm_mid(); + wait_iterator2 = 0; + } else if(staat2 == 1) { + wait_iterator2++; + if(wait_iterator2 > 200) + staat2 = 2; + } else { + arm_begin(); + } + } + if(arm_hoogte == 2) { + if(staat2 == 0) { + arm_hoog(); + wait_iterator2 = 0; + } else if(staat2 == 1) { + wait_iterator2++; + if(wait_iterator2 > 200) + staat2 = 2; + } else { + arm_begin(); + } + } } int main() { + + pwm_motor1.period_us(100); + motor1.setPosition(0); + pwm_motor2.period_us(100); + motor2.setPosition(0); pc.baud(115200); Ticker log_timer; @@ -350,12 +539,14 @@ * Please mind that the parentheses after looper are omitted when using attach. */ log_timer.attach(looper, 0.001); + + Ticker looptimer; + looptimer.attach(looper_motor,TSAMP); while(1) { //Loop /*Empty!*/ /*Everything is handled by the interrupt routine now!*/ while(1) { - static float count = 0; pc.printf("Span de biceps aan om het instellen te starten.\n"); do { ShineRed(); @@ -376,7 +567,7 @@ stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); - motor1_links(); + batje_hoek = 1; wait(4); break; } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) @@ -385,7 +576,7 @@ stopblinkgreen(); pc.printf("ShineRed.\n"); ShineRed(); - motor1_rechts(); + batje_hoek = 0; wait (4); break; } @@ -397,7 +588,7 @@ stopblinkgreen(); pc.printf("ShineGreen.\n"); ShineGreen(); - motor2_speed_mid (); + arm_hoogte = 2; wait (4); break; } @@ -405,7 +596,7 @@ stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); - motor2_speed_low (); + arm_hoogte = 0; wait(4); break; } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) @@ -414,7 +605,7 @@ stopblinkgreen(); pc.printf("ShineRed.\n"); ShineRed(); - motor2_speed_high (); + arm_hoogte = 1; wait (4); break; }