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@1:96cd4c9c5465, 2014-10-22 (annotated)

Committer:
Daanmk
Date:
Wed Oct 22 14:16:53 2014 +0000
Revision:
1:96cd4c9c5465
Parent:
0:b3809a8d9af1
Child:
2:3bf615031d7a
Cases, filters al toegevoegd

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 0:b3809a8d9af1 17
Daanmk 1:96cd4c9c5465 18 #define M2_PWM PTC8 //blauw
Daanmk 1:96cd4c9c5465 19 #define M2_DIR PTC9 //groen
Daanmk 1:96cd4c9c5465 20 #define M1_PWM PTA5 //kleine motor
Daanmk 1:96cd4c9c5465 21 #define M1_DIR PTA4 //kleine motor
Daanmk 0:b3809a8d9af1 22 #define TSAMP 0.005 // Sampletijd, 200Hz
Daanmk 0:b3809a8d9af1 23
Daanmk 1:96cd4c9c5465 24 Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor
Daanmk 1:96cd4c9c5465 25 Encoder motor1(PTD5,PTA13);//geel,wit
Daanmk 0:b3809a8d9af1 26 PwmOut pwm_motor1(M1_PWM);
Daanmk 0:b3809a8d9af1 27 PwmOut pwm_motor2(M2_PWM);
Daanmk 0:b3809a8d9af1 28 DigitalOut motordir2(M2_DIR);
Daanmk 0:b3809a8d9af1 29 DigitalOut motordir1(M1_DIR);
Daanmk 0:b3809a8d9af1 30 AnalogIn emg0(PTB0); //Biceps
Daanmk 0:b3809a8d9af1 31 AnalogIn emg1(PTB1); //Triceps
Daanmk 0:b3809a8d9af1 32 HIDScope scope(6);
Daanmk 0:b3809a8d9af1 33
Daanmk 1:96cd4c9c5465 34 MODSERIAL pc(USBTX,USBRX,64,1024);
Daanmk 1:96cd4c9c5465 35
Daanmk 0:b3809a8d9af1 36
Daanmk 1:96cd4c9c5465 37 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 38 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 39
Daanmk 1:96cd4c9c5465 40 arm_biquad_casd_df1_inst_f32 notch_biceps;
Daanmk 1:96cd4c9c5465 41 arm_biquad_casd_df1_inst_f32 notch_triceps;
Daanmk 1:96cd4c9c5465 42 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
Daanmk 1:96cd4c9c5465 43 float notch_biceps_states[4]; //state values
Daanmk 1:96cd4c9c5465 44 float notch_triceps_states[4];
Daanmk 0:b3809a8d9af1 45
Daanmk 1:96cd4c9c5465 46 arm_biquad_casd_df1_inst_f32 highpass_biceps;
Daanmk 1:96cd4c9c5465 47 arm_biquad_casd_df1_inst_f32 highpass_triceps;
Daanmk 1:96cd4c9c5465 48 //constants for 20Hz highpass
Daanmk 1:96cd4c9c5465 49 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 50 //state values
Daanmk 1:96cd4c9c5465 51 float highpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 52 float highpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 53 //constants for 80Hz lowpass
Daanmk 1:96cd4c9c5465 54 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
Daanmk 1:96cd4c9c5465 55 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
Daanmk 1:96cd4c9c5465 56 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
Daanmk 1:96cd4c9c5465 57 //state values
Daanmk 1:96cd4c9c5465 58 float lowpass_biceps_states[4];
Daanmk 1:96cd4c9c5465 59 float lowpass_triceps_states[4];
Daanmk 1:96cd4c9c5465 60
Daanmk 1:96cd4c9c5465 61 arm_biquad_casd_df1_inst_f32 envelop_biceps;
Daanmk 1:96cd4c9c5465 62 arm_biquad_casd_df1_inst_f32 envelop_triceps;
Daanmk 1:96cd4c9c5465 63 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
Daanmk 1:96cd4c9c5465 64 float envelop_biceps_states[4];
Daanmk 1:96cd4c9c5465 65 float envelop_triceps_states[4];
Daanmk 1:96cd4c9c5465 66
Daanmk 1:96cd4c9c5465 67 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN};
Daanmk 1:96cd4c9c5465 68 uint8_t state=RUST;
Daanmk 1:96cd4c9c5465 69
Daanmk 1:96cd4c9c5465 70 void emgmeten(){
Daanmk 0:b3809a8d9af1 71 /*put raw emg value in emg_value*/
Daanmk 0:b3809a8d9af1 72 emg0_value_f32 = emg0.read();
Daanmk 0:b3809a8d9af1 73 emg1_value_f32 = emg1.read();
Daanmk 0:b3809a8d9af1 74
Daanmk 0:b3809a8d9af1 75 //process emg biceps
Daanmk 1:96cd4c9c5465 76 arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
Daanmk 1:96cd4c9c5465 77 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 78 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 79 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 80 arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
Daanmk 0:b3809a8d9af1 81
Daanmk 0:b3809a8d9af1 82 //process emg triceps
Daanmk 1:96cd4c9c5465 83 arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
Daanmk 1:96cd4c9c5465 84 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
Daanmk 1:96cd4c9c5465 85 arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
Daanmk 1:96cd4c9c5465 86 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter
Daanmk 1:96cd4c9c5465 87 arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
Daanmk 0:b3809a8d9af1 88 }
Daanmk 1:96cd4c9c5465 89
Daanmk 0:b3809a8d9af1 90
Daanmk 0:b3809a8d9af1 91 int main()
Daanmk 0:b3809a8d9af1 92 {
Daanmk 1:96cd4c9c5465 93 while(1) {
Daanmk 1:96cd4c9c5465 94 pc.baud(38400); //PC baud rate is 38400 bits/seconde
Daanmk 1:96cd4c9c5465 95 Ticker emg_timer;
Daanmk 1:96cd4c9c5465 96 emg_timer.attach(emgmeten, TSAMP);
Daanmk 1:96cd4c9c5465 97 arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
Daanmk 1:96cd4c9c5465 98 arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
Daanmk 1:96cd4c9c5465 99 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
Daanmk 1:96cd4c9c5465 100 arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
Daanmk 1:96cd4c9c5465 101 arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
Daanmk 1:96cd4c9c5465 102 arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
Daanmk 1:96cd4c9c5465 103 arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
Daanmk 1:96cd4c9c5465 104 arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
Daanmk 1:96cd4c9c5465 105 switch(state) {
Daanmk 1:96cd4c9c5465 106 case RUST: { //Aanzetten
Daanmk 1:96cd4c9c5465 107
Daanmk 1:96cd4c9c5465 108 state = KALIBRATIE;
Daanmk 1:96cd4c9c5465 109 break;
Daanmk 1:96cd4c9c5465 110 }
Daanmk 1:96cd4c9c5465 111
Daanmk 1:96cd4c9c5465 112 case KALIBRATIE: {
Daanmk 1:96cd4c9c5465 113 pc.printf("Kalibratie");
Daanmk 1:96cd4c9c5465 114 wait(1);
Daanmk 1:96cd4c9c5465 115
Daanmk 1:96cd4c9c5465 116 state = RICHTEN;
Daanmk 1:96cd4c9c5465 117 break; //Stopt acties in deze case
Daanmk 1:96cd4c9c5465 118 }
Daanmk 1:96cd4c9c5465 119
Daanmk 1:96cd4c9c5465 120 case RICHTEN: { //Batje richten
Daanmk 1:96cd4c9c5465 121 pc.printf("Richten");
Daanmk 1:96cd4c9c5465 122 wait(1);
Daanmk 1:96cd4c9c5465 123 state = SLAAN;
Daanmk 1:96cd4c9c5465 124 break;
Daanmk 1:96cd4c9c5465 125 }
Daanmk 1:96cd4c9c5465 126 case SLAAN: { //Balletje slaan
Daanmk 1:96cd4c9c5465 127 pc.printf("Slaan!");
Daanmk 1:96cd4c9c5465 128 wait(1);
Daanmk 1:96cd4c9c5465 129 state = RICHTEN;
Daanmk 1:96cd4c9c5465 130 break;
Daanmk 1:96cd4c9c5465 131 }
Daanmk 1:96cd4c9c5465 132
Daanmk 1:96cd4c9c5465 133 default: {
Daanmk 1:96cd4c9c5465 134 state = RUST;
Daanmk 1:96cd4c9c5465 135 } //Accolade einde default
Daanmk 1:96cd4c9c5465 136
Daanmk 1:96cd4c9c5465 137 } //Accolade einde switch
Daanmk 1:96cd4c9c5465 138
Daanmk 1:96cd4c9c5465 139 } //Accolade einde main
Daanmk 1:96cd4c9c5465 140 }