Dominique Clevers / Mbed 2 deprecated testen_eindscript

voorlopige script getest (posities nog toevoegen)

Dependencies:   Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed

main.cpp@0:653af6a8a659, 2014-10-30 (annotated)

Committer:
DominiqueC
Date:
Thu Oct 30 16:59:01 2014 +0000
Revision:
0:653af6a8a659
Child:
1:0d5864272412
test 1

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 "HIDScope.h"
DominiqueC 0:653af6a8a659 14 #include "arm_math.h"
DominiqueC 0:653af6a8a659 15 #include "encoder.h"
DominiqueC 0:653af6a8a659 16 #include "MODSERIAL.h"
DominiqueC 0:653af6a8a659 17 //include "TextLCD.h"
DominiqueC 0:653af6a8a659 18
DominiqueC 0:653af6a8a659 19 #define M2_PWM PTC8 //blauw
DominiqueC 0:653af6a8a659 20 #define M2_DIR PTC9 //groen
DominiqueC 0:653af6a8a659 21 #define M1_PWM PTA5 //kleine motor
DominiqueC 0:653af6a8a659 22 #define M1_DIR PTA4 //kleine motor
DominiqueC 0:653af6a8a659 23 #define TSAMP 0.005 // Sampletijd, 200Hz
DominiqueC 0:653af6a8a659 24 #define K_P_KM (0.1)
DominiqueC 0:653af6a8a659 25 #define K_I_KM (0.03 *TSAMP)
DominiqueC 0:653af6a8a659 26 #define K_D_KM (0.001 /TSAMP)
DominiqueC 0:653af6a8a659 27 #define K_P_GM (2.9)
DominiqueC 0:653af6a8a659 28 #define K_I_GM (0.3 *TSAMP)
DominiqueC 0:653af6a8a659 29 #define K_D_GM (0.003 /TSAMP)
DominiqueC 0:653af6a8a659 30 #define I_LIMIT 1.
DominiqueC 0:653af6a8a659 31 #define RADTICKGM 0.003927
DominiqueC 0:653af6a8a659 32 #define THETADOT0 6.85
DominiqueC 0:653af6a8a659 33 #define THETADOT1 7.77
DominiqueC 0:653af6a8a659 34 #define THETADOT2 9.21
DominiqueC 0:653af6a8a659 35
DominiqueC 0:653af6a8a659 36 //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 37
DominiqueC 0:653af6a8a659 38 Encoder motor2(PTD2,PTD0); //geel,wit kleine motor
DominiqueC 0:653af6a8a659 39 Encoder motor1(PTD5,PTA13);//geel,wit
DominiqueC 0:653af6a8a659 40 PwmOut pwm_motor1(M1_PWM);
DominiqueC 0:653af6a8a659 41 PwmOut pwm_motor2(M2_PWM);
DominiqueC 0:653af6a8a659 42 DigitalOut motordir2(M2_DIR);
DominiqueC 0:653af6a8a659 43 DigitalOut motordir1(M1_DIR);
DominiqueC 0:653af6a8a659 44 AnalogIn emg0(PTB0); //Biceps
DominiqueC 0:653af6a8a659 45 AnalogIn emg1(PTB1); //Triceps
DominiqueC 0:653af6a8a659 46 HIDScope scope(6);
DominiqueC 0:653af6a8a659 47
DominiqueC 0:653af6a8a659 48 MODSERIAL pc(USBTX,USBRX,64,1024);
DominiqueC 0:653af6a8a659 49
DominiqueC 0:653af6a8a659 50
DominiqueC 0:653af6a8a659 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
DominiqueC 0:653af6a8a659 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
DominiqueC 0:653af6a8a659 53
DominiqueC 0:653af6a8a659 54 arm_biquad_casd_df1_inst_f32 notch_biceps;
DominiqueC 0:653af6a8a659 55 arm_biquad_casd_df1_inst_f32 notch_triceps;
DominiqueC 0:653af6a8a659 56 // constants for 50 Hz notch (bandbreedte 2 Hz)
DominiqueC 0:653af6a8a659 57 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
DominiqueC 0:653af6a8a659 58 //state values
DominiqueC 0:653af6a8a659 59 float notch_biceps_states[4];
DominiqueC 0:653af6a8a659 60 float notch_triceps_states[4];
DominiqueC 0:653af6a8a659 61
DominiqueC 0:653af6a8a659 62 arm_biquad_casd_df1_inst_f32 highpass_biceps;
DominiqueC 0:653af6a8a659 63 arm_biquad_casd_df1_inst_f32 highpass_triceps;
DominiqueC 0:653af6a8a659 64 //constants for 20Hz highpass
DominiqueC 0:653af6a8a659 65 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
DominiqueC 0:653af6a8a659 66 //state values
DominiqueC 0:653af6a8a659 67 float highpass_biceps_states[4];
DominiqueC 0:653af6a8a659 68 float highpass_triceps_states[4];
DominiqueC 0:653af6a8a659 69
DominiqueC 0:653af6a8a659 70 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
DominiqueC 0:653af6a8a659 71 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
DominiqueC 0:653af6a8a659 72 //constants for 80Hz lowpass
DominiqueC 0:653af6a8a659 73 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
DominiqueC 0:653af6a8a659 74 //state values
DominiqueC 0:653af6a8a659 75 float lowpass_biceps_states[4];
DominiqueC 0:653af6a8a659 76 float lowpass_triceps_states[4];
DominiqueC 0:653af6a8a659 77
DominiqueC 0:653af6a8a659 78 arm_biquad_casd_df1_inst_f32 envelop_biceps;
DominiqueC 0:653af6a8a659 79 arm_biquad_casd_df1_inst_f32 envelop_triceps;
DominiqueC 0:653af6a8a659 80 //constants for envelop
DominiqueC 0:653af6a8a659 81 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
DominiqueC 0:653af6a8a659 82 // state values
DominiqueC 0:653af6a8a659 83 float envelop_biceps_states[4];
DominiqueC 0:653af6a8a659 84 float envelop_triceps_states[4];
DominiqueC 0:653af6a8a659 85
DominiqueC 0:653af6a8a659 86 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES
DominiqueC 0:653af6a8a659 87 uint8_t state=RUST;
DominiqueC 0:653af6a8a659 88
DominiqueC 0:653af6a8a659 89 enum kalibratiestates {BICEPSMAX,TRICEPSMAX};
DominiqueC 0:653af6a8a659 90
DominiqueC 0:653af6a8a659 91 volatile bool looptimerflag;
DominiqueC 0:653af6a8a659 92 void setlooptimerflag(void)
DominiqueC 0:653af6a8a659 93 {
DominiqueC 0:653af6a8a659 94 looptimerflag = true;
DominiqueC 0:653af6a8a659 95 }
DominiqueC 0:653af6a8a659 96
DominiqueC 0:653af6a8a659 97 void clamp(float * in, float min, float max)
DominiqueC 0:653af6a8a659 98 {
DominiqueC 0:653af6a8a659 99 *in > min ? *in < max? : *in = max: *in = min;
DominiqueC 0:653af6a8a659 100 }
DominiqueC 0:653af6a8a659 101
DominiqueC 0:653af6a8a659 102 float pidkm(float setpointkm, float measurementkm) //PID Regelaar kleine motor
DominiqueC 0:653af6a8a659 103 {
DominiqueC 0:653af6a8a659 104 float error_km;
DominiqueC 0:653af6a8a659 105 static float prev_error_km = 0;
DominiqueC 0:653af6a8a659 106 float out_p_km = 0;
DominiqueC 0:653af6a8a659 107 static float out_i_km = 0; //static, want dan wordt vorige waarde onthouden
DominiqueC 0:653af6a8a659 108 float out_d_km = 0;
DominiqueC 0:653af6a8a659 109 error_km = setpointkm-measurementkm;
DominiqueC 0:653af6a8a659 110 out_p_km = error_km*K_P_KM;
DominiqueC 0:653af6a8a659 111 out_i_km += error_km*K_I_KM;
DominiqueC 0:653af6a8a659 112 out_d_km = (error_km-prev_error_km)*K_D_KM;
DominiqueC 0:653af6a8a659 113 clamp(&out_i_km,-I_LIMIT,I_LIMIT);
DominiqueC 0:653af6a8a659 114 prev_error_km = error_km;
DominiqueC 0:653af6a8a659 115 return out_p_km + out_i_km + out_d_km;
DominiqueC 0:653af6a8a659 116 }
DominiqueC 0:653af6a8a659 117
DominiqueC 0:653af6a8a659 118 float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
DominiqueC 0:653af6a8a659 119 {
DominiqueC 0:653af6a8a659 120 float error_gm;
DominiqueC 0:653af6a8a659 121 static float prev_error_gm = 0;
DominiqueC 0:653af6a8a659 122 float out_p_gm = 0;
DominiqueC 0:653af6a8a659 123 static float out_i_gm = 0;
DominiqueC 0:653af6a8a659 124 float out_d_gm = 0;
DominiqueC 0:653af6a8a659 125 error_gm = setpointgm-measurementgm;
DominiqueC 0:653af6a8a659 126 out_p_gm = error_gm*K_P_GM;
DominiqueC 0:653af6a8a659 127 out_i_gm += error_gm*K_I_GM;
DominiqueC 0:653af6a8a659 128 out_d_gm = (error_gm-prev_error_gm)*K_D_GM;
DominiqueC 0:653af6a8a659 129 clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
DominiqueC 0:653af6a8a659 130 prev_error_gm = error_gm;
DominiqueC 0:653af6a8a659 131 return out_p_gm + out_i_gm + out_d_gm;
DominiqueC 0:653af6a8a659 132 }
DominiqueC 0:653af6a8a659 133 void emgmeten()
DominiqueC 0:653af6a8a659 134 {
DominiqueC 0:653af6a8a659 135 /*put raw emg value in emg_value*/
DominiqueC 0:653af6a8a659 136 emg0_value_f32 = emg0.read();
DominiqueC 0:653af6a8a659 137 emg1_value_f32 = emg1.read();
DominiqueC 0:653af6a8a659 138
DominiqueC 0:653af6a8a659 139 //process emg biceps
DominiqueC 0:653af6a8a659 140 arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
DominiqueC 0:653af6a8a659 141 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
DominiqueC 0:653af6a8a659 142 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
DominiqueC 0:653af6a8a659 143 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter
DominiqueC 0:653af6a8a659 144 arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
DominiqueC 0:653af6a8a659 145
DominiqueC 0:653af6a8a659 146 //process emg triceps
DominiqueC 0:653af6a8a659 147 arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
DominiqueC 0:653af6a8a659 148 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
DominiqueC 0:653af6a8a659 149 arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
DominiqueC 0:653af6a8a659 150 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter
DominiqueC 0:653af6a8a659 151 arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
DominiqueC 0:653af6a8a659 152 }
DominiqueC 0:653af6a8a659 153
DominiqueC 0:653af6a8a659 154
DominiqueC 0:653af6a8a659 155 int main()
DominiqueC 0:653af6a8a659 156 {
DominiqueC 0:653af6a8a659 157 pc.baud(38400); //PC baud rate is 38400 bits/seconde
DominiqueC 0:653af6a8a659 158 Ticker emg_timer;
DominiqueC 0:653af6a8a659 159 emg_timer.attach(emgmeten, TSAMP);
DominiqueC 0:653af6a8a659 160 Ticker looptimer;
DominiqueC 0:653af6a8a659 161 looptimer.attach(setlooptimerflag,TSAMP);
DominiqueC 0:653af6a8a659 162 Timer tijdtimer;
DominiqueC 0:653af6a8a659 163 Timer tijdslaan;
DominiqueC 0:653af6a8a659 164 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
DominiqueC 0:653af6a8a659 165 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
DominiqueC 0:653af6a8a659 166 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
DominiqueC 0:653af6a8a659 167 arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
DominiqueC 0:653af6a8a659 168 arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
DominiqueC 0:653af6a8a659 169 arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
DominiqueC 0:653af6a8a659 170 arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
DominiqueC 0:653af6a8a659 171 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
DominiqueC 0:653af6a8a659 172 while(true) {
DominiqueC 0:653af6a8a659 173 switch(state) {
DominiqueC 0:653af6a8a659 174 case RUST: { //Aanzetten
DominiqueC 0:653af6a8a659 175 pc.printf("--THE SLAP -- GROEP 5"); //pc scherm
DominiqueC 0:653af6a8a659 176 wait(5);
DominiqueC 0:653af6a8a659 177 state = KALIBRATIE;
DominiqueC 0:653af6a8a659 178 break;
DominiqueC 0:653af6a8a659 179 }
DominiqueC 0:653af6a8a659 180
DominiqueC 0:653af6a8a659 181 case KALIBRATIE: { //kalibreren met maximale inspanning
DominiqueC 0:653af6a8a659 182 max_value_biceps=0;
DominiqueC 0:653af6a8a659 183 max_value_triceps=0;
DominiqueC 0:653af6a8a659 184 state = BICEPSMAX;
DominiqueC 0:653af6a8a659 185 switch(state) {
DominiqueC 0:653af6a8a659 186 case BICEPSMAX: { //maximale inspanning biceps
DominiqueC 0:653af6a8a659 187 pc.printf("Kalibratie. 1:BICEPS MAX"); //pc scherm
DominiqueC 0:653af6a8a659 188 wait(1);
DominiqueC 0:653af6a8a659 189 tijdtimer.start();
DominiqueC 0:653af6a8a659 190 pc.printf("Biceps meting, meting loopt"); //pc scherm
DominiqueC 0:653af6a8a659 191 while (tijdtimer <= 3) {
DominiqueC 0:653af6a8a659 192 if (envelop_emg0 > max_value_biceps) {
DominiqueC 0:653af6a8a659 193 max_value_biceps = envelop_emg0;
DominiqueC 0:653af6a8a659 194 }
DominiqueC 0:653af6a8a659 195 }
DominiqueC 0:653af6a8a659 196 if (tijdtimer >= 3) {
DominiqueC 0:653af6a8a659 197 tijdtimer.stop();
DominiqueC 0:653af6a8a659 198 tijdtimer.reset();
DominiqueC 0:653af6a8a659 199 pc.printf("max value %f\n\r", max_value_biceps); //pc scherm
DominiqueC 0:653af6a8a659 200 wait(1);
DominiqueC 0:653af6a8a659 201 state = TRICEPSMAX;
DominiqueC 0:653af6a8a659 202 } //einde if statement
DominiqueC 0:653af6a8a659 203 break;
DominiqueC 0:653af6a8a659 204 } //einde case bicepsmax
DominiqueC 0:653af6a8a659 205 case TRICEPSMAX: { //maximale inspanning triceps
DominiqueC 0:653af6a8a659 206 pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
DominiqueC 0:653af6a8a659 207 wait(1);
DominiqueC 0:653af6a8a659 208 tijdtimer.start();
DominiqueC 0:653af6a8a659 209 pc.printf("Triceps meting, meting loopt!"); //pc scherm
DominiqueC 0:653af6a8a659 210 while (tijdtimer <= 3) {
DominiqueC 0:653af6a8a659 211 if (envelop_emg1 > max_value_triceps) {
DominiqueC 0:653af6a8a659 212 max_value_triceps = envelop_emg1;
DominiqueC 0:653af6a8a659 213 }
DominiqueC 0:653af6a8a659 214 }
DominiqueC 0:653af6a8a659 215 if (tijdtimer >= 3) {
DominiqueC 0:653af6a8a659 216 tijdtimer.stop();
DominiqueC 0:653af6a8a659 217 tijdtimer.reset();
DominiqueC 0:653af6a8a659 218 pc.printf("max value %f\n\r", max_value_triceps);
DominiqueC 0:653af6a8a659 219 wait(1);
DominiqueC 0:653af6a8a659 220 } //einde if statement
DominiqueC 0:653af6a8a659 221 break;
DominiqueC 0:653af6a8a659 222 } //einde case tricepsmax
DominiqueC 0:653af6a8a659 223 default: {
DominiqueC 0:653af6a8a659 224 state = BICEPSMAX;
DominiqueC 0:653af6a8a659 225 } //einde default
DominiqueC 0:653af6a8a659 226 } //einde switch states
DominiqueC 0:653af6a8a659 227 break;
DominiqueC 0:653af6a8a659 228 }}}} // einde kalibratie case