File content as of revision 1:a602cde74178:

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

DigitalOut gpo(D0);
DigitalOut led(LED_RED);
AnalogIn pot1(A0);
AnalogIn pot2(A1);
QEI encoder1(D10, D11, NC, 32);
QEI encoder2(D12, D13, NC, 32);
FastPWM motor1_pwm(D6);
DigitalOut motor1_richting(D7);

MODSERIAL pc(USBTX, USBRX);

int count = 0;

Ticker MotorTicker;

void read_pots(double &pot1_value, double &pot2_value) // read pot 1 en pot 2
{
    pot1_value = pot1.read();
    pot2_value = pot2.read();
}

double det_ref(double ref_value) //zet referentiewaarde om in positie
{
    return ref_value * 2 * 3.1416;
}

double meas_pos () //Encoder functie
{
    return encoder1.getPulses()/32.0/131.25*2.0*3.1416;
}

double PID_controller (meas_pos, ref_pos, gain)
{
    double error = ref_pos - meas_pos;
    
    // Proportional control
    double u = gain * error;
    
        if (u > 1.0)
            {
                u = 1.0;
            }
            
        else if (u < -1.0)
            {
                u = -1.0);
            }
    return u;
}

void move_mot(double u)
{
    
        if (u <= 0) 
        {
            motor1_richting = 0;
            motor1_pwm.write(-u); //write Duty cycle 
        }
        
        if (u >= 0) 
        {
            motor1_richting = 1;
            motor1_pwm.write(u); //write Duty cycle 
        }
}


void Motor_control()
{    
    volatile double pot_value, volatile double gain;
    
    read_pots(pot_value, gain);
    
    volatile double yref = det_ref(pot_value); // reference position
    
    volatile double y = meas_pos(); // measured position
    
    volatile double u = PID_controller(y, yref, gain); // output PID controller
    
    move_mot(u); //functie die motor laat bewegen.
       
    count++;
    
    if (count == 400) // print 1x per seconde waardes.
    {
        pc.printf("u = %d, measure position y = %d, reference position yref = %d, gain = %d", u, y, yref, gain);
        count = 0;   
    }
}


int main()
{
    pc.baud(115200);
    
    int frequency_pwm = 16700; //16.7 kHz PWM
    motor1_pwm.period(1.0/frequency_pwm); // T = 1/f
    MotorTicker.attach(Motor_control, 0.0025);
    
    while (true) 
    {}
}