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@5:5383d9a80307, 2014-10-27 (annotated)

Committer:
Daanmk
Date:
Mon Oct 27 15:43:34 2014 +0000
Revision:
5:5383d9a80307
Parent:
4:a0b0c944846e
Child:
6:cb5daf35ba9b
tot richten gekomen

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 0:b3809a8d9af1 24
Daanmk 3:81a6009303a9 25 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 26
Daanmk 1:96cd4c9c5465 27 Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor
Daanmk 1:96cd4c9c5465 28 Encoder motor1(PTD5,PTA13);//geel,wit
Daanmk 0:b3809a8d9af1 29 PwmOut pwm_motor1(M1_PWM);
Daanmk 0:b3809a8d9af1 30 PwmOut pwm_motor2(M2_PWM);
Daanmk 0:b3809a8d9af1 31 DigitalOut motordir2(M2_DIR);
Daanmk 0:b3809a8d9af1 32 DigitalOut motordir1(M1_DIR);
Daanmk 0:b3809a8d9af1 33 AnalogIn emg0(PTB0); //Biceps
Daanmk 0:b3809a8d9af1 34 AnalogIn emg1(PTB1); //Triceps
Daanmk 0:b3809a8d9af1 35 HIDScope scope(6);
Daanmk 0:b3809a8d9af1 36
Daanmk 1:96cd4c9c5465 37 MODSERIAL pc(USBTX,USBRX,64,1024);
Daanmk 1:96cd4c9c5465 38
Daanmk 0:b3809a8d9af1 39
Daanmk 4:a0b0c944846e 40 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 4:a0b0c944846e 41 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; //variable to store value in for triceps
Daanmk 0:b3809a8d9af1 42
Daanmk 1:96cd4c9c5465 43 arm_biquad_casd_df1_inst_f32 notch_biceps;
Daanmk 1:96cd4c9c5465 44 arm_biquad_casd_df1_inst_f32 notch_triceps;
Daanmk 1:96cd4c9c5465 45 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
Daanmk 1:96cd4c9c5465 46 float notch_biceps_states[4]; //state values
Daanmk 1:96cd4c9c5465 47 float notch_triceps_states[4];
Daanmk 0:b3809a8d9af1 48
Daanmk 1:96cd4c9c5465 49 arm_biquad_casd_df1_inst_f32 highpass_biceps;
Daanmk 1:96cd4c9c5465 50 arm_biquad_casd_df1_inst_f32 highpass_triceps;
Daanmk 1:96cd4c9c5465 51 //constants for 20Hz highpass
Daanmk 1:96cd4c9c5465 52 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 53 //state values
Daanmk 1:96cd4c9c5465 54 float highpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 55 float highpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 56 //constants for 80Hz lowpass
Daanmk 1:96cd4c9c5465 57 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
Daanmk 1:96cd4c9c5465 58 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
Daanmk 1:96cd4c9c5465 59 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 60 //state values
Daanmk 1:96cd4c9c5465 61 float lowpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 62 float lowpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 63
Daanmk 1:96cd4c9c5465 64 arm_biquad_casd_df1_inst_f32 envelop_biceps;
Daanmk 1:96cd4c9c5465 65 arm_biquad_casd_df1_inst_f32 envelop_triceps;
Daanmk 1:96cd4c9c5465 66 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
Daanmk 1:96cd4c9c5465 67 float envelop_biceps_states[4];
Daanmk 1:96cd4c9c5465 68 float envelop_triceps_states[4];
Daanmk 1:96cd4c9c5465 69
Daanmk 5:5383d9a80307 70 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES
Daanmk 1:96cd4c9c5465 71 uint8_t state=RUST;
Daanmk 1:96cd4c9c5465 72
Daanmk 3:81a6009303a9 73 volatile bool looptimerflag;
Daanmk 3:81a6009303a9 74 void setlooptimerflag(void)
Daanmk 3:81a6009303a9 75 {
Daanmk 3:81a6009303a9 76 looptimerflag = true;
Daanmk 3:81a6009303a9 77 }
Daanmk 3:81a6009303a9 78
Daanmk 3:81a6009303a9 79 void keep_in_range(float * in, float min, float max); //keep in range
Daanmk 3:81a6009303a9 80
Daanmk 1:96cd4c9c5465 81 void emgmeten(){
Daanmk 0:b3809a8d9af1 82 /*put raw emg value in emg_value*/
Daanmk 0:b3809a8d9af1 83 emg0_value_f32 = emg0.read();
Daanmk 0:b3809a8d9af1 84 emg1_value_f32 = emg1.read();
Daanmk 0:b3809a8d9af1 85
Daanmk 0:b3809a8d9af1 86 //process emg biceps
Daanmk 1:96cd4c9c5465 87 arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
Daanmk 1:96cd4c9c5465 88 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 89 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 90 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 91 arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
Daanmk 0:b3809a8d9af1 92
Daanmk 0:b3809a8d9af1 93 //process emg triceps
Daanmk 1:96cd4c9c5465 94 arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
Daanmk 1:96cd4c9c5465 95 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 96 arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 97 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 98 arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
Daanmk 0:b3809a8d9af1 99 }
Daanmk 1:96cd4c9c5465 100
Daanmk 0:b3809a8d9af1 101 int main()
Daanmk 0:b3809a8d9af1 102 {
Daanmk 1:96cd4c9c5465 103 while(1) {
Daanmk 1:96cd4c9c5465 104 pc.baud(38400); //PC baud rate is 38400 bits/seconde
Daanmk 1:96cd4c9c5465 105 Ticker emg_timer;
Daanmk 1:96cd4c9c5465 106 emg_timer.attach(emgmeten, TSAMP);
Daanmk 3:81a6009303a9 107 Ticker looptimer;
Daanmk 3:81a6009303a9 108 looptimer.attach(setlooptimerflag,TSAMP);
Daanmk 4:a0b0c944846e 109 Timer tijdtimer;
Daanmk 1:96cd4c9c5465 110 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
Daanmk 1:96cd4c9c5465 111 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
Daanmk 1:96cd4c9c5465 112 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
Daanmk 1:96cd4c9c5465 113 arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
Daanmk 1:96cd4c9c5465 114 arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
Daanmk 1:96cd4c9c5465 115 arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
Daanmk 1:96cd4c9c5465 116 arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
Daanmk 1:96cd4c9c5465 117 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
Daanmk 2:3bf615031d7a 118
Daanmk 1:96cd4c9c5465 119 switch(state) {
Daanmk 2:3bf615031d7a 120 case RUST: { //Aanzetten
Daanmk 1:96cd4c9c5465 121
Daanmk 1:96cd4c9c5465 122 state = KALIBRATIE;
Daanmk 1:96cd4c9c5465 123 break;
Daanmk 1:96cd4c9c5465 124 }
Daanmk 1:96cd4c9c5465 125
Daanmk 2:3bf615031d7a 126 case KALIBRATIE: {
Daanmk 3:81a6009303a9 127 lcd.cls();
Daanmk 2:3bf615031d7a 128 lcd.locate(0,0);
Daanmk 2:3bf615031d7a 129 lcd.printf("Kalibratie"); //regel 1 LCD scherm
Daanmk 2:3bf615031d7a 130 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 131 lcd.printf("1:BICEPS RUST"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 132 wait(1);
Daanmk 3:81a6009303a9 133 max_value_biceps=0;
Daanmk 3:81a6009303a9 134 max_value_triceps=0;
Daanmk 4:a0b0c944846e 135 min_value_biceps=0;
Daanmk 4:a0b0c944846e 136 min_value_triceps=0;
Daanmk 4:a0b0c944846e 137 int metingstatus=1;
Daanmk 4:a0b0c944846e 138
Daanmk 4:a0b0c944846e 139 if (metingstatus==1) { //BICEPS RUST METING
Daanmk 4:a0b0c944846e 140
Daanmk 4:a0b0c944846e 141 lcd.cls();
Daanmk 4:a0b0c944846e 142 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 143 lcd.printf("BICEPS RUST"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 144 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 145 lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 146 wait(1);
Daanmk 4:a0b0c944846e 147 tijdtimer.start();
Daanmk 4:a0b0c944846e 148
Daanmk 4:a0b0c944846e 149 if (envelop_emg0 > min_value_biceps) {
Daanmk 4:a0b0c944846e 150 min_value_biceps = envelop_emg0;
Daanmk 4:a0b0c944846e 151 }
Daanmk 4:a0b0c944846e 152 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 153 tijdtimer.stop();
Daanmk 4:a0b0c944846e 154 tijdtimer.reset();
Daanmk 4:a0b0c944846e 155 metingstatus=2;
Daanmk 4:a0b0c944846e 156 }
Daanmk 3:81a6009303a9 157 }
Daanmk 4:a0b0c944846e 158 if (metingstatus==2) { //TRICEPS RUST METING
Daanmk 4:a0b0c944846e 159 lcd.cls();
Daanmk 4:a0b0c944846e 160 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 161 lcd.printf("TRICEPS RUST"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 162 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 163 lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 164 wait(1);
Daanmk 4:a0b0c944846e 165 tijdtimer.start();
Daanmk 4:a0b0c944846e 166 if (envelop_emg1 > min_value_triceps) {
Daanmk 4:a0b0c944846e 167 min_value_triceps = envelop_emg1; }
Daanmk 4:a0b0c944846e 168 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 169 tijdtimer.stop();
Daanmk 4:a0b0c944846e 170 tijdtimer.reset();
Daanmk 4:a0b0c944846e 171 metingstatus=3;
Daanmk 4:a0b0c944846e 172 }
Daanmk 4:a0b0c944846e 173 if (metingstatus==3) { //BICEPS KRACHT METING
Daanmk 4:a0b0c944846e 174 lcd.cls();
Daanmk 4:a0b0c944846e 175 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 176 lcd.printf("BICEPS MAX"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 177 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 178 lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 179 wait(1);
Daanmk 4:a0b0c944846e 180 tijdtimer.start();
Daanmk 4:a0b0c944846e 181 if (envelop_emg0 > max_value_biceps) {
Daanmk 4:a0b0c944846e 182 max_value_biceps = envelop_emg0; }
Daanmk 4:a0b0c944846e 183 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 184 tijdtimer.stop();
Daanmk 4:a0b0c944846e 185 tijdtimer.reset();
Daanmk 4:a0b0c944846e 186 metingstatus=4;
Daanmk 4:a0b0c944846e 187 }
Daanmk 4:a0b0c944846e 188 if (metingstatus==4) {//TRICEPS KRACHT METING
Daanmk 4:a0b0c944846e 189 lcd.cls();
Daanmk 4:a0b0c944846e 190 lcd.locate(0,0);
Daanmk 4:a0b0c944846e 191 lcd.printf("TRICEPS MAX"); //regel 1 LCD scherm
Daanmk 4:a0b0c944846e 192 lcd.locate(0,1);
Daanmk 4:a0b0c944846e 193 lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm
Daanmk 4:a0b0c944846e 194 tijdtimer.start();
Daanmk 4:a0b0c944846e 195 if (envelop_emg1 > max_value_triceps) {
Daanmk 4:a0b0c944846e 196 max_value_triceps = envelop_emg1; }
Daanmk 4:a0b0c944846e 197 if (tijdtimer == 3 ) {
Daanmk 4:a0b0c944846e 198 tijdtimer.stop();
Daanmk 4:a0b0c944846e 199 tijdtimer.reset();
Daanmk 4:a0b0c944846e 200 metingstatus=5;
Daanmk 4:a0b0c944846e 201 }
Daanmk 4:a0b0c944846e 202 if (metingstatus==5) {
Daanmk 5:5383d9a80307 203 lcd.cls();
Daanmk 5:5383d9a80307 204 lcd.locate(0,0);
Daanmk 5:5383d9a80307 205 lcd.printf("Kalibratie OK"); //regel 1 LCD scherm
Daanmk 5:5383d9a80307 206 lcd.locate(0,1);
Daanmk 5:5383d9a80307 207 lcd.printf(""); //regel 2 LCD scherm
Daanmk 5:5383d9a80307 208 wait(1);
Daanmk 4:a0b0c944846e 209 state = RICHTEN; }
Daanmk 2:3bf615031d7a 210 break;
Daanmk 1:96cd4c9c5465 211 }
Daanmk 1:96cd4c9c5465 212
Daanmk 2:3bf615031d7a 213 case RICHTEN: { //Batje richten
Daanmk 5:5383d9a80307 214 lcd.cls();
Daanmk 5:5383d9a80307 215 lcd.locate(0,0);
Daanmk 5:5383d9a80307 216 lcd.printf("RICHTEN"); //regel 1 LCD scherm
Daanmk 5:5383d9a80307 217 lcd.locate(0,1);
Daanmk 5:5383d9a80307 218 lcd.printf("KIES GOAL"); //regel 2 LCD scherm
Daanmk 5:5383d9a80307 219 wait(1);
Daanmk 5:5383d9a80307 220 int goalkeuze=1; //midden goadl
Daanmk 5:5383d9a80307 221 float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
Daanmk 5:5383d9a80307 222 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT
Daanmk 5:5383d9a80307 223 kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
Daanmk 5:5383d9a80307 224 tijdtimer.start();
Daanmk 5:5383d9a80307 225 if (kalibratiewaarde_biceps>0.3);{
Daanmk 5:5383d9a80307 226 goalkeuze = 0; //linker goal
Daanmk 5:5383d9a80307 227 }
Daanmk 5:5383d9a80307 228 if (kalibratiewaarde_triceps>0.3); {
Daanmk 5:5383d9a80307 229 goalkeuze = 2; //rechter goal
Daanmk 5:5383d9a80307 230 }
Daanmk 5:5383d9a80307 231 if (tijdtimer == 5) {
Daanmk 5:5383d9a80307 232 tijdtimer.stop();
Daanmk 5:5383d9a80307 233 tijdtimer.reset();
Daanmk 5:5383d9a80307 234 state = SLAAN;
Daanmk 5:5383d9a80307 235 }
Daanmk 5:5383d9a80307 236
Daanmk 5:5383d9a80307 237
Daanmk 5:5383d9a80307 238
Daanmk 3:81a6009303a9 239
Daanmk 3:81a6009303a9 240
Daanmk 3:81a6009303a9 241 keep_in_range(&pwm_to_motor, -1,1);
Daanmk 3:81a6009303a9 242 if(pwm_to_motor > 0)
Daanmk 3:81a6009303a9 243 motordir1.write(1);
Daanmk 3:81a6009303a9 244 else
Daanmk 3:81a6009303a9 245 motordir1.write(0);
Daanmk 3:81a6009303a9 246 pwm_motor1.write(abs(pwm_to_motor));
Daanmk 5:5383d9a80307 247
Daanmk 1:96cd4c9c5465 248 break;
Daanmk 1:96cd4c9c5465 249 }
Daanmk 2:3bf615031d7a 250 case SLAAN: { //Balletje slaan
Daanmk 3:81a6009303a9 251 lcd.printf("Slaan!");
Daanmk 3:81a6009303a9 252 wait(1);
Daanmk 3:81a6009303a9 253 state = RICHTEN;
Daanmk 1:96cd4c9c5465 254 break;
Daanmk 1:96cd4c9c5465 255 }
Daanmk 1:96cd4c9c5465 256
Daanmk 3:81a6009303a9 257 default: {
Daanmk 1:96cd4c9c5465 258 state = RUST;
Daanmk 2:3bf615031d7a 259 }
Daanmk 1:96cd4c9c5465 260
Daanmk 2:3bf615031d7a 261 }
Daanmk 1:96cd4c9c5465 262
Daanmk 2:3bf615031d7a 263 }
Daanmk 5:5383d9a80307 264 }
Daanmk 5:5383d9a80307 265
Daanmk 2:3bf615031d7a 266 }