File content as of revision 0:7098cdb29180:

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

// Defining pulses per revolution (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)
const double M1_timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS

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

// define ticker
Ticker aansturen;

QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) // ask victor whether it can be in if-loop for the two different motors

// defining flags
volatile bool flag_motor = false;


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


// defining reusable P controller
double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
{ 

  e_int = e_int + Ts * error;

    double PI_output = (Kp * error) + (Ki * e_int);
    return PI_output;
}

// Measure the error and apply the output to the plant
void motor1_Controller(DigitalOut directionPin(), PwmOut PWM(), double position, double Setpoint, double Kp, double Ki) //How to pass Digitalout and PWM, by reference?
{
    double reference = setpoint; // setpoint is in pulses
    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(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));

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



void move_motor(std::string state) //state can be left, right, or keypress
{
    
if (state == "left")
{
DigitalOut directionPin(D4); //direction
PwmOut PWM(D5); //speed
double position = wheel.getPulses(); //actual position
double Setpoint; //desired position
Kp; //controller gain
Ki; //set to zero
}

else if (state == "right")
{
DigitalOut directionPin(D4);
PwmOut PWM(D5);
double position = wheel.getPulses(); //actual position
double Setpoint; //desired position
double Kp; //controller gain, same as for left
double Ki; //set to zero
}

else if (state == "keypress") //different encoder than left and right
{
DigitalOut directionPin(D6); //direction
PwmOut PWM(D7); //speed
double position = wheel.getPulses(); //actual position, but different motor!
double Setpoint; //desired position, always the same actually
Kp; //controller gain, different from left and right
Ki; //set to zero
}



//call the flags and p-controller


aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker
while (1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm
{
// to make the motor move
        if(flag_motor) {
            flag_motor = false;
            motor1_Controller(directionPin, PWM, position, Setpoint, Kp, Ki); //How to pass Digitalout and PWM, by reference?
        }
    }
}