File content as of revision 23:34e97668550f:

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

//Motor 2
DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
PwmOut motor2speed(D5); // D5 is an input on the motor that controls the speed of motor number 2
DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
Encoder motor2(D13,D12,true); // The encoder is able to read and set position/speed values
MODSERIAL pc(USBTX,USBRX); // This input is used to send data to the pc
Ticker TickerController; // This adds a Ticker to the function +TickerController
HIDScope scope(2);
//AnalogIn potmeter2(A0); /NIEUW

// Defining constants
double counts_per_revolution=4200; 
double degrees_per_turn=360;
double counts_per_degree=counts_per_revolution/degrees_per_turn; //11.67 counts/degree
const double motor2_Kp = 1; //controller gain which will be multiplied with the error (*how fast will the error be corrected)
const double reference = 400;

// Function P-controller
double P(double error, const double Kp) //returns error * controller gain
{
    return Kp*error; 
}
// Function that calls the P-controller
double motor2_controller() // Void function that compares the current position with a reference value and outputs 
    {
        double position=motor2.getPosition(); //current motor position
        double error=reference-position;
        pc.printf("ik doe het positie = %d en error =%d en reference=%d \r\n", position, error, reference);
        scope.set(0,motor2.getPosition());
        return  P(error,motor2_Kp);
    }

// MAIN
int main()
{
    pc.baud(9600); // baud rate at which the information is processed to the computer
    //motor2.setPosition(0); // calls the current position of the motor zero
    while(true) {
        if (button.read() < 0.5) {   //if button pressed
            motor2direction = 0; // zero is clockwise (front view)
            motor2speed = 0.5f; // motorspeed
            pc.printf("positie = %d \r\n", motor2.getPosition());
        } else {                  // If button is not pressed
            double output_motor = motor2_controller();
            while(output_motor>0){
        motor2speed=output_motor;
        motor2direction=1;
        break;}
        while(output_motor<0){
        motor2speed=fabs(output_motor);
        motor2direction=0;
        break;}
            {
            pc.printf("else loop controller");
            }
        }
    
// Piece of code that keeps the value of the position between the counts per revolution
        while ((motor2.getPosition()>counts_per_revolution) || (motor2.getPosition()<-counts_per_revolution)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
        {
            motor2.setPosition(0);
            pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
            break;
        } 
    }
    }