File content as of revision 66:c922e18c9217:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "arm_math.h"
#include "TouchButton.h"

#define K_P (0.005)
#define K_I (0.00  *TSAMP1)
#define K_D (0  /TSAMP1)
#define I_LIMIT 1.
#define K_Pp (0.003)
#define K_Ip (0.0  *TSAMP2)
#define K_Dp (0.000003  /TSAMP2)

#define TSAMP1 0.01
#define TSAMP2 0.01
#define WACHTEN 1
#define SLAAN 2
#define TERUGKEREN 3
#define ANGLEMAX -280
#define ANGLEMIN 0

//initiating functions
void Triceps();
void Biceps();
void Calibratie_Triceps();
void Calibratie_Biceps();
float pid(float setspeed, float measurement, bool reset = false);
float pidpositie(float setposition, float measurement, bool reset =false);
void motor2aansturing();
void motor1aansturing();
void motor1aansturingdeel2();

//alle initiaties voor EMG
MODSERIAL pc(USBTX,USBRX);

HIDScope scope(5);

AnalogIn emgB(PTB1); //biceps
AnalogIn emgT(PTB2); //tricep

//*** OBJECTS ***
//bicep
uint16_t emg_valueB;
float emg_value_f32B;
float filtered_emgB;
float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
int yB1, yB2, yB3;
float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29, MOVAVG_B; //moving average objects
float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59;
int snelheidsstand;
//tricep
uint16_t emg_valueT;
float emg_value_f32T;
float filtered_emgT;
float drempelwaardeT;
int yT1, yT2;
float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects
float MOVAVG_Positie1, MOVAVG_Positie2;
int positie;

//*** FILTERS ***
arm_biquad_casd_df1_inst_f32 notchT;
arm_biquad_casd_df1_inst_f32 notchB;
//constants for 50Hz
float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
float notch_states[4];

arm_biquad_casd_df1_inst_f32 lowpassT;
arm_biquad_casd_df1_inst_f32 lowpassB;
//constants for 60Hz lowpass
float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
float lowpass_states[4];

arm_biquad_casd_df1_inst_f32 highpassT;
arm_biquad_casd_df1_inst_f32 highpassB;
//constants for 20Hz highpass
float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
float highpass_states[4];

/*bool stop;*/
float pwm;
float new_pwmpos;
float new_pwm;
float PWM2 = 0.3; //PWM voor instellen hoek batje
int toestand = WACHTEN;
float setspeed = 0, V3=150, V2=70, V1 =50;//V in counts/s

Encoder motor1(PTD5,PTD3, true);
Encoder motor2(PTD0,PTD2);
DigitalOut motordir1(PTA4);
DigitalOut motordir2(PTC9);
PwmOut pwm_motor1(PTA5);
PwmOut pwm_motor2(PTC8);

float prevgetpos=0;
float omega;
float deltacounts;

DigitalOut myled1(LED1);//red
DigitalOut myled2(LED2);//green
DigitalOut myled3(LED3);//blue

DigitalOut rood(PTD1);
DigitalOut groen(PTA13);
DigitalOut blauw(PTD4);
DigitalOut wit(PTA12);

/* FRDM-KL25Z built-in touch slider
*******************
*     *     *     *
*  1  *  2  *  3  *
*     *     *     *
*******************
* key 1 will light Red LED                  -> CALIBRATIE TRICEPS
* key 2 will light Green LED                -> CALIBRATIE BICEPS
* key 3 will light Blue LED                 -> START*/

enum standen {STAND1=0, STAND2=1, STAND3=2};
standen hoek2 = STAND1;

