File content as of revision 12:996f9f8e3acc:

#include "mbed.h"           //Mbed bibliotheek inladen, standaard functies
#include "MODSERIAL.h"      //MODSERIAL bibliotheek inladen, communicatie met PC
#include "encoder.h"        //Encoder bibliotheek inladen, communicatie met encoder
#include "TextLCD.h"        //LCD scherm bibliotheek inladen, communicatie met LCD scherm

//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR         0.005       //Sampletijd ticker regelaar motor
#define KP                          0.001       //Factor proprotionele regelaar
#define SAMPLETIME_EMG              0.005       //Sampletijd ticker EMG meten
#define PULS_ARM1_HOME_KALIBRATIE   180         //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
#define PULS_ARM2_HOME_KALIBRATIE   393         //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft

//Pinverdeling en naamgeving variabelen
TextLCD         lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2);      //LCD scherm
MODSERIAL       pc(USBTX, USBRX);                               //PC

PwmOut pwm_motor_arm1(PTA5);            //PWM naar motor arm 1
DigitalOut dir_motor_arm1(PTA4);        //Richting van motor arm 1
Encoder puls_motor_arm1(PTD0, PTD2);    //Encoder pulsen tellen van motor arm 1, (geel, wit)
PwmOut pwm_motor_arm2(PTC8);            //PWM naar motor arm 2
DigitalOut dir_motor_arm2(PTC9);        //Ricting van motor arm 2
Encoder puls_motor_arm2(PTD5, PTA13);   //Encoder pulsen tellen van motor arm 2, (geel, wit)
AnalogIn EMG_bi(PTB1);                  //Meten EMG signaal biceps
//Blauw op 3,3 V en groen op GND

Ticker ticker_regelaar;                 //Ticker voor regelaar motor
Ticker ticker_EMG;                      //Ticker voor EMG meten

//States definiëren
enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS};     //Alle states benoemen, ieder krijgt een getal beginnend met 0
uint8_t state = RUST;                   //State is rust aan het begin

//Gedefinieerde datatypen en naamgeving en beginwaarden
char *lcd_r1 = new char[16];            //Char voor tekst op eerste regel LCD scherm
char *lcd_r2 = new char[16];            //Char voor tekst op tweede regel LCD scherm

float pwm_to_motor_arm1;                //PWM output naar motor arm 1
float pwm_to_motor_arm2;                //PWM output naar motor arm 2
float xbk;                              //Gemeten EMG waarde biceps in de kalibratie
int i;                                  //Voor for-loop

volatile bool regelaar_ticker_flag;     //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_ticker_flag()          //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
{
    regelaar_ticker_flag = true;
}

volatile bool regelaar_EMG_flag;        //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_EMG_flag()             //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
{
    regelaar_EMG_flag = true;
}

void keep_in_range(float * in, float min, float max)        //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
{
    if (*in < min) {                    //Als de waarde kleiner is als het minimum wordt de waarde het minimum
        *in = min;
    }
    if (*in > max) {                    //Als de waarde groter is dan het maximum is de waarde het maximum
        *in = max;
    } else {                            //In alle andere gevallen is de waarde de waarde zelf
        *in = *in;
    }
}

void arm1_naar_thuispositie()
{
    pwm_to_motor_arm1 = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    keep_in_range(&pwm_to_motor_arm1, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)

    if (pwm_to_motor_arm1 > 0) {                        //Als PWM is positief, dan richting 1
        dir_motor_arm1.write(1);
    } else {                                            //Anders richting nul
        dir_motor_arm1.write(0);
    }
    pwm_motor_arm1.write(fabs(pwm_to_motor_arm1));      //Output PWM naar motor is de absolute waarde van de berekende PWM
    pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
    pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);

    if (pwm_to_motor_arm1 == 0) {                       //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
        state = KALIBRATIE_ARM2;                        //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
        pc.printf("KALIBRATIE_ARM1 afgerond");          //Tekst voor controle Arm 1 naar thuispositie
    }
}

void arm2_naar_thuispositie()
{
    pwm_to_motor_arm2 = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    keep_in_range(&pwm_to_motor_arm2, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)

    if (pwm_to_motor_arm2 > 0) {                        //Als PWM is positief, dan richting 1
        dir_motor_arm2.write(1);
    } else {                                            //Anders richting nul
        dir_motor_arm2.write(0);
    }
    pwm_motor_arm2.write(fabs(pwm_to_motor_arm2));      //Output PWM naar motor is de absolute waarde van de berekende PWM

    if (pwm_to_motor_arm2 == 0) {                       //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
        state = KALIBRATIE_BICEPS;                      //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
        pc.printf("KALIBRATIE_ARM2 afgerond");          //Tekst voor controle Arm 2 naar thuispositie
    }
}


int main()
{
    while(1) {                      //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
        //PC baud rate instellen
        pc.baud(38400);             //PC baud rate is 38400 bits/seconde

        switch(state) {             //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case

            case RUST: {            //Aanzetten
                lcd_r1 = " BMT M9   GR. 4 ";        //Tekst op eerste regel van LCD scherm
                lcd_r2 = "Hoi! Ik ben PIPO";        //Tekst op tweede regel van LCD scherm
                wait(2);                            //Twee seconden wachten
                pc.printf("RUST afgerond");         //Tekst voor controle Aanzetten
                state = KALIBRATIE_ARM1;            //State wordt kalibratie arm 1, zo door naar volgende onderdeel
                break;                              //Stopt acties in deze case
            }

            case KALIBRATIE_ARM1: {                 //Arm 1 naar thuispositie
                pc.printf("kalibratie-arm1");
                wait(1);                            //Een seconde wachten
                ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken

                while(state == KALIBRATIE_ARM1) {
                    while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    arm1_naar_thuispositie();                       //Voer acties uit om arm 1 naar thuispositie te krijgen
                }
                wait(1);                                            //Een seconde wachten
                break;                                              //Stopt acties in deze case
            }

            case KALIBRATIE_ARM2: {                 //Arm 2 naar thuispositie
                wait(1);                            //Een seconde wachten

                //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken

                while(state == KALIBRATIE_ARM2) {
                    while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    arm2_naar_thuispositie();                       //Voer acties uit om arm 2 naar thuispositie te krijgen
                }
                wait(1);                                            //Een seconde wachten
                ticker_regelaar.detach();                           //Ticker detachten, ticker doet nu niks meer
                break;                              //Stopt acties in deze case
            }

            case KALIBRATIE_BICEPS: {               //Kalibratie EMG signaal biceps
                wait(1);                            //Een seconde wachten

                ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);     //Ticker iedere zoveel seconden de flag laten opsteken

                pc.printf("Ticker voor kalibratie compleet");               //Tekst voor controle Ticker voor kalibratie

                //5 seconden EMG biceps meten

                wait(1);                            //Een seconde wachten
                lcd_r1 = " EMG kalibratie ";        //Tekst op eerste regel van LCD scherm
                lcd_r2 = " Span biceps aan";        //Tekst op tweede regel van LCD scherm

                for (i=0; i<1000; i++) {
                    while(regelaar_EMG_flag != true) ;              //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    xbk = EMG_bi.read();            //EMG signaal uitlezen
                }
                break;

            }
            default: {                              //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
                state = RUST;                       //Als dat gebeurt wordt de state rust en begint hij weer vooraan
            }
        }
        pc.printf("state: %u\n",state);
    }
}