File content as of revision 9:9000c5c1a0d6:

/***************************************/
/*                                     */
/*   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"
#include "TextLCD.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
#define K_P_KM (0.1)
#define K_I_KM (0.03  *TSAMP)
#define K_D_KM (0.001 /TSAMP)
#define K_P_GM (2.9)
#define K_I_GM (0.3  *TSAMP)
#define K_D_GM (0.003 /TSAMP)
#define I_LIMIT 1.
#define RADTICKGM 0.003927
#define THETA0 6.85             //hoe bepaald?????
#define THETA1 7.77
#define THETA2 9.21

TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x4); // rs, e, d4-d7 ok

Encoder motor2(PTD2,PTD0); //geel,wit kleine motor
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,pwm_to_motor1,max_value_biceps,min_value_biceps; //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,pwm_to_motor2,max_value_triceps,min_value_triceps,metingstatus; //variable to store value in for triceps 

arm_biquad_casd_df1_inst_f32 notch_biceps;
arm_biquad_casd_df1_inst_f32 notch_triceps;
// constants for 50 Hz notch (bandbreedte 2 Hz)
float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
//state values
float notch_biceps_states[4];
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];

arm_biquad_casd_df1_inst_f32 lowpass_biceps;
arm_biquad_casd_df1_inst_f32 lowpass_triceps;
//constants for 80Hz lowpass
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;
//constants for envelop
float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
// state values
float envelop_biceps_states[4];
float envelop_triceps_states[4];    

enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES
uint8_t state=RUST;

enum kalibratiestates {BICEPSMAX,TRICEPSMAX};

volatile bool looptimerflag;
void setlooptimerflag(void)
{
    looptimerflag = true;
}

void clamp(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}

float pidkm(float setpointkm, float measurementkm) //PID Regelaar Kleine motor
{
    float error_km;
    static float prev_error_km = 0;
    float           out_p_km = 0;
    static float    out_i_km = 0;
    float           out_d_km = 0;
    error_km  = setpointkm-measurementkm;
    out_p_km  = error_km*K_P_KM;
    out_i_km += error_km*K_I_KM;
    out_d_km  = (error_km-prev_error_km)*K_D_KM;
    clamp(&out_i_km,-I_LIMIT,I_LIMIT);
    prev_error_km = error_km;
    scope.set(1,out_p_km);
    scope.set(2,out_i_km);
    scope.set(3,out_d_km);
    return out_p_km + out_i_km + out_d_km;
}

float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
{
    float error_gm;
    static float prev_error_gm = 0;
    float           out_p_gm = 0;
    static float    out_i_gm = 0;
    float           out_d_gm = 0;
    error_gm  = setpointgm-measurementgm;
    out_p_gm  = error_gm*K_P_GM;
    out_i_gm += error_gm*K_I_GM;
    out_d_gm  = (error_gm-prev_error_gm)*K_D_GM;
    clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
    prev_error_gm = error_gm;
    scope.set(1,out_p_gm);
    scope.set(2,out_i_gm);
    scope.set(3,out_d_gm);
    return out_p_gm + out_i_gm + out_d_gm;
}
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()
{
        pc.baud(38400); //PC baud rate is 38400 bits/seconde
        Ticker emg_timer;
        emg_timer.attach(emgmeten, TSAMP);
        Ticker looptimer;
        looptimer.attach(setlooptimerflag,TSAMP);
        Timer tijdtimer;
        Timer tijdslaan;
        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);
    while(true) {
        switch(state) {
            case RUST: {                            //Aanzetten
                lcd.cls();
                lcd.locate(0,0);         
                lcd.printf(" -- THE SLAP -- ");     //regel 1 LCD scherm
                lcd.locate(0,1);
                lcd.printf("    GROEP 5     ");     //regel 2 LCD scherm
                wait(5);                
                state = KALIBRATIE;
                break;                         
            }                               

            case KALIBRATIE: {                                  //kalibreren met maximale inspanning
                max_value_biceps=0;
                max_value_triceps=0;
                state = BICEPSMAX;
                switch(state) {
                    case BICEPSMAX: {                           //maximale inspanning biceps
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("Kalibratie");               //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("1:BICEPS MAX");             //regel 2 LCD scherm
                        wait(1); 
                        tijdtimer.start();
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("Biceps meting");            //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("Meting loopt!");            //regel 2 LCD scherm
                        while (tijdtimer <= 3){
                            if (envelop_emg0 > max_value_biceps) {
                            max_value_biceps = envelop_emg0; }
                            } 
                        if (tijdtimer >= 3) {
                            tijdtimer.stop();
                            tijdtimer.reset();
                            lcd.cls();
                            lcd.locate(0,0);         
                            lcd.printf("Einde meting!");        //regel 1 LCD scherm
                            lcd.locate(0,1);
                            lcd.printf("waarde");               //METING WAARDE TOEVOEGEN 
                            wait(1);                     
                            state = TRICEPSMAX; 
                            }//einde if statement
                            break;
                            }//einde case bicepsmax
                    case TRICEPSMAX: {                          //maximale inspanning biceps
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("Kalibratie");               //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("2:TRICEPS MAX");            //regel 2 LCD scherm
                        wait(1); 
                        tijdtimer.start();
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("Triceps meting");           //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("Meting loopt!");            //regel 2 LCD scherm
                        while (tijdtimer <= 3){
                            if (envelop_emg1 > max_value_triceps) {
                            max_value_triceps = envelop_emg1; }
                            } 
                        if (tijdtimer >= 3) {
                            tijdtimer.stop();
                            tijdtimer.reset();
                            lcd.cls();
                            lcd.locate(0,0);         
                            lcd.printf("Einde meting!");        //regel 1 LCD scherm
                            lcd.locate(0,1);
                            lcd.printf("waarde");               //METING WAARDE TOEVOEGEN 
                            wait(1);                     
                            state = RICHTEN;   
                            } //einde if statement
                            break;
                            } //einde case tricepsmax  
                    default: {
                state = BICEPSMAX;
                        }  //einde default
                            } //einde switch states                                 
                break;                          
            }       // einde kalibratie case                        

           case RICHTEN: {                                  //batje richten (gebruik biceps en triceps)
                    lcd.cls();
                    lcd.locate(0,0);         
                    lcd.printf("Richten");                  //regel 1 LCD scherm
                    lcd.locate(0,1);
                    lcd.printf("Kies goal!");               //regel 2 LCD scherm
                    int16_t setpointkm;
                    float new_pwm_km;
                    wait(1);
                    float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
                    kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT
                    kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
                    tijdtimer.start();
                    while( tijdtimer <= 3) {
                        if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps < 0.3) { //linker goal!
                            setpointkm = -127;          //11,12graden naar rechts???????
                            new_pwm_km = pidkm(setpointkm, motor1.getPosition());
                            clamp(&new_pwm_km, -1,1);
                            if(new_pwm_km < 0)
                                motordir1 = 1;
                            else
                                motordir1 = 0;
                            pwm_motor1.write(abs(new_pwm_km));
                            if(motor1.getPosition() <= -400 ){
                                pwm_motor1.write(0);
                                }
                            }
                        if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
                            setpointkm = 127;        //11,12graden naar links??????
                            new_pwm_km = pidkm(setpointkm, motor1.getPosition());
                            clamp(&new_pwm_km, -1,1);
                            if(new_pwm_km < 0)
                                motordir1 = 1;
                            else
                                motordir1 = 0;
                            pwm_motor1.write(abs(new_pwm_km));
                            if(motor1.getPosition() >= 400 ){
                                pwm_motor1.write(0);
                                }                                                        
                            }
                        if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps < 0.3) { //middelste goal!
                            setpointkm = 0;        
                            new_pwm_km = pidkm(setpointkm, motor1.getPosition());
                            clamp(&new_pwm_km, -1,1);
                            if(new_pwm_km < 0)
                                motordir1 = 1;
                            else
                                motordir1 = 0;
                            pwm_motor1.write(abs(new_pwm_km));                            
                            }                        
                        }
                    if (tijdtimer >= 3) {
                        tijdtimer.stop();
                        tijdtimer.reset();
                        state = SLAAN;
                        }  
                
           break; 
            }                              
           case SLAAN: {                            //snelheid bepalen (gebruik alleen biceps)
                lcd.cls();
                lcd.locate(0,0);         
                lcd.printf("Slaan PingPong!");      //regel 1 LCD scherm
                lcd.locate(0,1);
                lcd.printf("Kies goal!");           //regel 2 LCD scherm
                wait(1);
                int16_t setpointgm;
                float new_pwm_gm;
                float kalibratiewaarde_biceps;
                float theta;
                kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
                tijdtimer.start();
                while (tijdtimer <=3 ) {
                    if (kalibratiewaarde_biceps<0.3){           //kalibratiewaarde_biceps<0.3 goal onderin
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("Onderste goal");            //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("Daar gaat ie!");            //regel 2 LCD scherm
                        tijdslaan.start();
                        theta=THETA0;
                        setpointgm = (theta*tijdslaan/RADTICKGM);
                        new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
                        clamp(&new_pwm_gm, -1,1);
                        if(new_pwm_gm < 0)
                        motordir2 = 0;
                        else
                        motordir2 = 1;
                        pwm_motor2.write(abs(new_pwm_gm));
                        if(motor2.getPosition() >= 450 ) {
                        pwm_motor2.write(0); 
                               }
                        wait(2);
                        state = RUST;
                        }
                    if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){        //0.3<kalibratiewaarde_biceps<0.6 goal midden
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("MIDDELSTE GOAL");            //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("DAAR GAAT IE!");             //regel 2 LCD scherm
                        theta=THETA1;
                        setpointgm = ((theta*tijdslaan)/RADTICKGM);
                        new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
                        clamp(&new_pwm_gm, -1,1);
                        if(new_pwm_gm < 0)
                        motordir2 = 0;
                        else
                        motordir2 = 1;
                        pwm_motor2.write(abs(new_pwm_gm));
                        if(motor2.getPosition() >= 450 ) {
                        pwm_motor2.write(0);                         
                        wait(2);
                        state = RUST;
                        }
                    if (kalibratiewaarde_biceps>0.6){             //kalibratiewaarde_biceps>0.6 goal bovenin
                        lcd.cls();
                        lcd.locate(0,0);         
                        lcd.printf("BOVENSTE GOAL");             //regel 1 LCD scherm
                        lcd.locate(0,1);
                        lcd.printf("DAAR GAAT IE!");             //regel 2 LCD scherm
                        theta=THETA2;
                        setpointgm = ((theta*tijdslaan)/RADTICKGM);
                        new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
                        clamp(&new_pwm_gm, -1,1);
                        if(new_pwm_gm < 0)
                        motordir2 = 0;
                        else
                        motordir2 = 1;
                        pwm_motor2.write(abs(new_pwm_gm));
                        if(motor2.getPosition() >= 450 ) {
                        pwm_motor2.write(0);                         
                        wait(2);
                        state = RUST;                       
                        
                        }
                } //einde whilelus
                if (tijdtimer >=3 ) {
                    tijdtimer.stop();
                    tijdtimer.reset();
                    wait(1);
                    //+ TERUGKEREN BEGINPOSITIE!
                    lcd.cls();
                    lcd.locate(0,0);         
                    lcd.printf("Goed Gedaan!");                 //regel 1 LCD scherm
                    lcd.locate(0,1);
                    lcd.printf("Terug naar begin");             //regel 2 LCD scherm
                    setpointgm = (0);
                    new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
                    clamp(&new_pwm_gm, -1,1);
                    state = RUST;
                } //einde if statement
                
                
                state = RUST;
           break; 
            }  

           default: {
                state = RUST;
            }                               

        }  //switch sate                            

    } //while
} //int main