File content as of revision 6:4e4df2f6157e:

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


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 = 150;
float translation = 0;
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);
}

void GetPosition()
{
    float pulses = motor2.getPulses();
    float Puls_degree = (8400/360);
    float degrees = (pulses/Puls_degree);
    float wheel1 = 16;
    float wheel2 = 31;
    float wheel3 = 41;
    float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
    float pi = 3.14159265359;
    float radians = (degrees/360)*2*pi;
    float translation = ((radians/overbrenging)*32.25);
    pc.printf("translation %f , degrees %f, radians %f, pulses %f \n \r ",translation, degrees, radians, pulses);


}
void Burgerflip()
{
    M2_Speed = 0.5;
    M2_Rotate = 1;
    M3_Speed = 0.5;
    M3_Rotate = 1;

}
int main()
{
    while (true) {
        GetDirections();
        if (draairechts == true) {
            M1_Speed = 0.5;
            M1_Rotate = 0;
        } else if (draailinks == true) {
            M1_Speed = 0.5;
            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;
        }
        GetPosition();
        /* 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);
            }
        }
        */