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@8:aa27423f4a4a, 2014-10-29 (annotated)

Committer:
Daanmk
Date:
Wed Oct 29 16:41:46 2014 +0000
Revision:
8:aa27423f4a4a
Parent:
7:dac6b30d43b3
Child:
9:9000c5c1a0d6
PROJECT MAIN SCRIPT

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
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(&notch_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(&notch_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(&notch_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(&notch_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 8:aa27423f4a4a 172 while(true) {
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 8:aa27423f4a4a 272 setpointkm = -125;
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 8:aa27423f4a4a 280 if(motor1.getPosition() <= -400 ){
Daanmk 8:aa27423f4a4a 281 pwm_motor1.write(0);
Daanmk 8:aa27423f4a4a 282 }
Daanmk 7:dac6b30d43b3 283 }
Daanmk 7:dac6b30d43b3 284 if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
Daanmk 8:aa27423f4a4a 285 setpointkm = 125;
Daanmk 7:dac6b30d43b3 286 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 7:dac6b30d43b3 287 clamp(&new_pwm_km, -1,1);
Daanmk 7:dac6b30d43b3 288 if(new_pwm_km < 0)
Daanmk 7:dac6b30d43b3 289 motordir1 = 1;
Daanmk 7:dac6b30d43b3 290 else
Daanmk 7:dac6b30d43b3 291 motordir1 = 0;
Daanmk 8:aa27423f4a4a 292 pwm_motor1.write(abs(new_pwm_km));
Daanmk 8:aa27423f4a4a 293 if(motor1.getPosition() >= 400 ){
Daanmk 8:aa27423f4a4a 294 pwm_motor1.write(0);
Daanmk 8:aa27423f4a4a 295 }
Daanmk 7:dac6b30d43b3 296 }
Daanmk 7:dac6b30d43b3 297 if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps < 0.3) { //middelste goal!
Daanmk 7:dac6b30d43b3 298 setpointkm = 0;
Daanmk 7:dac6b30d43b3 299 new_pwm_km = pidkm(setpointkm, motor1.getPosition());
Daanmk 7:dac6b30d43b3 300 clamp(&new_pwm_km, -1,1);
Daanmk 7:dac6b30d43b3 301 if(new_pwm_km < 0)
Daanmk 7:dac6b30d43b3 302 motordir1 = 1;
Daanmk 7:dac6b30d43b3 303 else
Daanmk 7:dac6b30d43b3 304 motordir1 = 0;
Daanmk 7:dac6b30d43b3 305 pwm_motor1.write(abs(new_pwm_km));
Daanmk 7:dac6b30d43b3 306 }
Daanmk 5:5383d9a80307 307 }
Daanmk 7:dac6b30d43b3 308 if (tijdtimer >= 3) {
Daanmk 5:5383d9a80307 309 tijdtimer.stop();
Daanmk 5:5383d9a80307 310 tijdtimer.reset();
Daanmk 5:5383d9a80307 311 state = SLAAN;
Daanmk 6:cb5daf35ba9b 312 }
Daanmk 5:5383d9a80307 313
Daanmk 1:96cd4c9c5465 314 break;
Daanmk 1:96cd4c9c5465 315 }
Daanmk 2:3bf615031d7a 316 case SLAAN: { //Balletje slaan
Daanmk 6:cb5daf35ba9b 317 lcd.cls();
Daanmk 6:cb5daf35ba9b 318 lcd.locate(0,0);
Daanmk 7:dac6b30d43b3 319 lcd.printf("Slaan PingPong!"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 320 lcd.locate(0,1);
Daanmk 7:dac6b30d43b3 321 lcd.printf("Kies goal!"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 322 wait(1);
Daanmk 6:cb5daf35ba9b 323 int16_t setpointgm;
Daanmk 6:cb5daf35ba9b 324 float new_pwm_gm;
Daanmk 6:cb5daf35ba9b 325 float kalibratiewaarde_biceps;
Daanmk 7:dac6b30d43b3 326 float theta;
Daanmk 6:cb5daf35ba9b 327 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
Daanmk 7:dac6b30d43b3 328 tijdtimer.start();
Daanmk 7:dac6b30d43b3 329 while (tijdtimer <=3 ) {
Daanmk 8:aa27423f4a4a 330 if (kalibratiewaarde_biceps<0.3){ //kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
Daanmk 6:cb5daf35ba9b 331 lcd.cls();
Daanmk 6:cb5daf35ba9b 332 lcd.locate(0,0);
Daanmk 7:dac6b30d43b3 333 lcd.printf("Onderste goal"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 334 lcd.locate(0,1);
Daanmk 7:dac6b30d43b3 335 lcd.printf("Daar gaat ie!"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 336 tijdslaan.start();
Daanmk 7:dac6b30d43b3 337 theta=THETA0;
Daanmk 7:dac6b30d43b3 338 setpointgm = (theta*tijdslaan/RADTICKGM);
Daanmk 6:cb5daf35ba9b 339 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 340 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 341 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 342 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 343 else
Daanmk 6:cb5daf35ba9b 344 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 345 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 8:aa27423f4a4a 346 if(motor2.getPosition() >= 450 ) {
Daanmk 8:aa27423f4a4a 347 pwm_motor2.write(0);
Daanmk 8:aa27423f4a4a 348 }
Daanmk 6:cb5daf35ba9b 349 wait(2);
Daanmk 6:cb5daf35ba9b 350 state = RUST;
Daanmk 6:cb5daf35ba9b 351 }
Daanmk 7:dac6b30d43b3 352 if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){
Daanmk 6:cb5daf35ba9b 353 lcd.cls();
Daanmk 6:cb5daf35ba9b 354 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 355 lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 356 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 357 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 358 theta=THETA1;
Daanmk 7:dac6b30d43b3 359 setpointgm = ((theta*tijdslaan)/RADTICKGM);
Daanmk 6:cb5daf35ba9b 360 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 361 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 362 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 363 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 364 else
Daanmk 6:cb5daf35ba9b 365 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 366 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 8:aa27423f4a4a 367 if(motor2.getPosition() >= 450 ) {
Daanmk 8:aa27423f4a4a 368 pwm_motor2.write(0);
Daanmk 6:cb5daf35ba9b 369 wait(2);
Daanmk 6:cb5daf35ba9b 370 state = RUST;
Daanmk 6:cb5daf35ba9b 371 }
Daanmk 7:dac6b30d43b3 372 if (kalibratiewaarde_biceps>0.6){
Daanmk 6:cb5daf35ba9b 373 lcd.cls();
Daanmk 6:cb5daf35ba9b 374 lcd.locate(0,0);
Daanmk 6:cb5daf35ba9b 375 lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm
Daanmk 6:cb5daf35ba9b 376 lcd.locate(0,1);
Daanmk 6:cb5daf35ba9b 377 lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
Daanmk 7:dac6b30d43b3 378 theta=THETA2;
Daanmk 7:dac6b30d43b3 379 setpointgm = ((theta*tijdslaan)/RADTICKGM);
Daanmk 6:cb5daf35ba9b 380 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 6:cb5daf35ba9b 381 clamp(&new_pwm_gm, -1,1);
Daanmk 6:cb5daf35ba9b 382 if(new_pwm_gm < 0)
Daanmk 6:cb5daf35ba9b 383 motordir2 = 0;
Daanmk 6:cb5daf35ba9b 384 else
Daanmk 6:cb5daf35ba9b 385 motordir2 = 1;
Daanmk 6:cb5daf35ba9b 386 pwm_motor2.write(abs(new_pwm_gm));
Daanmk 8:aa27423f4a4a 387 if(motor2.getPosition() >= 450 ) {
Daanmk 8:aa27423f4a4a 388 pwm_motor2.write(0);
Daanmk 6:cb5daf35ba9b 389 wait(2);
Daanmk 6:cb5daf35ba9b 390 state = RUST;
Daanmk 6:cb5daf35ba9b 391
Daanmk 6:cb5daf35ba9b 392 }
Daanmk 7:dac6b30d43b3 393 } //einde whilelus
Daanmk 7:dac6b30d43b3 394 if (tijdtimer >=3 ) {
Daanmk 7:dac6b30d43b3 395 tijdtimer.stop();
Daanmk 7:dac6b30d43b3 396 tijdtimer.reset();
Daanmk 8:aa27423f4a4a 397 wait(1);
Daanmk 7:dac6b30d43b3 398 //+ TERUGKEREN BEGINPOSITIE!
Daanmk 8:aa27423f4a4a 399 lcd.cls();
Daanmk 8:aa27423f4a4a 400 lcd.locate(0,0);
Daanmk 8:aa27423f4a4a 401 lcd.printf("Goed Gedaan!"); //regel 1 LCD scherm
Daanmk 8:aa27423f4a4a 402 lcd.locate(0,1);
Daanmk 8:aa27423f4a4a 403 lcd.printf("Terug naar begin"); //regel 2 LCD scherm
Daanmk 8:aa27423f4a4a 404 setpointgm = (0);
Daanmk 8:aa27423f4a4a 405 new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
Daanmk 8:aa27423f4a4a 406 clamp(&new_pwm_gm, -1,1);
Daanmk 7:dac6b30d43b3 407 state = RUST;
Daanmk 7:dac6b30d43b3 408 } //einde if statement
Daanmk 6:cb5daf35ba9b 409
Daanmk 6:cb5daf35ba9b 410
Daanmk 6:cb5daf35ba9b 411 state = RUST;
Daanmk 1:96cd4c9c5465 412 break;
Daanmk 1:96cd4c9c5465 413 }
Daanmk 1:96cd4c9c5465 414
Daanmk 3:81a6009303a9 415 default: {
Daanmk 1:96cd4c9c5465 416 state = RUST;
Daanmk 2:3bf615031d7a 417 }
Daanmk 1:96cd4c9c5465 418
Daanmk 6:cb5daf35ba9b 419 } //switch sate
Daanmk 1:96cd4c9c5465 420
Daanmk 6:cb5daf35ba9b 421 } //while
Daanmk 6:cb5daf35ba9b 422 } //int main
Daanmk 5:5383d9a80307 423