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@11:ea9fe75faf63, 2014-10-30 (annotated)

Committer:
DominiqueC
Date:
Thu Oct 30 09:12:18 2014 +0000
Revision:
11:ea9fe75faf63
Parent:
10:d156dc2efe5c 
pour Rianne

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 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
DominiqueC 10:d156dc2efe5c 32 #define THETADOT0 6.85
DominiqueC 10:d156dc2efe5c 33 #define THETADOT1 7.77
DominiqueC 10:d156dc2efe5c 34 #define THETADOT2 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
DominiqueC 9:9000c5c1a0d6 38 Encoder motor2(PTD2,PTD0); //geel,wit kleine motor
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;
DominiqueC 9:9000c5c1a0d6 56 // constants for 50 Hz notch (bandbreedte 2 Hz)
Daanmk 1:96cd4c9c5465 57 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
DominiqueC 9:9000c5c1a0d6 58 //state values
DominiqueC 9:9000c5c1a0d6 59 float notch_biceps_states[4];
Daanmk 1:96cd4c9c5465 60 float notch_triceps_states[4];
Daanmk 0:b3809a8d9af1 61
Daanmk 1:96cd4c9c5465 62 arm_biquad_casd_df1_inst_f32 highpass_biceps;
Daanmk 1:96cd4c9c5465 63 arm_biquad_casd_df1_inst_f32 highpass_triceps;
Daanmk 1:96cd4c9c5465 64 //constants for 20Hz highpass
Daanmk 1:96cd4c9c5465 65 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 66 //state values
Daanmk 1:96cd4c9c5465 67 float highpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 68 float highpass_triceps_states[4];
DominiqueC 9:9000c5c1a0d6 69
Daanmk 1:96cd4c9c5465 70 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
Daanmk 1:96cd4c9c5465 71 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
DominiqueC 9:9000c5c1a0d6 72 //constants for 80Hz lowpass
Daanmk 1:96cd4c9c5465 73 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 74 //state values
Daanmk 1:96cd4c9c5465 75 float lowpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 76 float lowpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 77
Daanmk 1:96cd4c9c5465 78 arm_biquad_casd_df1_inst_f32 envelop_biceps;
Daanmk 1:96cd4c9c5465 79 arm_biquad_casd_df1_inst_f32 envelop_triceps;
DominiqueC 9:9000c5c1a0d6 80 //constants for envelop
Daanmk 1:96cd4c9c5465 81 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
DominiqueC 9:9000c5c1a0d6 82 // state values
Daanmk 1:96cd4c9c5465 83 float envelop_biceps_states[4];
Daanmk 1:96cd4c9c5465 84 float envelop_triceps_states[4];
Daanmk 1:96cd4c9c5465 85
Daanmk 5:5383d9a80307 86 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES
Daanmk 1:96cd4c9c5465 87 uint8_t state=RUST;
Daanmk 1:96cd4c9c5465 88
Daanmk 7:dac6b30d43b3 89 enum kalibratiestates {BICEPSMAX,TRICEPSMAX};
Daanmk 7:dac6b30d43b3 90
Daanmk 3:81a6009303a9 91 volatile bool looptimerflag;
Daanmk 3:81a6009303a9 92 void setlooptimerflag(void)
Daanmk 3:81a6009303a9 93 {
Daanmk 3:81a6009303a9 94 looptimerflag = true;
Daanmk 3:81a6009303a9 95 }
Daanmk 3:81a6009303a9 96
Daanmk 6:cb5daf35ba9b 97 void clamp(float * in, float min, float max)
Daanmk 6:cb5daf35ba9b 98 {
Daanmk 6:cb5daf35ba9b 99 *in > min ? *in < max? : *in = max: *in = min;
Daanmk 6:cb5daf35ba9b 100 }
Daanmk 3:81a6009303a9 101
DominiqueC 11:ea9fe75faf63 102 float pidkm(float setpointkm, float measurementkm) //PID Regelaar kleine motor
Daanmk 6:cb5daf35ba9b 103 {
Daanmk 6:cb5daf35ba9b 104 float error_km;
Daanmk 6:cb5daf35ba9b 105 static float prev_error_km = 0;
Daanmk 6:cb5daf35ba9b 106 float out_p_km = 0;
DominiqueC 11:ea9fe75faf63 107 static float out_i_km = 0; //static, want dan wordt vorige waarde onthouden
Daanmk 6:cb5daf35ba9b 108 float out_d_km = 0;
Daanmk 6:cb5daf35ba9b 109 error_km = setpointkm-measurementkm;
Daanmk 6:cb5daf35ba9b 110 out_p_km = error_km*K_P_KM;
Daanmk 6:cb5daf35ba9b 111 out_i_km += error_km*K_I_KM;
Daanmk 6:cb5daf35ba9b 112 out_d_km = (error_km-prev_error_km)*K_D_KM;
Daanmk 6:cb5daf35ba9b 113 clamp(&out_i_km,-I_LIMIT,I_LIMIT);
Daanmk 6:cb5daf35ba9b 114 prev_error_km = error_km;
Daanmk 6:cb5daf35ba9b 115 scope.set(1,out_p_km);
Daanmk 6:cb5daf35ba9b 116 scope.set(2,out_i_km);
Daanmk 6:cb5daf35ba9b 117 scope.set(3,out_d_km);
Daanmk 6:cb5daf35ba9b 118 return out_p_km + out_i_km + out_d_km;
Daanmk 6:cb5daf35ba9b 119 }
Daanmk 6:cb5daf35ba9b 120
Daanmk 6:cb5daf35ba9b 121 float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
Daanmk 6:cb5daf35ba9b 122 {
Daanmk 6:cb5daf35ba9b 123 float error_gm;
Daanmk 6:cb5daf35ba9b 124 static float prev_error_gm = 0;
Daanmk 6:cb5daf35ba9b 125 float out_p_gm = 0;
Daanmk 6:cb5daf35ba9b 126 static float out_i_gm = 0;
Daanmk 6:cb5daf35ba9b 127 float out_d_gm = 0;
Daanmk 6:cb5daf35ba9b 128 error_gm = setpointgm-measurementgm;
Daanmk 6:cb5daf35ba9b 129 out_p_gm = error_gm*K_P_GM;
Daanmk 6:cb5daf35ba9b 130 out_i_gm += error_gm*K_I_GM;
Daanmk 6:cb5daf35ba9b 131 out_d_gm = (error_gm-prev_error_gm)*K_D_GM;
Daanmk 6:cb5daf35ba9b 132 clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
Daanmk 6:cb5daf35ba9b 133 prev_error_gm = error_gm;
Daanmk 6:cb5daf35ba9b 134 scope.set(1,out_p_gm);
Daanmk 6:cb5daf35ba9b 135 scope.set(2,out_i_gm);
Daanmk 6:cb5daf35ba9b 136 scope.set(3,out_d_gm);
Daanmk 6:cb5daf35ba9b 137 return out_p_gm + out_i_gm + out_d_gm;
Daanmk 6:cb5daf35ba9b 138 }
Daanmk 1:96cd4c9c5465 139 void emgmeten(){
Daanmk 0:b3809a8d9af1 140 /*put raw emg value in emg_value*/
Daanmk 0:b3809a8d9af1 141 emg0_value_f32 = emg0.read();
Daanmk 0:b3809a8d9af1 142 emg1_value_f32 = emg1.read();
Daanmk 0:b3809a8d9af1 143
Daanmk 0:b3809a8d9af1 144 //process emg biceps
Daanmk 1:96cd4c9c5465 145 arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
Daanmk 1:96cd4c9c5465 146 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 147 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 148 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 149 arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
Daanmk 0:b3809a8d9af1 150
Daanmk 0:b3809a8d9af1 151 //process emg triceps
Daanmk 1:96cd4c9c5465 152 arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
Daanmk 1:96cd4c9c5465 153 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 154 arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 155 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 156 arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
Daanmk 0:b3809a8d9af1 157 }
Daanmk 6:cb5daf35ba9b 158
Daanmk 1:96cd4c9c5465 159
Daanmk 0:b3809a8d9af1 160 int main()
Daanmk 0:b3809a8d9af1 161 {
Daanmk 1:96cd4c9c5465 162 pc.baud(38400); //PC baud rate is 38400 bits/seconde
Daanmk 1:96cd4c9c5465 163 Ticker emg_timer;
Daanmk 1:96cd4c9c5465 164 emg_timer.attach(emgmeten, TSAMP);
Daanmk 3:81a6009303a9 165 Ticker looptimer;
Daanmk 3:81a6009303a9 166 looptimer.attach(setlooptimerflag,TSAMP);
Daanmk 4:a0b0c944846e 167 Timer tijdtimer;
Daanmk 7:dac6b30d43b3 168 Timer tijdslaan;
Daanmk 1:96cd4c9c5465 169 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
Daanmk 1:96cd4c9c5465 170 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
Daanmk 1:96cd4c9c5465 171 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
Daanmk 1:96cd4c9c5465 172 arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
Daanmk 1:96cd4c9c5465 173 arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
Daanmk 1:96cd4c9c5465 174 arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
Daanmk 1:96cd4c9c5465 175 arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
Daanmk 1:96cd4c9c5465 176 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
Daanmk 8:aa27423f4a4a 177 while(true) {
Daanmk 1:96cd4c9c5465 178 switch(state) {
DominiqueC 9:9000c5c1a0d6 179 case RUST: { //Aanzetten
Daanmk 6:cb5daf35ba9b 180 lcd.cls();
Daanmk 6:cb5daf35ba9b 181 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 182 lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 183 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 184 lcd.printf(" GROEP 5 "); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 185 wait(5);
Daanmk 7:dac6b30d43b3 186 state = KALIBRATIE;
Daanmk 1:96cd4c9c5465 187 break;
Daanmk 1:96cd4c9c5465 188 }
Daanmk 1:96cd4c9c5465 189
DominiqueC 9:9000c5c1a0d6 190 case KALIBRATIE: { //kalibreren met maximale inspanning
Daanmk 3:81a6009303a9 191 max_value_biceps=0;
Daanmk 3:81a6009303a9 192 max_value_triceps=0;
Daanmk 7:dac6b30d43b3 193 state = BICEPSMAX;
Daanmk 7:dac6b30d43b3 194 switch(state) {
DominiqueC 9:9000c5c1a0d6 195 case BICEPSMAX: { //maximale inspanning biceps
Daanmk 7:dac6b30d43b3 196 lcd.cls();
Daanmk 7:dac6b30d43b3 197 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 198 lcd.printf("Kalibratie"); //regel 1 LCD scherm
Daanmk 7:dac6b30d43b3 199 lcd.locate(0,1);
Daanmk 7:dac6b30d43b3 200 lcd.printf("1:BICEPS MAX"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 201 wait(1);
Daanmk 7:dac6b30d43b3 202 tijdtimer.start();
Daanmk 7:dac6b30d43b3 203 lcd.cls();
Daanmk 7:dac6b30d43b3 204 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 205 lcd.printf("Biceps meting"); //regel 1 LCD scherm
Daanmk 7:dac6b30d43b3 206 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 207 lcd.printf("Meting loopt!"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 208 while (tijdtimer <= 3){
Daanmk 7:dac6b30d43b3 209 if (envelop_emg0 > max_value_biceps) {
Daanmk 7:dac6b30d43b3 210 max_value_biceps = envelop_emg0; }
Daanmk 7:dac6b30d43b3 211 }
Daanmk 7:dac6b30d43b3 212 if (tijdtimer >= 3) {
Daanmk 7:dac6b30d43b3 213 tijdtimer.stop();
Daanmk 7:dac6b30d43b3 214 tijdtimer.reset();
Daanmk 7:dac6b30d43b3 215 lcd.cls();
Daanmk 7:dac6b30d43b3 216 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 217 lcd.printf("Einde meting!"); //regel 1 LCD scherm
Daanmk 7:dac6b30d43b3 218 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 219 lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
Daanmk 7:dac6b30d43b3 220 wait(1);
Daanmk 7:dac6b30d43b3 221 state = TRICEPSMAX;
Daanmk 7:dac6b30d43b3 222 }//einde if statement
Daanmk 7:dac6b30d43b3 223 break;
Daanmk 7:dac6b30d43b3 224 }//einde case bicepsmax
DominiqueC 10:d156dc2efe5c 225 case TRICEPSMAX: { //maximale inspanning triceps
Daanmk 7:dac6b30d43b3 226 lcd.cls();
Daanmk 7:dac6b30d43b3 227 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 228 lcd.printf("Kalibratie"); //regel 1 LCD scherm
Daanmk 7:dac6b30d43b3 229 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 230 lcd.printf("2:TRICEPS MAX"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 231 wait(1);
Daanmk 7:dac6b30d43b3 232 tijdtimer.start();
Daanmk 7:dac6b30d43b3 233 lcd.cls();
Daanmk 7:dac6b30d43b3 234 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 235 lcd.printf("Triceps meting"); //regel 1 LCD scherm
Daanmk 7:dac6b30d43b3 236 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 237 lcd.printf("Meting loopt!"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 238 while (tijdtimer <= 3){
Daanmk 7:dac6b30d43b3 239 if (envelop_emg1 > max_value_triceps) {
Daanmk 7:dac6b30d43b3 240 max_value_triceps = envelop_emg1; }
Daanmk 7:dac6b30d43b3 241 }
Daanmk 7:dac6b30d43b3 242 if (tijdtimer >= 3) {
Daanmk 7:dac6b30d43b3 243 tijdtimer.stop();
Daanmk 7:dac6b30d43b3 244 tijdtimer.reset();
Daanmk 7:dac6b30d43b3 245 lcd.cls();
Daanmk 7:dac6b30d43b3 246 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 247 lcd.printf("Einde meting!"); //regel 1 LCD scherm
Daanmk 7:dac6b30d43b3 248 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 249 lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
Daanmk 7:dac6b30d43b3 250 wait(1);
Daanmk 7:dac6b30d43b3 251 state = RICHTEN;
Daanmk 7:dac6b30d43b3 252 } //einde if statement
Daanmk 7:dac6b30d43b3 253 break;
Daanmk 7:dac6b30d43b3 254 } //einde case tricepsmax
Daanmk 7:dac6b30d43b3 255 default: {
Daanmk 7:dac6b30d43b3 256 state = BICEPSMAX;
Daanmk 7:dac6b30d43b3 257 } //einde default
Daanmk 7:dac6b30d43b3 258 } //einde switch states
Daanmk 2:3bf615031d7a 259 break;
Daanmk 7:dac6b30d43b3 260 } // einde kalibratie case
Daanmk 1:96cd4c9c5465 261
DominiqueC 9:9000c5c1a0d6 262 case RICHTEN: { //batje richten (gebruik biceps en triceps)
Daanmk 5:5383d9a80307 263 lcd.cls();
Daanmk 5:5383d9a80307 264 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 265 lcd.printf("Richten"); //regel 1 LCD scherm
Daanmk 5:5383d9a80307 266 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 267 lcd.printf("Kies goal!"); //regel 2 LCD scherm
Daanmk 6:cb5daf35ba9b 268 int16_t setpointkm;
Daanmk 6:cb5daf35ba9b 269 float new_pwm_km;
Daanmk 5:5383d9a80307 270 wait(1);
Daanmk 5:5383d9a80307 271 float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
Daanmk 5:5383d9a80307 272 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT
Daanmk 5:5383d9a80307 273 kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
Daanmk 5:5383d9a80307 274 tijdtimer.start();
Daanmk 7:dac6b30d43b3 275 while( tijdtimer <= 3) {
Daanmk 7:dac6b30d43b3 276 if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps < 0.3) { //linker goal!
DominiqueC 10:d156dc2efe5c 277 setpointkm = -127; //11,12graden naar links
Daanmk 7:dac6b30d43b3 278 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 7:dac6b30d43b3 279 clamp(&new_pwm_km, -1,1);
Daanmk 7:dac6b30d43b3 280 if(new_pwm_km < 0)
Daanmk 7:dac6b30d43b3 281 motordir1 = 1;
Daanmk 7:dac6b30d43b3 282 else
Daanmk 7:dac6b30d43b3 283 motordir1 = 0;
Daanmk 7:dac6b30d43b3 284 pwm_motor1.write(abs(new_pwm_km));
Daanmk 8:aa27423f4a4a 285 if(motor1.getPosition() <= -400 ){
Daanmk 8:aa27423f4a4a 286 pwm_motor1.write(0);
Daanmk 8:aa27423f4a4a 287 }
Daanmk 7:dac6b30d43b3 288 }
Daanmk 7:dac6b30d43b3 289 if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
DominiqueC 10:d156dc2efe5c 290 setpointkm = 127; //11,12graden naar rechts
Daanmk 7:dac6b30d43b3 291 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 7:dac6b30d43b3 292 clamp(&new_pwm_km, -1,1);
Daanmk 7:dac6b30d43b3 293 if(new_pwm_km < 0)
Daanmk 7:dac6b30d43b3 294 motordir1 = 1;
Daanmk 7:dac6b30d43b3 295 else
Daanmk 7:dac6b30d43b3 296 motordir1 = 0;
Daanmk 8:aa27423f4a4a 297 pwm_motor1.write(abs(new_pwm_km));
Daanmk 8:aa27423f4a4a 298 if(motor1.getPosition() >= 400 ){
Daanmk 8:aa27423f4a4a 299 pwm_motor1.write(0);
Daanmk 8:aa27423f4a4a 300 }
Daanmk 7:dac6b30d43b3 301 }
Daanmk 7:dac6b30d43b3 302 if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps < 0.3) { //middelste goal!
Daanmk 7:dac6b30d43b3 303 setpointkm = 0;
Daanmk 7:dac6b30d43b3 304 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 7:dac6b30d43b3 305 clamp(&new_pwm_km, -1,1);
Daanmk 7:dac6b30d43b3 306 if(new_pwm_km < 0)
Daanmk 7:dac6b30d43b3 307 motordir1 = 1;
Daanmk 7:dac6b30d43b3 308 else
Daanmk 7:dac6b30d43b3 309 motordir1 = 0;
Daanmk 7:dac6b30d43b3 310 pwm_motor1.write(abs(new_pwm_km));
Daanmk 7:dac6b30d43b3 311 }
Daanmk 5:5383d9a80307 312 }
Daanmk 7:dac6b30d43b3 313 if (tijdtimer >= 3) {
Daanmk 5:5383d9a80307 314 tijdtimer.stop();
Daanmk 5:5383d9a80307 315 tijdtimer.reset();
Daanmk 5:5383d9a80307 316 state = SLAAN;
Daanmk 6:cb5daf35ba9b 317 }
Daanmk 5:5383d9a80307 318
Daanmk 1:96cd4c9c5465 319 break;
Daanmk 1:96cd4c9c5465 320 }
DominiqueC 9:9000c5c1a0d6 321 case SLAAN: { //snelheid bepalen (gebruik alleen biceps)
Daanmk 6:cb5daf35ba9b 322 lcd.cls();
Daanmk 6:cb5daf35ba9b 323 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 324 lcd.printf("Slaan PingPong!"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 325 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 326 lcd.printf("Kies goal!"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 327 wait(1);
Daanmk 6:cb5daf35ba9b 328 int16_t setpointgm;
Daanmk 6:cb5daf35ba9b 329 float new_pwm_gm;
Daanmk 6:cb5daf35ba9b 330 float kalibratiewaarde_biceps;
DominiqueC 10:d156dc2efe5c 331 float thetadot;
Daanmk 6:cb5daf35ba9b 332 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
Daanmk 7:dac6b30d43b3 333 tijdtimer.start();
Daanmk 7:dac6b30d43b3 334 while (tijdtimer <=3 ) {
DominiqueC 9:9000c5c1a0d6 335 if (kalibratiewaarde_biceps<0.3){ //kalibratiewaarde_biceps<0.3 goal onderin
Daanmk 6:cb5daf35ba9b 336 lcd.cls();
Daanmk 6:cb5daf35ba9b 337 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 338 lcd.printf("Onderste goal"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 339 lcd.locate(0,1);
DominiqueC 9:9000c5c1a0d6 340 lcd.printf("Daar gaat ie!"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 341 tijdslaan.start();
DominiqueC 10:d156dc2efe5c 342 thetadot=THETADOT0;
DominiqueC 10:d156dc2efe5c 343 setpointgm = (thetadot*tijdslaan/RADTICKGM);
DominiqueC 10:d156dc2efe5c 344 new_pwm_gm = pidgm(setpointgm, motor2.getPosition()); //wat gebeurt hier????
Daanmk 6:cb5daf35ba9b 345 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 346 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 347 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 348 else
Daanmk 6:cb5daf35ba9b 349 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 350 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 8:aa27423f4a4a 351 if(motor2.getPosition() >= 450 ) {
Daanmk 8:aa27423f4a4a 352 pwm_motor2.write(0);
Daanmk 8:aa27423f4a4a 353 }
Daanmk 6:cb5daf35ba9b 354 wait(2);
Daanmk 6:cb5daf35ba9b 355 state = RUST;
Daanmk 6:cb5daf35ba9b 356 }
DominiqueC 9:9000c5c1a0d6 357 if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){ //0.3<kalibratiewaarde_biceps<0.6 goal midden
Daanmk 6:cb5daf35ba9b 358 lcd.cls();
Daanmk 6:cb5daf35ba9b 359 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 360 lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 361 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 362 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
DominiqueC 10:d156dc2efe5c 363 thetadot=THETADOT1;
DominiqueC 10:d156dc2efe5c 364 setpointgm = ((thetadot*tijdslaan)/RADTICKGM);
Daanmk 6:cb5daf35ba9b 365 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 366 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 367 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 368 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 369 else
Daanmk 6:cb5daf35ba9b 370 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 371 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 8:aa27423f4a4a 372 if(motor2.getPosition() >= 450 ) {
Daanmk 8:aa27423f4a4a 373 pwm_motor2.write(0);
Daanmk 6:cb5daf35ba9b 374 wait(2);
Daanmk 6:cb5daf35ba9b 375 state = RUST;
Daanmk 6:cb5daf35ba9b 376 }
DominiqueC 9:9000c5c1a0d6 377 if (kalibratiewaarde_biceps>0.6){ //kalibratiewaarde_biceps>0.6 goal bovenin
Daanmk 6:cb5daf35ba9b 378 lcd.cls();
Daanmk 6:cb5daf35ba9b 379 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 380 lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 381 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 382 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
DominiqueC 10:d156dc2efe5c 383 thetadot=THETADOT2;
DominiqueC 10:d156dc2efe5c 384 setpointgm = ((thetadot*tijdslaan)/RADTICKGM);
Daanmk 6:cb5daf35ba9b 385 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 386 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 387 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 388 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 389 else
Daanmk 6:cb5daf35ba9b 390 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 391 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 8:aa27423f4a4a 392 if(motor2.getPosition() >= 450 ) {
Daanmk 8:aa27423f4a4a 393 pwm_motor2.write(0);
Daanmk 6:cb5daf35ba9b 394 wait(2);
Daanmk 6:cb5daf35ba9b 395 state = RUST;
Daanmk 6:cb5daf35ba9b 396
Daanmk 6:cb5daf35ba9b 397 }
Daanmk 7:dac6b30d43b3 398 } //einde whilelus
Daanmk 7:dac6b30d43b3 399 if (tijdtimer >=3 ) {
Daanmk 7:dac6b30d43b3 400 tijdtimer.stop();
Daanmk 7:dac6b30d43b3 401 tijdtimer.reset();
Daanmk 8:aa27423f4a4a 402 wait(1);
Daanmk 7:dac6b30d43b3 403 //+ TERUGKEREN BEGINPOSITIE!
Daanmk 8:aa27423f4a4a 404 lcd.cls();
Daanmk 8:aa27423f4a4a 405 lcd.locate(0,0);
DominiqueC 9:9000c5c1a0d6 406 lcd.printf("Goed Gedaan!"); //regel 1 LCD scherm
Daanmk 8:aa27423f4a4a 407 lcd.locate(0,1);
Daanmk 8:aa27423f4a4a 408 lcd.printf("Terug naar begin"); //regel 2 LCD scherm
Daanmk 8:aa27423f4a4a 409 setpointgm = (0);
Daanmk 8:aa27423f4a4a 410 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 8:aa27423f4a4a 411 clamp(&new_pwm_gm, -1,1);
Daanmk 7:dac6b30d43b3 412 state = RUST;
Daanmk 7:dac6b30d43b3 413 } //einde if statement
Daanmk 6:cb5daf35ba9b 414
Daanmk 6:cb5daf35ba9b 415
Daanmk 6:cb5daf35ba9b 416 state = RUST;
Daanmk 1:96cd4c9c5465 417 break;
Daanmk 1:96cd4c9c5465 418 }
Daanmk 1:96cd4c9c5465 419
Daanmk 3:81a6009303a9 420 default: {
Daanmk 1:96cd4c9c5465 421 state = RUST;
Daanmk 2:3bf615031d7a 422 }
Daanmk 1:96cd4c9c5465 423
Daanmk 6:cb5daf35ba9b 424 } //switch sate
Daanmk 1:96cd4c9c5465 425
Daanmk 6:cb5daf35ba9b 426 } //while
Daanmk 6:cb5daf35ba9b 427 } //int main
DominiqueC 10:d156dc2efe5c 428 }}