int main ()
{
    pc.baud(115200);

    drempelwaardeT=0;
    drempelwaardeB1=0;
    drempelwaardeB2=0;
    drempelwaardeB3=0;

    TouchButton TButton;

    myled1=1;
    myled2=1;
    myled3=1;

    int key=0;

    pc.printf("key 1 calibratie tricep\r\n");
    pc.printf("key 2 caliratie biceps\r\n");
    pc.printf("key 3 START\r\n");

    while(true) {
        rood=0;
        wit=0;
        blauw=0;
        groen=0;

        key = TButton.PressedButton();

        if (key==1) {
            //rood
            myled1 = 0;
            myled2 = 1;
            myled3 = 1;
            rood=1;

            pc.printf("calibratie tricep aan\n");
            wait(2);

            Calibratie_Triceps();
            drempelwaardeT=MOVAVG_T-1;
            pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);

            pc.printf("calibratie tricep klaar,\n");
            myled1 = 0;
            myled2 = 0;
            myled3 = 0;
            wit=1;

            wait(2);

            myled1=1;
            myled2=1;
            myled3=1;
            rood=0;
            wit=0;
        }
        if (key==2) {
            //green
            myled1 = 1;
            myled2 = 0;
            myled3 = 1;
            rood=0;
            groen=1;

            pc.printf("calibratie bicep snelheid 1 aan\n");
            wait(2);

            Calibratie_Biceps();
            drempelwaardeB1=MOVAVG_B-1;
            pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
            myled1 = 0;
            myled2 = 0;
            myled3 = 0;
            wit=1;
            wait(2);

            wit=0;
            myled1 = 1;
            myled2 = 0;
            myled3 = 1;
            rood=0;

            pc.printf("calibratie biceps snelheid 2 aan\n");
            wait(2);

            Calibratie_Biceps();
            drempelwaardeB2=MOVAVG_B-1;
            pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
            myled1 = 0;
            myled2 = 0;
            myled3 = 0;
            wit=1;

            wait(2);

            wit=0;
            myled1 = 1;
            myled2 = 0;
            myled3 = 1;
            rood=0;

            pc.printf("calibratie biceps snelheid 3 aan\n");
            wait(2);
            Calibratie_Biceps();
            drempelwaardeB3=MOVAVG_B-1;
            pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
            myled1 = 0;
            myled2 = 0;
            myled3 = 0;
            wit=1;

            wait(2);

            pc.printf("caliratie biceps is klaar\n");
            wit=0;
            groen=0;
            myled1=1;
            myled2=1;
            myled3=1;
        }
        if (key==3) {
            motor2.setPosition(0);
            //blue

            myled1 = 1;
            myled2 = 1;
            myled3 = 0;
            blauw=1;
            wait(2);

            if(drempelwaardeT==0) {
                pc.printf("geen waarde calibratie TRICEPS \n");
                myled1 = 0;
                myled2 = 0;
                myled3 = 0;
                wit=1;
                wait(0.5);
                wit=0;
            }
            if (drempelwaardeB1==0) {
                pc.printf("geen waarde calibratie BICEPS 1 \n");
                myled1 = 0;
                myled2 = 0;
                myled3 = 0;
                wit=1;
                wait(0.5);
                wit=0;
            }
            if (drempelwaardeB2==0) {
                pc.printf("geen waarde calibratie BICEPS 2 \n");
                myled1 = 0;
                myled2 = 0;
                myled3 = 0;
                wit=1;
                wait(0.5);
                wit=0;
            }
            if (drempelwaardeB3==0) {
                pc.printf("geen waarde calibratie BICEPS 3 \n");
                myled1 = 0;
                myled2 = 0;
                myled3 = 0;
                wit=1;
                wait(0.5);
                wit=0;
            } else {
                pc.printf("eerst positie dan snelheid aangeven /n");

                //bepaling van positie met triceps 1
                Ticker log_timerT1;

                arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
                arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
                arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);

                myled1 = 0;
                myled2 = 1;
                myled3 = 1;
                rood=1;

                log_timerT1.attach(Triceps, 0.005);
                wait(2);
                log_timerT1.detach();

                // positie van batje met behulp van Triceps

                if (MOVAVG_T >= drempelwaardeT) {
                    yT1=1;
                } else {
                    yT1=0;
                }

                pc.printf("Triceps meting 1 is klaar.\n");
                myled1 = 1;
                myled2 = 1;
                myled3 = 0;
                rood=0;

                wait(1);

                //bepaling van positie met tricep 2
                Ticker log_timerT2;

                arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
                arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
                arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);

                myled1 = 0;
                myled2 = 1;
                myled3 = 1;
                rood=1;

                log_timerT2.attach(Triceps, 0.005);
                wait(2);
                log_timerT2.detach();

                if (MOVAVG_T >= drempelwaardeT) {
                    yT2=1;
                } else {
                    yT2=0;
                }

                pc.printf("Triceps meting 2 is klaar.\n");
                myled1 = 1;
                myled2 = 1;
                myled3 = 0;
                rood=0;

                // *** INPUT MOTOR 2 ***
                positie=yT1+yT2;

                //controle positie op scherm
                if (positie==0) {
                    pc.printf("Motor 2 blijft op stand 1\n");
                } else {
                    if (positie==1) {
                        pc.printf("Motor 2 gaat naar stand 2\n");
                    } else {
                        if (positie==2) {
                            pc.printf("Motor 2 gaat naar stand 3\n");
                        }
                    }
                }

                Ticker looptimer2;
                looptimer2.attach(motor2aansturing,TSAMP1);
                wait(3);
                looptimer2.detach();
                pc.printf("Detach Motor 2\n");

                //   eind aansturing motor 2

                wait(1);
                Ticker log_timerB;

                arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
                arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
                arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);

                myled1 = 1;
                myled2 = 0;
                myled3 = 1;
                rood=0;
                groen=1;

                log_timerB.attach(Biceps,0.005);
                wait(2);
                log_timerB.detach();

                //bepaling van snelheidsstand met biceps

                if (MOVAVG_B >= drempelwaardeB1) {
                    yB1=1;
                    if (MOVAVG_B >= drempelwaardeB2) {
                        yB2=1;
                        if (MOVAVG_B >= drempelwaardeB3) {
                            yB3=1;
                        } else {
                            yB3=0;
                        }
                    } else {
                        yB2=0;
                    }
                } else {
                    yB1=0;
                }

                pc.printf("Biceps meting is klaar.\n");
                myled1 = 1;
                myled2 = 1;
                myled3 = 0;
                groen=0;

                // *** INPUT MOTOR 1 ***
                snelheidsstand=yB1+yB2+yB3;

                //controle snelheidsstand op scherm
                if (snelheidsstand==0) {
                    pc.printf("Motor 1 beweegt niet \n");
                } else {
                    if (snelheidsstand==1) {
                        pc.printf("Motor 1 beweegt met snelheid 1 \n");
                    } else {
                        if (snelheidsstand==2) {
                            pc.printf("Motor 1 beweegt met snelheid 2 \n");
                        } else {
                            if (snelheidsstand==3) {
                                pc.printf("Motor 1 beweegt met snelheid 3 \n");
                            }
                        }
                    }
                }

                Ticker looptimer1;
                motordir1 = 1;
                looptimer1.attach(motor1aansturing,TSAMP1);
                wait(2);
                looptimer1.detach();
                pc.printf("detachMotor1\n");

                pid(0,0,true);

                Ticker looptimer3;
                looptimer3.attach(motor1aansturingdeel2,TSAMP1);
                wait(3); //is aan te passen (tijd die nodig is om balletje te slaan)
                looptimer3.detach();
                pc.printf("detachMotor1\n");

                pidpositie(0,0,true);
                pwm_motor1.write(0);
                toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!

                myled1=1;
                myled2=1;
                myled3=1;
                groen=0;
                blauw=0;
            }
        }
    }
}//end int main

