File content as of revision 40:cac08f589732:

#include "mbed.h"
#include "QEI.h"
#include "math.h"


// Motor 1 & 2
DigitalOut      Direction(D4); 
PwmOut          PowerMotor(D5); 
QEI             Encoder(D10,D11,NC,32,QEI::X2_ENCODING); 
DigitalOut      Direction2(D7);
PwmOut          PowerMotor2(D6);
QEI             Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
PwmOut          PowerServo(D3);

// Buttons & EMG
AnalogIn        Button(A3); // Calibration button  
AnalogIn        Button2(A4); // Fire button
DigitalIn       Button3(D9); // Home button
AnalogIn        EMG(A0);
AnalogIn        EMG2(A1);

// Tickers & timers

Ticker          biquadTicker;
Ticker          MotorWrite;
Timer           timer;
Timer           TijdEMGCal;

// LED
DigitalOut      ledF(LED_RED);
DigitalOut      ledC(LED_GREEN);



// Values
const double    twopi = 6.2831853071795;
const double    Fs=100; // Sample Frequency
int             Fired = 0;

// Motor 1 (Translation)
const double    n1 = 3.3; // The amount of the rotations that is allowed
int             Pulses;
double          Rotatie = 0; //Rotation in radial
double          Goal; // Setpoint for Motor 
double          Error = 0; // Setting initial values
double          Errord = 0;
double          Errori = 0;
double          Errorp = 0;
const double    Kp = 0.2; // PID controll parameters
const double    Kd = 10;
const double    Ki = 0.2;
double          v = 0;
double          upperlimit; //Maximum rounds up
const double    downlimit = 0.8; //Lowest limit of the robot
const double    margin = 0.8; //Margin for safe range
bool            OutRange = false; //In Range or not

// Motor 2 (Rotation)
const double    n2 = 0.334;
int             Pulses2;
double          Rotatie2 = 0;
double          Goal2;
double          Error2 = 0;
double          Errord2 = 0;
double          Errori2 = 0;
double          Errorp2 = 0;
const double    Kp2 = 0.2;
const double    Kd2 = 10;
const double    Ki2 = 0.2;
double          v2 = 0;
double          turnlimit = 0.4;
// Margin 2 = 0
bool            OutRange2 = false;

// Switch between firing mode (Excecute) and return to Home mode
bool            Excecute = false;
bool            Home = false;

// Filter
double          Fs2 = 500; // Frequency in Hz
const double    TijdCal = 5; // Time for calibration
double          Max = 0; // Max value for EMG
double          Max2 = 0; // Max value for EMG2
bool            Cali = false; // Switch for Calibration
double          T1 = 0; // Thresholds for EMG
double          T2 = 0;
double          T3 = 0;
double          T4 = 0;

double u1; //All variables for filtering
double y1;
double y2;
double y3;
double y4;
double y5;
double y6;
double y7;
double y8;
double y9;
double y10;

double u10;
double y11;
double y12;
double y13;
double y14;
double y15;
double y16;
double y17;
double y18;

double f1_v1=0,f1_v2=0;
double f2_v1=0,f2_v2=0;
double f3_v1=0,f3_v2=0;
double f4_v1=0,f4_v2=0;
double f5_v1=0,f5_v2=0;
double f6_v1=0,f6_v2=0;
double f7_v1=0,f7_v2=0;

double f1_v3=0,f1_v4=0;
double f2_v3=0,f2_v4=0;
double f3_v3=0,f3_v4=0;
double f4_v3=0,f4_v4=0;
double f5_v3=0,f5_v4=0;
double f6_v3=0,f6_v4=0;
double f7_v3=0,f7_v4=0;

// Notch
const double gainNotch3=0.969922;
const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000;
const double gainNotch4=0.975183;
const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000;
const double gainNotch5=0.993678;
const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000;

// High pass
const double gainHP1=0.96860379641660377;
const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.000000000;
const double gainHP2=0.935820;
const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000;

// Low pass
const double gainLP6=0.000048;
const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000;
const double gainLP7=0.000048;
const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000;



// Voids for calculations in the main programme

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;
}

void myController()
{
    // EMG 1
    u1 = EMG.read();
    // Notch
    y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
    y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
    y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);

    // HP
    y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
    y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);

    // LP
    y6 = fabs(y5);
    y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
    y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
    
    
    // EMG 2
    u10 = EMG2.read();
    // Notch
    y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
    y12 = biquad(y11,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
    y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);

    // HP
    y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
    y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);

    // LP
    y16 = fabs(y15);
    y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
    y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);

    if (Cali == true) {
        if (y8 >= Max) {
            Max = y8;
        }
        if (y18 >= Max2) {
            Max2 = y18;
        }
    }
}

