File content as of revision 8:f8f58e48b352:

/***************************************/
/*                                     */
/*   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 "arm_math.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "TextLCD.h"

#define M2_PWM PTC8 //kleine motor
#define M2_DIR PTC9 //kleine motor
#define M1_PWM PTA5 //grote motor
#define M1_DIR PTA4 //grote motor
#define TSAMP 0.005  // Sampletijd, 200Hz
#define K_P_KM (0.01)
#define K_I_KM (0.0003  *TSAMP)
#define K_D_KM (0.0 /TSAMP)
#define K_P_GM (0.0022)
#define K_I_GM (0.0001  *TSAMP)        //oud 0.0001
#define K_D_GM (0.00000001 /TSAMP)
#define I_LIMIT 1.
#define RADTICKGM 0.003927
#define THETADOT0 30.0       //oud 6.85
#define THETADOT1 20.0       //oud 7.77
#define THETADOT2 14.0       //oud 9.21

TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13);

Encoder motor2(PTD3,PTD1); //geel,wit kleine motor  oud:PTD2,PTD0
Encoder motor1(PTD5,PTD0);//geel,wit grote motor   oud:PTD5,PTA13
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); //deltoid

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_deltoid,min_value_deltoid,metingstatus; //variable to store value in for deltoid

arm_biquad_casd_df1_inst_f32 notch_biceps;
arm_biquad_casd_df1_inst_f32 notch_deltoid;
// 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_deltoid_states[4];

arm_biquad_casd_df1_inst_f32 highpass_biceps;
arm_biquad_casd_df1_inst_f32 highpass_deltoid;
//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_deltoid_states[4];

arm_biquad_casd_df1_inst_f32 lowpass_biceps;
arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
//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_deltoid_states[4];

arm_biquad_casd_df1_inst_f32 envelop_biceps;
arm_biquad_casd_df1_inst_f32 envelop_deltoid;
//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_deltoid_states[4];

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

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;              //static, want dan wordt vorige waarde onthouden
    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;
    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;
    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 deltoid
    arm_biquad_cascade_df1_f32(&notch_deltoid, &emg1_value_f32, &filtered_emg1_notch, 1 );
    arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
    arm_biquad_cascade_df1_f32(&lowpass_deltoid, &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_deltoid, &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;
    tijdtimer.start();
    tijdslaan.start();
    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_deltoid,1 , notch_const, notch_deltoid_states);
    arm_biquad_cascade_df1_init_f32(&highpass_deltoid,1 ,highpass_const,highpass_deltoid_states);
    arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 ,lowpass_const,lowpass_deltoid_states);
    arm_biquad_cascade_df1_init_f32(&envelop_deltoid,1 ,envelop_const,envelop_deltoid_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_deltoid=0;

                //maximale inspanning biceps
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Kalibratie1:");
                lcd.locate(0,1);
                lcd.printf("Span biceps!");
                wait(5);
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Meting loopt");
                tijdtimer.reset();
                tijdtimer.start();
                while (tijdtimer.read() <= 3) {
                    if (envelop_emg0 > max_value_biceps) {
                        max_value_biceps = envelop_emg0;
                    }
                }
                tijdtimer.stop();
                tijdtimer.reset();
                //pc.printf("max value %f\n\r", max_value_biceps);
                lcd.cls();
                wait(5);

                //maximale inspanning deltoid
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Kalibratie2:");
                lcd.locate(0,1);
                lcd.printf("Span deltoid!");
                wait(5);
                tijdtimer.start();
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Meting loopt");
                while (tijdtimer.read() <= 3) {
                    if (envelop_emg1 > max_value_deltoid) {
                        max_value_deltoid = envelop_emg1;
                    }
                }
                // tijdtimer.stop();
                tijdtimer.reset();
                //pc.printf("max value %f\n\r", max_value_deltoid);
                lcd.cls();
                wait(5);

                state = RICHTEN;
                break;
            }// einde kalibratie case

            case RICHTEN: {                                  //batje richten (gebruik deltoid)
                wait(3);
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Richten");                  //regel 1 LCD scherm
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("Kies goal!");               //regel 2 LCD scherm
                wait(5);
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Meting loopt");
                wait(2);
                float setpointkm;
                float new_pwm_km;
                float kalibratiewaarde_deltoid;
                kalibratiewaarde_deltoid=(envelop_emg1/max_value_deltoid);
                if (kalibratiewaarde_deltoid >= 0.35) {
                    setpointkm = -329;          //11,12 graden naar links
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("links");
                } else if (kalibratiewaarde_deltoid>0.2 && kalibratiewaarde_deltoid<=0.35) {
                    setpointkm = 0;        //11,12graden naar rechts
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("midden");
                } else {
                    setpointkm = 280;       //11,12 graden naar rechts //oud 127    //30 graden
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("rechts");
                }
                //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
                tijdtimer.reset();
                while (tijdtimer.read() <= 3) {
                    while(looptimerflag == false);
                    looptimerflag = false;
                    new_pwm_km = pidkm(setpointkm, motor2.getPosition());   //bewegen naar setpoint
                    clamp(&new_pwm_km, -1,1);
                    if(new_pwm_km < 0)
                        motordir2 = 1;  //links
                    else
                        motordir2 = 0;  //rechts
                    pwm_motor2.write(abs(new_pwm_km));
                }
                wait(2);
                state = SLAAN;
                break;
            }

            case SLAAN: {                               //batje slaan (gebruik biceps)
                wait(3);
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Richten");                  //regel 1 LCD scherm
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("Kies goal!");               //regel 2 LCD scherm
                wait(5);
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Meting loopt");
                wait(2);
                float thetadot;
                float setpointgm;
                float new_pwm_gm;
                float setpointkm;
                float new_pwm_km;
                float kalibratiewaarde_biceps;
                kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
                if (kalibratiewaarde_biceps <= 0.2) {                                     //kalibratiewaarde_biceps<0.2 goal onderin
                    thetadot=THETADOT0;
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("ONDERSTE GOAL");
                } else if (kalibratiewaarde_biceps>0.2 && kalibratiewaarde_biceps<=0.4) { //0.2<kalibratiewaarde_biceps<0.4 goal midden
                    thetadot=THETADOT1;
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("MIDDELSTE GOAL");
                } else {                                                                  //goal bovenin
                    thetadot=THETADOT2;
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("BOVENSTE GOAL");
                }
                wait(3);
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Daar gaat ie!");

                //MOTOR 1 LATEN BEWEGEN met snelheid thetadot
                tijdtimer.reset();
                tijdslaan.reset();
                while (tijdtimer.read() <=3) {
                    while(looptimerflag == false);
                    looptimerflag = false;
                    if (motor1.getPosition()>= 444 ) {
                        setpointgm = 444;
                    } else {
                        setpointgm = ((thetadot*tijdslaan.read())/RADTICKGM);
                    }
                    new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
                    clamp(&new_pwm_gm, -1,1);
                    if(new_pwm_gm < 0) {
                        motordir1 = 1;  //links
                    } else {
                        motordir1 = 0;  //rechts
                    }
                    pwm_motor1.write(abs(new_pwm_gm));
                }
                //pc.printf("pos %d %f\n\r", motor1.getPosition(),setpointgm);

                //MOTOR 2 OP POSITIE HOUDEN
                new_pwm_km = pidkm(setpointkm, motor2.getPosition());   //bewegen naar setpoint
                clamp(&new_pwm_km, -1,1);
                if(new_pwm_km < 0)
                    motordir2 = 1;  //links
                else
                    motordir2 = 0;  //rechts
                pwm_motor2.write(abs(new_pwm_km));

            }
            state = HOME;
            break;

            case HOME: {                    //terugbewegen naar beginpositie
                float setpointgm;
                float new_pwm_gm;
                float setpointkm;
                float new_pwm_km;

                //MOTOR 1 naar home
                tijdtimer.reset();
                tijdslaan.reset();
                while (tijdtimer.read() <=6) {
                    while(looptimerflag == false);
                    looptimerflag = false;
                    setpointgm = 0;
                    new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
                    clamp(&new_pwm_gm, -1,1);
                    if(new_pwm_gm < 0)
                        motordir1 = 1;  //links
                    else
                        motordir1 = 0;  //rechts
                    pwm_motor1.write(abs(new_pwm_gm));

                    //MOTOR 2 naar home
                    setpointkm=0;
                    new_pwm_km = pidkm(setpointkm, motor2.getPosition());   //bewegen naar setpoint
                    clamp(&new_pwm_km, -1,1);
                    if(new_pwm_km < 0)
                        motordir2 = 1;  //links
                    else
                        motordir2 = 0;  //rechts
                    pwm_motor2.write(abs(new_pwm_km));
                }
                pwm_motor1.write(0);
                pwm_motor2.write(0);
                wait(20);
                state = RICHTEN;      //optioneel
                break;
            }
            default: {
                state = RUST;
            }
        }
    }
}