File content as of revision 2:0cb899f2800a:

#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
#define KP                      1

//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(PTD1);        //Richting van motor arm 1
Encoder puls_motor_arm1(PTD0, PTC9);    //Encoder pulsen tellen van motor arm 1

Ticker ticker_regelaar;                  

//Gedefinieerde datatypen en naamgeving
bool rust = false;                      //Bool voor controleren volgorde in programma
bool kalibratie_positie = false;        //Bool voor controleren volgorde in programma
bool kalibratie_EMG = false;            //Bool voor controleren volgorde in programma

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

volatile bool regelaar_ticker_flag;
void setregelaar_ticker_flag(){
    regelaar_ticker_flag = true;
}

void keep_in_range(float * in, float min, float max){
    if (*in < min){
        *in = min;
    }
    if (*in > max){
        *in = max;
    }
    else {
        *in = *in;
    }
}
    

int puls_arm1_home = 363;
float pwm_to_motor_arm1;

//Beginwaarden voor variabelen


int main() {
    
    //PC baud rate instellen
    pc.baud(38400);             //PC baud rate is 38400 bits/seconde
    
    //Aanzetten
    if (rust == false && kalibratie_positie == false && kalibratie_EMG == false){
        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("Aanzetten compleet");    //Tekst voor controle Aanzetten
        rust = true;                        //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
    }
    
    
    
    //Arm 1 naar thuispositie
    if (rust == true && kalibratie_positie == false && kalibratie_EMG == false){
        wait(1);                            //Een seconde wachten
        
        ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
        
        while(rust == true && kalibratie_positie == false && kalibratie_EMG == false) {
            while(regelaar_ticker_flag != true) ;
            regelaar_ticker_flag = false;
            
            pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;
            keep_in_range(&pwm_to_motor_arm1, -1, 1);
            
            if (pwm_to_motor_arm1 > 0){
                dir_motor_arm1.write(1);
            }
            else {
                dir_motor_arm1.write(0);
            }
            pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
            
            if (pwm_to_motor_arm1 == 0) {
                 kalibratie_positie = true;
                 pc.printf("Arm 1 naar thuispositie compleet");
            }        
            wait(1);
            
        }   
    }    
}