float pid(float setspeed, float measurement, bool reset )
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    if(reset==true) {
        out_i = 0;
        prev_error = 0;
    }
    error  = setspeed-measurement;
    out_p  = error*K_P;
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    prev_error = error;
    return out_p + out_i + out_d;
}

float pidpositie(float setposition, float measurement, bool reset)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    if(reset==true) {
        out_i = 0;
        prev_error = 0;
    }
    error  = setposition-measurement;
    out_p  = error*K_Pp;
    out_i += error*K_Ip;
    out_d  = (error-prev_error)*K_Dp;
    prev_error = error;
    return out_p + out_i + out_d;
}

void Triceps()
{
    //Triceps lezen
    emg_valueT = emgT.read_u16();
    emg_value_f32T = emgT.read();

    //Triceps filteren
    arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, &filtered_emgT, 1);
    arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
    filtered_emgT = fabs(filtered_emgT);
    arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
    filtered_emgT = fabs(filtered_emgT);

    //Triceps moving average
    T0=filtered_emgT*1000;
    MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
    T29=T28;
    T28=T27;
    T27=T26;
    T26=T25;
    T25=T24;
    T24=T23;
    T23=T22;
    T22=T21;
    T21=T20;
    T20=T19;
    T19=T18;
    T18=T17;
    T17=T16;
    T16=T15;
    T15=T14;
    T14=T13;
    T13=T12;
    T12=T11;
    T11=T10;
    T10=T9;
    T9=T8;
    T8=T7;
    T7=T6;
    T6=T5;
    T5=T4;
    T4=T3;
    T3=T2;
    T2=T1;
    T1=T0;

    //sturen naar scherm (Realterm)
    pc.printf("Moving average T %f\r\n",MOVAVG_T);

    //sturen naar HID Scope
    /*scope.set(0,emg_valueT);        //ruwe data
    scope.set(1,filtered_emgT);     //filtered
    scope.send();*/
}

