File content as of revision 1:96cd4c9c5465:

/***************************************/
/*                                     */
/*   BRONCODE GROEP 5, MODULE 9, 2014  */
/*       *****-THE SLAP-******         */
/*                                     */
/* -Dominique Clevers                  */
/* -Rianne van Dommelen                */
/* -Daan de Muinck Keizer              */
/* -David den Houting                  */
/* -Marjolein Thijssen                 */
/***************************************/
#include "mbed.h"
#include "HIDScope.h"
#include "arm_math.h"
#include "encoder.h"
#include "MODSERIAL.h"

#define M2_PWM PTC8 //blauw
#define M2_DIR PTC9 //groen
#define M1_PWM PTA5 //kleine motor
#define M1_DIR PTA4 //kleine motor
#define TSAMP 0.005  // Sampletijd, 200Hz

Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor
Encoder motor1(PTD5,PTA13);//geel,wit
PwmOut pwm_motor1(M1_PWM);
PwmOut pwm_motor2(M2_PWM);
DigitalOut motordir2(M2_DIR);
DigitalOut motordir1(M1_DIR);
AnalogIn emg0(PTB0); //Biceps
AnalogIn emg1(PTB1); //Triceps
HIDScope scope(6);

MODSERIAL pc(USBTX,USBRX,64,1024);


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

arm_biquad_casd_df1_inst_f32 notch_biceps;
arm_biquad_casd_df1_inst_f32 notch_triceps;
float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
float notch_biceps_states[4]; //state values
float notch_triceps_states[4];

arm_biquad_casd_df1_inst_f32 highpass_biceps;
arm_biquad_casd_df1_inst_f32 highpass_triceps;
//constants for 20Hz highpass
float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
//state values
float highpass_biceps_states[4];
float highpass_triceps_states[4];
//constants for 80Hz lowpass
arm_biquad_casd_df1_inst_f32 lowpass_biceps;
arm_biquad_casd_df1_inst_f32 lowpass_triceps;
float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
//state values
float lowpass_biceps_states[4];
float lowpass_triceps_states[4];

arm_biquad_casd_df1_inst_f32 envelop_biceps;
arm_biquad_casd_df1_inst_f32 envelop_triceps;
float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
float envelop_biceps_states[4];
float envelop_triceps_states[4];    

enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN};
uint8_t state=RUST;

void emgmeten(){
    /*put raw emg value in emg_value*/
    emg0_value_f32 = emg0.read();   
    emg1_value_f32 = emg1.read();
 
    //process emg biceps
    arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
    arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
    arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
    filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass);  //gelijkrichter
    arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
    
    //process emg triceps
    arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
    arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
    arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
    filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass);  //gelijkrichter
    arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
    }


int main()
{
    while(1) {
        pc.baud(38400); //PC baud rate is 38400 bits/seconde
        Ticker emg_timer;
        emg_timer.attach(emgmeten, TSAMP);
        arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
        arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
        arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
        arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
        arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
        arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
        arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
        arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
        switch(state) {
            case RUST: {                    //Aanzetten
                
                state = KALIBRATIE;
                break;                         
            }                               

            case KALIBRATIE: {         
                pc.printf("Kalibratie");
                wait(1);   
                                              
                state = RICHTEN;                       
                break;                          //Stopt acties in deze case
            }                               

           case RICHTEN: {         //Batje richten
           pc.printf("Richten");
           wait(1);
           state = SLAAN;
           break; 
            }                              
           case SLAAN: {         //Balletje slaan
           pc.printf("Slaan!");
           wait(1);
           state = RICHTEN;
           break; 
            }  

            default: {
                state = RUST;
            }                               //Accolade einde default

        }                               //Accolade einde switch

    }                               //Accolade einde main
    }