File content as of revision 2:4fb733fffd1f:

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

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

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

MODSERIAL pc(USBTX, USBRX);

int main()
{
    pc.baud(115200);
    pc.printf("hoi\n");
    M1_rotate = 1;
    M2_rotate = 1;
    M3_rotate = 1;
    a = 1;

    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);
        }
    }
    bool GetDirections() {
        bool draairechts = false;
        bool draailinks = false;
        if (rechts == 1 && draairechts = false) { //Als je rechts aanspant en hij draait nog niet naar rechts
            draairechts = true;
        } else if (rechts == 1 && draairechts = true) { //Als je rechts aanspant en hij draait al naar rechts
            draairechts = false;
        } else if (links == 1 && draailinks = false) { //Als je links aanspant en hij draait nog niet naar links
            draailinks = true;
        } else if (links == 1 && draailinks = true) { // Als je links aanspant en hij draait al naar links
            draailinks = false;
        }

        else if (links == 1 && draairechts = true) { // Als je links aanspant en hij draait nog naar rechts
            draailinks = true;
            draairechts = false;
        } else if (rechts == 1 && draailinks = true) { // Als je rechts aanspant en hij draait nog naar links
            draairechts = true;
            draailinks = false;
        }
        return draailinks;
        return draairechts;
    }

    while (true) {
        GetDirections();
        if (draailinks == true) {
            M1_Speed = 0.5;
            M1_Rotate = 0;
        } else if (draairechts == true) {
            M1_Speed = 0.5;
            M1_Rotate = 1;
        } else {
            M1_Speed = 0;
        }



        /*
        float M2 = M2_rotate.read();
        float potje1 = pot1.read();
        float potje2 = pot2.read();
        M1_Speed.write(potje1);
        M3_Speed.write(potje2);
        wait(0.1);
        pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
        */
        
    }
}