void Biceps()
{
    //Biceps lezen
    emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
    emg_value_f32B = emgB.read();

    //Biceps filteren
    arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
    arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
    filtered_emgB = fabs(filtered_emgB);
    arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
    filtered_emgB = fabs(filtered_emgB);

    //Biceps moving average
    B0=filtered_emgB*1000;
    MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667;
    B59=B58;
    B58=B57;
    B57=B56;
    B56=B55;
    B55=B54;
    B54=B53;
    B53=B52;
    B52=B51;
    B51=B50;
    B50=B48;
    B49=B49;
    B48=B47;
    B47=B46;
    B46=B45;
    B45=B44;
    B44=B43;
    B43=B42;
    B42=B41;
    B41=B40;
    B40=B39;
    B39=B38;
    B38=B37;
    B37=B36;
    B36=B35;
    B35=B34;
    B34=B33;
    B33=B32;
    B32=B31;
    B31=B30;
    B30=B29;
    B29=B28;
    B28=B27;
    B27=B26;
    B26=B25;
    B25=B24;
    B24=B23;
    B23=B22;
    B22=B21;
    B21=B20;
    B20=B19;
    B19=B18;
    B18=B17;
    B17=B16;
    B16=B15;
    B15=B14;
    B14=B13;
    B13=B12;
    B12=B11;
    B11=B10;
    B10=B9;
    B9=B8;
    B8=B7;
    B7=B6;
    B6=B5;
    B5=B4;
    B4=B3;
    B3=B2;
    B2=B1;
    B1=B0;

    //sturen naar scherm
    pc.printf("Moving average B %f\r\n",MOVAVG_B);

    //naar HID Scope
    /*scope.set(2,emg_valueB);        //ruwe data
    scope.set(3,filtered_emgB);     //filtered
    scope.send();*/
}

void Calibratie_Triceps()
{
    Ticker log_timerT;

    arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
    arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
    arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);

    log_timerT.attach(Triceps, 0.005);
    wait(2);
    log_timerT.detach();
}

void Calibratie_Biceps()
{
    Ticker log_timerB;

    arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
    arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
    arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);

    log_timerB.attach(Biceps, 0.005);
    wait(2);
    log_timerB.detach();
}

void motor2aansturing()
{
    switch(positie) {
        case 0:
            if (motor2.getPosition()>= 0) {
                motordir2 = 1;
                pwm_motor2.write(PWM2);
            } else {
                pwm_motor2.write(0);
            }
            break;
        case 1:
            if (motor2.getPosition()>= 40) {
                motordir2 = 1;
                pwm_motor2.write(PWM2);
            }
            if (motor2.getPosition()<= 40) {
                motordir2 = 0;
                pwm_motor2.write(PWM2);
            } else {
                pwm_motor2.write(0);
            }
            break;
        case 2:
            if (motor2.getPosition()<= 50) {
                motordir2 = 0;
                pwm_motor2.write(PWM2);
            } else {
                pwm_motor2.write(0);
            }
            break;
    }
}

void motor1aansturing()
{
    deltacounts = motor1.getPosition()- prevgetpos;
    prevgetpos=motor1.getPosition();
    omega=(deltacounts/TSAMP1);

    switch(toestand) {
        case SLAAN:
            pc.printf("SLAAN\n");
            new_pwm = pid(setspeed, omega);
            pwm_motor1.write(new_pwm);
            pc.printf("new-Pwm %f/r/n", new_pwm);
            motordir1 = 1;
            if (motor1.getPosition() <= ANGLEMAX) {
                toestand = TERUGKEREN;
                pwm_motor1.write(0.0);
                pc.printf("toestand = terugkeren, wacht tot 2e ticker\n\r");
            }
            break;

        case WACHTEN:
            pwm_motor1.write(0);
            pc.printf("ifwachten\n");
            if (snelheidsstand != 0 ) {
                toestand = SLAAN;
                pc.printf("slaan \n");
                switch(snelheidsstand) {
                    case 1:
                        setspeed = V1;
                        pc.printf("Snel 1 \n");
                        break;
                    case 2:
                        setspeed = V2;
                        pc.printf("Snel 2\n");
                        break;
                    case 3:
                        setspeed = V3;
                        pc.printf("Snel 3 \n");
                        break;
                }//end switch
            } //end if
            break;
    } //end switch

    scope.set(0,motor1.getPosition());
    scope.set(1,ANGLEMAX);
    scope.set(3,omega);
    scope.set(4, setspeed);
    scope.send();
}

void motor1aansturingdeel2()
{
    deltacounts = motor1.getPosition()- prevgetpos;
    prevgetpos=motor1.getPosition();
    omega=(deltacounts/TSAMP1);

    switch(toestand) {
        case TERUGKEREN:
            new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());
            if (new_pwmpos > 0) {
                motordir1 = 0;
                //pc.printf("motor1.getPosition %d\r\n", motor1.getPosition()); voor controle
            } else {
                motordir1 = 1;
            }
            pwm_motor1.write(fabs(new_pwmpos));
            break;
    } //end switch

    scope.set(0,motor1.getPosition());
    scope.set(2,ANGLEMIN);
    scope.set(3, omega);
    scope.set(4, setspeed);
    scope.send();
}