File content as of revision 4:4d4d7343406d:

///////////////////////////////////////////////#INCLUDES//////////////////////////////////////////
#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"

//////////////////////////////////////////////CONSTANTS AND SHIZZLE////////////////////////////////////////////////////////////////////////////
//info uit
HIDScope scope(3);
//encoders
QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +

//ingangen
AnalogIn pot1(A2);

//uitgangen
DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
//PwmOut motor2_speed_control(D5);
//DigitalOut motor2_direction(D4);
const int CW=1; //clockwise
const int CCW=0; //counterclockwise

//frequencies
//const float pwm_frequency=1000;
const double hidscope_frequency=100;
const double pid_control_frequency=5;

//tickers
Ticker hidscope_ticker;
Ticker pid_control_ticker;

//constants
const int cpr=32*131;
const float PI=3.1415;
const float counttorad=((2*PI)/cpr);

//////////////////////////////////////////////////////////FILTERS///////////////////////////////////////////////////////

//IK WEET NIET HOE GOED DEZE WAARDES KLOPPEN, VOORZICHTIG DUS!! WAARDES ZIJN TE VERKRIJGEN UIT ASN
//Biquad #1
const double gain_f1 = 0.004756;
const double B_f1[] = { 1.00000000000, -0.34966580719,  1.00000000000};
const double A_f1[] = { 1.00000000000, -1.86848759822,  0.87558390333};

//Biquad #2
const double gain_f2 = 0.036454;
const double B_f2[] = { 1.00000000000, -1.56455717372,  1.00000000000};
const double A_f2[] = { 1.00000000000, -1.93013247482,  0.94815823632};


//filterwaardes uit asn naar aparte waardes omschrijven
double v1 = 0, v2 = 0, u = 0, y = 0;
const double a1_f1 = gain_f1*A_f1[1], a2_f1 = gain_f1*A_f1[2], b0_f1 = gain_f1*B_f1[0], b1_f1 = gain_f1*B_f1[1], b2_f1 = gain_f1*B_f1[2]; //filter coefficients filter 1
const double a1_f2 = gain_f2*A_f1[1], a2_f2 = gain_f2*A_f2[2], b0_f2 = gain_f2*B_f2[0], b1_f2 = gain_f2*B_f2[1], b2_f2 = gain_f2*B_f2[2]; //filter coefficients filter 1


//biquad filter
double biquad( double u, double &v1, double &v2, const double a1, const double a2,
               const double b0, const double b1, const double b2 )
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}
///////////////////////////////////////////////////////////CONTROLLERS//////////////////////////////////////////////////////////
//DIT ZIJN ZOMAAR WAARDEN, AANPASSEN DUS WAARDES BEREKENEN MET MATLAB
const float motor1_pid_kp=0.5;
const float motor1_pid_ki=0.01;
const float motor1_pid_kd=0.01;

double motor1_error_int=0;
double motor1_error_prev=0;

// Reusable PID controller with filter
double pid_control( double e, const double Kp, const double Ki, const double Kd, double Ts,//Ts is dus sampletijd
                    double &motor1_error_int, double &motor1_error_prev)
{
// Derivative
    double e_der = (e - motor1_error_prev)/Ts;
    e_der = biquad( e_der, v1, v2, a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);//twee keer hetzelfde met andere filterwaardes, dit klopt
    e_der = biquad( e_der, v1, v2, a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);//is omdat een biquad filter maar 6 polen en nulpunten heeft
    motor1_error_prev = e;
// Integral
    motor1_error_int = motor1_error_int + Ts * e;
// PID
    return Kp * e + Ki * motor1_error_int + Kd * e_der;
}


///////////////////////////////////////////////////////////GO-FLAGS AND ACTIVATION FUNCTIONS///////////////////////////////
//go flags
volatile bool scopedata_go=false, pid_control_go=false;

//acvitator functions

void scopedata_activate()
{
    scopedata_go=true;
}
void pid_control_activate()
{
    pid_control_go=true;
}
///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////

//scopedata
void scopedata()
{
    scope.set(0,2*PI*pot1.read());
    scope.set(1,counttorad*encoder1.getPulses());
    scope.set(2,motor1_speed_control.read());
    scope.send();
}

//////////////////////////////////////////////////MAIN////////////////////////////////////////////////////////////////////
int main()
{
    //set initial shizzle
    motor1_speed_control.write(0);

    //tickers
    hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
    pid_control_ticker.attach(&pid_control_activate,1.0/pid_control_frequency);

    //make variables
    volatile float signal_motor1=0;
    while(1) {

        //control motor 1 with a pID controller
        if (pid_control_go==true) {
            double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
            double signal_motor1=pid_control(error,motor1_pid_kp,motor1_pid_ki,motor1_pid_kd,
                                             1.0/pid_control_frequency,motor1_error_int,motor1_error_prev);//send error to pid controller 1.0/pid_control_frequency=sampletijd
            if (signal_motor1>=0) {//determine CW or CCW rotation
                motor1_direction.write(CW);
            } else {
                motor1_direction.write(CCW);
            }

            if (fabs(signal_motor1)>=1) { //check if signal is <1
                signal_motor1=1;//if signal >1 make it 1 to not damage motor
            } else {
                signal_motor1=fabs(signal_motor1);// if signal<1 use signal
            }
            motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
            pid_control_go=false;
        }
        //call scopedata
        if (scopedata_go==true) {
            scopedata();
            scopedata_go=false;
        }

    }
    return 0;
}