void MotorSet()
{
    // Calculation for position of motor
    Pulses = Encoder.getPulses();
    Rotatie = (Pulses*twopi)/4200;
    Pulses2 = Encoder2.getPulses();
    Rotatie2 = (Pulses2*twopi)/4200;
    // Motor 1 (translation)
    if (OutRange) { //If the Robot is OutRange or is going Home
        Error = Goal-Rotatie; 
        Errord = (Error-Errorp)/Fs;
        Errorp = Error;
        if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small
            Errori = Errori + Error*1/Fs;
        } else {
            Errori = 0;
        }
        if (Error>=0) { // Determine the direction
            Direction=1;
        } else {
            Direction=0;
        }
        v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed
         if (Home == true) { //Maximum speed when OutRange
            if (v >0.15) {
                v = 0.15;
            }
        }
    }
    PowerMotor.write(fabs(v));

    // Dan motor 2 (rotatie)
    if (OutRange2) {
        Error2 = Goal2-Rotatie2; 
        Errord2 = (Error2-Errorp2)/Fs;
        Errorp2 = Error2;
        if (fabs(Error2) <= 0.5) {
            Errori2 = Errori2 + Error2*1/Fs;
        } else {
            Errori2 = 0;
        }
        if (Error2>=0) {
            Direction2 = 0;
        } else {
            Direction2 = 1;
        }
        v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
    }
    PowerMotor2.write(fabs(v2));
}

void Calibration ()
{
    if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range
        if (Button >= 0.8) {
            wait (0.2);
            Cali = true; // Calibration part in the EMG void is being activated
            TijdEMGCal.start();
            Excecute = false;
            ledC = 0;
            v = 0;
            v2 = 0;
        }
    }
    if (TijdEMGCal.read() >= TijdCal) {
        Cali = false; // Calibration part in the EMG void is stopped
        ledC = 1;
        TijdEMGCal.stop();
        TijdEMGCal.reset();
        T1 = 0.35*Max; // Determination of thresholds
        T2 = 0.5*Max;
        T3 = 0.35*Max2;
        T4 = 0.5*Max2;
        wait (3);
        Excecute = true;
    }
}

int main()
{
    upperlimit = n1*twopi; // Calculation for limits of the robot
    turnlimit = n2*twopi;
    ledF = 1;
    ledC = 1;
    PowerMotor.write(0);
    PowerMotor2.write(0);
    MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers
    biquadTicker.attach(myController,1/Fs2);
    PowerServo.period_ms(20);
    while (true) {
        Calibration(); // Makes it able to calibrate during firing mode
        while (Excecute) {
            // Motor 1
            if (Rotatie >= upperlimit) { // If OutRange
                Goal = upperlimit - margin-0.2;
                OutRange = true;
            }
            if (Rotatie <= downlimit) { // If OutRange
                Goal = 0 + margin +0.2;
                OutRange = true;
            }
            if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange
                OutRange = false;
            }
            if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange
                if (y8 >= T2 ) {
                    Direction = 1;
                    v = 0.13;
                }
                if (y8 > T1 && y8 < T2) {
                    Direction = 0;
                    v = 0.07;
                }
                if (y8 <= T1) {
                    Direction = 0;
                    v = 0;
                }
            }

            // Motor
            if (Rotatie2 >= turnlimit) { // If OutRange
                Goal2 = turnlimit-0.1;
                OutRange2 = true;
            }
            if (Rotatie2 <= -turnlimit) { // If OutRange
                Goal2 = -turnlimit+0.1;
                OutRange2 = true;
            }
            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange
                OutRange2 = false;
            }
            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange
                if (y18 >= T4 ) {
                    Direction2 = 1;
                    v2 = 0.06;
                }
                if (y18 > T3 && y18 < T4) {
                    Direction2 = 0;
                    v2 = 0.06;
                }
                if (y18 <= T3) {
                    Direction2 = 0;
                    v2 = 0;
                }
            }
            if (Button2 >= 0.8) { // Firing Rubber Band Gun
                wait(0.2);
                ledF = 0;
                PowerServo.write(0.27);
                wait (1);
                PowerServo.write(0.04);
                wait (1);
                PowerServo.write(0.27);
                wait (1);
                PowerServo.write(0.04);
                ledF = 1;
                Fired=Fired+1;
                if (Fired == 3) {
                    wait (1);
                    Home = true;
                    Excecute = false;
                }
            }
            if (Button3 == 0) {
                Excecute = false;
                Home = true;
            }
            Calibration();
        }

        while (Home) { // Return to base position
            OutRange = true; // Activation of PID control part
            OutRange2 = true;
            Goal = margin;
            Goal2 = 0;
            if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again
                timer.start();
            } else {
                timer.stop();
                timer.reset();
            }
            if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset.
                Errori = 0;
                Errord = 0;
                Errorp = 0;
                Errori2 = 0;
                Errord2 = 0;
                Errorp2 = 0;
                Fired = 0;
                wait (5);
                Excecute = true;
                Home = false;
            }
        }
    }
}