the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of The_SLAP_5_1 by
main.cpp@7:dac6b30d43b3, 2014-10-28 (annotated)
- Committer:
- Daanmk
- Date:
- Tue Oct 28 14:53:54 2014 +0000
- Revision:
- 7:dac6b30d43b3
- Parent:
- 6:cb5daf35ba9b
- Child:
- 8:aa27423f4a4a
volledig script V2 met nieuwe while loops, zonder return to home;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Daanmk | 0:b3809a8d9af1 | 1 | /***************************************/ |
Daanmk | 0:b3809a8d9af1 | 2 | /* */ |
Daanmk | 0:b3809a8d9af1 | 3 | /* BRONCODE GROEP 5, MODULE 9, 2014 */ |
Daanmk | 0:b3809a8d9af1 | 4 | /* *****-THE SLAP-****** */ |
Daanmk | 0:b3809a8d9af1 | 5 | /* */ |
Daanmk | 0:b3809a8d9af1 | 6 | /* -Dominique Clevers */ |
Daanmk | 0:b3809a8d9af1 | 7 | /* -Rianne van Dommelen */ |
Daanmk | 0:b3809a8d9af1 | 8 | /* -Daan de Muinck Keizer */ |
Daanmk | 0:b3809a8d9af1 | 9 | /* -David den Houting */ |
Daanmk | 0:b3809a8d9af1 | 10 | /* -Marjolein Thijssen */ |
Daanmk | 0:b3809a8d9af1 | 11 | /***************************************/ |
Daanmk | 0:b3809a8d9af1 | 12 | #include "mbed.h" |
Daanmk | 0:b3809a8d9af1 | 13 | #include "HIDScope.h" |
Daanmk | 0:b3809a8d9af1 | 14 | #include "arm_math.h" |
Daanmk | 0:b3809a8d9af1 | 15 | #include "encoder.h" |
Daanmk | 1:96cd4c9c5465 | 16 | #include "MODSERIAL.h" |
Daanmk | 2:3bf615031d7a | 17 | #include "TextLCD.h" |
Daanmk | 0:b3809a8d9af1 | 18 | |
Daanmk | 1:96cd4c9c5465 | 19 | #define M2_PWM PTC8 //blauw |
Daanmk | 1:96cd4c9c5465 | 20 | #define M2_DIR PTC9 //groen |
Daanmk | 1:96cd4c9c5465 | 21 | #define M1_PWM PTA5 //kleine motor |
Daanmk | 1:96cd4c9c5465 | 22 | #define M1_DIR PTA4 //kleine motor |
Daanmk | 0:b3809a8d9af1 | 23 | #define TSAMP 0.005 // Sampletijd, 200Hz |
Daanmk | 7:dac6b30d43b3 | 24 | #define K_P_KM (0.1) |
Daanmk | 7:dac6b30d43b3 | 25 | #define K_I_KM (0.03 *TSAMP) |
Daanmk | 7:dac6b30d43b3 | 26 | #define K_D_KM (0.001 /TSAMP) |
Daanmk | 6:cb5daf35ba9b | 27 | #define K_P_GM (2.9) |
Daanmk | 6:cb5daf35ba9b | 28 | #define K_I_GM (0.3 *TSAMP) |
Daanmk | 6:cb5daf35ba9b | 29 | #define K_D_GM (0.003 /TSAMP) |
Daanmk | 6:cb5daf35ba9b | 30 | #define I_LIMIT 1. |
Daanmk | 7:dac6b30d43b3 | 31 | #define RADTICKGM 0.003927 |
Daanmk | 7:dac6b30d43b3 | 32 | #define THETA0 6.85 |
Daanmk | 7:dac6b30d43b3 | 33 | #define THETA1 7.77 |
Daanmk | 7:dac6b30d43b3 | 34 | #define THETA2 9.21 |
Daanmk | 0:b3809a8d9af1 | 35 | |
Daanmk | 3:81a6009303a9 | 36 | 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 |
Daanmk | 2:3bf615031d7a | 37 | |
Daanmk | 1:96cd4c9c5465 | 38 | Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor |
Daanmk | 1:96cd4c9c5465 | 39 | Encoder motor1(PTD5,PTA13);//geel,wit |
Daanmk | 0:b3809a8d9af1 | 40 | PwmOut pwm_motor1(M1_PWM); |
Daanmk | 0:b3809a8d9af1 | 41 | PwmOut pwm_motor2(M2_PWM); |
Daanmk | 0:b3809a8d9af1 | 42 | DigitalOut motordir2(M2_DIR); |
Daanmk | 0:b3809a8d9af1 | 43 | DigitalOut motordir1(M1_DIR); |
Daanmk | 0:b3809a8d9af1 | 44 | AnalogIn emg0(PTB0); //Biceps |
Daanmk | 0:b3809a8d9af1 | 45 | AnalogIn emg1(PTB1); //Triceps |
Daanmk | 0:b3809a8d9af1 | 46 | HIDScope scope(6); |
Daanmk | 0:b3809a8d9af1 | 47 | |
Daanmk | 1:96cd4c9c5465 | 48 | MODSERIAL pc(USBTX,USBRX,64,1024); |
Daanmk | 1:96cd4c9c5465 | 49 | |
Daanmk | 0:b3809a8d9af1 | 50 | |
Daanmk | 4:a0b0c944846e | 51 | 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 |
Daanmk | 6:cb5daf35ba9b | 52 | 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 |
Daanmk | 0:b3809a8d9af1 | 53 | |
Daanmk | 1:96cd4c9c5465 | 54 | arm_biquad_casd_df1_inst_f32 notch_biceps; |
Daanmk | 1:96cd4c9c5465 | 55 | arm_biquad_casd_df1_inst_f32 notch_triceps; |
Daanmk | 1:96cd4c9c5465 | 56 | float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch |
Daanmk | 1:96cd4c9c5465 | 57 | float notch_biceps_states[4]; //state values |
Daanmk | 1:96cd4c9c5465 | 58 | float notch_triceps_states[4]; |
Daanmk | 0:b3809a8d9af1 | 59 | |
Daanmk | 1:96cd4c9c5465 | 60 | arm_biquad_casd_df1_inst_f32 highpass_biceps; |
Daanmk | 1:96cd4c9c5465 | 61 | arm_biquad_casd_df1_inst_f32 highpass_triceps; |
Daanmk | 1:96cd4c9c5465 | 62 | //constants for 20Hz highpass |
Daanmk | 1:96cd4c9c5465 | 63 | float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189}; |
Daanmk | 1:96cd4c9c5465 | 64 | //state values |
Daanmk | 1:96cd4c9c5465 | 65 | float highpass_biceps_states[4]; |
Daanmk | 1:96cd4c9c5465 | 66 | float highpass_triceps_states[4]; |
Daanmk | 1:96cd4c9c5465 | 67 | //constants for 80Hz lowpass |
Daanmk | 1:96cd4c9c5465 | 68 | arm_biquad_casd_df1_inst_f32 lowpass_biceps; |
Daanmk | 1:96cd4c9c5465 | 69 | arm_biquad_casd_df1_inst_f32 lowpass_triceps; |
Daanmk | 1:96cd4c9c5465 | 70 | float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189}; |
Daanmk | 1:96cd4c9c5465 | 71 | //state values |
Daanmk | 1:96cd4c9c5465 | 72 | float lowpass_biceps_states[4]; |
Daanmk | 1:96cd4c9c5465 | 73 | float lowpass_triceps_states[4]; |
Daanmk | 1:96cd4c9c5465 | 74 | |
Daanmk | 1:96cd4c9c5465 | 75 | arm_biquad_casd_df1_inst_f32 envelop_biceps; |
Daanmk | 1:96cd4c9c5465 | 76 | arm_biquad_casd_df1_inst_f32 envelop_triceps; |
Daanmk | 1:96cd4c9c5465 | 77 | float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016}; |
Daanmk | 1:96cd4c9c5465 | 78 | float envelop_biceps_states[4]; |
Daanmk | 1:96cd4c9c5465 | 79 | float envelop_triceps_states[4]; |
Daanmk | 1:96cd4c9c5465 | 80 | |
Daanmk | 5:5383d9a80307 | 81 | enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES |
Daanmk | 1:96cd4c9c5465 | 82 | uint8_t state=RUST; |
Daanmk | 1:96cd4c9c5465 | 83 | |
Daanmk | 7:dac6b30d43b3 | 84 | enum kalibratiestates {BICEPSMAX,TRICEPSMAX}; |
Daanmk | 7:dac6b30d43b3 | 85 | |
Daanmk | 3:81a6009303a9 | 86 | volatile bool looptimerflag; |
Daanmk | 3:81a6009303a9 | 87 | void setlooptimerflag(void) |
Daanmk | 3:81a6009303a9 | 88 | { |
Daanmk | 3:81a6009303a9 | 89 | looptimerflag = true; |
Daanmk | 3:81a6009303a9 | 90 | } |
Daanmk | 3:81a6009303a9 | 91 | |
Daanmk | 6:cb5daf35ba9b | 92 | void clamp(float * in, float min, float max) |
Daanmk | 6:cb5daf35ba9b | 93 | { |
Daanmk | 6:cb5daf35ba9b | 94 | *in > min ? *in < max? : *in = max: *in = min; |
Daanmk | 6:cb5daf35ba9b | 95 | } |
Daanmk | 3:81a6009303a9 | 96 | |
Daanmk | 6:cb5daf35ba9b | 97 | float pidkm(float setpointkm, float measurementkm) //PID Regelaar Kleine motor |
Daanmk | 6:cb5daf35ba9b | 98 | { |
Daanmk | 6:cb5daf35ba9b | 99 | float error_km; |
Daanmk | 6:cb5daf35ba9b | 100 | static float prev_error_km = 0; |
Daanmk | 6:cb5daf35ba9b | 101 | float out_p_km = 0; |
Daanmk | 6:cb5daf35ba9b | 102 | static float out_i_km = 0; |
Daanmk | 6:cb5daf35ba9b | 103 | float out_d_km = 0; |
Daanmk | 6:cb5daf35ba9b | 104 | error_km = setpointkm-measurementkm; |
Daanmk | 6:cb5daf35ba9b | 105 | out_p_km = error_km*K_P_KM; |
Daanmk | 6:cb5daf35ba9b | 106 | out_i_km += error_km*K_I_KM; |
Daanmk | 6:cb5daf35ba9b | 107 | out_d_km = (error_km-prev_error_km)*K_D_KM; |
Daanmk | 6:cb5daf35ba9b | 108 | clamp(&out_i_km,-I_LIMIT,I_LIMIT); |
Daanmk | 6:cb5daf35ba9b | 109 | prev_error_km = error_km; |
Daanmk | 6:cb5daf35ba9b | 110 | scope.set(1,out_p_km); |
Daanmk | 6:cb5daf35ba9b | 111 | scope.set(2,out_i_km); |
Daanmk | 6:cb5daf35ba9b | 112 | scope.set(3,out_d_km); |
Daanmk | 6:cb5daf35ba9b | 113 | return out_p_km + out_i_km + out_d_km; |
Daanmk | 6:cb5daf35ba9b | 114 | } |
Daanmk | 6:cb5daf35ba9b | 115 | |
Daanmk | 6:cb5daf35ba9b | 116 | float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor |
Daanmk | 6:cb5daf35ba9b | 117 | { |
Daanmk | 6:cb5daf35ba9b | 118 | float error_gm; |
Daanmk | 6:cb5daf35ba9b | 119 | static float prev_error_gm = 0; |
Daanmk | 6:cb5daf35ba9b | 120 | float out_p_gm = 0; |
Daanmk | 6:cb5daf35ba9b | 121 | static float out_i_gm = 0; |
Daanmk | 6:cb5daf35ba9b | 122 | float out_d_gm = 0; |
Daanmk | 6:cb5daf35ba9b | 123 | error_gm = setpointgm-measurementgm; |
Daanmk | 6:cb5daf35ba9b | 124 | out_p_gm = error_gm*K_P_GM; |
Daanmk | 6:cb5daf35ba9b | 125 | out_i_gm += error_gm*K_I_GM; |
Daanmk | 6:cb5daf35ba9b | 126 | out_d_gm = (error_gm-prev_error_gm)*K_D_GM; |
Daanmk | 6:cb5daf35ba9b | 127 | clamp(&out_i_gm,-I_LIMIT,I_LIMIT); |
Daanmk | 6:cb5daf35ba9b | 128 | prev_error_gm = error_gm; |
Daanmk | 6:cb5daf35ba9b | 129 | scope.set(1,out_p_gm); |
Daanmk | 6:cb5daf35ba9b | 130 | scope.set(2,out_i_gm); |
Daanmk | 6:cb5daf35ba9b | 131 | scope.set(3,out_d_gm); |
Daanmk | 6:cb5daf35ba9b | 132 | return out_p_gm + out_i_gm + out_d_gm; |
Daanmk | 6:cb5daf35ba9b | 133 | } |
Daanmk | 1:96cd4c9c5465 | 134 | void emgmeten(){ |
Daanmk | 0:b3809a8d9af1 | 135 | /*put raw emg value in emg_value*/ |
Daanmk | 0:b3809a8d9af1 | 136 | emg0_value_f32 = emg0.read(); |
Daanmk | 0:b3809a8d9af1 | 137 | emg1_value_f32 = emg1.read(); |
Daanmk | 0:b3809a8d9af1 | 138 | |
Daanmk | 0:b3809a8d9af1 | 139 | //process emg biceps |
Daanmk | 1:96cd4c9c5465 | 140 | arm_biquad_cascade_df1_f32(¬ch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 ); |
Daanmk | 1:96cd4c9c5465 | 141 | arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 142 | arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 143 | filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter |
Daanmk | 1:96cd4c9c5465 | 144 | arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 ); |
Daanmk | 0:b3809a8d9af1 | 145 | |
Daanmk | 0:b3809a8d9af1 | 146 | //process emg triceps |
Daanmk | 1:96cd4c9c5465 | 147 | arm_biquad_cascade_df1_f32(¬ch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 ); |
Daanmk | 1:96cd4c9c5465 | 148 | arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 149 | arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 150 | filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter |
Daanmk | 1:96cd4c9c5465 | 151 | arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 ); |
Daanmk | 0:b3809a8d9af1 | 152 | } |
Daanmk | 6:cb5daf35ba9b | 153 | |
Daanmk | 1:96cd4c9c5465 | 154 | |
Daanmk | 0:b3809a8d9af1 | 155 | int main() |
Daanmk | 0:b3809a8d9af1 | 156 | { |
Daanmk | 1:96cd4c9c5465 | 157 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
Daanmk | 1:96cd4c9c5465 | 158 | Ticker emg_timer; |
Daanmk | 1:96cd4c9c5465 | 159 | emg_timer.attach(emgmeten, TSAMP); |
Daanmk | 3:81a6009303a9 | 160 | Ticker looptimer; |
Daanmk | 3:81a6009303a9 | 161 | looptimer.attach(setlooptimerflag,TSAMP); |
Daanmk | 4:a0b0c944846e | 162 | Timer tijdtimer; |
Daanmk | 7:dac6b30d43b3 | 163 | Timer tijdslaan; |
Daanmk | 1:96cd4c9c5465 | 164 | arm_biquad_cascade_df1_init_f32(¬ch_biceps,1 , notch_const, notch_biceps_states); |
Daanmk | 1:96cd4c9c5465 | 165 | arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states); |
Daanmk | 1:96cd4c9c5465 | 166 | arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states); |
Daanmk | 1:96cd4c9c5465 | 167 | arm_biquad_cascade_df1_init_f32(¬ch_triceps,1 , notch_const, notch_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 168 | arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 169 | arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 170 | arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 171 | arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states); |
Daanmk | 7:dac6b30d43b3 | 172 | while(1) { |
Daanmk | 1:96cd4c9c5465 | 173 | switch(state) { |
Daanmk | 2:3bf615031d7a | 174 | case RUST: { //Aanzetten |
Daanmk | 6:cb5daf35ba9b | 175 | lcd.cls(); |
Daanmk | 6:cb5daf35ba9b | 176 | lcd.locate(0,0); |
Daanmk | 6:cb5daf35ba9b | 177 | lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm |
Daanmk | 6:cb5daf35ba9b | 178 | lcd.locate(0,1); |
Daanmk | 6:cb5daf35ba9b | 179 | lcd.printf(" GROEP 5 "); |
Daanmk | 6:cb5daf35ba9b | 180 | wait(5); |
Daanmk | 7:dac6b30d43b3 | 181 | state = KALIBRATIE; |
Daanmk | 1:96cd4c9c5465 | 182 | break; |
Daanmk | 1:96cd4c9c5465 | 183 | } |
Daanmk | 1:96cd4c9c5465 | 184 | |
Daanmk | 2:3bf615031d7a | 185 | case KALIBRATIE: { |
Daanmk | 3:81a6009303a9 | 186 | max_value_biceps=0; |
Daanmk | 3:81a6009303a9 | 187 | max_value_triceps=0; |
Daanmk | 7:dac6b30d43b3 | 188 | state = BICEPSMAX; |
Daanmk | 7:dac6b30d43b3 | 189 | switch(state) { |
Daanmk | 7:dac6b30d43b3 | 190 | case BICEPSMAX: { |
Daanmk | 7:dac6b30d43b3 | 191 | lcd.cls(); |
Daanmk | 7:dac6b30d43b3 | 192 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 193 | lcd.printf("Kalibratie"); //regel 1 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 194 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 195 | lcd.printf("1:BICEPS MAX"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 196 | wait(1); |
Daanmk | 7:dac6b30d43b3 | 197 | tijdtimer.start(); |
Daanmk | 7:dac6b30d43b3 | 198 | lcd.cls(); |
Daanmk | 7:dac6b30d43b3 | 199 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 200 | lcd.printf("Biceps meting"); //regel 1 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 201 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 202 | lcd.printf("Meting loopt!"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 203 | while (tijdtimer <= 3){ |
Daanmk | 7:dac6b30d43b3 | 204 | if (envelop_emg0 > max_value_biceps) { |
Daanmk | 7:dac6b30d43b3 | 205 | max_value_biceps = envelop_emg0; } |
Daanmk | 7:dac6b30d43b3 | 206 | } |
Daanmk | 7:dac6b30d43b3 | 207 | if (tijdtimer >= 3) { |
Daanmk | 7:dac6b30d43b3 | 208 | tijdtimer.stop(); |
Daanmk | 7:dac6b30d43b3 | 209 | tijdtimer.reset(); |
Daanmk | 7:dac6b30d43b3 | 210 | lcd.cls(); |
Daanmk | 7:dac6b30d43b3 | 211 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 212 | lcd.printf("Einde meting!"); //regel 1 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 213 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 214 | lcd.printf("waarde"); //METING WAARDE TOEVOEGEN |
Daanmk | 7:dac6b30d43b3 | 215 | wait(1); |
Daanmk | 7:dac6b30d43b3 | 216 | state = TRICEPSMAX; |
Daanmk | 7:dac6b30d43b3 | 217 | }//einde if statement |
Daanmk | 7:dac6b30d43b3 | 218 | break; |
Daanmk | 7:dac6b30d43b3 | 219 | }//einde case bicepsmax |
Daanmk | 7:dac6b30d43b3 | 220 | case TRICEPSMAX: { |
Daanmk | 7:dac6b30d43b3 | 221 | lcd.cls(); |
Daanmk | 7:dac6b30d43b3 | 222 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 223 | lcd.printf("Kalibratie"); //regel 1 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 224 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 225 | lcd.printf("2:TRICEPS MAX"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 226 | wait(1); |
Daanmk | 7:dac6b30d43b3 | 227 | tijdtimer.start(); |
Daanmk | 7:dac6b30d43b3 | 228 | lcd.cls(); |
Daanmk | 7:dac6b30d43b3 | 229 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 230 | lcd.printf("Triceps meting"); //regel 1 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 231 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 232 | lcd.printf("Meting loopt!"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 233 | while (tijdtimer <= 3){ |
Daanmk | 7:dac6b30d43b3 | 234 | if (envelop_emg1 > max_value_triceps) { |
Daanmk | 7:dac6b30d43b3 | 235 | max_value_triceps = envelop_emg1; } |
Daanmk | 7:dac6b30d43b3 | 236 | } |
Daanmk | 7:dac6b30d43b3 | 237 | if (tijdtimer >= 3) { |
Daanmk | 7:dac6b30d43b3 | 238 | tijdtimer.stop(); |
Daanmk | 7:dac6b30d43b3 | 239 | tijdtimer.reset(); |
Daanmk | 7:dac6b30d43b3 | 240 | lcd.cls(); |
Daanmk | 7:dac6b30d43b3 | 241 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 242 | lcd.printf("Einde meting!"); //regel 1 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 243 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 244 | lcd.printf("waarde"); //METING WAARDE TOEVOEGEN |
Daanmk | 7:dac6b30d43b3 | 245 | wait(1); |
Daanmk | 7:dac6b30d43b3 | 246 | state = RICHTEN; |
Daanmk | 7:dac6b30d43b3 | 247 | } //einde if statement |
Daanmk | 7:dac6b30d43b3 | 248 | break; |
Daanmk | 7:dac6b30d43b3 | 249 | } //einde case tricepsmax |
Daanmk | 7:dac6b30d43b3 | 250 | default: { |
Daanmk | 7:dac6b30d43b3 | 251 | state = BICEPSMAX; |
Daanmk | 7:dac6b30d43b3 | 252 | } //einde default |
Daanmk | 7:dac6b30d43b3 | 253 | } //einde switch states |
Daanmk | 2:3bf615031d7a | 254 | break; |
Daanmk | 7:dac6b30d43b3 | 255 | } // einde kalibratie case |
Daanmk | 1:96cd4c9c5465 | 256 | |
Daanmk | 2:3bf615031d7a | 257 | case RICHTEN: { //Batje richten |
Daanmk | 5:5383d9a80307 | 258 | lcd.cls(); |
Daanmk | 5:5383d9a80307 | 259 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 260 | lcd.printf("Richten"); //regel 1 LCD scherm |
Daanmk | 5:5383d9a80307 | 261 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 262 | lcd.printf("Kies goal!"); //regel 2 LCD scherm |
Daanmk | 6:cb5daf35ba9b | 263 | int16_t setpointkm; |
Daanmk | 6:cb5daf35ba9b | 264 | float new_pwm_km; |
Daanmk | 5:5383d9a80307 | 265 | wait(1); |
Daanmk | 5:5383d9a80307 | 266 | float kalibratiewaarde_biceps,kalibratiewaarde_triceps; |
Daanmk | 5:5383d9a80307 | 267 | kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT |
Daanmk | 5:5383d9a80307 | 268 | kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps); |
Daanmk | 5:5383d9a80307 | 269 | tijdtimer.start(); |
Daanmk | 7:dac6b30d43b3 | 270 | while( tijdtimer <= 3) { |
Daanmk | 7:dac6b30d43b3 | 271 | if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps < 0.3) { //linker goal! |
Daanmk | 7:dac6b30d43b3 | 272 | setpointkm = -168; |
Daanmk | 7:dac6b30d43b3 | 273 | new_pwm_km = pidkm(setpointkm, motor1.getPosition()); |
Daanmk | 7:dac6b30d43b3 | 274 | clamp(&new_pwm_km, -1,1); |
Daanmk | 7:dac6b30d43b3 | 275 | if(new_pwm_km < 0) |
Daanmk | 7:dac6b30d43b3 | 276 | motordir1 = 1; |
Daanmk | 7:dac6b30d43b3 | 277 | else |
Daanmk | 7:dac6b30d43b3 | 278 | motordir1 = 0; |
Daanmk | 7:dac6b30d43b3 | 279 | pwm_motor1.write(abs(new_pwm_km)); |
Daanmk | 7:dac6b30d43b3 | 280 | } |
Daanmk | 7:dac6b30d43b3 | 281 | if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal! |
Daanmk | 7:dac6b30d43b3 | 282 | setpointkm = 168; |
Daanmk | 7:dac6b30d43b3 | 283 | new_pwm_km = pidkm(setpointkm, motor1.getPosition()); |
Daanmk | 7:dac6b30d43b3 | 284 | clamp(&new_pwm_km, -1,1); |
Daanmk | 7:dac6b30d43b3 | 285 | if(new_pwm_km < 0) |
Daanmk | 7:dac6b30d43b3 | 286 | motordir1 = 1; |
Daanmk | 7:dac6b30d43b3 | 287 | else |
Daanmk | 7:dac6b30d43b3 | 288 | motordir1 = 0; |
Daanmk | 7:dac6b30d43b3 | 289 | pwm_motor1.write(abs(new_pwm_km)); |
Daanmk | 7:dac6b30d43b3 | 290 | } |
Daanmk | 7:dac6b30d43b3 | 291 | if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps < 0.3) { //middelste goal! |
Daanmk | 7:dac6b30d43b3 | 292 | setpointkm = 0; |
Daanmk | 7:dac6b30d43b3 | 293 | new_pwm_km = pidkm(setpointkm, motor1.getPosition()); |
Daanmk | 7:dac6b30d43b3 | 294 | clamp(&new_pwm_km, -1,1); |
Daanmk | 7:dac6b30d43b3 | 295 | if(new_pwm_km < 0) |
Daanmk | 7:dac6b30d43b3 | 296 | motordir1 = 1; |
Daanmk | 7:dac6b30d43b3 | 297 | else |
Daanmk | 7:dac6b30d43b3 | 298 | motordir1 = 0; |
Daanmk | 7:dac6b30d43b3 | 299 | pwm_motor1.write(abs(new_pwm_km)); |
Daanmk | 7:dac6b30d43b3 | 300 | } |
Daanmk | 5:5383d9a80307 | 301 | } |
Daanmk | 7:dac6b30d43b3 | 302 | if (tijdtimer >= 3) { |
Daanmk | 5:5383d9a80307 | 303 | tijdtimer.stop(); |
Daanmk | 5:5383d9a80307 | 304 | tijdtimer.reset(); |
Daanmk | 5:5383d9a80307 | 305 | state = SLAAN; |
Daanmk | 6:cb5daf35ba9b | 306 | } |
Daanmk | 5:5383d9a80307 | 307 | |
Daanmk | 1:96cd4c9c5465 | 308 | break; |
Daanmk | 1:96cd4c9c5465 | 309 | } |
Daanmk | 2:3bf615031d7a | 310 | case SLAAN: { //Balletje slaan |
Daanmk | 6:cb5daf35ba9b | 311 | lcd.cls(); |
Daanmk | 6:cb5daf35ba9b | 312 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 313 | lcd.printf("Slaan PingPong!"); //regel 1 LCD scherm |
Daanmk | 6:cb5daf35ba9b | 314 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 315 | lcd.printf("Kies goal!"); //regel 2 LCD scherm |
Daanmk | 3:81a6009303a9 | 316 | wait(1); |
Daanmk | 6:cb5daf35ba9b | 317 | int16_t setpointgm; |
Daanmk | 6:cb5daf35ba9b | 318 | float new_pwm_gm; |
Daanmk | 6:cb5daf35ba9b | 319 | float kalibratiewaarde_biceps; |
Daanmk | 7:dac6b30d43b3 | 320 | float theta; |
Daanmk | 6:cb5daf35ba9b | 321 | kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); |
Daanmk | 7:dac6b30d43b3 | 322 | tijdtimer.start(); |
Daanmk | 7:dac6b30d43b3 | 323 | while (tijdtimer <=3 ) { |
Daanmk | 7:dac6b30d43b3 | 324 | if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){ |
Daanmk | 6:cb5daf35ba9b | 325 | lcd.cls(); |
Daanmk | 6:cb5daf35ba9b | 326 | lcd.locate(0,0); |
Daanmk | 7:dac6b30d43b3 | 327 | lcd.printf("Onderste goal"); //regel 1 LCD scherm |
Daanmk | 6:cb5daf35ba9b | 328 | lcd.locate(0,1); |
Daanmk | 7:dac6b30d43b3 | 329 | lcd.printf("Daar gaat ie!"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 330 | tijdslaan.start(); |
Daanmk | 7:dac6b30d43b3 | 331 | theta=THETA0; |
Daanmk | 7:dac6b30d43b3 | 332 | setpointgm = (theta*tijdslaan/RADTICKGM); |
Daanmk | 6:cb5daf35ba9b | 333 | new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); |
Daanmk | 6:cb5daf35ba9b | 334 | clamp(&new_pwm_gm, -1,1); |
Daanmk | 6:cb5daf35ba9b | 335 | if(new_pwm_gm < 0) |
Daanmk | 6:cb5daf35ba9b | 336 | motordir2 = 0; |
Daanmk | 6:cb5daf35ba9b | 337 | else |
Daanmk | 6:cb5daf35ba9b | 338 | motordir2 = 1; |
Daanmk | 6:cb5daf35ba9b | 339 | pwm_motor2.write(abs(new_pwm_gm)); |
Daanmk | 6:cb5daf35ba9b | 340 | wait(2); |
Daanmk | 6:cb5daf35ba9b | 341 | state = RUST; |
Daanmk | 6:cb5daf35ba9b | 342 | } |
Daanmk | 7:dac6b30d43b3 | 343 | if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){ |
Daanmk | 6:cb5daf35ba9b | 344 | lcd.cls(); |
Daanmk | 6:cb5daf35ba9b | 345 | lcd.locate(0,0); |
Daanmk | 6:cb5daf35ba9b | 346 | lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm |
Daanmk | 6:cb5daf35ba9b | 347 | lcd.locate(0,1); |
Daanmk | 6:cb5daf35ba9b | 348 | lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 349 | theta=THETA1; |
Daanmk | 7:dac6b30d43b3 | 350 | setpointgm = ((theta*tijdslaan)/RADTICKGM); |
Daanmk | 6:cb5daf35ba9b | 351 | new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); |
Daanmk | 6:cb5daf35ba9b | 352 | clamp(&new_pwm_gm, -1,1); |
Daanmk | 6:cb5daf35ba9b | 353 | if(new_pwm_gm < 0) |
Daanmk | 6:cb5daf35ba9b | 354 | motordir2 = 0; |
Daanmk | 6:cb5daf35ba9b | 355 | else |
Daanmk | 6:cb5daf35ba9b | 356 | motordir2 = 1; |
Daanmk | 6:cb5daf35ba9b | 357 | pwm_motor2.write(abs(new_pwm_gm)); |
Daanmk | 6:cb5daf35ba9b | 358 | wait(2); |
Daanmk | 6:cb5daf35ba9b | 359 | state = RUST; |
Daanmk | 6:cb5daf35ba9b | 360 | } |
Daanmk | 7:dac6b30d43b3 | 361 | if (kalibratiewaarde_biceps>0.6){ |
Daanmk | 6:cb5daf35ba9b | 362 | lcd.cls(); |
Daanmk | 6:cb5daf35ba9b | 363 | lcd.locate(0,0); |
Daanmk | 6:cb5daf35ba9b | 364 | lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm |
Daanmk | 6:cb5daf35ba9b | 365 | lcd.locate(0,1); |
Daanmk | 6:cb5daf35ba9b | 366 | lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm |
Daanmk | 7:dac6b30d43b3 | 367 | theta=THETA2; |
Daanmk | 7:dac6b30d43b3 | 368 | setpointgm = ((theta*tijdslaan)/RADTICKGM); |
Daanmk | 6:cb5daf35ba9b | 369 | new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); |
Daanmk | 6:cb5daf35ba9b | 370 | clamp(&new_pwm_gm, -1,1); |
Daanmk | 6:cb5daf35ba9b | 371 | if(new_pwm_gm < 0) |
Daanmk | 6:cb5daf35ba9b | 372 | motordir2 = 0; |
Daanmk | 6:cb5daf35ba9b | 373 | else |
Daanmk | 6:cb5daf35ba9b | 374 | motordir2 = 1; |
Daanmk | 6:cb5daf35ba9b | 375 | pwm_motor2.write(abs(new_pwm_gm)); |
Daanmk | 6:cb5daf35ba9b | 376 | wait(2); |
Daanmk | 6:cb5daf35ba9b | 377 | state = RUST; |
Daanmk | 6:cb5daf35ba9b | 378 | |
Daanmk | 6:cb5daf35ba9b | 379 | } |
Daanmk | 7:dac6b30d43b3 | 380 | } //einde whilelus |
Daanmk | 7:dac6b30d43b3 | 381 | if (tijdtimer >=3 ) { |
Daanmk | 7:dac6b30d43b3 | 382 | tijdtimer.stop(); |
Daanmk | 7:dac6b30d43b3 | 383 | tijdtimer.reset(); |
Daanmk | 7:dac6b30d43b3 | 384 | |
Daanmk | 7:dac6b30d43b3 | 385 | //+ TERUGKEREN BEGINPOSITIE! |
Daanmk | 7:dac6b30d43b3 | 386 | state = RUST; |
Daanmk | 7:dac6b30d43b3 | 387 | } //einde if statement |
Daanmk | 6:cb5daf35ba9b | 388 | |
Daanmk | 6:cb5daf35ba9b | 389 | |
Daanmk | 6:cb5daf35ba9b | 390 | state = RUST; |
Daanmk | 1:96cd4c9c5465 | 391 | break; |
Daanmk | 1:96cd4c9c5465 | 392 | } |
Daanmk | 1:96cd4c9c5465 | 393 | |
Daanmk | 3:81a6009303a9 | 394 | default: { |
Daanmk | 1:96cd4c9c5465 | 395 | state = RUST; |
Daanmk | 2:3bf615031d7a | 396 | } |
Daanmk | 1:96cd4c9c5465 | 397 | |
Daanmk | 6:cb5daf35ba9b | 398 | } //switch sate |
Daanmk | 1:96cd4c9c5465 | 399 | |
Daanmk | 6:cb5daf35ba9b | 400 | } //while |
Daanmk | 6:cb5daf35ba9b | 401 | } //int main |
Daanmk | 5:5383d9a80307 | 402 |