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@0:b3809a8d9af1, 2014-10-21 (annotated)

Committer:
Daanmk
Date:
Tue Oct 21 13:35:52 2014 +0000
Revision:
0:b3809a8d9af1
Child:
1:96cd4c9c5465
porject;

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 0:b3809a8d9af1 16
Daanmk 0:b3809a8d9af1 17 #define M1_PWM PTC8 //blauw
Daanmk 0:b3809a8d9af1 18 #define M1_DIR PTC9 //groen
Daanmk 0:b3809a8d9af1 19 #define M2_PWM PTA5 //blauw
Daanmk 0:b3809a8d9af1 20 #define M2_DIR PTA4 //groen
Daanmk 0:b3809a8d9af1 21 #define TSAMP 0.005 // Sampletijd, 200Hz
Daanmk 0:b3809a8d9af1 22 #define PI 3.1415926535897
Daanmk 0:b3809a8d9af1 23
Daanmk 0:b3809a8d9af1 24 Encoder motor1(PTD2,PTD0); //geel,wit
Daanmk 0:b3809a8d9af1 25 Encoder motor2(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 0:b3809a8d9af1 34 MODSERIAL pc (USBTX,USBRX,64,1024);
Daanmk 0:b3809a8d9af1 35 pc.baud(115200);
Daanmk 0:b3809a8d9af1 36
Daanmk 0:b3809a8d9af1 37 int const windowsamples = 60; //aantal samples waaruit het window voor MOVAG bestaat
Daanmk 0:b3809a8d9af1 38
Daanmk 0:b3809a8d9af1 39 float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs; //variable to store value in for biceps
Daanmk 0:b3809a8d9af1 40 float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs; //variable to store value in for triceps
Daanmk 0:b3809a8d9af1 41 float xpos_max,xpos_min,xneg_max,xneg_min; //kalibratiewaardes
Daanmk 0:b3809a8d9af1 42
Daanmk 0:b3809a8d9af1 43 float emg_biceps [windowsamples];
Daanmk 0:b3809a8d9af1 44 float emg_triceps [windowsamples];
Daanmk 0:b3809a8d9af1 45
Daanmk 0:b3809a8d9af1 46 void meting()
Daanmk 0:b3809a8d9af1 47 {
Daanmk 0:b3809a8d9af1 48 arm_biquad_casd_df1_inst_f32 notch;
Daanmk 0:b3809a8d9af1 49 //constants for 50Hz notch
Daanmk 0:b3809a8d9af1 50 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924};
Daanmk 0:b3809a8d9af1 51 //state values
Daanmk 0:b3809a8d9af1 52 float notch_states[4];
Daanmk 0:b3809a8d9af1 53 arm_biquad_casd_df1_inst_f32 highpass;
Daanmk 0:b3809a8d9af1 54 //constants for 20Hz highpass
Daanmk 0:b3809a8d9af1 55 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
Daanmk 0:b3809a8d9af1 56 //state values
Daanmk 0:b3809a8d9af1 57 float highpass_states[4];
Daanmk 0:b3809a8d9af1 58 //constants for 80Hz lowpass
Daanmk 0:b3809a8d9af1 59 arm_biquad_casd_df1_inst_f32 lowpass;
Daanmk 0:b3809a8d9af1 60 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
Daanmk 0:b3809a8d9af1 61 //state values
Daanmk 0:b3809a8d9af1 62 float lowpass_states[4];
Daanmk 0:b3809a8d9af1 63
Daanmk 0:b3809a8d9af1 64 /*put raw emg value in emg_value*/
Daanmk 0:b3809a8d9af1 65 emg0_value_f32 = emg0.read();
Daanmk 0:b3809a8d9af1 66 emg1_value_f32 = emg1.read();
Daanmk 0:b3809a8d9af1 67
Daanmk 0:b3809a8d9af1 68 //process emg biceps
Daanmk 0:b3809a8d9af1 69 arm_biquad_cascade_df1_f32(&notch, &emg0_value_f32, &filtered_emg0_notch, 1 );
Daanmk 0:b3809a8d9af1 70 arm_biquad_cascade_df1_f32(&highpass, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
Daanmk 0:b3809a8d9af1 71 arm_biquad_cascade_df1_f32(&lowpass, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
Daanmk 0:b3809a8d9af1 72 filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass);
Daanmk 0:b3809a8d9af1 73 emg_biceps [0]= filtered_emg0_eindsignaal_abs;
Daanmk 0:b3809a8d9af1 74
Daanmk 0:b3809a8d9af1 75 //process emg triceps
Daanmk 0:b3809a8d9af1 76 arm_biquad_cascade_df1_f32(&notch, &emg1_value_f32, &filtered_emg1_notch, 1 );
Daanmk 0:b3809a8d9af1 77 arm_biquad_cascade_df1_f32(&highpass, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
Daanmk 0:b3809a8d9af1 78 arm_biquad_cascade_df1_f32(&lowpass, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
Daanmk 0:b3809a8d9af1 79 filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass);
Daanmk 0:b3809a8d9af1 80 emg_triceps [0]= filtered_emg1_eindsignaal_abs;
Daanmk 0:b3809a8d9af1 81
Daanmk 0:b3809a8d9af1 82 //Movag
Daanmk 0:b3809a8d9af1 83 //Variabelen voor berekenen gemiddelden
Daanmk 0:b3809a8d9af1 84 float avg0,avg1;
Daanmk 0:b3809a8d9af1 85 avg0=avg1=0;
Daanmk 0:b3809a8d9af1 86
Daanmk 0:b3809a8d9af1 87 //Inhoud van een buffer (=gefilterd signaal) optellen
Daanmk 0:b3809a8d9af1 88 for(int x=0; x<windowsamples; x++) {
Daanmk 0:b3809a8d9af1 89 avg0 = avg0 + (emg_biceps[x]);
Daanmk 0:b3809a8d9af1 90 avg1 = avg1 + (emg_triceps[x]);
Daanmk 0:b3809a8d9af1 91 }
Daanmk 0:b3809a8d9af1 92
Daanmk 0:b3809a8d9af1 93 //Gemiddelde berekenen en relativeren tov maximum voluntary contraction
Daanmk 0:b3809a8d9af1 94 avg0 = gain_biceps*avg0/windowsamples/biceps_max;
Daanmk 0:b3809a8d9af1 95 avg1 = gain_triceps*avg1/windowsamples/triceps_max;
Daanmk 0:b3809a8d9af1 96 }
Daanmk 0:b3809a8d9af1 97
Daanmk 0:b3809a8d9af1 98 int main()
Daanmk 0:b3809a8d9af1 99 {
Daanmk 0:b3809a8d9af1 100 Ticker log_timer;
Daanmk 0:b3809a8d9af1 101 log_timer.attach(meting, TSAMP);
Daanmk 0:b3809a8d9af1 102 while(1) //Loop
Daanmk 0:b3809a8d9af1 103 {
Daanmk 0:b3809a8d9af1 104
Daanmk 0:b3809a8d9af1 105 }
Daanmk 0:b3809a8d9af1 106 }