voorlopige script getest (posities nog toevoegen)
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
main.cpp@4:527e5b5283c2, 2014-10-31 (annotated)
- Committer:
- DominiqueC
- Date:
- Fri Oct 31 16:52:26 2014 +0000
- Revision:
- 4:527e5b5283c2
- Parent:
- 3:b06ada67fa4f
- Child:
- 5:5085197c02be
Eindscript nog hangend in Home case;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DominiqueC | 0:653af6a8a659 | 1 | /***************************************/ |
DominiqueC | 0:653af6a8a659 | 2 | /* */ |
DominiqueC | 0:653af6a8a659 | 3 | /* BRONCODE GROEP 5, MODULE 9, 2014 */ |
DominiqueC | 0:653af6a8a659 | 4 | /* *****-THE SLAP-****** */ |
DominiqueC | 0:653af6a8a659 | 5 | /* */ |
DominiqueC | 0:653af6a8a659 | 6 | /* -Dominique Clevers */ |
DominiqueC | 0:653af6a8a659 | 7 | /* -Rianne van Dommelen */ |
DominiqueC | 0:653af6a8a659 | 8 | /* -Daan de Muinck Keizer */ |
DominiqueC | 0:653af6a8a659 | 9 | /* -David den Houting */ |
DominiqueC | 0:653af6a8a659 | 10 | /* -Marjolein Thijssen */ |
DominiqueC | 0:653af6a8a659 | 11 | /***************************************/ |
DominiqueC | 0:653af6a8a659 | 12 | #include "mbed.h" |
DominiqueC | 0:653af6a8a659 | 13 | #include "arm_math.h" |
DominiqueC | 0:653af6a8a659 | 14 | #include "encoder.h" |
DominiqueC | 0:653af6a8a659 | 15 | #include "MODSERIAL.h" |
DominiqueC | 0:653af6a8a659 | 16 | //include "TextLCD.h" |
DominiqueC | 0:653af6a8a659 | 17 | |
DominiqueC | 4:527e5b5283c2 | 18 | #define M2_PWM PTC8 //kleine motor |
DominiqueC | 4:527e5b5283c2 | 19 | #define M2_DIR PTC9 //kleine motor |
DominiqueC | 4:527e5b5283c2 | 20 | #define M1_PWM PTA5 //grote motor |
DominiqueC | 4:527e5b5283c2 | 21 | #define M1_DIR PTA4 //grote motor |
DominiqueC | 0:653af6a8a659 | 22 | #define TSAMP 0.005 // Sampletijd, 200Hz |
DominiqueC | 3:b06ada67fa4f | 23 | #define K_P_KM (0.01) |
DominiqueC | 3:b06ada67fa4f | 24 | #define K_I_KM (0.0003 *TSAMP) |
DominiqueC | 3:b06ada67fa4f | 25 | #define K_D_KM (0.0 /TSAMP) |
DominiqueC | 3:b06ada67fa4f | 26 | #define K_P_GM (0.0022) |
DominiqueC | 3:b06ada67fa4f | 27 | #define K_I_GM (0.0001 *TSAMP) |
DominiqueC | 3:b06ada67fa4f | 28 | #define K_D_GM (0.00000001 /TSAMP) |
DominiqueC | 0:653af6a8a659 | 29 | #define I_LIMIT 1. |
DominiqueC | 0:653af6a8a659 | 30 | #define RADTICKGM 0.003927 |
DominiqueC | 0:653af6a8a659 | 31 | #define THETADOT0 6.85 |
DominiqueC | 0:653af6a8a659 | 32 | #define THETADOT1 7.77 |
DominiqueC | 0:653af6a8a659 | 33 | #define THETADOT2 9.21 |
DominiqueC | 0:653af6a8a659 | 34 | |
DominiqueC | 0:653af6a8a659 | 35 | //TextLCD pc(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //Textpc pc(p15, p16, p17, p18, p19, p20, Textpc::pc16x4); // rs, e, d4-d7 ok |
DominiqueC | 0:653af6a8a659 | 36 | |
DominiqueC | 2:8596695c56df | 37 | Encoder motor2(PTD2,PTD0); //geel,wit kleine motor MOTOR 2 |
DominiqueC | 0:653af6a8a659 | 38 | Encoder motor1(PTD5,PTA13);//geel,wit |
DominiqueC | 0:653af6a8a659 | 39 | PwmOut pwm_motor1(M1_PWM); |
DominiqueC | 0:653af6a8a659 | 40 | PwmOut pwm_motor2(M2_PWM); |
DominiqueC | 0:653af6a8a659 | 41 | DigitalOut motordir2(M2_DIR); |
DominiqueC | 0:653af6a8a659 | 42 | DigitalOut motordir1(M1_DIR); |
DominiqueC | 0:653af6a8a659 | 43 | AnalogIn emg0(PTB0); //Biceps |
DominiqueC | 0:653af6a8a659 | 44 | AnalogIn emg1(PTB1); //Triceps |
DominiqueC | 0:653af6a8a659 | 45 | |
DominiqueC | 0:653af6a8a659 | 46 | MODSERIAL pc(USBTX,USBRX,64,1024); |
DominiqueC | 0:653af6a8a659 | 47 | |
DominiqueC | 0:653af6a8a659 | 48 | |
DominiqueC | 0:653af6a8a659 | 49 | 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 |
DominiqueC | 0:653af6a8a659 | 50 | 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 |
DominiqueC | 0:653af6a8a659 | 51 | |
DominiqueC | 0:653af6a8a659 | 52 | arm_biquad_casd_df1_inst_f32 notch_biceps; |
DominiqueC | 0:653af6a8a659 | 53 | arm_biquad_casd_df1_inst_f32 notch_triceps; |
DominiqueC | 0:653af6a8a659 | 54 | // constants for 50 Hz notch (bandbreedte 2 Hz) |
DominiqueC | 0:653af6a8a659 | 55 | float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch |
DominiqueC | 0:653af6a8a659 | 56 | //state values |
DominiqueC | 0:653af6a8a659 | 57 | float notch_biceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 58 | float notch_triceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 59 | |
DominiqueC | 0:653af6a8a659 | 60 | arm_biquad_casd_df1_inst_f32 highpass_biceps; |
DominiqueC | 0:653af6a8a659 | 61 | arm_biquad_casd_df1_inst_f32 highpass_triceps; |
DominiqueC | 0:653af6a8a659 | 62 | //constants for 20Hz highpass |
DominiqueC | 0:653af6a8a659 | 63 | float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189}; |
DominiqueC | 0:653af6a8a659 | 64 | //state values |
DominiqueC | 0:653af6a8a659 | 65 | float highpass_biceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 66 | float highpass_triceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 67 | |
DominiqueC | 0:653af6a8a659 | 68 | arm_biquad_casd_df1_inst_f32 lowpass_biceps; |
DominiqueC | 0:653af6a8a659 | 69 | arm_biquad_casd_df1_inst_f32 lowpass_triceps; |
DominiqueC | 0:653af6a8a659 | 70 | //constants for 80Hz lowpass |
DominiqueC | 0:653af6a8a659 | 71 | float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189}; |
DominiqueC | 0:653af6a8a659 | 72 | //state values |
DominiqueC | 0:653af6a8a659 | 73 | float lowpass_biceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 74 | float lowpass_triceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 75 | |
DominiqueC | 0:653af6a8a659 | 76 | arm_biquad_casd_df1_inst_f32 envelop_biceps; |
DominiqueC | 0:653af6a8a659 | 77 | arm_biquad_casd_df1_inst_f32 envelop_triceps; |
DominiqueC | 0:653af6a8a659 | 78 | //constants for envelop |
DominiqueC | 0:653af6a8a659 | 79 | float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016}; |
DominiqueC | 0:653af6a8a659 | 80 | // state values |
DominiqueC | 0:653af6a8a659 | 81 | float envelop_biceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 82 | float envelop_triceps_states[4]; |
DominiqueC | 0:653af6a8a659 | 83 | |
DominiqueC | 0:653af6a8a659 | 84 | enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES |
DominiqueC | 0:653af6a8a659 | 85 | uint8_t state=RUST; |
DominiqueC | 0:653af6a8a659 | 86 | |
DominiqueC | 0:653af6a8a659 | 87 | volatile bool looptimerflag; |
DominiqueC | 0:653af6a8a659 | 88 | void setlooptimerflag(void) |
DominiqueC | 0:653af6a8a659 | 89 | { |
DominiqueC | 0:653af6a8a659 | 90 | looptimerflag = true; |
DominiqueC | 0:653af6a8a659 | 91 | } |
DominiqueC | 0:653af6a8a659 | 92 | |
DominiqueC | 0:653af6a8a659 | 93 | void clamp(float * in, float min, float max) |
DominiqueC | 0:653af6a8a659 | 94 | { |
DominiqueC | 0:653af6a8a659 | 95 | *in > min ? *in < max? : *in = max: *in = min; |
DominiqueC | 0:653af6a8a659 | 96 | } |
DominiqueC | 0:653af6a8a659 | 97 | |
DominiqueC | 0:653af6a8a659 | 98 | float pidkm(float setpointkm, float measurementkm) //PID Regelaar kleine motor |
DominiqueC | 0:653af6a8a659 | 99 | { |
DominiqueC | 0:653af6a8a659 | 100 | float error_km; |
DominiqueC | 0:653af6a8a659 | 101 | static float prev_error_km = 0; |
DominiqueC | 0:653af6a8a659 | 102 | float out_p_km = 0; |
DominiqueC | 0:653af6a8a659 | 103 | static float out_i_km = 0; //static, want dan wordt vorige waarde onthouden |
DominiqueC | 0:653af6a8a659 | 104 | float out_d_km = 0; |
DominiqueC | 0:653af6a8a659 | 105 | error_km = setpointkm-measurementkm; |
DominiqueC | 0:653af6a8a659 | 106 | out_p_km = error_km*K_P_KM; |
DominiqueC | 0:653af6a8a659 | 107 | out_i_km += error_km*K_I_KM; |
DominiqueC | 0:653af6a8a659 | 108 | out_d_km = (error_km-prev_error_km)*K_D_KM; |
DominiqueC | 0:653af6a8a659 | 109 | clamp(&out_i_km,-I_LIMIT,I_LIMIT); |
DominiqueC | 0:653af6a8a659 | 110 | prev_error_km = error_km; |
DominiqueC | 0:653af6a8a659 | 111 | return out_p_km + out_i_km + out_d_km; |
DominiqueC | 0:653af6a8a659 | 112 | } |
DominiqueC | 0:653af6a8a659 | 113 | |
DominiqueC | 0:653af6a8a659 | 114 | float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor |
DominiqueC | 0:653af6a8a659 | 115 | { |
DominiqueC | 0:653af6a8a659 | 116 | float error_gm; |
DominiqueC | 0:653af6a8a659 | 117 | static float prev_error_gm = 0; |
DominiqueC | 0:653af6a8a659 | 118 | float out_p_gm = 0; |
DominiqueC | 0:653af6a8a659 | 119 | static float out_i_gm = 0; |
DominiqueC | 0:653af6a8a659 | 120 | float out_d_gm = 0; |
DominiqueC | 0:653af6a8a659 | 121 | error_gm = setpointgm-measurementgm; |
DominiqueC | 0:653af6a8a659 | 122 | out_p_gm = error_gm*K_P_GM; |
DominiqueC | 0:653af6a8a659 | 123 | out_i_gm += error_gm*K_I_GM; |
DominiqueC | 0:653af6a8a659 | 124 | out_d_gm = (error_gm-prev_error_gm)*K_D_GM; |
DominiqueC | 0:653af6a8a659 | 125 | clamp(&out_i_gm,-I_LIMIT,I_LIMIT); |
DominiqueC | 0:653af6a8a659 | 126 | prev_error_gm = error_gm; |
DominiqueC | 0:653af6a8a659 | 127 | return out_p_gm + out_i_gm + out_d_gm; |
DominiqueC | 0:653af6a8a659 | 128 | } |
DominiqueC | 0:653af6a8a659 | 129 | void emgmeten() |
DominiqueC | 0:653af6a8a659 | 130 | { |
DominiqueC | 0:653af6a8a659 | 131 | /*put raw emg value in emg_value*/ |
DominiqueC | 0:653af6a8a659 | 132 | emg0_value_f32 = emg0.read(); |
DominiqueC | 0:653af6a8a659 | 133 | emg1_value_f32 = emg1.read(); |
DominiqueC | 0:653af6a8a659 | 134 | |
DominiqueC | 0:653af6a8a659 | 135 | //process emg biceps |
DominiqueC | 0:653af6a8a659 | 136 | arm_biquad_cascade_df1_f32(¬ch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 ); |
DominiqueC | 0:653af6a8a659 | 137 | arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 ); |
DominiqueC | 0:653af6a8a659 | 138 | arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 ); |
DominiqueC | 0:653af6a8a659 | 139 | filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter |
DominiqueC | 0:653af6a8a659 | 140 | arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 ); |
DominiqueC | 0:653af6a8a659 | 141 | |
DominiqueC | 0:653af6a8a659 | 142 | //process emg triceps |
DominiqueC | 0:653af6a8a659 | 143 | arm_biquad_cascade_df1_f32(¬ch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 ); |
DominiqueC | 0:653af6a8a659 | 144 | arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 ); |
DominiqueC | 0:653af6a8a659 | 145 | arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 ); |
DominiqueC | 0:653af6a8a659 | 146 | filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter |
DominiqueC | 0:653af6a8a659 | 147 | arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 ); |
DominiqueC | 0:653af6a8a659 | 148 | } |
DominiqueC | 0:653af6a8a659 | 149 | |
DominiqueC | 0:653af6a8a659 | 150 | |
DominiqueC | 0:653af6a8a659 | 151 | int main() |
DominiqueC | 0:653af6a8a659 | 152 | { |
DominiqueC | 0:653af6a8a659 | 153 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
DominiqueC | 0:653af6a8a659 | 154 | Ticker emg_timer; |
DominiqueC | 0:653af6a8a659 | 155 | emg_timer.attach(emgmeten, TSAMP); |
DominiqueC | 0:653af6a8a659 | 156 | Ticker looptimer; |
DominiqueC | 0:653af6a8a659 | 157 | looptimer.attach(setlooptimerflag,TSAMP); |
DominiqueC | 0:653af6a8a659 | 158 | Timer tijdtimer; |
DominiqueC | 0:653af6a8a659 | 159 | Timer tijdslaan; |
DominiqueC | 3:b06ada67fa4f | 160 | tijdtimer.start(); |
DominiqueC | 3:b06ada67fa4f | 161 | tijdslaan.start(); |
DominiqueC | 0:653af6a8a659 | 162 | arm_biquad_cascade_df1_init_f32(¬ch_biceps,1 , notch_const, notch_biceps_states); |
DominiqueC | 0:653af6a8a659 | 163 | arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states); |
DominiqueC | 0:653af6a8a659 | 164 | arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states); |
DominiqueC | 0:653af6a8a659 | 165 | arm_biquad_cascade_df1_init_f32(¬ch_triceps,1 , notch_const, notch_triceps_states); |
DominiqueC | 0:653af6a8a659 | 166 | arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states); |
DominiqueC | 0:653af6a8a659 | 167 | arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states); |
DominiqueC | 0:653af6a8a659 | 168 | arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states); |
DominiqueC | 0:653af6a8a659 | 169 | arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states); |
DominiqueC | 3:b06ada67fa4f | 170 | |
DominiqueC | 0:653af6a8a659 | 171 | while(true) { |
DominiqueC | 0:653af6a8a659 | 172 | switch(state) { |
DominiqueC | 0:653af6a8a659 | 173 | case RUST: { //Aanzetten |
DominiqueC | 0:653af6a8a659 | 174 | pc.printf("--THE SLAP -- GROEP 5"); //pc scherm |
DominiqueC | 0:653af6a8a659 | 175 | wait(5); |
DominiqueC | 0:653af6a8a659 | 176 | state = KALIBRATIE; |
DominiqueC | 0:653af6a8a659 | 177 | break; |
DominiqueC | 0:653af6a8a659 | 178 | } |
DominiqueC | 0:653af6a8a659 | 179 | |
DominiqueC | 0:653af6a8a659 | 180 | case KALIBRATIE: { //kalibreren met maximale inspanning |
DominiqueC | 0:653af6a8a659 | 181 | max_value_biceps=0; |
DominiqueC | 0:653af6a8a659 | 182 | max_value_triceps=0; |
DominiqueC | 2:8596695c56df | 183 | //maximale inspanning biceps |
DominiqueC | 4:527e5b5283c2 | 184 | pc.printf("Kalibratie1: Span Biceps!"); //pc scherm |
DominiqueC | 4:527e5b5283c2 | 185 | wait(5); |
DominiqueC | 4:527e5b5283c2 | 186 | pc.printf("Meting loopt"); //pc scherm |
DominiqueC | 4:527e5b5283c2 | 187 | tijdtimer.reset(); |
DominiqueC | 2:8596695c56df | 188 | tijdtimer.start(); |
DominiqueC | 4:527e5b5283c2 | 189 | while (tijdtimer.read() <= 3) { |
DominiqueC | 4:527e5b5283c2 | 190 | if (envelop_emg0 > max_value_biceps) { |
DominiqueC | 2:8596695c56df | 191 | max_value_biceps = envelop_emg0; |
DominiqueC | 2:8596695c56df | 192 | } |
DominiqueC | 2:8596695c56df | 193 | } |
DominiqueC | 2:8596695c56df | 194 | tijdtimer.stop(); |
DominiqueC | 2:8596695c56df | 195 | tijdtimer.reset(); |
DominiqueC | 4:527e5b5283c2 | 196 | pc.printf("max value %f\n\r", max_value_biceps); |
DominiqueC | 4:527e5b5283c2 | 197 | wait(5); |
DominiqueC | 2:8596695c56df | 198 | |
DominiqueC | 2:8596695c56df | 199 | //maximale inspanning triceps |
DominiqueC | 4:527e5b5283c2 | 200 | pc.printf("Kalibratie2: Span Triceps!"); //pc scherm |
DominiqueC | 4:527e5b5283c2 | 201 | wait(5); |
DominiqueC | 2:8596695c56df | 202 | tijdtimer.start(); |
DominiqueC | 4:527e5b5283c2 | 203 | pc.printf("Meting loopt"); //pc scherm |
DominiqueC | 4:527e5b5283c2 | 204 | while (tijdtimer.read() <= 3) { |
DominiqueC | 2:8596695c56df | 205 | if (envelop_emg1 > max_value_triceps) { |
DominiqueC | 2:8596695c56df | 206 | max_value_triceps = envelop_emg1; |
DominiqueC | 2:8596695c56df | 207 | } |
DominiqueC | 2:8596695c56df | 208 | } |
DominiqueC | 4:527e5b5283c2 | 209 | // tijdtimer.stop(); |
DominiqueC | 2:8596695c56df | 210 | tijdtimer.reset(); |
DominiqueC | 2:8596695c56df | 211 | pc.printf("max value %f\n\r", max_value_triceps); |
DominiqueC | 4:527e5b5283c2 | 212 | wait(5); |
DominiqueC | 4:527e5b5283c2 | 213 | |
DominiqueC | 2:8596695c56df | 214 | state = RICHTEN; |
DominiqueC | 0:653af6a8a659 | 215 | break; |
DominiqueC | 1:0d5864272412 | 216 | }// einde kalibratie case |
DominiqueC | 2:8596695c56df | 217 | |
DominiqueC | 2:8596695c56df | 218 | case RICHTEN: { //batje richten (gebruik biceps en triceps) |
DominiqueC | 2:8596695c56df | 219 | pc.printf("Richten"); //regel 1 LCD scherm |
DominiqueC | 2:8596695c56df | 220 | pc.printf("Kies goal!"); //regel 2 LCD scherm |
DominiqueC | 2:8596695c56df | 221 | float setpointkm; |
DominiqueC | 2:8596695c56df | 222 | float new_pwm_km; |
DominiqueC | 2:8596695c56df | 223 | wait(3); |
DominiqueC | 2:8596695c56df | 224 | float kalibratiewaarde_biceps,kalibratiewaarde_triceps; |
DominiqueC | 2:8596695c56df | 225 | kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); |
DominiqueC | 2:8596695c56df | 226 | kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps); |
DominiqueC | 4:527e5b5283c2 | 227 | pc.printf("biceps %f\n\r", kalibratiewaarde_biceps); //WEGHALEN LATER |
DominiqueC | 2:8596695c56df | 228 | if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal! |
DominiqueC | 2:8596695c56df | 229 | setpointkm = -127; //11,12graden naar links |
DominiqueC | 2:8596695c56df | 230 | pc.printf("links"); |
DominiqueC | 2:8596695c56df | 231 | } else if (kalibratiewaarde_biceps <= 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal! |
DominiqueC | 2:8596695c56df | 232 | setpointkm = 127; //11,12graden naar rechts |
DominiqueC | 2:8596695c56df | 233 | pc.printf("rechts"); |
DominiqueC | 2:8596695c56df | 234 | } else { //middelste goal! |
DominiqueC | 2:8596695c56df | 235 | setpointkm = 0; |
DominiqueC | 2:8596695c56df | 236 | pc.printf("midden"); |
DominiqueC | 2:8596695c56df | 237 | } |
DominiqueC | 3:b06ada67fa4f | 238 | //MOTOR 2 LATEN BEWEGEN NAAR setpointkm |
DominiqueC | 3:b06ada67fa4f | 239 | tijdtimer.reset(); |
DominiqueC | 3:b06ada67fa4f | 240 | while (tijdtimer.read() <= 3) { |
DominiqueC | 3:b06ada67fa4f | 241 | while(looptimerflag == false); |
DominiqueC | 3:b06ada67fa4f | 242 | looptimerflag = false; |
DominiqueC | 3:b06ada67fa4f | 243 | new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint |
DominiqueC | 3:b06ada67fa4f | 244 | clamp(&new_pwm_km, -1,1); |
DominiqueC | 3:b06ada67fa4f | 245 | if(new_pwm_km < 0) |
DominiqueC | 3:b06ada67fa4f | 246 | motordir2 = 1; //links |
DominiqueC | 3:b06ada67fa4f | 247 | else |
DominiqueC | 3:b06ada67fa4f | 248 | motordir2 = 0; //rechts |
DominiqueC | 3:b06ada67fa4f | 249 | pwm_motor2.write(abs(new_pwm_km)); |
DominiqueC | 3:b06ada67fa4f | 250 | } |
DominiqueC | 4:527e5b5283c2 | 251 | wait(2); |
DominiqueC | 2:8596695c56df | 252 | state = SLAAN; |
DominiqueC | 2:8596695c56df | 253 | break; |
DominiqueC | 2:8596695c56df | 254 | } |
DominiqueC | 3:b06ada67fa4f | 255 | |
DominiqueC | 2:8596695c56df | 256 | case SLAAN: { |
DominiqueC | 4:527e5b5283c2 | 257 | pc.printf("Slaan"); //regel 1 LCD scherm |
DominiqueC | 4:527e5b5283c2 | 258 | pc.printf("Kies goal"); //regel 2 LCD scherm |
DominiqueC | 2:8596695c56df | 259 | float thetadot; |
DominiqueC | 2:8596695c56df | 260 | float setpointgm; |
DominiqueC | 2:8596695c56df | 261 | float new_pwm_gm; |
DominiqueC | 3:b06ada67fa4f | 262 | float setpointkm; |
DominiqueC | 3:b06ada67fa4f | 263 | float new_pwm_km; |
DominiqueC | 2:8596695c56df | 264 | wait(3); |
DominiqueC | 2:8596695c56df | 265 | float kalibratiewaarde_biceps; |
DominiqueC | 2:8596695c56df | 266 | kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); |
DominiqueC | 4:527e5b5283c2 | 267 | pc.printf("biceps %f\n\r", kalibratiewaarde_biceps); //WEGHALEN LATER |
DominiqueC | 2:8596695c56df | 268 | if (kalibratiewaarde_biceps <= 0.3) { //kalibratiewaarde_biceps<0.3 goal onderin |
DominiqueC | 2:8596695c56df | 269 | thetadot=THETADOT0; |
DominiqueC | 4:527e5b5283c2 | 270 | pc.printf("ONDERSTE GOAL"); |
DominiqueC | 2:8596695c56df | 271 | } else if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<=0.6) { //0.3<kalibratiewaarde_biceps<0.6 goal midden |
DominiqueC | 2:8596695c56df | 272 | thetadot=THETADOT1; |
DominiqueC | 3:b06ada67fa4f | 273 | pc.printf("MIDDELSTE GOAL"); |
DominiqueC | 2:8596695c56df | 274 | } else { //goal bovenin |
DominiqueC | 2:8596695c56df | 275 | thetadot=THETADOT2; |
DominiqueC | 3:b06ada67fa4f | 276 | pc.printf("BOVENSTE GOAL"); |
DominiqueC | 2:8596695c56df | 277 | } |
DominiqueC | 2:8596695c56df | 278 | pc.printf("Daar gaat ie!"); |
DominiqueC | 3:b06ada67fa4f | 279 | |
DominiqueC | 4:527e5b5283c2 | 280 | //MOTOR 1 LATEN BEWEGEN met snelheid thetadot |
DominiqueC | 3:b06ada67fa4f | 281 | tijdtimer.reset(); |
DominiqueC | 3:b06ada67fa4f | 282 | tijdslaan.reset(); |
DominiqueC | 3:b06ada67fa4f | 283 | while (tijdtimer.read() <=3) { |
DominiqueC | 3:b06ada67fa4f | 284 | while(looptimerflag == false); |
DominiqueC | 3:b06ada67fa4f | 285 | looptimerflag = false; |
DominiqueC | 3:b06ada67fa4f | 286 | if (motor1.getPosition()>= 444 ) { |
DominiqueC | 3:b06ada67fa4f | 287 | setpointgm = 444; |
DominiqueC | 3:b06ada67fa4f | 288 | } else { |
DominiqueC | 3:b06ada67fa4f | 289 | setpointgm = ((thetadot*tijdslaan.read())/RADTICKGM); |
DominiqueC | 3:b06ada67fa4f | 290 | } |
DominiqueC | 3:b06ada67fa4f | 291 | new_pwm_gm = pidgm(setpointgm, motor1.getPosition()); |
DominiqueC | 3:b06ada67fa4f | 292 | clamp(&new_pwm_gm, -1,1); |
DominiqueC | 3:b06ada67fa4f | 293 | if(new_pwm_gm < 0) { |
DominiqueC | 3:b06ada67fa4f | 294 | motordir1 = 1; //links |
DominiqueC | 3:b06ada67fa4f | 295 | } else { |
DominiqueC | 3:b06ada67fa4f | 296 | motordir1 = 0; //rechts |
DominiqueC | 3:b06ada67fa4f | 297 | } |
DominiqueC | 3:b06ada67fa4f | 298 | pwm_motor1.write(abs(new_pwm_gm)); |
DominiqueC | 3:b06ada67fa4f | 299 | } |
DominiqueC | 3:b06ada67fa4f | 300 | pc.printf("pos %d %f\n\r", motor1.getPosition(),setpointgm); |
DominiqueC | 3:b06ada67fa4f | 301 | |
DominiqueC | 4:527e5b5283c2 | 302 | //MOTOR 2 OP POSITIE HOUDEN |
DominiqueC | 3:b06ada67fa4f | 303 | new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint |
DominiqueC | 3:b06ada67fa4f | 304 | clamp(&new_pwm_km, -1,1); |
DominiqueC | 3:b06ada67fa4f | 305 | if(new_pwm_km < 0) |
DominiqueC | 3:b06ada67fa4f | 306 | motordir2 = 1; //links |
DominiqueC | 3:b06ada67fa4f | 307 | else |
DominiqueC | 3:b06ada67fa4f | 308 | motordir2 = 0; //rechts |
DominiqueC | 3:b06ada67fa4f | 309 | pwm_motor2.write(abs(new_pwm_km)); |
DominiqueC | 3:b06ada67fa4f | 310 | |
DominiqueC | 3:b06ada67fa4f | 311 | } |
DominiqueC | 3:b06ada67fa4f | 312 | state = HOME; |
DominiqueC | 3:b06ada67fa4f | 313 | break; |
DominiqueC | 3:b06ada67fa4f | 314 | |
DominiqueC | 3:b06ada67fa4f | 315 | case HOME: { |
DominiqueC | 3:b06ada67fa4f | 316 | pc.printf("HOMESTATE"); |
DominiqueC | 3:b06ada67fa4f | 317 | float setpointgm; |
DominiqueC | 3:b06ada67fa4f | 318 | float new_pwm_gm; |
DominiqueC | 3:b06ada67fa4f | 319 | float setpointkm; |
DominiqueC | 3:b06ada67fa4f | 320 | float new_pwm_km; |
DominiqueC | 3:b06ada67fa4f | 321 | |
DominiqueC | 4:527e5b5283c2 | 322 | //MOTOR 1 naar home |
DominiqueC | 3:b06ada67fa4f | 323 | tijdtimer.reset(); |
DominiqueC | 3:b06ada67fa4f | 324 | tijdslaan.reset(); |
DominiqueC | 3:b06ada67fa4f | 325 | while (tijdtimer.read() <=3) { |
DominiqueC | 3:b06ada67fa4f | 326 | while(looptimerflag == false); |
DominiqueC | 3:b06ada67fa4f | 327 | looptimerflag = false; |
DominiqueC | 3:b06ada67fa4f | 328 | setpointgm = 0; |
DominiqueC | 3:b06ada67fa4f | 329 | new_pwm_gm = pidgm(setpointgm, motor1.getPosition()); |
DominiqueC | 3:b06ada67fa4f | 330 | clamp(&new_pwm_gm, -1,1); |
DominiqueC | 3:b06ada67fa4f | 331 | if(new_pwm_gm < 0) |
DominiqueC | 3:b06ada67fa4f | 332 | motordir1 = 1; //links |
DominiqueC | 3:b06ada67fa4f | 333 | else |
DominiqueC | 3:b06ada67fa4f | 334 | motordir1 = 0; //rechts |
DominiqueC | 3:b06ada67fa4f | 335 | pwm_motor1.write(abs(new_pwm_gm)); |
DominiqueC | 3:b06ada67fa4f | 336 | |
DominiqueC | 4:527e5b5283c2 | 337 | //MOTOR 2 naar home |
DominiqueC | 3:b06ada67fa4f | 338 | setpointkm=0; |
DominiqueC | 3:b06ada67fa4f | 339 | new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint |
DominiqueC | 3:b06ada67fa4f | 340 | clamp(&new_pwm_km, -1,1); |
DominiqueC | 3:b06ada67fa4f | 341 | if(new_pwm_km < 0) |
DominiqueC | 3:b06ada67fa4f | 342 | motordir2 = 1; //links |
DominiqueC | 3:b06ada67fa4f | 343 | else |
DominiqueC | 3:b06ada67fa4f | 344 | motordir2 = 0; //rechts |
DominiqueC | 3:b06ada67fa4f | 345 | pwm_motor2.write(abs(new_pwm_km)); |
DominiqueC | 3:b06ada67fa4f | 346 | } |
DominiqueC | 3:b06ada67fa4f | 347 | //state = RICHTEN; //optioneel |
DominiqueC | 2:8596695c56df | 348 | break; |
DominiqueC | 2:8596695c56df | 349 | } |
DominiqueC | 3:b06ada67fa4f | 350 | default: { |
DominiqueC | 3:b06ada67fa4f | 351 | state = RUST; |
DominiqueC | 2:8596695c56df | 352 | } |
DominiqueC | 1:0d5864272412 | 353 | } |
DominiqueC | 1:0d5864272412 | 354 | } |
DominiqueC | 2:8596695c56df | 355 | } |