Dominique Clevers / Mbed 2 deprecated testen_eindscript

voorlopige script getest (posities nog toevoegen)

Dependencies:   Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed

main.cpp@8:988ea0092101, 2014-11-01 (annotated)

Committer:
DominiqueC
Date:
Sat Nov 01 15:00:47 2014 +0000
Revision:
8:988ea0092101
Parent:
7:c2c3d1ade6bd 
EINDSCRIPT DELTOID EN BICEPS

Who changed what in which revision?

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