File content as of revision 7:9a1007e35bac:

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

// Algemeen
DigitalIn button2(SW2);
DigitalIn button3(SW3);
AnalogIn But2(A5);
AnalogIn But1(A3);
DigitalOut led(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
MODSERIAL pc(USBTX, USBRX);

//Motoren
DigitalOut direction1(D4);
PwmOut pwmpin1(D5);
PwmOut pwmpin2(D6);
DigitalOut direction2(D7);
volatile float PWM1;
volatile float PWM2;
volatile float pwm2;

//Encoder
DigitalIn EncoderA(D13);
DigitalIn EncoderB(D12);
QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
double Pulses2;
double motor_position2;

//Pot meter
AnalogIn pot(A1);
AnalogIn pot0(A0);
float Pot2;
float Pot1;

//Ticker
Ticker Pwm;
Ticker PotRead;
Ticker Kdc;


//Kinematica
double stap;
double KPot;
float KPotabs;
float ElbowReference;
float Ellebooghoek1;
float Ellebooghoek2;
float Ellebooghoek3;
float Ellebooghoek4;
float Hoeknieuw;

//Limiet in graden
float lowerlim = 0;
float upperlim = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor

// VARIABLES PID CONTROLLER
double Kp = 6;          // Zonder arm: 6,0,1
double Ki = 0;          //
double Kd = 1;          //
double Ts = 0.001;      // Sample time in seconds

float Kinematics(float KPot)
{

    if (KPot > 0.45f) {
        stap = KPot*150*Ts;         // 144 graden van de arm in 5 seconden
        Hoeknieuw = ElbowReference + stap;
        return Hoeknieuw;
    }

    else if (KPot < -0.45f) {
        stap = KPot*150*Ts;
        Hoeknieuw = ElbowReference + stap;
        return Hoeknieuw;
    }

    else {
        return ElbowReference;
    }
}

float Limits(float Ellebooghoek2)
{

    if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) {       //Binnen de limieten
        Ellebooghoek3 = Ellebooghoek2;
    }

    else {
        if (Ellebooghoek2 >= upperlim) {                            //Boven de limiet
            Ellebooghoek3 = upperlim;
        } else {                                                    //Onder de limiet
            Ellebooghoek3 = lowerlim;
        }
    }

    return Ellebooghoek3;
}




// ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~

double PID_controller(double error)
{
    static double error_integral = 0;
    static double error_prev = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
    
    /* PID testing
    Kp = 10 * Pot2;
    Ki = 10 * Pot1;
    
    if (!But2) {
        Kd = Kd + 0.01;
    }
    if (!But1){
        Kd = Kd - 0.01;
    }
    */
    
    // Proportional part:
    double u_k = Kp * error;

    // Integral part
    error_integral = error_integral + error * Ts;
    double u_i = Ki * error_integral;

    // Derivative part
    double error_derivative = (error - error_prev)/Ts;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error;

    // Sum all parts and return it
    return u_k + u_i + u_d;
}

void moter2_control(double u)
{
    direction2= u < 0.0f; //positief = CW
    if (fabs(u)> 0.7f) {
        u = 0.7f;
    } else {
        u= u;
    }
    pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

void PwmMotor(void)
{
    // Reference hoek berekenen, in graden
    float Ellebooghoek1 = Kinematics(pwm2);
    float Ellebooghoek4 = Limits(Ellebooghoek1);
    ElbowReference = Ellebooghoek4;

    // Positie motor berekenen, in graden
    Pulses2 = encoder2.getPulses();
    motor_position2 = -(Pulses2/8400)*360;

    double error = ElbowReference - motor_position2;
    double u = PID_controller(error);
    moter2_control(u);

}

void MotorOn(void)
{
    pwmpin2 = 0;
    Pwm.attach (PwmMotor, Ts);

}
void MotorOff(void)
{
    Pwm.detach ();
    pwmpin2 = 0;
}

void ContinuousReader(void)
{
    Pot2 = pot.read();
    Pot1 = pot0.read();
    pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
}

/*
void Kdcount (void)             // Voor het testen van de PID waardes
{
    int count = 0;
    ElbowReference = ElbowReference + 10;
    if (count == 7) {
        ElbowReference = 0;
        count = 0;
    }
    count ++;
}
*/

int main()
{
    Timer t;
    t.start();
    int counter = 0;
    pwmpin2.period_us(60);
    PotRead.attach(ContinuousReader,Ts);
    //Kdc.attach(Kdcount,5);                //Voor PID waarde testen
    pc.baud(115200);
    //pc.printf("start\r\n");
    led = 1;
    led2 =1;
    led3 =1;

    while (true) {
        led3 = 0;
        if (!button2) {
            led3 = 1;
            led = 0;
            //pc.printf("MotorOn\r\n");
            MotorOn();
        }
        if (!button3) {
            //pc.printf("MotorOff\r\n");
            PotRead.detach();
            MotorOff();
        }
        led = 0;
        if(counter==10) {
            float tmp = t.read();
            printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Kp,Ki,Kd);
            counter = 0;
        }
        counter++;
        wait(0.001);
    }
}