File content as of revision 2:866a8a9f2b93:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"

DigitalOut  motor1_direction(D4);
PwmOut      motor1_speed(D5);
PwmOut      led(D9);
DigitalIn   button_1(PTC6); //counterclockwise
DigitalIn   button_2(PTA4); //clockwise
Encoder     motor1(D12,D13);
HIDScope    scope(1);

MODSERIAL   pc(USBTX, USBRX);
volatile bool regelaar_ticker_flag;

void setregelaar_ticker_flag()
{
    regelaar_ticker_flag = true;
}

#define SAMPLETIME_REGELAAR 0.005
float referentie_arm1 = 0;

//define states
enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION};
uint8_t state = GOTOHOMEPOSITION;

const int pressed = 0;

double H;
double P;
double D;


void sethome(){
    motor1.setPosition(0);
    H = motor1.getPosition();
}

void move_motor1_ccw (){
    motor1_direction = 0;
    motor1_speed = 0.8;
}

void move_motor1_cw (){
    motor1_direction = 1;
    motor1_speed = 0.8;
}

void movetohome(){
    P = motor1.getPosition();
    D = (P - H);

    if (D > -5 && D < 5){
        motor1_speed = 0;
    }
    else if (D > 0){
        move_motor1_cw();
    }
    else if (D < 0){
        move_motor1_ccw();
    }
}

void move_motor1()
{
    if (button_1 == pressed) {
        move_motor1_cw ();
    } else if (button_2 == pressed) {
        move_motor1_ccw ();
    } else { 
        motor1_speed = 0;
    }
}

void read_encoder1 ()    // aflezen van encoder via hidscope??
{
    scope.set(0,motor1.getPosition());
    led.write(motor1.getPosition()/100.0);
    scope.send();
    wait(0.2f);
}

int main()
{    
    while (true) {
        pc.baud(9600);          //pc baud rate van de computer
        
    switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
        
        case GOTOHOMEPOSITION:      //positie op 0 zetten voor arm 1
         {
            pc.printf("gotohomeposition\n\r");
            read_encoder1();
            sethome();
            state = MOVE_MOTOR;
            break;
        }
        
        case MOVE_MOTOR:            //motor kan cw en ccw bewegen a.d.h.v. buttons
        {
            pc.printf("move_motor\n\r");
            while (state == MOVE_MOTOR){
            pc.printf("whiletrue\n\r");
            move_motor1();
            if (button_1 == pressed && button_2 == pressed){
            state = BACKTOHOMEPOSITION;
            }
            }
            wait (1);
            break; 
        }
        
        case BACKTOHOMEPOSITION:    //motor gaat terug naar homeposition
        {
            pc.printf("backhomeposition\n\r");
            wait (1);
            
            ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
            
            while(state == BACKTOHOMEPOSITION){
                while(regelaar_ticker_flag != true);
                regelaar_ticker_flag = false;
                
                pc.printf("pulsmotorposition1 %d", motor1.getPosition());
                pc.printf("referentie %f\n\r", referentie_arm1);
                
                if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
                    referentie_arm1 = 0;
                }
                
            break;
        }
    }
}
}