the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of The_SLAP_5_1 by
main.cpp
- Committer:
- Daanmk
- Date:
- 2014-10-28
- Revision:
- 6:cb5daf35ba9b
- Parent:
- 5:5383d9a80307
- Child:
- 7:dac6b30d43b3
File content as of revision 6:cb5daf35ba9b:
/***************************************/ /* */ /* BRONCODE GROEP 5, MODULE 9, 2014 */ /* *****-THE SLAP-****** */ /* */ /* -Dominique Clevers */ /* -Rianne van Dommelen */ /* -Daan de Muinck Keizer */ /* -David den Houting */ /* -Marjolein Thijssen */ /***************************************/ #include "mbed.h" #include "HIDScope.h" #include "arm_math.h" #include "encoder.h" #include "MODSERIAL.h" #include "TextLCD.h" #define M2_PWM PTC8 //blauw #define M2_DIR PTC9 //groen #define M1_PWM PTA5 //kleine motor #define M1_DIR PTA4 //kleine motor #define TSAMP 0.005 // Sampletijd, 200Hz #define K_P_KM (14) #define K_I_KM (28 *TSAMP) #define K_D_KM (0.01 /TSAMP) #define K_P_GM (2.9) #define K_I_GM (0.3 *TSAMP) #define K_D_GM (0.003 /TSAMP) #define I_LIMIT 1. TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x4); // rs, e, d4-d7 ok Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor Encoder motor1(PTD5,PTA13);//geel,wit PwmOut pwm_motor1(M1_PWM); PwmOut pwm_motor2(M2_PWM); DigitalOut motordir2(M2_DIR); DigitalOut motordir1(M1_DIR); AnalogIn emg0(PTB0); //Biceps AnalogIn emg1(PTB1); //Triceps HIDScope scope(6); MODSERIAL pc(USBTX,USBRX,64,1024); float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0,pwm_to_motor1,max_value_biceps,min_value_biceps; //variable to store value in for biceps float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1,pwm_to_motor2,max_value_triceps,min_value_triceps,metingstatus; //variable to store value in for triceps arm_biquad_casd_df1_inst_f32 notch_biceps; arm_biquad_casd_df1_inst_f32 notch_triceps; float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch float notch_biceps_states[4]; //state values float notch_triceps_states[4]; arm_biquad_casd_df1_inst_f32 highpass_biceps; arm_biquad_casd_df1_inst_f32 highpass_triceps; //constants for 20Hz highpass float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189}; //state values float highpass_biceps_states[4]; float highpass_triceps_states[4]; //constants for 80Hz lowpass arm_biquad_casd_df1_inst_f32 lowpass_biceps; arm_biquad_casd_df1_inst_f32 lowpass_triceps; float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189}; //state values float lowpass_biceps_states[4]; float lowpass_triceps_states[4]; arm_biquad_casd_df1_inst_f32 envelop_biceps; arm_biquad_casd_df1_inst_f32 envelop_triceps; float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016}; float envelop_biceps_states[4]; float envelop_triceps_states[4]; enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES uint8_t state=RUST; volatile bool looptimerflag; void setlooptimerflag(void) { looptimerflag = true; } void clamp(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; } float pidkm(float setpointkm, float measurementkm) //PID Regelaar Kleine motor { float error_km; static float prev_error_km = 0; float out_p_km = 0; static float out_i_km = 0; float out_d_km = 0; error_km = setpointkm-measurementkm; out_p_km = error_km*K_P_KM; out_i_km += error_km*K_I_KM; out_d_km = (error_km-prev_error_km)*K_D_KM; clamp(&out_i_km,-I_LIMIT,I_LIMIT); prev_error_km = error_km; scope.set(1,out_p_km); scope.set(2,out_i_km); scope.set(3,out_d_km); return out_p_km + out_i_km + out_d_km; } float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor { float error_gm; static float prev_error_gm = 0; float out_p_gm = 0; static float out_i_gm = 0; float out_d_gm = 0; error_gm = setpointgm-measurementgm; out_p_gm = error_gm*K_P_GM; out_i_gm += error_gm*K_I_GM; out_d_gm = (error_gm-prev_error_gm)*K_D_GM; clamp(&out_i_gm,-I_LIMIT,I_LIMIT); prev_error_gm = error_gm; scope.set(1,out_p_gm); scope.set(2,out_i_gm); scope.set(3,out_d_gm); return out_p_gm + out_i_gm + out_d_gm; } void emgmeten(){ /*put raw emg value in emg_value*/ emg0_value_f32 = emg0.read(); emg1_value_f32 = emg1.read(); //process emg biceps arm_biquad_cascade_df1_f32(¬ch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 ); arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 ); arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 ); filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 ); //process emg triceps arm_biquad_cascade_df1_f32(¬ch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 ); arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 ); arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 ); filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 ); } int main() { while(1) { pc.baud(38400); //PC baud rate is 38400 bits/seconde Ticker emg_timer; emg_timer.attach(emgmeten, TSAMP); Ticker looptimer; looptimer.attach(setlooptimerflag,TSAMP); Timer tijdtimer; arm_biquad_cascade_df1_init_f32(¬ch_biceps,1 , notch_const, notch_biceps_states); arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states); arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states); arm_biquad_cascade_df1_init_f32(¬ch_triceps,1 , notch_const, notch_triceps_states); arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states); arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states); arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states); arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states); switch(state) { case RUST: { //Aanzetten lcd.cls(); lcd.locate(0,0); lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(" GROEP 5 "); wait(5); if (metingstatus<5); state = KALIBRATIE; if (metingstatus==5); state = RICHTEN; break; } case KALIBRATIE: { lcd.cls(); lcd.locate(0,0); lcd.printf("Kalibratie"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("1:BICEPS RUST"); //regel 2 LCD scherm wait(1); max_value_biceps=0; max_value_triceps=0; min_value_biceps=0; min_value_triceps=0; int metingstatus=1; if (metingstatus==1) { //BICEPS RUST METING lcd.cls(); lcd.locate(0,0); lcd.printf("BICEPS RUST"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm wait(1); tijdtimer.start(); if (envelop_emg0 > min_value_biceps) { min_value_biceps = envelop_emg0; } if (tijdtimer == 3 ) { tijdtimer.stop(); tijdtimer.reset(); metingstatus=2; } } if (metingstatus==2) { //TRICEPS RUST METING lcd.cls(); lcd.locate(0,0); lcd.printf("TRICEPS RUST"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm wait(1); tijdtimer.start(); if (envelop_emg1 > min_value_triceps) { min_value_triceps = envelop_emg1; } if (tijdtimer == 3 ) { tijdtimer.stop(); tijdtimer.reset(); metingstatus=3; } } if (metingstatus==3) { //BICEPS KRACHT METING lcd.cls(); lcd.locate(0,0); lcd.printf("BICEPS MAX"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm wait(1); tijdtimer.start(); if (envelop_emg0 > max_value_biceps) { max_value_biceps = envelop_emg0; } if (tijdtimer == 3 ) { tijdtimer.stop(); tijdtimer.reset(); metingstatus=4; } } if (metingstatus==4) {//TRICEPS KRACHT METING lcd.cls(); lcd.locate(0,0); lcd.printf("TRICEPS MAX"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm tijdtimer.start(); if (envelop_emg1 > max_value_triceps) { max_value_triceps = envelop_emg1; } if (tijdtimer == 3 ) { tijdtimer.stop(); tijdtimer.reset(); metingstatus=5; } } if (metingstatus==5) { lcd.cls(); lcd.locate(0,0); lcd.printf("Kalibratie OK"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(""); //regel 2 LCD scherm wait(1); state = RICHTEN; } break; } case RICHTEN: { //Batje richten lcd.cls(); lcd.locate(0,0); lcd.printf("RICHTEN"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("KIES GOAL"); //regel 2 LCD scherm int16_t setpointkm; float new_pwm_km; wait(1); int goalkeuze=1; //midden goal float kalibratiewaarde_biceps,kalibratiewaarde_triceps; kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps); tijdtimer.start(); if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_triceps<0.3){ goalkeuze = 0; //goalkeuze lcd.cls(); lcd.locate(0,0); lcd.printf("LINKER GOAL!"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(""); //regel 2 LCD scherm setpointkm = -241; new_pwm_km = pidkm(setpointkm, motor1.getPosition()); clamp(&new_pwm_km, -1,1); if(new_pwm_km < 0) motordir1 = 1; else motordir1 = 0; pwm_motor1.write(abs(new_pwm_km)); wait(1); state = SLAAN; //-241 encoder tics tov midden goal } if (kalibratiewaarde_triceps>0.3 && kalibratiewaarde_biceps<0.3) { goalkeuze = 2; //rechter goal lcd.cls(); lcd.locate(0,0); lcd.printf("RECHTER GOAL!"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(""); //regel 2 LCD scherm //+11 encoder tics tov midden goal setpointkm = 11; new_pwm_km = pidkm(setpointkm, motor1.getPosition()); clamp(&new_pwm_km, -1,1); if(new_pwm_km < 0) motordir1 = 1; else motordir1 = 0; pwm_motor1.write(abs(new_pwm_km)); wait(1); state = SLAAN; } if (kalibratiewaarde_biceps <0.3 && kalibratiewaarde_triceps<0.3) { goalkeuze=1; // middelste goal! lcd.cls(); lcd.locate(0,0); lcd.printf("MIDDEN GOAL!"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(""); //regel 2 LCD scherm //0 encoder tics. Ook start positie! setpointkm = 0; new_pwm_km = pidkm(setpointkm, motor1.getPosition()); clamp(&new_pwm_km, -1,1); if(new_pwm_km < 0) motordir1 = 1; else motordir1 = 0; pwm_motor1.write(abs(new_pwm_km)); wait(1); state = SLAAN; } if (tijdtimer == 5) { tijdtimer.stop(); tijdtimer.reset(); state = SLAAN; } break; } case SLAAN: { //Balletje slaan lcd.cls(); lcd.locate(0,0); lcd.printf("SLAAN!"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("KIES GOAL!"); //regel 2 LCD scherm wait(1); int16_t setpointgm; float new_pwm_gm; int goalhoogte=0; float kalibratiewaarde_biceps; kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){ goalhoogte = 0; //goalkeuze, onderste goal lcd.cls(); lcd.locate(0,0); lcd.printf("ONDERSTE GOAL"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm setpointgm = //onbekend; new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); clamp(&new_pwm_gm, -1,1); if(new_pwm_gm < 0) motordir2 = 0; else motordir2 = 1; pwm_motor2.write(abs(new_pwm_gm)); wait(2); state = RUST; } if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){ goalhoogte = 1; //goalkeuze, middelste goal lcd.cls(); lcd.locate(0,0); lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm setpointgm = //onbekend; new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); clamp(&new_pwm_gm, -1,1); if(new_pwm_gm < 0) motordir2 = 0; else motordir2 = 1; pwm_motor2.write(abs(new_pwm_gm)); wait(2); state = RUST; } if (kalibratiewaarde_biceps>0.6){ goalhoogte = 2; //goalkeuze, Bovenste goal lcd.cls(); lcd.locate(0,0); lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm setpointgm = //onbekend; new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); clamp(&new_pwm_gm, -1,1); if(new_pwm_gm < 0) motordir2 = 0; else motordir2 = 1; pwm_motor2.write(abs(new_pwm_gm)); wait(2); state = RUST; } state = RUST; break; } default: { state = RUST; } } //switch sate } //while } //int main