File content as of revision 16:75884d07334e:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "FastPWM.h"
// Algemeen
DigitalIn button2(SW2);
DigitalIn button3(SW3);
DigitalIn But2(D12);
DigitalIn But1(D13);
DigitalOut led(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
MODSERIAL pc(USBTX, USBRX);

//Motoren
DigitalOut direction1(D4);
FastPWM pwmpin1(D5);
FastPWM pwmpin2(D6);
DigitalOut direction2(D7);
volatile float pwm1;
volatile float pwm2;

//Encoder
QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
double Pulses1;
double motor_position1;
double Pulses2;
double motor_position2;
double error1;

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

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


//Kinematica
double stap1;
double stap2;
double KPot;
float KPotabs;

float ElbowReference;
float Ellebooghoek1;
float Ellebooghoek2;
float Ellebooghoek3;
float Ellebooghoek4;

float PolsReference;
float Polshoek1;
float Polshoek2;
float Polshoek3;
float Polshoek4;

float Hoeknieuw1;
float Hoeknieuw2;

//Limiet in graden
float lowerlim1 = -900;
float upperlim1 = 900;
float lowerlim2 = 0;
float upperlim2 = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor

// VARIABLES PID CONTROLLER
double Kp1 = 0;          
double Ki1 = 0;          
double Kd1 = 1;
double Kp2 = 6;          // Zonder arm: 6,0,1
double Ki2 = 0;          
double Kd2 = 1;          
double Ts = 0.00006;      // Sample time in seconds

float countpwm = 0;



float Kinematics1(float KPot)
{

    if (KPot > 0.45f) {
        stap1 = KPot*450*0.000001;
        Hoeknieuw1 = PolsReference + stap1;
        return Hoeknieuw1;
    }

    else if (KPot < -0.45f) {
        stap1 = KPot*450*0.000001;
        Hoeknieuw1 = PolsReference + stap1;
        return Hoeknieuw1;
    }

    else {
        return PolsReference;
    }
}
float Kinematics2(float KPot)
{

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

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

    else {
        return ElbowReference;
    }
}

float Limits1(float Polshoek2)
{

    if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) {       //Binnen de limieten
        Polshoek3 = Polshoek2;
    }

    else {
        if (Polshoek2 >= upperlim1) {                            //Boven de limiet
            Polshoek3 = upperlim1;
        } else {                                                    //Onder de limiet
            Polshoek3 = lowerlim1;
        }
    }

    return Polshoek3;
}


float Limits2(float Ellebooghoek2)
{

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

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

    return Ellebooghoek3;
}




// ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
double PID_controller1(double error1)
{
    static double error1_integral = 0;
    static double error1_prev = error1; // 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
    Kp1 = 30 * Pot2;
    Ki1 = 10 * Pot1;
    
    if (!But2) {
        Kd1 = Kd1 + 0.001;
    }
    if (!But1){
        Kd1 = Kd1 - 0.001;
    }
    
    
    // Proportional part:
    double u_k1 = Kp1 * error1;

    // Integral part
    error1_integral = error1_integral + error1 * Ts;
    double u_i1 = Ki1* error1_integral;

    // Derivative part
    double error1_derivative = (error1 - error1_prev)/Ts;
    double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
    double u_d1 = Kd1 * filtered_error1_derivative;
    error1_prev = error1;

    // Sum all parts and return it
    return u_k1 + u_i1 + u_d1;
}
double PID_controller2(double error2)
{
    static double error2_integral = 0;
    static double error2_prev = error2; // 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
    Kp2 = 10 * Pot2;
    Ki2 = 10 * Pot1;
    
    if (!But2) {
        Kd2 = Kd2 + 0.01;
    }
    if (!But1){
        Kd2 = Kd2 - 0.01;
    }
    */
    
    // Proportional part:
    double u_k2 = Kp2 * error2;

    // Integral part
    error2_integral = error2_integral + error2 * Ts;
    double u_i2 = Ki2 * error2_integral;

    // Derivative part
    double error2_derivative = (error2 - error2_prev)/Ts;
    double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
    double u_d2 = Kd2 * filtered_error2_derivative;
    error2_prev = error2;

    // Sum all parts and return it
    return u_k2 + u_i2 + u_d2;
}

void moter1_control(double u1)
{
    direction1= u1 > 0.0f; //positief = CW
    if (fabs(u1)> 0.5f) {
        u1 = 0.5f;
    } else {
        u1= u1;
    }
    pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

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

void PwmMotor(void)
{
    //Reference hoek berekenen, in graden
    float Ellebooghoek1 = Kinematics2(pwm2);
    float Ellebooghoek4 = Limits2(Ellebooghoek1);
    //ElbowReference = Ellebooghoek4;
    
    //float Polshoek1 = Kinematics1(pwm2);
    //float Polshoek4 = Limits1(Polshoek1);
    //PolsReference = Polshoek4;

    // Positie motor berekenen, in graden
    Pulses1 = encoder1.getPulses();
    motor_position1 = -(Pulses1/1200)*360;
    Pulses2 = encoder2.getPulses();
    motor_position2 = -(Pulses2/4800)*360;

    double error1 = PolsReference - motor_position1;
    double u1 = PID_controller1(error1);
    moter1_control(u1);
    
    double error2 = ElbowReference - motor_position2;
    double u2 = PID_controller2(error2);
    moter2_control(u2);

}

void Servod(void)
{
     if (countpwm == 0 ){
      //Reference hoek berekenen, in graden
    float Ellebooghoek1 = Kinematics2(pwm2);
    float Ellebooghoek4 = Limits2(Ellebooghoek1);
    //ElbowReference = Ellebooghoek4;
    
    //float Polshoek1 = Kinematics1(pwm2);
    //float Polshoek4 = Limits1(Polshoek1);
    //PolsReference = Polshoek4;

    // Positie motor berekenen, in graden
    Pulses1 = encoder1.getPulses();
    motor_position1 = -(Pulses1/1200)*360;
    Pulses2 = encoder2.getPulses();
    motor_position2 = -(Pulses2/4800)*360;

    double error1 = PolsReference - motor_position1;
    double u1 = PID_controller1(error1);
    moter1_control(u1);
    
    double error2 = ElbowReference - motor_position2;
    double u2 = PID_controller2(error2);
    moter2_control(u2);
      }   
     else if (countpwm == 9){
      countpwm = 0;
     } 
     led3 = !led3;
}
     
void MotorOn(void)
{
    pwmpin1 = 0;
    pwmpin2 = 0;
    //Pwm.attach (PwmMotor, Ts);
    Pwm.attach (Servod, Ts);

}

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

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


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


int main()
{
    Timer t;
    t.start();
    int counter = 0;
    pwmpin2.period_us(60);
    PotRead.attach(ContinuousReader,0.0005);
    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,%f\n\r",tmp,motor_position1,PolsReference,error1,Kp1,Ki1,Kd1);
            counter = 0;
        }
        counter++;
        wait(0.001);
    }
}