File content as of revision 18:264812048611:

/********************************************/
/*                                          */
/*   BRONCODE GROEP 7, MODULE 9, 2014       */
/*       *-THE SLAP-*                       */
/*                                          */
/* -Anne ten Dam                            */
/* -Laura de Heus                           */
/* -Moniek Strijdveen                       */
/* -Bart Arendshorst                        */
/* -Peter Bartels                           */
/********************************************/

#include "TextLCD.h"
#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "PwmOut.h"
#include "arm_math.h"
#include "MODSERIAL.h"

/*definieren pinnen Motoren*/
#define M1_PWM PTA5
#define M1_DIR PTA4
#define M2_PWM PTC8
#define M2_DIR PTC9
/*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
#define TSAMP 0.005
#define K_P (2000)
#define K_I (0)
#define K_D (0.1)
#define K_P_motor2 (75)
#define K_D_motor2 (0.01)
#define I_LIMIT 100.
#define lengte_arm 0.5

/*
Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden.
*/
TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
Encoder motor1(PTD3,PTD1);
Encoder motor2(PTD5, PTD0);
PwmOut pwm_motor1(M1_PWM);
PwmOut pwm_motor2(M2_PWM);
DigitalOut motordir1(M1_DIR);
DigitalOut motordir2(M2_DIR);
DigitalOut LEDGREEN(LED_GREEN);
DigitalOut LEDRED(LED_RED);
MODSERIAL pc(USBTX,USBRX);
HIDScope scope(3);
AnalogIn emg(PTB1);

Timer statetimer;
/*
definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
*/
Ticker statemachine;
Ticker screen;
arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz
arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz
arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz
arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
int previous_herhalingen = 0;
int current_herhalingen = 0;
int current_herhalingen_1 = 0;
int previous_herhalingen_1 = 0;
int previous_pos_motor1 = 0;
int previous_pos_motor2 = 0;
int current_pos_motor1;
int current_pos_motor2;
int EMG = 1;
int aantal_pieken = 0;
int doel;
int doel_richting;
int doel_hoogte;
int puls_richting1 = 4000;
int puls_richting2;
int puls_richting3 = 4000;
bool aanspan;
void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
float pid_motor2(float setpoint, float measurement);
float pos_motor1_rad;
float pos_motor2_rad;
float PWM1_percentage = 0;
float PWM2_percentage = 0;
float PWM1;
float PWM2;
float prev_setpoint = 0;
float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
float lowpass_1_states[4];
float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684};
float lowpass_2_states[4];
float highpass_const[] = {0.638945525159022 , -1.277891050318045 ,  0.638945525159022 , 1.142980502539901 , -0.412801598096189};
float highpass_states[4];
float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362};
float notch_states[4];
float emg_filtered;
float emg_max = 0;
float emg_treshhold_laag = 0;
float emg_treshhold_hoog = 0;
float marge = 0;
float PWM_richting1;
float PWM_richting2;
float PWM_richting3;
float position2_setpoint;
float snelheid;

enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES
state_t state = RUST;

void rust()
{
    current_herhalingen = previous_herhalingen + 1;
    previous_herhalingen = current_herhalingen;
}//void rust

void pieken_tellen()
{
    if (emg_filtered>=emg_treshhold_hoog) {
        aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
    }//if
    if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt
        aanspan=false;
        aantal_pieken++;
        doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;

    }//if
}//void pieken_tellen

void emg_filtering()
{
    uint16_t emg_value;
    float emg_value_f32;
    emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
    emg_value_f32 = emg.read();

    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
    arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
    arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
    emg_filtered = fabs(emg_filtered);
    arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
    //scope.set(0,emg_value);     //uint value
    //scope.set(1,emg_filtered);  //processed float
    if(state!=EMG_KALIBRATIE) {
        pieken_tellen();
    }//if
    //pc.printf("%d\n\r",doel);
    scope.set(0, doel);
    scope.set(1, aantal_pieken);
    scope.send();
}//void emg_filtering()

void emg_max_meting()
{
    emg_filtering();
    if (emg_filtered>=emg_max) {
        emg_max=emg_filtered;
    }//if
    emg_treshhold_laag = 0.4*emg_max;
    emg_treshhold_hoog = 0.7*emg_max;
}//void emg_max_meting

void akkoord_geven()
{
    emg_filtering();
}

void emg_kalibratie()
{
    //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het  nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
    current_herhalingen = previous_herhalingen + 1;
    previous_herhalingen = current_herhalingen;
    //}
    if(current_herhalingen<=1000) {
        emg_max_meting();
    }//if
}//void emg_kalibratie

