File content as of revision 8:8a3ab396853f:

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

DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);
DigitalOut led_r(LED_RED);

DigitalOut M1_Rotate(D2); // voltage only base rotation
PwmOut M1_Speed(D3);      // voltage only base rotation

MODSERIAL pc(USBTX, USBRX);

//QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);

DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
PwmOut M2_Speed(D5);        // encoder side pot 2 translation

DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation

DigitalIn links(SW3);
DigitalIn rechts(SW2);
//DigitalOut M2_rotate(D6);
//PwmOut M2_Speed(D7);

AnalogIn pot1(A4); // pot 1 motor 1
AnalogIn pot2(A3); // pot 2 motor 3

bool draairechts;
bool draailinks;
bool turn = 0;
float waiter = 0.1;
float afstand = 200;
float translation = 0;
float degrees3 = 0;

float Puls_degree = (8400/360);
float wheel1 = 16;
float wheel2 = 31;
float wheel3 = 41;
float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
float pi = 3.14159265359;

void GetDirections()
{
    pc.baud(115200);
    if ((rechts == 0) && (links == 0) && (turn == 0)) {
        draailinks = 0;
        draairechts = 0;
        turn = 1;
        pc.printf("begin de actie \n \r ");
        wait(waiter);

    } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
        draailinks = 0;
        draairechts = 0;
        turn = 0;
        pc.printf("breek de actie af \n \r ");
        wait(waiter);
    } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {

    } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
        /* if the right button is pressed and the motor isn't rotating to the left,
        then start rotating to the right etc*/
        draairechts = !draairechts;
        pc.printf("draai naar rechts \n \r ");
        wait(waiter);
    } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
        draailinks = 0;
        draairechts = !draairechts;
        pc.printf("draai naar rechts na links \n \r ");
        wait(waiter);
    } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
        draailinks = !draailinks;
        pc.printf("draai naar links \n \r ");
        wait(waiter);
    } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
        draairechts = 0;
        draailinks = !draailinks;
        pc.printf("draai naar links na rechts \n \r ");
        wait(waiter);
    }
    wait(2*waiter);
}

float GetPositionM2()
{
    float pulses2 = motor2.getPulses();
    float degrees2 = (pulses2/Puls_degree);
    float radians2 = (degrees2/360)*2*pi;
    float translation = ((radians2/overbrenging)*32.25);

    return translation;
}
float GetRotationM3()
{
    float pulses3 = motor3.getPulses();
    float degrees3 = (pulses3/Puls_degree);
    float radians3 = (degrees3/360)*2*pi;

    return degrees3;
}
void GoBack()
{
    while (GetPositionM2() > 0) {
        M3_Speed = 0;
        M2_Speed = 1;
        M2_Rotate = 0;
        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
        led_r = 0;
    }
    M2_Speed = 0;


    while (GetRotationM3() > 0) {
        M3_Rotate = 1;
        M3_Speed = 0.2;
        led_r = 1;
        led_b = 0;
        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());

    }
        M3_Speed = 0;

        turn = 0;
    

}
void Burgerflip()
{
    if (GetPositionM2() > afstand) {
        M3_Speed = 0.2;
        M3_Rotate = 0;
        M2_Speed = 0;
    } else if (GetPositionM2() < afstand) {
        M2_Speed = 1;
        M2_Rotate = 1;

    }
    if (GetRotationM3() > 150) {
        GoBack();
    }
}

int main()
{
    led_g = 1;
    led_b = 1;
    led_r = 1;

    while (true) {

        GetDirections();
        if (draairechts == true) {
            M1_Speed = 0.2;
            M1_Rotate = 0;
        } else if (draailinks == true) {
            M1_Speed = 0.2;
            M1_Rotate = 1;
        } else if (turn == 1) {
            /*M2_Speed = 0.5;
            M2_Rotate = 1;
            M3_Speed = 0.5;
            M3_Rotate = 1;*/
            Burgerflip();
        } else if (turn == 0) {
            M2_Speed = 0;
            M3_Speed = 0;
        }
        if ((draailinks == false) && (draairechts == false)) {
            M1_Speed = 0;
        }
        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
        /* pulses = 8400 */
    }
}
/*
        float GetReferenceVelocity() {
            // Returns reference velocity in rad/s.
            // Positive value means clockwise rotation.
            const float maxVelocity=8.4; // in rad/s of course!    float referenceVelocity;  // in rad/s
            if (a)   {
                // Clockwise rotation
                referenceVelocity = potMeterIn * maxVelocity;
                
            } else {
                // Counterclockwise rotation
                referenceVelocity = -1*potMeterIn * maxVelocity;
                
            }
            return referenceVelocity;
        }

        void SetMotor1(float motorValue) {
            // Given -1<=motorValue<=1, this sets the PWM and direction
            // bits for motor 1. Positive value makes motor rotating
            // clockwise. motorValues outside range are truncated to
            // within range
            if (motorValue >=0) {
                motor1DirectionPin=1;
            } else {
                motor1DirectionPin=0;
            }
            if (fabs(motorValue)>1) {
                motor1MagnitudePin = 1;
            } else {
                motor1MagnitudePin = fabs(motorValue);
            }
        }

        void SetMotor2(float motorValue) {
            // Given -1<=motorValue<=1, this sets the PWM and direction
            // bits for motor 1. Positive value makes motor rotating
            // clockwise. motorValues outside range are truncated to
            // within range
            if (motorValue >=0) {
                motor2DirectionPin=1;
            } else {
                motor2DirectionPin=0;
            }
            if (fabs(motorValue)>1) {
                motor2MagnitudePin = 1;
            } else {
                motor2MagnitudePin = fabs(motorValue);
            }
        }

        void SetMotor3(float motorValue) {
            // Given -1<=motorValue<=1, this sets the PWM and direction
            // bits for motor 1. Positive value makes motor rotating
            // clockwise. motorValues outside range are truncated to
            // within range
            if (motorValue >=0) {
                motor3DirectionPin=1;
            } else {
                motor3DirectionPin=0;
            }
            if (fabs(motorValue)>1) {
                motor3MagnitudePin = 1;
            } else {
                motor3MagnitudePin = fabs(motorValue);
            }
        }
        */