File content as of revision 18:ef413d2fd0b1:

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

float CW=1;
float CCW=0;
float direction1,direction2;
float speed1,speed2;
float signaal1,signaal2;
float error1_int,error2_int;
float error1_prev,error2_prev;

float maxvaluepwm=0;
float minimal_error;
float Ts;
const float pi=3.14159265359;
const float degtorad=(pi/180);


DigitalOut motor1_dir(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
PwmOut motor1_speed(D6);//aanstuursnelheid motor 1
PwmOut motor2_speed(D5);
DigitalOut motor2_dir(D4);



controlandadjust::controlandadjust(float errorband, float controlfrequency)
{
    minimal_error=errorband*degtorad/2;
    Ts=1.0/controlfrequency;
}

void controlandadjust::verwerksignaal(float signaal1,float signaal2)
{
    //motor1
    if (signaal1>=0) {//determine CW or CCW rotation
        direction1=CCW;
    } else {
        direction1=CW;
    }

    if (fabs(signaal1)>=maxvaluepwm) { //check if signal is <1
        speed1=maxvaluepwm;//if signal >1 make it 1 to not damage motor
    } else {
        speed1=fabs(signaal1);// if signal<1 use signal
    }

    //motor2
    if (signaal2>=0) {//determine CW or CCW rotation
        direction2=CW;
    } else {
        direction2=CCW;
    }

    if (fabs(signaal2)>=maxvaluepwm) { //check if signal is <1
        speed2=maxvaluepwm;//if signal >1 make it 1 to not damage motor
    } else {
        speed2=fabs(signaal2);// if signal<1 use signal
    }
    //write to motor
    motor1_dir.write(direction1);
    motor1_speed.write(speed1);
    motor2_dir.write(direction2);
    motor2_speed.write(speed2);

}
float controlandadjust::motor1pwm()
{
    return speed1;
}

float controlandadjust::motor2pwm()
{
    return speed2;
}


void controlandadjust::P(float error1,float error2,float Kp)
{
    signaal1=error1*Kp;
    signaal2=error2*Kp;

    //check if error is big enough to produce signal, else signal is 0 to save motors
    if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(signaal1,signaal2);
    } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(0,0);
    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(signaal1,0);
    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(0,signaal2);
    }
}

void controlandadjust::PI(float error1, float error2, float Kp, float Ki)
{
    error1_int = error1_int + Ts * error1;
    signaal1=Kp*error1+Ki*error1_int;

    error2_int = error2_int + Ts * error2;
    signaal2=Kp*error2+Ki*error2_int;

    //check if error is big enough to produce signal, else signal is 0 to save motors
    //and make error_int 0 if the error is small enough to protect from "spooling up"
    if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(signaal1,signaal2);
    } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(0,0);
        error1_int=0,error2_int=0;
    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(signaal1,0);
        error2_int=0;
    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(0,signaal2);
        error1_int=0;
    }
}

void controlandadjust::PID(float error1, float error2, float Kp, float Ki,float Kd)
{
    // Derivative
    double error1_der = (error1 - error1_prev)/Ts;
    error1_prev = error1;

    double error2_der = (error2 - error2_prev)/Ts;
    error2_prev = error2;

// Integral
    error1_int = error1_int + Ts * error1;
    error2_int = error2_int + Ts * error2;
// PID
    signaal1= Kp * error1 + Ki * error1_int + Kd * error1_der;
    signaal2= Kp * error2 + Ki * error2_int + Kd * error2_der;

    //check if error is big enough to produce signal, else signal is 0 to save motors
    //and make error_int 0 if the error is small enough to protect from "spooling up"
    if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(signaal1,signaal2);
    } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(0,0);
        error1_int=0,error2_int=0;
    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(signaal1,0);
        error2_int=0;
    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(0,signaal2);
        error1_int=0;
    }
}
//////////////////////////////////////PID WITH LOW PASS FILTER
void controlandadjust::PIDLowPass(float error1, float error2, float Kp, float Ki,float Kd, float tau_p)
{
    // Derivative
    double error1_der = (error1 - error1_prev)/Ts; //(e(k)-e(k-1))/Ts
    error1_prev = error1;

    double error2_der = (error2 - error2_prev)/Ts;
    error2_prev = error2;

// Integral
    error1_int = error1_int + Ts * error1;
    error2_int = error2_int + Ts * error2;
// PID
     signaal1= (signaal1*(tau_p/Ts) + Kp * error1 + Ki * error1_int + Kd * error1_der)*( 1/( 1+ (tau_p/Ts) ) );
     signaal2= (signaal2*(tau_p/Ts) + Kp * error2 + Ki * error2_int + Kd * error2_der)*( 1/( 1+ (tau_p/Ts) ) );

    //check if error is big enough to produce signal, else signal is 0 to save motors
    //and make error_int 0 if the error is small enough to protect from "spooling up"
    if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(signaal1,signaal2);
    } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(0,0);
        error1_int=0,error2_int=0;
    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
        verwerksignaal(signaal1,0);
        error2_int=0;
    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
        verwerksignaal(0,signaal2);
        error1_int=0;
    }
}

void controlandadjust::STOP()
{
    motor1_speed.write(0);
    motor2_speed.write(0);
}
void controlandadjust::cutoff(float maxvalue)
{
    maxvaluepwm=maxvalue;
}