void arm_kalibratie()
{
    //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
    motor1.setPosition(0);
    //motor2.setPosition(0);
    akkoord_geven();
}//void arm_kalibratie

void doel_bepaling()
{
    if(200<=current_herhalingen && current_herhalingen <1200) {
        emg_filtering();
        doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
    }//if
    else if(current_herhalingen == 1200 && state==METEN_HOOGTE) {
        doel_hoogte = doel;
        aantal_pieken = 0;
        doel = 0;
    } else if(current_herhalingen == 1200 && state==METEN_RICHTING) {
        doel_richting = doel;
        aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
        doel = 0;
    } else if(1200<current_herhalingen && current_herhalingen<=2200) {
        akkoord_geven();
    }//else if
    else {
    }//else
}//void doel_bepaling

void meten_hoogte()
{
    current_herhalingen = previous_herhalingen + 1;
    previous_herhalingen = current_herhalingen;
    doel_bepaling();
}//void meten_hoogte

void meten_richting()
{
    current_herhalingen = previous_herhalingen + 1;
    previous_herhalingen = current_herhalingen;
    doel_bepaling();
}//void meten_richting

void GotoPosition (float position_setpoint_rad, float speed_radpersecond)
{
    static float setpoint = 0; 
    if (setpoint < position_setpoint_rad) {
        setpoint += (0.005 * speed_radpersecond);
        if (setpoint > position_setpoint_rad) {
            setpoint = position_setpoint_rad;
        }
    }

    else if (setpoint > position_setpoint_rad) {
        setpoint -= (0.005 * speed_radpersecond);
        if (setpoint < position_setpoint_rad) {
            setpoint = position_setpoint_rad;
        }
    } else if (setpoint == position_setpoint_rad) {
        current_herhalingen = previous_herhalingen + 1;
        previous_herhalingen = current_herhalingen;
    }
    
    pc.printf("%f\r\n", setpoint); 
    current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
    pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
    PWM1_percentage = pid(setpoint, pos_motor1_rad);

    if (PWM1_percentage < -100) {
        PWM1_percentage = -100;
    } else if (PWM1_percentage >100) {
        PWM1_percentage =100;
    }

    if(PWM1_percentage < 0) {
        motordir1 = 1;
    }//if
    else {
        motordir1 = 0;
    }//else

    pwm_motor1.write(abs(PWM1_percentage/100.));
}//void GotoPosition

float pid(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = (setpoint-measurement);
    out_p  = error*K_P;
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    return out_p + out_i + out_d;
}//float pid

void translatie (float position2_setpoint_rad, float speed2_radpersecond)
{
    static float setpoint = 0; 
    if (setpoint < position2_setpoint_rad) {
        setpoint += (0.005 * speed2_radpersecond);
        if (setpoint > position2_setpoint_rad) {
            setpoint = position2_setpoint_rad;
        }
    }

    else if (setpoint > position2_setpoint_rad) {
        setpoint -= (0.005 * speed2_radpersecond);
        if (setpoint < position2_setpoint_rad) {
            setpoint = position2_setpoint_rad;
        }
    } else if (setpoint == position2_setpoint_rad) {
        current_herhalingen = previous_herhalingen + 1;
        previous_herhalingen = current_herhalingen;
    }

    current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
    pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
    PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad);

    if (PWM2_percentage < -100) {
        PWM2_percentage = -100;
    } else if (PWM2_percentage >100) {
        PWM2_percentage =100;
    }

    if(PWM2_percentage < 0) {
        motordir2 = 1;
    }//if
    else {
        motordir2 = 0;
    }//else

    pwm_motor2.write(abs(PWM2_percentage/100.));
    prev_setpoint = setpoint;
}

float pid_motor2(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    float           out_d = 0;
    error  = (setpoint-measurement);
    out_p  = error*K_P_motor2;
    out_d  = (error-prev_error)*K_D_motor2;
    prev_error = error;
    return out_p + out_d;
}//float pid

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

