Dominique Clevers / Mbed 2 deprecated The_SLAP_5_1

the slap

Dependencies:   Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed

Fork of The_SLAP_5_1 by Daan

main.cpp@6:cb5daf35ba9b, 2014-10-28 (annotated)

Committer:
Daanmk
Date:
Tue Oct 28 09:46:57 2014 +0000
Revision:
6:cb5daf35ba9b
Parent:
5:5383d9a80307
Child:
7:dac6b30d43b3
Volledige script af, versie1

Who changed what in which revision?

UserRevisionLine numberNew 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 6:cb5daf35ba9b 24 #define K_P_KM (14)
Daanmk 6:cb5daf35ba9b 25 #define K_I_KM (28 *TSAMP)
Daanmk 6:cb5daf35ba9b 26 #define K_D_KM (0.01 /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 0:b3809a8d9af1 31
Daanmk 3:81a6009303a9 32 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 33
Daanmk 1:96cd4c9c5465 34 Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor
Daanmk 1:96cd4c9c5465 35 Encoder motor1(PTD5,PTA13);//geel,wit
Daanmk 0:b3809a8d9af1 36 PwmOut pwm_motor1(M1_PWM);
Daanmk 0:b3809a8d9af1 37 PwmOut pwm_motor2(M2_PWM);
Daanmk 0:b3809a8d9af1 38 DigitalOut motordir2(M2_DIR);
Daanmk 0:b3809a8d9af1 39 DigitalOut motordir1(M1_DIR);
Daanmk 0:b3809a8d9af1 40 AnalogIn emg0(PTB0); //Biceps
Daanmk 0:b3809a8d9af1 41 AnalogIn emg1(PTB1); //Triceps
Daanmk 0:b3809a8d9af1 42 HIDScope scope(6);
Daanmk 0:b3809a8d9af1 43
Daanmk 1:96cd4c9c5465 44 MODSERIAL pc(USBTX,USBRX,64,1024);
Daanmk 1:96cd4c9c5465 45
Daanmk 0:b3809a8d9af1 46
Daanmk 4:a0b0c944846e 47 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 48 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 49
Daanmk 1:96cd4c9c5465 50 arm_biquad_casd_df1_inst_f32 notch_biceps;
Daanmk 1:96cd4c9c5465 51 arm_biquad_casd_df1_inst_f32 notch_triceps;
Daanmk 1:96cd4c9c5465 52 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
Daanmk 1:96cd4c9c5465 53 float notch_biceps_states[4]; //state values
Daanmk 1:96cd4c9c5465 54 float notch_triceps_states[4];
Daanmk 0:b3809a8d9af1 55
Daanmk 1:96cd4c9c5465 56 arm_biquad_casd_df1_inst_f32 highpass_biceps;
Daanmk 1:96cd4c9c5465 57 arm_biquad_casd_df1_inst_f32 highpass_triceps;
Daanmk 1:96cd4c9c5465 58 //constants for 20Hz highpass
Daanmk 1:96cd4c9c5465 59 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 60 //state values
Daanmk 1:96cd4c9c5465 61 float highpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 62 float highpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 63 //constants for 80Hz lowpass
Daanmk 1:96cd4c9c5465 64 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
Daanmk 1:96cd4c9c5465 65 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
Daanmk 1:96cd4c9c5465 66 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 67 //state values
Daanmk 1:96cd4c9c5465 68 float lowpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 69 float lowpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 70
Daanmk 1:96cd4c9c5465 71 arm_biquad_casd_df1_inst_f32 envelop_biceps;
Daanmk 1:96cd4c9c5465 72 arm_biquad_casd_df1_inst_f32 envelop_triceps;
Daanmk 1:96cd4c9c5465 73 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
Daanmk 1:96cd4c9c5465 74 float envelop_biceps_states[4];
Daanmk 1:96cd4c9c5465 75 float envelop_triceps_states[4];
Daanmk 1:96cd4c9c5465 76
Daanmk 5:5383d9a80307 77 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES
Daanmk 1:96cd4c9c5465 78 uint8_t state=RUST;
Daanmk 1:96cd4c9c5465 79
Daanmk 3:81a6009303a9 80 volatile bool looptimerflag;
Daanmk 3:81a6009303a9 81 void setlooptimerflag(void)
Daanmk 3:81a6009303a9 82 {
Daanmk 3:81a6009303a9 83 looptimerflag = true;
Daanmk 3:81a6009303a9 84 }
Daanmk 3:81a6009303a9 85
Daanmk 6:cb5daf35ba9b 86 void clamp(float * in, float min, float max)
Daanmk 6:cb5daf35ba9b 87 {
Daanmk 6:cb5daf35ba9b 88 *in > min ? *in < max? : *in = max: *in = min;
Daanmk 6:cb5daf35ba9b 89 }
Daanmk 3:81a6009303a9 90
Daanmk 6:cb5daf35ba9b 91 float pidkm(float setpointkm, float measurementkm) //PID Regelaar Kleine motor
Daanmk 6:cb5daf35ba9b 92 {
Daanmk 6:cb5daf35ba9b 93 float error_km;
Daanmk 6:cb5daf35ba9b 94 static float prev_error_km = 0;
Daanmk 6:cb5daf35ba9b 95 float out_p_km = 0;
Daanmk 6:cb5daf35ba9b 96 static float out_i_km = 0;
Daanmk 6:cb5daf35ba9b 97 float out_d_km = 0;
Daanmk 6:cb5daf35ba9b 98 error_km = setpointkm-measurementkm;
Daanmk 6:cb5daf35ba9b 99 out_p_km = error_km*K_P_KM;
Daanmk 6:cb5daf35ba9b 100 out_i_km += error_km*K_I_KM;
Daanmk 6:cb5daf35ba9b 101 out_d_km = (error_km-prev_error_km)*K_D_KM;
Daanmk 6:cb5daf35ba9b 102 clamp(&out_i_km,-I_LIMIT,I_LIMIT);
Daanmk 6:cb5daf35ba9b 103 prev_error_km = error_km;
Daanmk 6:cb5daf35ba9b 104 scope.set(1,out_p_km);
Daanmk 6:cb5daf35ba9b 105 scope.set(2,out_i_km);
Daanmk 6:cb5daf35ba9b 106 scope.set(3,out_d_km);
Daanmk 6:cb5daf35ba9b 107 return out_p_km + out_i_km + out_d_km;
Daanmk 6:cb5daf35ba9b 108 }
Daanmk 6:cb5daf35ba9b 109
Daanmk 6:cb5daf35ba9b 110 float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
Daanmk 6:cb5daf35ba9b 111 {
Daanmk 6:cb5daf35ba9b 112 float error_gm;
Daanmk 6:cb5daf35ba9b 113 static float prev_error_gm = 0;
Daanmk 6:cb5daf35ba9b 114 float out_p_gm = 0;
Daanmk 6:cb5daf35ba9b 115 static float out_i_gm = 0;
Daanmk 6:cb5daf35ba9b 116 float out_d_gm = 0;
Daanmk 6:cb5daf35ba9b 117 error_gm = setpointgm-measurementgm;
Daanmk 6:cb5daf35ba9b 118 out_p_gm = error_gm*K_P_GM;
Daanmk 6:cb5daf35ba9b 119 out_i_gm += error_gm*K_I_GM;
Daanmk 6:cb5daf35ba9b 120 out_d_gm = (error_gm-prev_error_gm)*K_D_GM;
Daanmk 6:cb5daf35ba9b 121 clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
Daanmk 6:cb5daf35ba9b 122 prev_error_gm = error_gm;
Daanmk 6:cb5daf35ba9b 123 scope.set(1,out_p_gm);
Daanmk 6:cb5daf35ba9b 124 scope.set(2,out_i_gm);
Daanmk 6:cb5daf35ba9b 125 scope.set(3,out_d_gm);
Daanmk 6:cb5daf35ba9b 126 return out_p_gm + out_i_gm + out_d_gm;
Daanmk 6:cb5daf35ba9b 127 }
Daanmk 1:96cd4c9c5465 128 void emgmeten(){
Daanmk 0:b3809a8d9af1 129 /*put raw emg value in emg_value*/
Daanmk 0:b3809a8d9af1 130 emg0_value_f32 = emg0.read();
Daanmk 0:b3809a8d9af1 131 emg1_value_f32 = emg1.read();
Daanmk 0:b3809a8d9af1 132
Daanmk 0:b3809a8d9af1 133 //process emg biceps
Daanmk 1:96cd4c9c5465 134 arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
Daanmk 1:96cd4c9c5465 135 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 136 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 137 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 138 arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
Daanmk 0:b3809a8d9af1 139
Daanmk 0:b3809a8d9af1 140 //process emg triceps
Daanmk 1:96cd4c9c5465 141 arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
Daanmk 1:96cd4c9c5465 142 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 143 arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 144 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 145 arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
Daanmk 0:b3809a8d9af1 146 }
Daanmk 6:cb5daf35ba9b 147
Daanmk 1:96cd4c9c5465 148
Daanmk 0:b3809a8d9af1 149 int main()
Daanmk 0:b3809a8d9af1 150 {
Daanmk 1:96cd4c9c5465 151 while(1) {
Daanmk 1:96cd4c9c5465 152 pc.baud(38400); //PC baud rate is 38400 bits/seconde
Daanmk 1:96cd4c9c5465 153 Ticker emg_timer;
Daanmk 1:96cd4c9c5465 154 emg_timer.attach(emgmeten, TSAMP);
Daanmk 3:81a6009303a9 155 Ticker looptimer;
Daanmk 3:81a6009303a9 156 looptimer.attach(setlooptimerflag,TSAMP);
Daanmk 4:a0b0c944846e 157 Timer tijdtimer;
Daanmk 1:96cd4c9c5465 158 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
Daanmk 1:96cd4c9c5465 159 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
Daanmk 1:96cd4c9c5465 160 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
Daanmk 1:96cd4c9c5465 161 arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
Daanmk 1:96cd4c9c5465 162 arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
Daanmk 1:96cd4c9c5465 163 arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
Daanmk 1:96cd4c9c5465 164 arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
Daanmk 1:96cd4c9c5465 165 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
Daanmk 2:3bf615031d7a 166
Daanmk 1:96cd4c9c5465 167 switch(state) {
Daanmk 2:3bf615031d7a 168 case RUST: { //Aanzetten
Daanmk 6:cb5daf35ba9b 169 lcd.cls();
Daanmk 6:cb5daf35ba9b 170 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 171 lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 172 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 173 lcd.printf(" GROEP 5 ");
Daanmk 6:cb5daf35ba9b 174 wait(5);
Daanmk 6:cb5daf35ba9b 175 if (metingstatus<5);
Daanmk 6:cb5daf35ba9b 176 state = KALIBRATIE;
Daanmk 6:cb5daf35ba9b 177 if (metingstatus==5);
Daanmk 6:cb5daf35ba9b 178 state = RICHTEN;
Daanmk 1:96cd4c9c5465 179 break;
Daanmk 1:96cd4c9c5465 180 }
Daanmk 1:96cd4c9c5465 181
Daanmk 2:3bf615031d7a 182 case KALIBRATIE: {
Daanmk 3:81a6009303a9 183 lcd.cls();
Daanmk 2:3bf615031d7a 184 lcd.locate(0,0);
Daanmk 2:3bf615031d7a 185 lcd.printf("Kalibratie"); //regel 1 LCD scherm
Daanmk 2:3bf615031d7a 186 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 187 lcd.printf("1:BICEPS RUST"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 188 wait(1);
Daanmk 3:81a6009303a9 189 max_value_biceps=0;
Daanmk 3:81a6009303a9 190 max_value_triceps=0;
Daanmk 4:a0b0c944846e 191 min_value_biceps=0;
Daanmk 4:a0b0c944846e 192 min_value_triceps=0;
Daanmk 4:a0b0c944846e 193 int metingstatus=1;
Daanmk 4:a0b0c944846e 194
Daanmk 4:a0b0c944846e 195 if (metingstatus==1) { //BICEPS RUST METING
Daanmk 4:a0b0c944846e 196
Daanmk 4:a0b0c944846e 197 lcd.cls();
Daanmk 4:a0b0c944846e 198 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 199 lcd.printf("BICEPS RUST"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 200 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 201 lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 202 wait(1);
Daanmk 4:a0b0c944846e 203 tijdtimer.start();
Daanmk 4:a0b0c944846e 204
Daanmk 4:a0b0c944846e 205 if (envelop_emg0 > min_value_biceps) {
Daanmk 4:a0b0c944846e 206 min_value_biceps = envelop_emg0;
Daanmk 4:a0b0c944846e 207 }
Daanmk 4:a0b0c944846e 208 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 209 tijdtimer.stop();
Daanmk 4:a0b0c944846e 210 tijdtimer.reset();
Daanmk 4:a0b0c944846e 211 metingstatus=2;
Daanmk 4:a0b0c944846e 212 }
Daanmk 3:81a6009303a9 213 }
Daanmk 4:a0b0c944846e 214 if (metingstatus==2) { //TRICEPS RUST METING
Daanmk 4:a0b0c944846e 215 lcd.cls();
Daanmk 4:a0b0c944846e 216 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 217 lcd.printf("TRICEPS RUST"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 218 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 219 lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 220 wait(1);
Daanmk 4:a0b0c944846e 221 tijdtimer.start();
Daanmk 4:a0b0c944846e 222 if (envelop_emg1 > min_value_triceps) {
Daanmk 4:a0b0c944846e 223 min_value_triceps = envelop_emg1; }
Daanmk 4:a0b0c944846e 224 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 225 tijdtimer.stop();
Daanmk 4:a0b0c944846e 226 tijdtimer.reset();
Daanmk 4:a0b0c944846e 227 metingstatus=3;
Daanmk 4:a0b0c944846e 228 }
Daanmk 6:cb5daf35ba9b 229 }
Daanmk 4:a0b0c944846e 230 if (metingstatus==3) { //BICEPS KRACHT METING
Daanmk 4:a0b0c944846e 231 lcd.cls();
Daanmk 4:a0b0c944846e 232 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 233 lcd.printf("BICEPS MAX"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 234 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 235 lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 236 wait(1);
Daanmk 4:a0b0c944846e 237 tijdtimer.start();
Daanmk 4:a0b0c944846e 238 if (envelop_emg0 > max_value_biceps) {
Daanmk 4:a0b0c944846e 239 max_value_biceps = envelop_emg0; }
Daanmk 4:a0b0c944846e 240 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 241 tijdtimer.stop();
Daanmk 4:a0b0c944846e 242 tijdtimer.reset();
Daanmk 4:a0b0c944846e 243 metingstatus=4;
Daanmk 4:a0b0c944846e 244 }
Daanmk 6:cb5daf35ba9b 245 }
Daanmk 4:a0b0c944846e 246 if (metingstatus==4) {//TRICEPS KRACHT METING
Daanmk 4:a0b0c944846e 247 lcd.cls();
Daanmk 4:a0b0c944846e 248 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 249 lcd.printf("TRICEPS MAX"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 250 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 251 lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 252 tijdtimer.start();
Daanmk 4:a0b0c944846e 253 if (envelop_emg1 > max_value_triceps) {
Daanmk 4:a0b0c944846e 254 max_value_triceps = envelop_emg1; }
Daanmk 4:a0b0c944846e 255 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 256 tijdtimer.stop();
Daanmk 4:a0b0c944846e 257 tijdtimer.reset();
Daanmk 4:a0b0c944846e 258 metingstatus=5;
Daanmk 6:cb5daf35ba9b 259 }
Daanmk 6:cb5daf35ba9b 260 }
Daanmk 4:a0b0c944846e 261 if (metingstatus==5) {
Daanmk 5:5383d9a80307 262 lcd.cls();
Daanmk 5:5383d9a80307 263 lcd.locate(0,0);
Daanmk 5:5383d9a80307 264 lcd.printf("Kalibratie OK"); //regel 1 LCD scherm
Daanmk 5:5383d9a80307 265 lcd.locate(0,1);
Daanmk 5:5383d9a80307 266 lcd.printf(""); //regel 2 LCD scherm
Daanmk 5:5383d9a80307 267 wait(1);
Daanmk 4:a0b0c944846e 268 state = RICHTEN; }
Daanmk 2:3bf615031d7a 269 break;
Daanmk 1:96cd4c9c5465 270 }
Daanmk 1:96cd4c9c5465 271
Daanmk 2:3bf615031d7a 272 case RICHTEN: { //Batje richten
Daanmk 5:5383d9a80307 273 lcd.cls();
Daanmk 5:5383d9a80307 274 lcd.locate(0,0);
Daanmk 5:5383d9a80307 275 lcd.printf("RICHTEN"); //regel 1 LCD scherm
Daanmk 5:5383d9a80307 276 lcd.locate(0,1);
Daanmk 5:5383d9a80307 277 lcd.printf("KIES GOAL"); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 278 int16_t setpointkm;
Daanmk 6:cb5daf35ba9b 279 float new_pwm_km;
Daanmk 5:5383d9a80307 280 wait(1);
Daanmk 6:cb5daf35ba9b 281 int goalkeuze=1; //midden goal
Daanmk 5:5383d9a80307 282 float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
Daanmk 5:5383d9a80307 283 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT
Daanmk 5:5383d9a80307 284 kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
Daanmk 5:5383d9a80307 285 tijdtimer.start();
Daanmk 6:cb5daf35ba9b 286
Daanmk 6:cb5daf35ba9b 287 if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_triceps<0.3){
Daanmk 6:cb5daf35ba9b 288 goalkeuze = 0; //goalkeuze
Daanmk 6:cb5daf35ba9b 289 lcd.cls();
Daanmk 6:cb5daf35ba9b 290 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 291 lcd.printf("LINKER GOAL!"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 292 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 293 lcd.printf(""); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 294 setpointkm = -241;
Daanmk 6:cb5daf35ba9b 295 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 6:cb5daf35ba9b 296 clamp(&new_pwm_km, -1,1);
Daanmk 6:cb5daf35ba9b 297 if(new_pwm_km < 0)
Daanmk 6:cb5daf35ba9b 298 motordir1 = 1;
Daanmk 6:cb5daf35ba9b 299 else
Daanmk 6:cb5daf35ba9b 300 motordir1 = 0;
Daanmk 6:cb5daf35ba9b 301 pwm_motor1.write(abs(new_pwm_km));
Daanmk 6:cb5daf35ba9b 302 wait(1);
Daanmk 6:cb5daf35ba9b 303 state = SLAAN;
Daanmk 6:cb5daf35ba9b 304 //-241 encoder tics tov midden goal
Daanmk 6:cb5daf35ba9b 305
Daanmk 5:5383d9a80307 306 }
Daanmk 6:cb5daf35ba9b 307 if (kalibratiewaarde_triceps>0.3 && kalibratiewaarde_biceps<0.3) {
Daanmk 5:5383d9a80307 308 goalkeuze = 2; //rechter goal
Daanmk 6:cb5daf35ba9b 309 lcd.cls();
Daanmk 6:cb5daf35ba9b 310 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 311 lcd.printf("RECHTER GOAL!"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 312 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 313 lcd.printf(""); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 314 //+11 encoder tics tov midden goal
Daanmk 6:cb5daf35ba9b 315 setpointkm = 11;
Daanmk 6:cb5daf35ba9b 316 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 6:cb5daf35ba9b 317 clamp(&new_pwm_km, -1,1);
Daanmk 6:cb5daf35ba9b 318 if(new_pwm_km < 0)
Daanmk 6:cb5daf35ba9b 319 motordir1 = 1;
Daanmk 6:cb5daf35ba9b 320 else
Daanmk 6:cb5daf35ba9b 321 motordir1 = 0;
Daanmk 6:cb5daf35ba9b 322 pwm_motor1.write(abs(new_pwm_km));
Daanmk 6:cb5daf35ba9b 323 wait(1);
Daanmk 6:cb5daf35ba9b 324 state = SLAAN;
Daanmk 6:cb5daf35ba9b 325 }
Daanmk 6:cb5daf35ba9b 326 if (kalibratiewaarde_biceps <0.3 && kalibratiewaarde_triceps<0.3) {
Daanmk 6:cb5daf35ba9b 327 goalkeuze=1; // middelste goal!
Daanmk 6:cb5daf35ba9b 328 lcd.cls();
Daanmk 6:cb5daf35ba9b 329 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 330 lcd.printf("MIDDEN GOAL!"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 331 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 332 lcd.printf(""); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 333 //0 encoder tics. Ook start positie!
Daanmk 6:cb5daf35ba9b 334 setpointkm = 0;
Daanmk 6:cb5daf35ba9b 335 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 6:cb5daf35ba9b 336 clamp(&new_pwm_km, -1,1);
Daanmk 6:cb5daf35ba9b 337 if(new_pwm_km < 0)
Daanmk 6:cb5daf35ba9b 338 motordir1 = 1;
Daanmk 6:cb5daf35ba9b 339 else
Daanmk 6:cb5daf35ba9b 340 motordir1 = 0;
Daanmk 6:cb5daf35ba9b 341 pwm_motor1.write(abs(new_pwm_km));
Daanmk 6:cb5daf35ba9b 342 wait(1);
Daanmk 6:cb5daf35ba9b 343 state = SLAAN;
Daanmk 5:5383d9a80307 344 }
Daanmk 5:5383d9a80307 345 if (tijdtimer == 5) {
Daanmk 5:5383d9a80307 346 tijdtimer.stop();
Daanmk 5:5383d9a80307 347 tijdtimer.reset();
Daanmk 5:5383d9a80307 348 state = SLAAN;
Daanmk 6:cb5daf35ba9b 349 }
Daanmk 5:5383d9a80307 350
Daanmk 1:96cd4c9c5465 351 break;
Daanmk 1:96cd4c9c5465 352 }
Daanmk 2:3bf615031d7a 353 case SLAAN: { //Balletje slaan
Daanmk 6:cb5daf35ba9b 354 lcd.cls();
Daanmk 6:cb5daf35ba9b 355 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 356 lcd.printf("SLAAN!"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 357 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 358 lcd.printf("KIES GOAL!"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 359 wait(1);
Daanmk 6:cb5daf35ba9b 360 int16_t setpointgm;
Daanmk 6:cb5daf35ba9b 361 float new_pwm_gm;
Daanmk 6:cb5daf35ba9b 362 int goalhoogte=0;
Daanmk 6:cb5daf35ba9b 363 float kalibratiewaarde_biceps;
Daanmk 6:cb5daf35ba9b 364 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
Daanmk 6:cb5daf35ba9b 365 if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
Daanmk 6:cb5daf35ba9b 366 goalhoogte = 0; //goalkeuze, onderste goal
Daanmk 6:cb5daf35ba9b 367 lcd.cls();
Daanmk 6:cb5daf35ba9b 368 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 369 lcd.printf("ONDERSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 370 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 371 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 372 setpointgm = //onbekend;
Daanmk 6:cb5daf35ba9b 373 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 374 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 375 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 376 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 377 else
Daanmk 6:cb5daf35ba9b 378 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 379 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 6:cb5daf35ba9b 380 wait(2);
Daanmk 6:cb5daf35ba9b 381 state = RUST;
Daanmk 6:cb5daf35ba9b 382 }
Daanmk 6:cb5daf35ba9b 383 if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){
Daanmk 6:cb5daf35ba9b 384 goalhoogte = 1; //goalkeuze, middelste goal
Daanmk 6:cb5daf35ba9b 385 lcd.cls();
Daanmk 6:cb5daf35ba9b 386 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 387 lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 388 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 389 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 390 setpointgm = //onbekend;
Daanmk 6:cb5daf35ba9b 391 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 392 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 393 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 394 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 395 else
Daanmk 6:cb5daf35ba9b 396 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 397 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 6:cb5daf35ba9b 398 wait(2);
Daanmk 6:cb5daf35ba9b 399 state = RUST;
Daanmk 6:cb5daf35ba9b 400 }
Daanmk 6:cb5daf35ba9b 401 if (kalibratiewaarde_biceps>0.6){
Daanmk 6:cb5daf35ba9b 402 goalhoogte = 2; //goalkeuze, Bovenste goal
Daanmk 6:cb5daf35ba9b 403 lcd.cls();
Daanmk 6:cb5daf35ba9b 404 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 405 lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 406 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 407 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 408 setpointgm = //onbekend;
Daanmk 6:cb5daf35ba9b 409 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 410 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 411 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 412 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 413 else
Daanmk 6:cb5daf35ba9b 414 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 415 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 6:cb5daf35ba9b 416 wait(2);
Daanmk 6:cb5daf35ba9b 417 state = RUST;
Daanmk 6:cb5daf35ba9b 418
Daanmk 6:cb5daf35ba9b 419 }
Daanmk 6:cb5daf35ba9b 420
Daanmk 6:cb5daf35ba9b 421
Daanmk 6:cb5daf35ba9b 422
Daanmk 6:cb5daf35ba9b 423
Daanmk 6:cb5daf35ba9b 424 state = RUST;
Daanmk 1:96cd4c9c5465 425 break;
Daanmk 1:96cd4c9c5465 426 }
Daanmk 1:96cd4c9c5465 427
Daanmk 3:81a6009303a9 428 default: {
Daanmk 1:96cd4c9c5465 429 state = RUST;
Daanmk 2:3bf615031d7a 430 }
Daanmk 1:96cd4c9c5465 431
Daanmk 6:cb5daf35ba9b 432 } //switch sate
Daanmk 1:96cd4c9c5465 433
Daanmk 6:cb5daf35ba9b 434 } //while
Daanmk 6:cb5daf35ba9b 435 } //int main
Daanmk 5:5383d9a80307 436