File content as of revision 36:077fe5b3189b:

// Inclusion of libraries
#include    "mbed.h"
#include    "FastPWM.h"    
#include    "QEI.h"        // Includes library for encoder
#include    "MODSERIAL.h"
#include    "HIDScope.h"
#include    "BiQuad.h"

// Input
AnalogIn    pot1(A1);
AnalogIn    pot2(A2);
InterruptIn button1(D0);
InterruptIn button2(D1);
InterruptIn emergencybutton(SW2);   //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible

DigitalIn   pin8(D8);       // Encoder 1 B
DigitalIn   pin9(D9);       // Encoder 1 A
DigitalIn   pin10(D10);     // Encoder 2 B
DigitalIn   pin11(D11);     // Encoder 2 A
DigitalIn   pin12(D12);     // Encoder 3 B
DigitalIn   pin13(D13);     // Encoder 3 A

// Output
DigitalOut  pin2(D2);       // Motor 3 direction = motor flip
FastPWM     pin3(D3);       // Motor 3 pwm = motor flip
DigitalOut  pin4(D4);       // Motor 2 direction = motor right
FastPWM     pin5(D5);       // Motor 2 pwm = motor right
FastPWM     pin6(D6);       // Motor 1 pwm = motor left
DigitalOut  pin7(D7);       // Motor 1 direction = motor left

DigitalOut  greenled(LED_GREEN,1);
DigitalOut  redled(LED_RED,1);
DigitalOut  blueled(LED_BLUE,1);

// Utilisation of libraries
MODSERIAL   pc(USBTX, USBRX);
QEI         Encoderl(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
QEI         Encoderr(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
QEI         Encoderf(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
Ticker      motor;
Ticker      encoders;

// Global variables
const float     pi = 3.14159265358979f;
float           u3 = 0.0f;         // Normalised variable for the movement of motor 3
const float     fCountsRad = 4200.0f;
const float     dt = 0.001f;

// Functions
void Emergency()        // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
{   greenled = 1;       // Red light on
    blueled = 1;
    redled = 0;
    pin3 = 0;           // All motor forces to zero
    pin5 = 0;
    pin6 = 0;
    exit (0);           // Abort mission!!
}

    // Subfunctions
    int Countslinput()      // Gets the counts from encoder 1
    {   int     countsl;
        countsl = Encoderl.getPulses();
        return  countsl;
    }
    int Countsrinput()      // Gets the counts from encoder 2
    {   int     countsr;
        countsr = Encoderr.getPulses();
        return  countsr;
    }
    int Countsfinput()      // Gets the counts from encoder 3
    {   int     countsf;
        countsf = Encoderf.getPulses();
        return  countsf;
    }

    float   CurrentAngle(float counts)      // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
    {   float   angle = ((float)counts*2.0f*pi)/fCountsRad;
        while   (angle > 2.0f*pi)
        {   angle = angle-2.0f*pi;
        }
        while   (angle < -2.0f*pi)
        {   angle = angle+2.0f*pi;
        }
        return angle;
    }

    float  ErrorCalc(float refvalue,float CurAngle)     // Calculates the error of the system, based on the current angle and the reference value
    {   float   error = refvalue - CurAngle;
        return  error;
    }
        
        float Kpcontr()     // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
        {   float   Kp = 20.0f*pot2;
            return  Kp;
        }
        
        float Kdcontr()     // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
        {   float   Kd = 0.25f*pot1;
            return  Kd;
        }               
               
    float PIDcontroller(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
    {   //float   Kp = Kpcontr();
        float   Kp = 10.37f; 
        float   Ki = 1.02f;
        float   Kd = 0.0514f;
        //float   Kd = Kdcontr();
        float   error = ErrorCalc(refvalue,CurAngle);
        static float    error_integral = 0.0;
        static float    error_prev = error; // initialization with this value only done once!
        static  BiQuad  PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
        // Proportional part:
        float   u_k = Kp * error;
        // Integral part
        error_integral = error_integral + error * dt;
        float   u_i = Ki * error_integral;
        // Derivative part
        float   error_derivative = (error - error_prev)/dt;
        float   filtered_error_derivative = PIDfilter.step(error_derivative);
        float   u_d = Kd * filtered_error_derivative;
        error_prev = error;
        // Sum all parts and return it
        pc.printf ("Kp = %f     Kd = %f",Kp,Kd);
        return  u_k + u_i + u_d;
    }

        float DesiredAnglel()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
        {   float   angle = (pot1*2.0f*pi)-pi;
            return  angle;
        }
        
float*    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
{   
    //float   refvalue = pi/4.0f;
    float   refvalue = DesiredAnglel();                             // different minus sign per motor
    int     counts = Countslinput();                               // different encoder pins per motor
    float   currentangle = CurrentAngle(counts);                  // different minus sign per motor
    float   PIDcontrol = PIDcontroller(refvalue,currentangle);   // same for every motor
    float   error = ErrorCalc(refvalue,currentangle);            // same for every motor
    
    pin6 = fabs(PIDcontrol);                                       // different pins for every motor
    pin7 = PIDcontrol > 0.0f;                                      // different pins for every motor
    //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
    //pin6 = fabs(PIDcontr)/pi;
    //pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue,currentangle);
    float*  outcome;
    float   turninfo[3] = {error, refvalue, currentangle};
    //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
    outcome = turninfo;
    
    return outcome;
}

float ActualPosition(int counts, int offsetcounts)      // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
{   float MotorPosition = - (counts - offsetcounts) / fCountsRad;
    // minus sign to correct for direction convention
    return MotorPosition;
}

// Main program
int main()
{   
    pc.baud(115200);  
    pin3.period_us(15);     // If you give a period on one pin, c++ gives all pins this period
        
    //float motoroutcome1 = motor.attach(turn1,dt);
    
    emergencybutton.rise(Emergency);              //If the button is pressed, stop program
    //pin6 = 0.01; 
            
    while   (true)
    {   
        float* motoroutcomel = turnl();
        float motorl1 = motoroutcomel[0];
        float motorl2 = motoroutcomel[1];
        float motorl3 = motoroutcomel[2];
        
        pc.printf("     error1:  %f      ref1: %f     angle1:  %f \r\n",motorl1,motorl2,motorl3);
        wait(dt);
        //wait(dt*10);
        //wait(printingfreq);  --> still needs to be defined   
    }
}