void statemachinefunction()
{
    statetimer.reset();
    //pc.printf(".");
    switch(state) {
        case RUST: {
            rust();
            /*voorwaarde wanneer hij door kan naar de volgende case*/
            if (current_herhalingen == 100 && EMG == 1) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                state = EMG_KALIBRATIE;
                EMG = 0; //door EMG op 0 te zetten word deze loop nooit meer doorlopen, daarna zal altijd else worden uitgevoerd. Wat ook gelijk het kalibreren van de arm overslaat. Men kan na 1 keer kalibreren dus vaker achter elkaar schieten
            }//if (current_herhalingen == 100 && EMG == 1)
            else if(current_herhalingen == 100) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                state = ARM_KALIBRATIE;
            }//else
            break;
        }//case RUST:

        case EMG_KALIBRATIE: {
            emg_kalibratie();
            if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/
                current_herhalingen = 0;
                previous_herhalingen = 0;
                aantal_pieken = 0;
                doel = 0;
                state = ARM_KALIBRATIE;
            }//if (current_herhalingen >=1000)
            break;
        }//case EMG_KALIBRATIE

        case ARM_KALIBRATIE: {
            arm_kalibratie();
            if (aantal_pieken == 1) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                aantal_pieken = 0;
                doel = 0;
                state = METEN_HOOGTE;
            }//if (current_herhalingen == 100)
            break;
        }//case ARM_KALIBRATIE:

        case METEN_HOOGTE: {
            meten_hoogte();
            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                aantal_pieken = 0;
                doel = 0;
                //doel_hoogte = 0;
                state = METEN_RICHTING;
            }//if (current_herhalingen == 2800 && aantal_pieken == 1)
            else if (current_herhalingen == 2200) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                aantal_pieken = 0;
                doel = 0;
                state = METEN_HOOGTE;
            }///else
            break;
        }//case METEN_HOOGTE

        case METEN_RICHTING: {
            meten_richting();
            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                aantal_pieken = 0;
                doel = 0;
                state = INSTELLEN_RICHTING;
            }//if (current_herhalingen == 2800 && aantal_pieken == 1)
            else if (current_herhalingen == 2200) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                aantal_pieken = 0;
                doel = 0;
                state = METEN_RICHTING;
            }///else
            break;
        }//case METEN_RICHTING


        case TEST: {
            state = RUST;
            break;
        }

        case INSTELLEN_RICHTING: {
            //instellen_richting();
            if (doel_richting == 1) {
                position2_setpoint = 0;
            } else if (doel_richting ==2) {
                position2_setpoint = 14.5;
            } else {
                position2_setpoint = 25;
            }

            translatie(position2_setpoint, 1);

            if (current_herhalingen >= 400) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                doel_richting = 0; 
                state = SLAAN;
            }//if (current_herhalingen == 100
            break;
        }//case INSTELLEN_RICHTING

        case SLAAN: {
            if (doel_hoogte == 1) {
                snelheid = 2;
            } else if (doel_hoogte ==2) {
                snelheid = 2.25;
            } else {
                snelheid = 5;
            }
            GotoPosition(1.9, snelheid);
            if (current_herhalingen == 400) {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                prev_setpoint =0;
                //setpoint = 0;
                state = RETURN2RUST;
            }//if (current_herhalingen == 100)
            break;
        }//case SLAAN

        case RETURN2RUST: {
            GotoPosition(0,0.1);
            doel_richting = 0;
            doel_hoogte = 0;
            if (current_herhalingen >= 200) {
                current_herhalingen = 0; 
                translatie(0,1.6);
                if (current_herhalingen >=200){
                state = RUST;
                current_herhalingen = 0;
                previous_herhalingen = 0;
                current_herhalingen = 0;
                current_herhalingen = 0;
                }
            }//if (current_herhalingen == 100)
            
            break;
        }// case RETURN2RUST

        default: {
            state = RUST;
        }//default

    }//switch(state)
}//void statemachinefunction


