File content as of revision 1:2f23368e8638:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
// #include "HIDScope.h"

Serial pc(USBTX,USBRX);
// HIDScope scope(4);          // definieerd het aantal kanalen van de scope

// Define Tickers and stuff
Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
double controlfreq = 10 ;    // controlloops frequentie (Hz)
double controlstep = 1/controlfreq; // timestep derived from controlfreq

//MOTOR INPUTPINS
// motor 1
PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
DigitalOut motor1_rich(D7); // digitaal signaal voor richting
// motor 2
PwmOut motor2_aan(D5);
DigitalOut motor2_rich(D4);

// ENCODER
Encoder motor1_enc(D12,D11);        // encoder outputpins
Encoder motor2_enc(D10,D9);

//POTMETERS
AnalogIn potright(A0);      // define the potmeter outputpins
AnalogIn potleft(A1);

// RESETBUTTON              
DigitalIn   button(PTA4);      // defines the button used for a encoder reset
int button_pressed = 0;

int reference1 = 0;         // set the reference position of the encoders
int reference2 = 0;

// CONTROLLER SETTINGS
    // motor 1
    const double m1_Kp = 1;         // Proportional constant     
    const double m1_Ki = 0;         // integration constant
    const double m1_Kd = 0;         // differentiation constant
    // motor 2
    const double m2_Kp = 1;
    const double m2_Ki = 0;
    const double m2_Kd = 0;
// storage variables
    // motor 1
    double m1_err_int = 0; 
    double m1_prev_err = 0;
    // motor 2
    double m2_err_int = 0;
    double m2_prev_err = 0;


// SETPOINT SETTINGS
// define storage variables for setpoint values
    double c_setpoint1 = 0;
    double c_setpoint2 = 0;
// define the maximum rate of change for the setpoint (velocity)
    double Vmax = 5;            // rad/sec


//// FILTER VARIABLES
// storage variables
        // differential action filter, same is used for both controllers
        double m_f_v1 = 0;
        double m_f_v2 = 0;

// Filter coefficients
        // differential action filter
        const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0;      // coefficients from sheets are used as first test.



////////////////////////////////////////////////////////////////
/////////////////// START OF SIDE FUNCTIONS ////////////////////
//////////////////////////////////////////////////////////////
// these functions are tailored to perform 1 specific function


// counts 2 radians
// this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
// It has been set up for standard 2X DECODING!!!!!!
double get_radians(double counts)       
{
    double pi = 3.14159265359;
    double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
    return radians;
}


// This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_setpoint)
// to create a reference that moves with a variable speed. It is now set up for potmeter values.  
double setpoint_f(double input, double &c_setpoint)
{
    double offset = 0.5, gain = 2;                
    double potset = (input-offset)*gain;
    double setpoint = c_setpoint + potset * controlstep * Vmax ;
        c_setpoint = setpoint;
    return setpoint;
}


// This function takes the inputvalue and ensures it is between -1 and 1
// this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
// I am aware it has unnecesary steps but it works.
double outputlimiter (double output)
{
    if(output>1)                                
    {
        output = 1;
    }
    else if(output < 1 && output > 0)
    {
        output = output;
    }
    else if(output > -1 && output < 0)
    {
        output = output;
    }
    else if(output < -1)
    {
        (output = -1);
    }
    return output;
}


// BIQUADFILTER CODE GIVEN IN SHEETS 
// used for d-control
double biquadfilter(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;
    }


// PID Controller given in sheets
// aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
double PID( double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
{
// Proportional
double P = Kp * e;

// Derivative
double e_derr = (e - e_prev)/Ts;
e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
e_prev = e;
double D = Kd* e_derr;

// Integral
e_int = e_int + Ts * e;
double I = e_int * Ki;

// PID
double output = P + I + D;
return output;
}



// this function is a simple K control called by the motor function
double K_control(double error,double K)
{
    double output = K*error;                            // controller output K*e
    output = outputlimiter(output);             // limit the output to -1->1
    return output;
}

/////////////////////////////////////////////////////////////////////
////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
///////////////////////////////////////////////////////////////////
// these functions are used to control all aspects of a single electric motor and are called by the main function

// MOTOR 1
void motor1_control()
{
    double setpoint1 = setpoint_f(potright.read(), c_setpoint1);      // determine the setpoint that has been set by the inputsignal
    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
    double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
    // scope.set(0,setpoint1);                                                                                                              // disabled because HIDScope is acting strangely
    // scope.set(1,rads1);
    // scope.send();
    
    // double output1 = K_control(error1, m1_Kp);        // bereken de controller output voor motor 1(zie hierboven)
    double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
    if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
        motor1_rich.write(0);
        motor1_aan.write(output1);
    } else if(output1 < 0) {
        motor1_rich.write(1);
        motor1_aan.write(abs(output1));
    }
}

// MOTOR 2
void motor2_control()
{
    double setpoint2 = setpoint_f(potleft.read(), c_setpoint2);      // determine the setpoint that has been set by the inputsignal
    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
    double error2 = (setpoint2 - rads2);                       // determine the error (reference - position)
    // scope.set(3,setpoint2);                                                                                                              // disabled because HIDScope is acting strangely
    // scope.set(4,rads2);
    // scope.send();
    
    // double output2 = K_control(error2, m2_Kp);        // bereken de controller output voor motor 1(zie hierboven)
    double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
    if(output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
        motor2_rich.write(0);
        motor2_aan.write(output2);
    } else if(output2 < 0) {
        motor2_rich.write(1);
        motor2_aan.write(abs(output2));
    }
}


int main()
{
    // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop as shown in the sheets.
    controller1.attach(&motor1_control, controlstep);
    controller2.attach(&motor2_control, controlstep);
    while(true) 
    {
        if(button.read() == button_pressed)             // reset the encoder to reference position
        {
            motor1_enc.setPosition(reference1);
            motor2_enc.setPosition(reference2);
        }
    }
}