File content as of revision 4:1b4885298ade:

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

Serial pc(USBTX, USBRX);
QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in)

// Define pin for motor control
DigitalOut directionPin(D4);
PwmOut PWM(D5);

// define ticker
Ticker aansturen;
Ticker Printen;

// define rotation direction
const int cw = 1;
const int ccw = 0;

// Controller gain proportional and intergrator
const double motor1_Kp = 5; // more or les random number.


// calculating pulses to rotations in degree.
const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4.  10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
double Rotation = -2; // rotation in degree
double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.

// defining flags
volatile bool flag_motor = false;
volatile bool flag_pcprint = false;

// making function flags.
void Go_flag_motor()
{
    flag_motor = true;
}

void Go_flag_pcprint()
{
    flag_pcprint = true;
}

// Reusable P controller
double P(double error, const double Kp)
{ 

    double P_output = Kp * error;
    return P_output;
}
// Next task, measure the error and apply the output to the plant
void motor1_Controller()
{
    double reference = movement;
    double position = wheel.getPulses();
    double error_pulses = (reference - position); // calculate the error in pulses
    double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
    
    double output = abs(P( error_rotation, motor1_Kp));

    if(error_pulses > 0) {
        directionPin.write(cw);
    
    }
    if(error_pulses < 0) {
        directionPin.write(ccw); 
    }
PWM.write(output); // out of the if loop due to abs output


}


void counts_showing()
{

    double kijken = wheel.getPulses();
    pc.printf("ref %.0f count%.0f \n",movement,kijken);

}

int main()
{
    aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
    Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
    while( 1 ) {

        if(flag_motor) {
            flag_motor = false;
            motor1_Controller();
        }

        if(flag_pcprint) {
            flag_pcprint = false;
            counts_showing();
        }

    }
}