void screenupdate()
{
    if(state==RUST) {
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
        lcd.locate(0,1);
        lcd.printf("  GROEP 7   ");
    }//if(state==RUST)

    else if(state==EMG_KALIBRATIE) {
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("Max. aanspannen");
        if(current_herhalingen<=200) {
            lcd.locate(0,1);
            lcd.printf("nog 5 sec.");
        }//if(current_herhalingen<=200)
        else if(current_herhalingen<=400) {
            lcd.locate(0,1);
            lcd.printf("nog 4 sec.");
        }//else if(current_herhalingen<=400)
        else if(current_herhalingen<=600) {
            lcd.locate(0,1);
            lcd.printf("nog 3 sec.");
        }//else if(current_herhalingen<=600)
        else if(current_herhalingen<=800) {
            lcd.locate(0,1);
            lcd.printf("nog 2 sec.");
        }//else if(current_herhalingen<=800)
        else if(current_herhalingen<=1000) {
            lcd.locate(0,1);
            lcd.printf("nog 1 sec.");
        }//else if(current_herhalingen<=1000)
    }//else if(state==EMG_KALIBRATIE)

    else if(state==ARM_KALIBRATIE) {
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("Set arm to zero");
        lcd.locate(0,1);
        lcd.printf("Klaar? Span aan");
    }//else if(state==ARM_KALIBRATIE)

    else if(state==METEN_HOOGTE) {
        lcd.cls();
        if(current_herhalingen<=200) {
            lcd.locate(0,0);
            lcd.printf("Hoogte bepalen:");
            lcd.locate(0,1);
            lcd.printf("span aan per vak");
        }//if(current_herhalingen<=200){
        else if(200<=current_herhalingen && current_herhalingen<1200) {
            lcd.locate(0,0);
            lcd.printf("Vak %d",doel);
            if(current_herhalingen<=400) {
                lcd.locate(0,1);
                lcd.printf("nog 5 sec.");
            }//if(current_herhalingen<=400)
            else if(current_herhalingen<=600) {
                lcd.locate(0,1);
                lcd.printf("nog 4 sec.");
            }//else if(current_herhalingen<=600)
            else if(current_herhalingen<=800) {
                lcd.locate(0,1);
                lcd.printf("nog 3 sec.");
            }//else if(current_herhalingen<=800)
            else if(current_herhalingen<=1000) {
                lcd.locate(0,1);
                lcd.printf("nog 2 sec.");
            }//else if(current_herhalingen<=1000)
            else if(current_herhalingen<1200) {
                lcd.locate(0,1);
                lcd.printf("nog 1 sec.");
            }//else if(current_herhalingen<=1200)
        }//else if(200<=current_herhalingen<=1200)
        else if(current_herhalingen<=2200) {
            lcd.locate(0,0);
            lcd.printf("Vak %d akkoord?",doel_hoogte);
            lcd.locate(0,1);
            lcd.printf("Span aan");
        }//else if(current_herhalingen<=1600){
        else {
            lcd.locate(0,0);
            lcd.printf("Opnieuw hoogte");
            lcd.locate(0,1);
            lcd.printf("bepalen");
        }//else{
    }//else if(state==METEN_HOOGTE){

    else if(state==METEN_RICHTING) {
        lcd.cls();
        if(current_herhalingen<=200) {
            lcd.locate(0,0);
            lcd.printf("Richting bepalen:");
            lcd.locate(0,1);
            lcd.printf("span aan per vak");
        }//if(current_herhalingen<=200)
        else if(200<=current_herhalingen && current_herhalingen<1200) {
            lcd.locate(0,0);
            lcd.printf("Vak %d",doel);
            if(current_herhalingen<=400) {
                lcd.locate(0,1);
                lcd.printf("nog 5 sec.");
            }//if(current_herhalingen<=400)
            else if(current_herhalingen<=600) {
                lcd.locate(0,1);
                lcd.printf("nog 4 sec.");
            }//else if(current_herhalingen<=600)
            else if(current_herhalingen<=800) {
                lcd.locate(0,1);
                lcd.printf("nog 3 sec.");
            }//else if(current_herhalingen<=800)
            else if(current_herhalingen<=1000) {
                lcd.locate(0,1);
                lcd.printf("nog 2 sec.");
            }//else if(current_herhalingen<=1000)
            else if(current_herhalingen<1200) {
                lcd.locate(0,1);
                lcd.printf("nog 1 sec.");
            }//else if(current_herhalingen<=1200)
        }//else if(200<=current_herhalingen<=1200)
        else if(current_herhalingen<=2200) {
            lcd.locate(0,0);
            lcd.printf("Vak %d akkoord?",doel_richting);
            lcd.locate(0,1);
            lcd.printf("Span aan");
        }//else if(current_herhalingen<=1600)
        else {
            lcd.locate(0,0);
            lcd.printf("Opnieuw richting");
            lcd.locate(0,1);
            lcd.printf("bepalen");
        }//else
    }//else if(state==METEN_RICHTING){

    else if(state==INSTELLEN_RICHTING) {
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("Instellen hoek");
        lcd.locate(0,1);
        lcd.printf("Even wachten...");
    }//else if(state==INSTELLEN_RICHTING){

    else if(state==SLAAN) {
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("Slaan, pas op");
        lcd.locate(0,1);
        lcd.printf("Let's pray");
    }//else if(state==INSTELLEN_RICHTING){

    else if(state==RETURN2RUST) {
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("Terug naar");
        lcd.locate(0,1);
        lcd.printf("0-positie");
    }//else if(state==INSTELLEN_RICHTING){

    else {
        lcd.cls();
        lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
    }//else{
}

int main()
{
    pwm_motor1.period_us(100);
    pwm_motor2.period_us(100);
    pc.baud(115200);
    statetimer.start();
    arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
    arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
    arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
    arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
    state = TEST;
    statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
    //screen.attach(&screenupdate, 0.2);
    while(1) {
        screenupdate();
        wait(0.2);
    };
}