File content as of revision 1:7d5e6bc2b314:

#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);


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 == 0){
        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
         {
            read_encoder1();
            sethome();
            state = move_motor;
            break;
        }
        
        case move_motor:            //motor kan cw en ccw bewegen a.d.h.v. buttons
        {
            move_motor1 ();
            state = homeposition;
            break;
        }
        
        case backtohomeposition:    //motor gaat terug naar homeposition
        {
            movetohome();
            break;
        }
}