the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of The_SLAP_5_1 by
main.cpp@2:3bf615031d7a, 2014-10-23 (annotated)
- Committer:
- Daanmk
- Date:
- Thu Oct 23 15:03:01 2014 +0000
- Revision:
- 2:3bf615031d7a
- Parent:
- 1:96cd4c9c5465
- Child:
- 3:81a6009303a9
LCD added;
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:3bf615031d7a | 25 | TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2, NC, NC, TextLCD::HD44780); // 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 | 1:96cd4c9c5465 | 40 | float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0; //variable to store value in for biceps |
Daanmk | 1:96cd4c9c5465 | 41 | float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1; //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 | 1:96cd4c9c5465 | 70 | enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; |
Daanmk | 1:96cd4c9c5465 | 71 | uint8_t state=RUST; |
Daanmk | 1:96cd4c9c5465 | 72 | |
Daanmk | 1:96cd4c9c5465 | 73 | void emgmeten(){ |
Daanmk | 0:b3809a8d9af1 | 74 | /*put raw emg value in emg_value*/ |
Daanmk | 0:b3809a8d9af1 | 75 | emg0_value_f32 = emg0.read(); |
Daanmk | 0:b3809a8d9af1 | 76 | emg1_value_f32 = emg1.read(); |
Daanmk | 0:b3809a8d9af1 | 77 | |
Daanmk | 0:b3809a8d9af1 | 78 | //process emg biceps |
Daanmk | 1:96cd4c9c5465 | 79 | arm_biquad_cascade_df1_f32(¬ch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 ); |
Daanmk | 1:96cd4c9c5465 | 80 | arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 81 | arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 82 | filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter |
Daanmk | 1:96cd4c9c5465 | 83 | arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 ); |
Daanmk | 0:b3809a8d9af1 | 84 | |
Daanmk | 0:b3809a8d9af1 | 85 | //process emg triceps |
Daanmk | 1:96cd4c9c5465 | 86 | arm_biquad_cascade_df1_f32(¬ch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 ); |
Daanmk | 1:96cd4c9c5465 | 87 | arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 88 | arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 ); |
Daanmk | 1:96cd4c9c5465 | 89 | filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter |
Daanmk | 1:96cd4c9c5465 | 90 | arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 ); |
Daanmk | 0:b3809a8d9af1 | 91 | } |
Daanmk | 1:96cd4c9c5465 | 92 | |
Daanmk | 0:b3809a8d9af1 | 93 | |
Daanmk | 0:b3809a8d9af1 | 94 | int main() |
Daanmk | 0:b3809a8d9af1 | 95 | { |
Daanmk | 1:96cd4c9c5465 | 96 | while(1) { |
Daanmk | 1:96cd4c9c5465 | 97 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
Daanmk | 1:96cd4c9c5465 | 98 | Ticker emg_timer; |
Daanmk | 1:96cd4c9c5465 | 99 | emg_timer.attach(emgmeten, TSAMP); |
Daanmk | 1:96cd4c9c5465 | 100 | arm_biquad_cascade_df1_init_f32(¬ch_biceps,1 , notch_const, notch_biceps_states); |
Daanmk | 1:96cd4c9c5465 | 101 | arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states); |
Daanmk | 1:96cd4c9c5465 | 102 | arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states); |
Daanmk | 1:96cd4c9c5465 | 103 | arm_biquad_cascade_df1_init_f32(¬ch_triceps,1 , notch_const, notch_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 104 | arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 105 | arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 106 | arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states); |
Daanmk | 1:96cd4c9c5465 | 107 | arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states); |
Daanmk | 2:3bf615031d7a | 108 | |
Daanmk | 1:96cd4c9c5465 | 109 | switch(state) { |
Daanmk | 2:3bf615031d7a | 110 | case RUST: { //Aanzetten |
Daanmk | 1:96cd4c9c5465 | 111 | |
Daanmk | 1:96cd4c9c5465 | 112 | state = KALIBRATIE; |
Daanmk | 1:96cd4c9c5465 | 113 | break; |
Daanmk | 1:96cd4c9c5465 | 114 | } |
Daanmk | 1:96cd4c9c5465 | 115 | |
Daanmk | 2:3bf615031d7a | 116 | case KALIBRATIE: { |
Daanmk | 2:3bf615031d7a | 117 | lc.cls(); |
Daanmk | 2:3bf615031d7a | 118 | lcd.locate(0,0); |
Daanmk | 2:3bf615031d7a | 119 | lcd.printf("Kalibratie"); //regel 1 LCD scherm |
Daanmk | 2:3bf615031d7a | 120 | lcd.locate(0,1); |
Daanmk | 2:3bf615031d7a | 121 | lcd.printf(""); //regel 2 LCD scherm |
Daanmk | 1:96cd4c9c5465 | 122 | wait(1); |
Daanmk | 1:96cd4c9c5465 | 123 | |
Daanmk | 1:96cd4c9c5465 | 124 | state = RICHTEN; |
Daanmk | 2:3bf615031d7a | 125 | break; |
Daanmk | 1:96cd4c9c5465 | 126 | } |
Daanmk | 1:96cd4c9c5465 | 127 | |
Daanmk | 2:3bf615031d7a | 128 | case RICHTEN: { //Batje richten |
Daanmk | 2:3bf615031d7a | 129 | lcd.printf("Richten"); |
Daanmk | 1:96cd4c9c5465 | 130 | wait(1); |
Daanmk | 1:96cd4c9c5465 | 131 | state = SLAAN; |
Daanmk | 1:96cd4c9c5465 | 132 | break; |
Daanmk | 1:96cd4c9c5465 | 133 | } |
Daanmk | 2:3bf615031d7a | 134 | case SLAAN: { //Balletje slaan |
Daanmk | 2:3bf615031d7a | 135 | lcd.printf("Slaan!"); |
Daanmk | 1:96cd4c9c5465 | 136 | wait(1); |
Daanmk | 1:96cd4c9c5465 | 137 | state = RICHTEN; |
Daanmk | 1:96cd4c9c5465 | 138 | break; |
Daanmk | 1:96cd4c9c5465 | 139 | } |
Daanmk | 1:96cd4c9c5465 | 140 | |
Daanmk | 1:96cd4c9c5465 | 141 | default: { |
Daanmk | 1:96cd4c9c5465 | 142 | state = RUST; |
Daanmk | 2:3bf615031d7a | 143 | } |
Daanmk | 1:96cd4c9c5465 | 144 | |
Daanmk | 2:3bf615031d7a | 145 | } |
Daanmk | 1:96cd4c9c5465 | 146 | |
Daanmk | 2:3bf615031d7a | 147 | } |
Daanmk | 2:3bf615031d7a | 148 | } |