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@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?

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 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(&notch_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(&notch_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(&notch_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(&notch_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 }