Dominique Clevers / Mbed 2 deprecated THESLAP_EINDSCRIPT

eindscript the slap

Dependencies:   Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed

main.cpp@8:f8f58e48b352, 2014-11-05 (annotated)

Committer:
DominiqueC
Date:
Wed Nov 05 15:57:39 2014 +0000
Revision:
8:f8f58e48b352
Parent:
7:20c2ebe52306 
eindeindeindscript :)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DominiqueC 0:8d63bed88781 1 /***************************************/
DominiqueC 0:8d63bed88781 2 /* */
DominiqueC 0:8d63bed88781 3 /* BRONCODE GROEP 5, MODULE 9, 2014 */
DominiqueC 0:8d63bed88781 4 /* *****-THE SLAP-****** */
DominiqueC 0:8d63bed88781 5 /* */
DominiqueC 0:8d63bed88781 6 /* -Dominique Clevers */
DominiqueC 0:8d63bed88781 7 /* -Rianne van Dommelen */
DominiqueC 0:8d63bed88781 8 /* -Daan de Muinck Keizer */
DominiqueC 0:8d63bed88781 9 /* -David den Houting */
DominiqueC 0:8d63bed88781 10 /* -Marjolein Thijssen */
DominiqueC 0:8d63bed88781 11 /***************************************/
DominiqueC 0:8d63bed88781 12 #include "mbed.h"
DominiqueC 0:8d63bed88781 13 #include "arm_math.h"
DominiqueC 0:8d63bed88781 14 #include "encoder.h"
DominiqueC 0:8d63bed88781 15 #include "MODSERIAL.h"
DominiqueC 0:8d63bed88781 16 #include "TextLCD.h"
DominiqueC 0:8d63bed88781 17
DominiqueC 0:8d63bed88781 18 #define M2_PWM PTC8 //kleine motor
DominiqueC 0:8d63bed88781 19 #define M2_DIR PTC9 //kleine motor
DominiqueC 0:8d63bed88781 20 #define M1_PWM PTA5 //grote motor
DominiqueC 0:8d63bed88781 21 #define M1_DIR PTA4 //grote motor
DominiqueC 0:8d63bed88781 22 #define TSAMP 0.005 // Sampletijd, 200Hz
DominiqueC 0:8d63bed88781 23 #define K_P_KM (0.01)
DominiqueC 0:8d63bed88781 24 #define K_I_KM (0.0003 *TSAMP)
DominiqueC 0:8d63bed88781 25 #define K_D_KM (0.0 /TSAMP)
DominiqueC 0:8d63bed88781 26 #define K_P_GM (0.0022)
DominiqueC 6:2c11ee0c05b4 27 #define K_I_GM (0.0001 *TSAMP) //oud 0.0001
DominiqueC 0:8d63bed88781 28 #define K_D_GM (0.00000001 /TSAMP)
DominiqueC 0:8d63bed88781 29 #define I_LIMIT 1.
DominiqueC 0:8d63bed88781 30 #define RADTICKGM 0.003927
DominiqueC 8:f8f58e48b352 31 #define THETADOT0 30.0 //oud 6.85
DominiqueC 8:f8f58e48b352 32 #define THETADOT1 20.0 //oud 7.77
DominiqueC 8:f8f58e48b352 33 #define THETADOT2 14.0 //oud 9.21
DominiqueC 0:8d63bed88781 34
DominiqueC 1:5187f57ad4b0 35 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13);
DominiqueC 0:8d63bed88781 36
DominiqueC 3:fbe46d4e4b3f 37 Encoder motor2(PTD3,PTD1); //geel,wit kleine motor oud:PTD2,PTD0
DominiqueC 3:fbe46d4e4b3f 38 Encoder motor1(PTD5,PTD0);//geel,wit grote motor oud:PTD5,PTA13
DominiqueC 0:8d63bed88781 39 PwmOut pwm_motor1(M1_PWM);
DominiqueC 0:8d63bed88781 40 PwmOut pwm_motor2(M2_PWM);
DominiqueC 0:8d63bed88781 41 DigitalOut motordir2(M2_DIR);
DominiqueC 0:8d63bed88781 42 DigitalOut motordir1(M1_DIR);
DominiqueC 0:8d63bed88781 43 AnalogIn emg0(PTB0); //Biceps
DominiqueC 0:8d63bed88781 44 AnalogIn emg1(PTB1); //deltoid
DominiqueC 0:8d63bed88781 45
DominiqueC 0:8d63bed88781 46 MODSERIAL pc(USBTX,USBRX,64,1024);
DominiqueC 0:8d63bed88781 47
DominiqueC 0:8d63bed88781 48
DominiqueC 0:8d63bed88781 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:8d63bed88781 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_deltoid,min_value_deltoid,metingstatus; //variable to store value in for deltoid
DominiqueC 0:8d63bed88781 51
DominiqueC 0:8d63bed88781 52 arm_biquad_casd_df1_inst_f32 notch_biceps;
DominiqueC 0:8d63bed88781 53 arm_biquad_casd_df1_inst_f32 notch_deltoid;
DominiqueC 0:8d63bed88781 54 // constants for 50 Hz notch (bandbreedte 2 Hz)
DominiqueC 0:8d63bed88781 55 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
DominiqueC 0:8d63bed88781 56 //state values
DominiqueC 0:8d63bed88781 57 float notch_biceps_states[4];
DominiqueC 0:8d63bed88781 58 float notch_deltoid_states[4];
DominiqueC 0:8d63bed88781 59
DominiqueC 0:8d63bed88781 60 arm_biquad_casd_df1_inst_f32 highpass_biceps;
DominiqueC 0:8d63bed88781 61 arm_biquad_casd_df1_inst_f32 highpass_deltoid;
DominiqueC 0:8d63bed88781 62 //constants for 20Hz highpass
DominiqueC 0:8d63bed88781 63 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
DominiqueC 0:8d63bed88781 64 //state values
DominiqueC 0:8d63bed88781 65 float highpass_biceps_states[4];
DominiqueC 0:8d63bed88781 66 float highpass_deltoid_states[4];
DominiqueC 0:8d63bed88781 67
DominiqueC 0:8d63bed88781 68 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
DominiqueC 0:8d63bed88781 69 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
DominiqueC 0:8d63bed88781 70 //constants for 80Hz lowpass
DominiqueC 0:8d63bed88781 71 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
DominiqueC 0:8d63bed88781 72 //state values
DominiqueC 0:8d63bed88781 73 float lowpass_biceps_states[4];
DominiqueC 0:8d63bed88781 74 float lowpass_deltoid_states[4];
DominiqueC 0:8d63bed88781 75
DominiqueC 0:8d63bed88781 76 arm_biquad_casd_df1_inst_f32 envelop_biceps;
DominiqueC 0:8d63bed88781 77 arm_biquad_casd_df1_inst_f32 envelop_deltoid;
DominiqueC 0:8d63bed88781 78 //constants for envelop
DominiqueC 0:8d63bed88781 79 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
DominiqueC 0:8d63bed88781 80 // state values
DominiqueC 0:8d63bed88781 81 float envelop_biceps_states[4];
DominiqueC 0:8d63bed88781 82 float envelop_deltoid_states[4];
DominiqueC 0:8d63bed88781 83
DominiqueC 0:8d63bed88781 84 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES
DominiqueC 0:8d63bed88781 85 uint8_t state=RUST;
DominiqueC 0:8d63bed88781 86
DominiqueC 0:8d63bed88781 87 volatile bool looptimerflag;
DominiqueC 0:8d63bed88781 88 void setlooptimerflag(void)
DominiqueC 0:8d63bed88781 89 {
DominiqueC 0:8d63bed88781 90 looptimerflag = true;
DominiqueC 0:8d63bed88781 91 }
DominiqueC 0:8d63bed88781 92
DominiqueC 0:8d63bed88781 93 void clamp(float * in, float min, float max)
DominiqueC 0:8d63bed88781 94 {
DominiqueC 0:8d63bed88781 95 *in > min ? *in < max? : *in = max: *in = min;
DominiqueC 0:8d63bed88781 96 }
DominiqueC 0:8d63bed88781 97
DominiqueC 0:8d63bed88781 98 float pidkm(float setpointkm, float measurementkm) //PID Regelaar kleine motor
DominiqueC 0:8d63bed88781 99 {
DominiqueC 0:8d63bed88781 100 float error_km;
DominiqueC 0:8d63bed88781 101 static float prev_error_km = 0;
DominiqueC 0:8d63bed88781 102 float out_p_km = 0;
DominiqueC 0:8d63bed88781 103 static float out_i_km = 0; //static, want dan wordt vorige waarde onthouden
DominiqueC 0:8d63bed88781 104 float out_d_km = 0;
DominiqueC 0:8d63bed88781 105 error_km = setpointkm-measurementkm;
DominiqueC 0:8d63bed88781 106 out_p_km = error_km*K_P_KM;
DominiqueC 0:8d63bed88781 107 out_i_km += error_km*K_I_KM;
DominiqueC 0:8d63bed88781 108 out_d_km = (error_km-prev_error_km)*K_D_KM;
DominiqueC 0:8d63bed88781 109 clamp(&out_i_km,-I_LIMIT,I_LIMIT);
DominiqueC 0:8d63bed88781 110 prev_error_km = error_km;
DominiqueC 0:8d63bed88781 111 return out_p_km + out_i_km + out_d_km;
DominiqueC 0:8d63bed88781 112 }
DominiqueC 0:8d63bed88781 113
DominiqueC 0:8d63bed88781 114 float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
DominiqueC 0:8d63bed88781 115 {
DominiqueC 0:8d63bed88781 116 float error_gm;
DominiqueC 0:8d63bed88781 117 static float prev_error_gm = 0;
DominiqueC 0:8d63bed88781 118 float out_p_gm = 0;
DominiqueC 0:8d63bed88781 119 static float out_i_gm = 0;
DominiqueC 0:8d63bed88781 120 float out_d_gm = 0;
DominiqueC 0:8d63bed88781 121 error_gm = setpointgm-measurementgm;
DominiqueC 0:8d63bed88781 122 out_p_gm = error_gm*K_P_GM;
DominiqueC 0:8d63bed88781 123 out_i_gm += error_gm*K_I_GM;
DominiqueC 0:8d63bed88781 124 out_d_gm = (error_gm-prev_error_gm)*K_D_GM;
DominiqueC 0:8d63bed88781 125 clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
DominiqueC 0:8d63bed88781 126 prev_error_gm = error_gm;
DominiqueC 0:8d63bed88781 127 return out_p_gm + out_i_gm + out_d_gm;
DominiqueC 0:8d63bed88781 128 }
DominiqueC 0:8d63bed88781 129 void emgmeten()
DominiqueC 0:8d63bed88781 130 {
DominiqueC 0:8d63bed88781 131 /*put raw emg value in emg_value*/
DominiqueC 0:8d63bed88781 132 emg0_value_f32 = emg0.read();
DominiqueC 0:8d63bed88781 133 emg1_value_f32 = emg1.read();
DominiqueC 0:8d63bed88781 134
DominiqueC 0:8d63bed88781 135 //process emg biceps
DominiqueC 0:8d63bed88781 136 arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
DominiqueC 0:8d63bed88781 137 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
DominiqueC 0:8d63bed88781 138 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
DominiqueC 0:8d63bed88781 139 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter
DominiqueC 0:8d63bed88781 140 arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
DominiqueC 0:8d63bed88781 141
DominiqueC 0:8d63bed88781 142 //process emg deltoid
DominiqueC 0:8d63bed88781 143 arm_biquad_cascade_df1_f32(&notch_deltoid, &emg1_value_f32, &filtered_emg1_notch, 1 );
DominiqueC 0:8d63bed88781 144 arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
DominiqueC 0:8d63bed88781 145 arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
DominiqueC 0:8d63bed88781 146 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter
DominiqueC 0:8d63bed88781 147 arm_biquad_cascade_df1_f32(&envelop_deltoid, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
DominiqueC 0:8d63bed88781 148 }
DominiqueC 0:8d63bed88781 149
DominiqueC 0:8d63bed88781 150
DominiqueC 0:8d63bed88781 151 int main()
DominiqueC 0:8d63bed88781 152 {
DominiqueC 0:8d63bed88781 153 pc.baud(38400); //PC baud rate is 38400 bits/seconde
DominiqueC 0:8d63bed88781 154 Ticker emg_timer;
DominiqueC 0:8d63bed88781 155 emg_timer.attach(emgmeten, TSAMP);
DominiqueC 0:8d63bed88781 156 Ticker looptimer;
DominiqueC 0:8d63bed88781 157 looptimer.attach(setlooptimerflag,TSAMP);
DominiqueC 0:8d63bed88781 158 Timer tijdtimer;
DominiqueC 0:8d63bed88781 159 Timer tijdslaan;
DominiqueC 0:8d63bed88781 160 tijdtimer.start();
DominiqueC 0:8d63bed88781 161 tijdslaan.start();
DominiqueC 0:8d63bed88781 162 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
DominiqueC 0:8d63bed88781 163 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
DominiqueC 0:8d63bed88781 164 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
DominiqueC 0:8d63bed88781 165 arm_biquad_cascade_df1_init_f32(&notch_deltoid,1 , notch_const, notch_deltoid_states);
DominiqueC 0:8d63bed88781 166 arm_biquad_cascade_df1_init_f32(&highpass_deltoid,1 ,highpass_const,highpass_deltoid_states);
DominiqueC 0:8d63bed88781 167 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 ,lowpass_const,lowpass_deltoid_states);
DominiqueC 0:8d63bed88781 168 arm_biquad_cascade_df1_init_f32(&envelop_deltoid,1 ,envelop_const,envelop_deltoid_states);
DominiqueC 0:8d63bed88781 169 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
DominiqueC 0:8d63bed88781 170
DominiqueC 0:8d63bed88781 171 while(true) {
DominiqueC 0:8d63bed88781 172 switch(state) {
DominiqueC 0:8d63bed88781 173 case RUST: { //Aanzetten
DominiqueC 4:305671faeb2b 174 lcd.cls();
DominiqueC 0:8d63bed88781 175 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 176 lcd.printf(" --THE SLAP -- "); //regel 1 LCD scherm
DominiqueC 4:305671faeb2b 177 lcd.locate(0,1);
DominiqueC 0:8d63bed88781 178 lcd.printf(" GROEP 5 "); //regel 2 LCD scherm
DominiqueC 0:8d63bed88781 179 wait(5);
DominiqueC 0:8d63bed88781 180 state = KALIBRATIE;
DominiqueC 0:8d63bed88781 181 break;
DominiqueC 0:8d63bed88781 182 }
DominiqueC 0:8d63bed88781 183
DominiqueC 0:8d63bed88781 184 case KALIBRATIE: { //kalibreren met maximale inspanning
DominiqueC 0:8d63bed88781 185 max_value_biceps=0;
DominiqueC 0:8d63bed88781 186 max_value_deltoid=0;
DominiqueC 4:305671faeb2b 187
DominiqueC 0:8d63bed88781 188 //maximale inspanning biceps
DominiqueC 4:305671faeb2b 189 lcd.cls();
DominiqueC 0:8d63bed88781 190 lcd.locate(0,0);
DominiqueC 4:305671faeb2b 191 lcd.printf("Kalibratie1:");
DominiqueC 0:8d63bed88781 192 lcd.locate(0,1);
DominiqueC 4:305671faeb2b 193 lcd.printf("Span biceps!");
DominiqueC 0:8d63bed88781 194 wait(5);
DominiqueC 4:305671faeb2b 195 lcd.cls();
DominiqueC 0:8d63bed88781 196 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 197 lcd.printf("Meting loopt");
DominiqueC 0:8d63bed88781 198 tijdtimer.reset();
DominiqueC 0:8d63bed88781 199 tijdtimer.start();
DominiqueC 0:8d63bed88781 200 while (tijdtimer.read() <= 3) {
DominiqueC 0:8d63bed88781 201 if (envelop_emg0 > max_value_biceps) {
DominiqueC 0:8d63bed88781 202 max_value_biceps = envelop_emg0;
DominiqueC 0:8d63bed88781 203 }
DominiqueC 0:8d63bed88781 204 }
DominiqueC 0:8d63bed88781 205 tijdtimer.stop();
DominiqueC 0:8d63bed88781 206 tijdtimer.reset();
DominiqueC 0:8d63bed88781 207 //pc.printf("max value %f\n\r", max_value_biceps);
DominiqueC 5:060d376291b5 208 lcd.cls();
DominiqueC 0:8d63bed88781 209 wait(5);
DominiqueC 0:8d63bed88781 210
DominiqueC 0:8d63bed88781 211 //maximale inspanning deltoid
DominiqueC 4:305671faeb2b 212 lcd.cls();
DominiqueC 0:8d63bed88781 213 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 214 lcd.printf("Kalibratie2:");
DominiqueC 4:305671faeb2b 215 lcd.locate(0,1);
DominiqueC 0:8d63bed88781 216 lcd.printf("Span deltoid!");
DominiqueC 0:8d63bed88781 217 wait(5);
DominiqueC 0:8d63bed88781 218 tijdtimer.start();
DominiqueC 4:305671faeb2b 219 lcd.cls();
DominiqueC 0:8d63bed88781 220 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 221 lcd.printf("Meting loopt");
DominiqueC 0:8d63bed88781 222 while (tijdtimer.read() <= 3) {
DominiqueC 0:8d63bed88781 223 if (envelop_emg1 > max_value_deltoid) {
DominiqueC 0:8d63bed88781 224 max_value_deltoid = envelop_emg1;
DominiqueC 0:8d63bed88781 225 }
DominiqueC 0:8d63bed88781 226 }
DominiqueC 0:8d63bed88781 227 // tijdtimer.stop();
DominiqueC 0:8d63bed88781 228 tijdtimer.reset();
DominiqueC 0:8d63bed88781 229 //pc.printf("max value %f\n\r", max_value_deltoid);
DominiqueC 5:060d376291b5 230 lcd.cls();
DominiqueC 0:8d63bed88781 231 wait(5);
DominiqueC 0:8d63bed88781 232
DominiqueC 0:8d63bed88781 233 state = RICHTEN;
DominiqueC 0:8d63bed88781 234 break;
DominiqueC 0:8d63bed88781 235 }// einde kalibratie case
DominiqueC 0:8d63bed88781 236
DominiqueC 4:305671faeb2b 237 case RICHTEN: { //batje richten (gebruik deltoid)
DominiqueC 0:8d63bed88781 238 wait(3);
DominiqueC 4:305671faeb2b 239 lcd.cls();
DominiqueC 0:8d63bed88781 240 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 241 lcd.printf("Richten"); //regel 1 LCD scherm
DominiqueC 5:060d376291b5 242 lcd.cls();
DominiqueC 0:8d63bed88781 243 lcd.locate(0,1);
DominiqueC 5:060d376291b5 244 lcd.printf("Kies goal!"); //regel 2 LCD scherm
DominiqueC 0:8d63bed88781 245 wait(5);
DominiqueC 4:305671faeb2b 246 lcd.cls();
DominiqueC 0:8d63bed88781 247 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 248 lcd.printf("Meting loopt");
DominiqueC 5:060d376291b5 249 wait(2);
DominiqueC 5:060d376291b5 250 float setpointkm;
DominiqueC 5:060d376291b5 251 float new_pwm_km;
DominiqueC 0:8d63bed88781 252 float kalibratiewaarde_deltoid;
DominiqueC 0:8d63bed88781 253 kalibratiewaarde_deltoid=(envelop_emg1/max_value_deltoid);
DominiqueC 0:8d63bed88781 254 if (kalibratiewaarde_deltoid >= 0.35) {
DominiqueC 7:20c2ebe52306 255 setpointkm = -329; //11,12 graden naar links
DominiqueC 4:305671faeb2b 256 lcd.cls();
DominiqueC 0:8d63bed88781 257 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 258 lcd.printf("links");
DominiqueC 6:2c11ee0c05b4 259 } else if (kalibratiewaarde_deltoid>0.2 && kalibratiewaarde_deltoid<=0.35) {
DominiqueC 0:8d63bed88781 260 setpointkm = 0; //11,12graden naar rechts
DominiqueC 4:305671faeb2b 261 lcd.cls();
DominiqueC 0:8d63bed88781 262 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 263 lcd.printf("midden");
DominiqueC 0:8d63bed88781 264 } else {
DominiqueC 7:20c2ebe52306 265 setpointkm = 280; //11,12 graden naar rechts //oud 127 //30 graden
DominiqueC 4:305671faeb2b 266 lcd.cls();
DominiqueC 0:8d63bed88781 267 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 268 lcd.printf("rechts");
DominiqueC 0:8d63bed88781 269 }
DominiqueC 0:8d63bed88781 270 //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
DominiqueC 0:8d63bed88781 271 tijdtimer.reset();
DominiqueC 0:8d63bed88781 272 while (tijdtimer.read() <= 3) {
DominiqueC 0:8d63bed88781 273 while(looptimerflag == false);
DominiqueC 0:8d63bed88781 274 looptimerflag = false;
DominiqueC 0:8d63bed88781 275 new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
DominiqueC 0:8d63bed88781 276 clamp(&new_pwm_km, -1,1);
DominiqueC 0:8d63bed88781 277 if(new_pwm_km < 0)
DominiqueC 0:8d63bed88781 278 motordir2 = 1; //links
DominiqueC 0:8d63bed88781 279 else
DominiqueC 0:8d63bed88781 280 motordir2 = 0; //rechts
DominiqueC 0:8d63bed88781 281 pwm_motor2.write(abs(new_pwm_km));
DominiqueC 0:8d63bed88781 282 }
DominiqueC 0:8d63bed88781 283 wait(2);
DominiqueC 0:8d63bed88781 284 state = SLAAN;
DominiqueC 0:8d63bed88781 285 break;
DominiqueC 0:8d63bed88781 286 }
DominiqueC 0:8d63bed88781 287
DominiqueC 4:305671faeb2b 288 case SLAAN: { //batje slaan (gebruik biceps)
DominiqueC 0:8d63bed88781 289 wait(3);
DominiqueC 4:305671faeb2b 290 lcd.cls();
DominiqueC 0:8d63bed88781 291 lcd.locate(0,0);
DominiqueC 5:060d376291b5 292 lcd.printf("Richten"); //regel 1 LCD scherm
DominiqueC 5:060d376291b5 293 lcd.cls();
DominiqueC 0:8d63bed88781 294 lcd.locate(0,1);
DominiqueC 5:060d376291b5 295 lcd.printf("Kies goal!"); //regel 2 LCD scherm
DominiqueC 5:060d376291b5 296 wait(5);
DominiqueC 5:060d376291b5 297 lcd.cls();
DominiqueC 5:060d376291b5 298 lcd.locate(0,0);
DominiqueC 5:060d376291b5 299 lcd.printf("Meting loopt");
DominiqueC 5:060d376291b5 300 wait(2);
DominiqueC 0:8d63bed88781 301 float thetadot;
DominiqueC 0:8d63bed88781 302 float setpointgm;
DominiqueC 0:8d63bed88781 303 float new_pwm_gm;
DominiqueC 0:8d63bed88781 304 float setpointkm;
DominiqueC 0:8d63bed88781 305 float new_pwm_km;
DominiqueC 0:8d63bed88781 306 float kalibratiewaarde_biceps;
DominiqueC 0:8d63bed88781 307 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
DominiqueC 4:305671faeb2b 308 if (kalibratiewaarde_biceps <= 0.2) { //kalibratiewaarde_biceps<0.2 goal onderin
DominiqueC 0:8d63bed88781 309 thetadot=THETADOT0;
DominiqueC 4:305671faeb2b 310 lcd.cls();
DominiqueC 0:8d63bed88781 311 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 312 lcd.printf("ONDERSTE GOAL");
DominiqueC 4:305671faeb2b 313 } else if (kalibratiewaarde_biceps>0.2 && kalibratiewaarde_biceps<=0.4) { //0.2<kalibratiewaarde_biceps<0.4 goal midden
DominiqueC 0:8d63bed88781 314 thetadot=THETADOT1;
DominiqueC 4:305671faeb2b 315 lcd.cls();
DominiqueC 0:8d63bed88781 316 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 317 lcd.printf("MIDDELSTE GOAL");
DominiqueC 4:305671faeb2b 318 } else { //goal bovenin
DominiqueC 0:8d63bed88781 319 thetadot=THETADOT2;
DominiqueC 4:305671faeb2b 320 lcd.cls();
DominiqueC 0:8d63bed88781 321 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 322 lcd.printf("BOVENSTE GOAL");
DominiqueC 0:8d63bed88781 323 }
DominiqueC 0:8d63bed88781 324 wait(3);
DominiqueC 4:305671faeb2b 325 lcd.cls();
DominiqueC 0:8d63bed88781 326 lcd.locate(0,0);
DominiqueC 0:8d63bed88781 327 lcd.printf("Daar gaat ie!");
DominiqueC 0:8d63bed88781 328
DominiqueC 0:8d63bed88781 329 //MOTOR 1 LATEN BEWEGEN met snelheid thetadot
DominiqueC 0:8d63bed88781 330 tijdtimer.reset();
DominiqueC 0:8d63bed88781 331 tijdslaan.reset();
DominiqueC 0:8d63bed88781 332 while (tijdtimer.read() <=3) {
DominiqueC 0:8d63bed88781 333 while(looptimerflag == false);
DominiqueC 0:8d63bed88781 334 looptimerflag = false;
DominiqueC 0:8d63bed88781 335 if (motor1.getPosition()>= 444 ) {
DominiqueC 0:8d63bed88781 336 setpointgm = 444;
DominiqueC 0:8d63bed88781 337 } else {
DominiqueC 0:8d63bed88781 338 setpointgm = ((thetadot*tijdslaan.read())/RADTICKGM);
DominiqueC 0:8d63bed88781 339 }
DominiqueC 0:8d63bed88781 340 new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
DominiqueC 0:8d63bed88781 341 clamp(&new_pwm_gm, -1,1);
DominiqueC 0:8d63bed88781 342 if(new_pwm_gm < 0) {
DominiqueC 0:8d63bed88781 343 motordir1 = 1; //links
DominiqueC 0:8d63bed88781 344 } else {
DominiqueC 0:8d63bed88781 345 motordir1 = 0; //rechts
DominiqueC 0:8d63bed88781 346 }
DominiqueC 0:8d63bed88781 347 pwm_motor1.write(abs(new_pwm_gm));
DominiqueC 0:8d63bed88781 348 }
DominiqueC 0:8d63bed88781 349 //pc.printf("pos %d %f\n\r", motor1.getPosition(),setpointgm);
DominiqueC 0:8d63bed88781 350
DominiqueC 0:8d63bed88781 351 //MOTOR 2 OP POSITIE HOUDEN
DominiqueC 0:8d63bed88781 352 new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
DominiqueC 0:8d63bed88781 353 clamp(&new_pwm_km, -1,1);
DominiqueC 0:8d63bed88781 354 if(new_pwm_km < 0)
DominiqueC 0:8d63bed88781 355 motordir2 = 1; //links
DominiqueC 0:8d63bed88781 356 else
DominiqueC 0:8d63bed88781 357 motordir2 = 0; //rechts
DominiqueC 0:8d63bed88781 358 pwm_motor2.write(abs(new_pwm_km));
DominiqueC 0:8d63bed88781 359
DominiqueC 0:8d63bed88781 360 }
DominiqueC 0:8d63bed88781 361 state = HOME;
DominiqueC 0:8d63bed88781 362 break;
DominiqueC 0:8d63bed88781 363
DominiqueC 4:305671faeb2b 364 case HOME: { //terugbewegen naar beginpositie
DominiqueC 0:8d63bed88781 365 float setpointgm;
DominiqueC 0:8d63bed88781 366 float new_pwm_gm;
DominiqueC 0:8d63bed88781 367 float setpointkm;
DominiqueC 0:8d63bed88781 368 float new_pwm_km;
DominiqueC 0:8d63bed88781 369
DominiqueC 0:8d63bed88781 370 //MOTOR 1 naar home
DominiqueC 0:8d63bed88781 371 tijdtimer.reset();
DominiqueC 0:8d63bed88781 372 tijdslaan.reset();
DominiqueC 0:8d63bed88781 373 while (tijdtimer.read() <=6) {
DominiqueC 0:8d63bed88781 374 while(looptimerflag == false);
DominiqueC 0:8d63bed88781 375 looptimerflag = false;
DominiqueC 0:8d63bed88781 376 setpointgm = 0;
DominiqueC 0:8d63bed88781 377 new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
DominiqueC 0:8d63bed88781 378 clamp(&new_pwm_gm, -1,1);
DominiqueC 0:8d63bed88781 379 if(new_pwm_gm < 0)
DominiqueC 0:8d63bed88781 380 motordir1 = 1; //links
DominiqueC 0:8d63bed88781 381 else
DominiqueC 0:8d63bed88781 382 motordir1 = 0; //rechts
DominiqueC 0:8d63bed88781 383 pwm_motor1.write(abs(new_pwm_gm));
DominiqueC 0:8d63bed88781 384
DominiqueC 0:8d63bed88781 385 //MOTOR 2 naar home
DominiqueC 0:8d63bed88781 386 setpointkm=0;
DominiqueC 0:8d63bed88781 387 new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
DominiqueC 0:8d63bed88781 388 clamp(&new_pwm_km, -1,1);
DominiqueC 0:8d63bed88781 389 if(new_pwm_km < 0)
DominiqueC 0:8d63bed88781 390 motordir2 = 1; //links
DominiqueC 0:8d63bed88781 391 else
DominiqueC 0:8d63bed88781 392 motordir2 = 0; //rechts
DominiqueC 0:8d63bed88781 393 pwm_motor2.write(abs(new_pwm_km));
DominiqueC 0:8d63bed88781 394 }
DominiqueC 6:2c11ee0c05b4 395 pwm_motor1.write(0);
DominiqueC 6:2c11ee0c05b4 396 pwm_motor2.write(0);
DominiqueC 6:2c11ee0c05b4 397 wait(20);
DominiqueC 0:8d63bed88781 398 state = RICHTEN; //optioneel
DominiqueC 0:8d63bed88781 399 break;
DominiqueC 0:8d63bed88781 400 }
DominiqueC 0:8d63bed88781 401 default: {
DominiqueC 0:8d63bed88781 402 state = RUST;
DominiqueC 0:8d63bed88781 403 }
DominiqueC 0:8d63bed88781 404 }
DominiqueC 0:8d63bed88781 405 }
DominiqueC 0:8d63bed88781 406 }