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@3:81a6009303a9, 2014-10-27 (annotated)

Committer:
Daanmk
Date:
Mon Oct 27 14:31:43 2014 +0000
Revision:
3:81a6009303a9
Parent:
2:3bf615031d7a
Child:
4:a0b0c944846e
mogelijke kalibratie

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 3:81a6009303a9 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; //variable to store value in for biceps
Daanmk 3:81a6009303a9 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; //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 3:81a6009303a9 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 1:96cd4c9c5465 109 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
Daanmk 1:96cd4c9c5465 110 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
Daanmk 1:96cd4c9c5465 111 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
Daanmk 1:96cd4c9c5465 112 arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
Daanmk 1:96cd4c9c5465 113 arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
Daanmk 1:96cd4c9c5465 114 arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
Daanmk 1:96cd4c9c5465 115 arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
Daanmk 1:96cd4c9c5465 116 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
Daanmk 2:3bf615031d7a 117
Daanmk 1:96cd4c9c5465 118 switch(state) {
Daanmk 2:3bf615031d7a 119 case RUST: { //Aanzetten
Daanmk 1:96cd4c9c5465 120
Daanmk 1:96cd4c9c5465 121 state = KALIBRATIE;
Daanmk 1:96cd4c9c5465 122 break;
Daanmk 1:96cd4c9c5465 123 }
Daanmk 1:96cd4c9c5465 124
Daanmk 2:3bf615031d7a 125 case KALIBRATIE: {
Daanmk 3:81a6009303a9 126 lcd.cls();
Daanmk 2:3bf615031d7a 127 lcd.locate(0,0);
Daanmk 2:3bf615031d7a 128 lcd.printf("Kalibratie"); //regel 1 LCD scherm
Daanmk 2:3bf615031d7a 129 lcd.locate(0,1);
Daanmk 3:81a6009303a9 130 lcd.printf("STAP 1: BICEPS"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 131 wait(1);
Daanmk 3:81a6009303a9 132 max_value_biceps=0;
Daanmk 3:81a6009303a9 133 max_value_triceps=0;
Daanmk 3:81a6009303a9 134 for(int x=0; x<200; x++);{ //BICEPS METING
Daanmk 3:81a6009303a9 135 if (envelop_emg0 > max_value_biceps); {
Daanmk 3:81a6009303a9 136 max_value_biceps = envelop_emg0;
Daanmk 3:81a6009303a9 137 }
Daanmk 3:81a6009303a9 138 }
Daanmk 3:81a6009303a9 139 lcd.cls();
Daanmk 3:81a6009303a9 140 lcd.locate(0,0);
Daanmk 3:81a6009303a9 141 lcd.printf("BICEPS OK"); //regel 1 LCD scherm
Daanmk 3:81a6009303a9 142 lcd.locate(0,1);
Daanmk 3:81a6009303a9 143 lcd.printf("STAP 2:TRICEPS"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 144 wait(1);
Daanmk 3:81a6009303a9 145 for(int x=0; x<200; x++); {//TRICEPS METING
Daanmk 3:81a6009303a9 146 if (envelop_emg1 > max_value_triceps); {
Daanmk 3:81a6009303a9 147 max_value_triceps = envelop_emg1;
Daanmk 3:81a6009303a9 148 }
Daanmk 3:81a6009303a9 149 }
Daanmk 3:81a6009303a9 150 lcd.cls();
Daanmk 3:81a6009303a9 151 lcd.locate(0,0);
Daanmk 3:81a6009303a9 152 lcd.printf("TRICEPS OK"); //regel 1 LCD scherm
Daanmk 3:81a6009303a9 153 lcd.locate(0,1);
Daanmk 3:81a6009303a9 154 lcd.printf("De waardes:"); //regel 2 LCD scherm
Daanmk 3:81a6009303a9 155 wait(0.5);
Daanmk 3:81a6009303a9 156 lcd.printf("%f,%f\n",max_value_biceps,max_value_triceps);
Daanmk 1:96cd4c9c5465 157 state = RICHTEN;
Daanmk 2:3bf615031d7a 158 break;
Daanmk 1:96cd4c9c5465 159 }
Daanmk 1:96cd4c9c5465 160
Daanmk 2:3bf615031d7a 161 case RICHTEN: { //Batje richten
Daanmk 3:81a6009303a9 162 lcd.printf("Richten");
Daanmk 3:81a6009303a9 163 wait(1);
Daanmk 3:81a6009303a9 164 max_value = 0;
Daanmk 3:81a6009303a9 165
Daanmk 3:81a6009303a9 166
Daanmk 3:81a6009303a9 167 keep_in_range(&pwm_to_motor, -1,1);
Daanmk 3:81a6009303a9 168 if(pwm_to_motor > 0)
Daanmk 3:81a6009303a9 169 motordir1.write(1);
Daanmk 3:81a6009303a9 170 else
Daanmk 3:81a6009303a9 171 motordir1.write(0);
Daanmk 3:81a6009303a9 172 pwm_motor1.write(abs(pwm_to_motor));
Daanmk 3:81a6009303a9 173 state = SLAAN;
Daanmk 1:96cd4c9c5465 174 break;
Daanmk 1:96cd4c9c5465 175 }
Daanmk 2:3bf615031d7a 176 case SLAAN: { //Balletje slaan
Daanmk 3:81a6009303a9 177 lcd.printf("Slaan!");
Daanmk 3:81a6009303a9 178 wait(1);
Daanmk 3:81a6009303a9 179 state = RICHTEN;
Daanmk 1:96cd4c9c5465 180 break;
Daanmk 1:96cd4c9c5465 181 }
Daanmk 1:96cd4c9c5465 182
Daanmk 3:81a6009303a9 183 default: {
Daanmk 1:96cd4c9c5465 184 state = RUST;
Daanmk 2:3bf615031d7a 185 }
Daanmk 1:96cd4c9c5465 186
Daanmk 2:3bf615031d7a 187 }
Daanmk 1:96cd4c9c5465 188
Daanmk 2:3bf615031d7a 189 }
Daanmk 2:3bf615031d7a 190 }