File content as of revision 14:14f42a87cbfb:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "MovingAverage.h"
#define NSAMPLE 200

//states
enum States {Waiting, Homing, HomingDemo, Emergency, EMGCal, MotorCal, Operation, Demo};
enum OPStates {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};

// Global variables
States CurrentState;
OPStates CurrentOperationState;
Ticker TickerLoop;
Timer TimerLoop;
Timer TimerCheck;
int CountTime=0;

//Communication
MODSERIAL pc(USBTX,USBRX);
QEI Encoder1(D10,D11,NC,32);    // Encoder motor q1 (arm)
QEI Encoder2(D12,D13,NC,32);    // Encoder motor q2 (end-effector)

//EMG settings
AnalogIn emg0(A0); //Biceps Left
AnalogIn emg1(A1); //Biceps Right
MovingAverage <float>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
MovingAverage <float>Movag_RB(NSAMPLE,0.0);

// filters
//Make Biquad filters Left(a0, a1, a2, b1, b2)
BiQuadChain bqc1; //chain voor High Pass en Notch
BiQuad bq1(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //High Pass Filter
BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
BiQuad bq5(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter
//Make Biquad filters Right
BiQuadChain bqc2; //chain voor High Pass en Notch
BiQuad bq3(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //High Pass Filter
BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
BiQuad bq6(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter


//Global pin variables Motors 1
PwmOut PwmPin1(D5);
DigitalOut DirectionPin1(D4);

//Global pin variables Motors 2
PwmOut PwmPin2(D6);
DigitalOut DirectionPin2(D7);

//Output LED
DigitalOut LedRed (LED_RED);
DigitalOut LedGreen (LED_GREEN);
DigitalOut LedBlue (LED_BLUE);

// Buttons
DigitalIn BUTSW2(SW2);
DigitalIn BUTSW3(SW3);
DigitalIn BUT1(D8);
DigitalIn BUT2(D9);

//Constant variables
const float L0 = 0.1; // Distance between origin frame and joint 1 [m[
const float L1 = 0.326; // Length link 1 (shaft to shaft) [m]
const float L2 = 0.185; // Length link 2 (end-effector length) [m]
const float Ts = 0.002; //Sampling time 500 Hz

// Homing boolean
bool MoveHome = false;
bool Switch2Demo = false;
bool Switch2OP = false;

// variables
//Motor calibration
bool NextMotorFlag = false;

//EMG
bool EMGBoolLeft; // Boolean EMG 1 (left)
bool EMGBoolRight; // Boolean EMG 2 (right)
float LeftBicepsOut; // Final value left of processed EMG
float RightBicepsOut; // Final value right of processed EMG
float ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1)
float ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1)
float MaxLeft = 0.00001;
float MaxRight = 0.00001;
float NormLeft;
float NormRight;

// Reference positions
float q1Ref = 0;
float q2Ref = 0;

//Motors
float q1 = 0; // Current angle of motor 1 (arm)
float q2 = 0; // Current angle of motor 2 (end-effector)
float qArm = 0; //Current angle of arm
float qEndEff = 0; //Current angle of end-effector
float MotorValue1; // Inputvalue to set motor 1
float MotorValue2; // Inputvalue to set motor 2

//Inverse kinematics
float VdesX; // Desired velocity in x direction
float VdesY; // Desired velocity in y direction
float Error1; // Difference in reference angle and current angle motor 1
float Error2; // Difference in reference angle and current angle motor 2
int xDir;
int yDir;
int Direction;
float RatioGears = 134.0/30.0;
float RatioPulleys = 87.4/27.5;
//Print to screen
int PrintFlag = false;
int CounterPrint = 0;


//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

void PrintToScreen()
{
    CounterPrint++;
    if (CounterPrint == 100) {
        PrintFlag = true;
        CounterPrint = 0;
    }
}

// Function to set motors off
void SetMotorsOff()
{
    // Turn motors off
    PwmPin1 = 0;
    PwmPin2 = 0;

}

// Function for processing EMG
void ProcessingEMG()
{
    //Filter Left Biceps
    float LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
    float LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
    Movag_LB.Insert(LB_Rectify); //Moving Average
    LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
    NormLeft = LeftBicepsOut/MaxLeft; // Normalized value

    //Filter Right Biceps
    float RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
    float RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
    Movag_RB.Insert(RB_Rectify); //Moving Average
    RightBicepsOut = Movag_RB.GetAverage();  //Get final value
    NormRight = RightBicepsOut/MaxRight; // Normalized value

    if (NormLeft > ThresholdLeft) {
        EMGBoolLeft = true;
    } else {
        EMGBoolLeft = false;
    }
    if (NormRight > ThresholdRight) {
        EMGBoolRight = true;
    } else {
        EMGBoolRight = false;
    }

}

void MeasureAll()
{
    //Processing and reading EMG
    ProcessingEMG();

    //Measure current motor angles
    float Counts1 = Encoder1.getPulses();
    float Counts2 = Encoder2.getPulses();
    q1 = Counts1/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
    q2 = Counts2/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)


    qArm = -1*q1/RatioGears; //Adjust for opposite direction due to gears
    qEndEff = q2/RatioPulleys;



}

// Function to set desired cartesian velocities of end-effector
void Vdesired()
{
    if(BUTSW3 == false)
    {
    Direction = -1; //negative Direction 
    }
    else
    {
    Direction = 1; //Positive Direction
    }
    xDir = 1-BUT1.read();
    yDir = 1-BUT2.read();
    float Vconst = 0.05; // m/s (3 cm per second)
    VdesX = Direction*EMGBoolLeft*Vconst; // Left biceps is X-direction
    VdesY = Direction*EMGBoolRight*Vconst; // Right biceps is Y-direction
}

// Inverse Kinematics
void InverseKinematics()
{
    // matrix inverse Jacobian
    float InvJac11 = (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
    float InvJac12 = -(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
    float InvJac21 = -(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
    float InvJac22 = (L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));



    float q1DotRef = (InvJac11*VdesX + InvJac12*VdesY); //reference angular velocity motor 1
    float q2DotRef = (InvJac21*VdesX + InvJac22*VdesY); //reference angular velocity motor 2

    q1Ref += q1DotRef*Ts; // set new reference position motor angle 1
    q2Ref += q2DotRef*Ts; // set new reference position motor angle 1
}

//State machine
void StateMachine()
{
    switch (CurrentState) {
        case Waiting:
            SetMotorsOff();
            Encoder1.reset();
            q1Ref = 0;
            q1 = 0;
            Error1 = 0;
            Encoder2.reset();
            q2Ref = 0;
            q2 = 0;
            Error2 = 0;

            LedRed = 0;
            LedGreen = 0;
            LedBlue = 1; //Yellow

            if (BUT2 == false) {
                CurrentState = EMGCal;
                TimerLoop.reset();
            }

            break;

        case Homing:
            LedRed = 0;
            LedGreen = 0;
            LedBlue = 0; //White


            if (MoveHome == true) {
                if (q1Ref >= 0) {
                    q1Ref -= 0.0005*(6.380);
                }
                if (q1Ref <= 0) {
                    q1Ref += 0.0005*(6.380);
                }
                if (q2Ref >= 0) {
                    q2Ref -= 0.0005*(6.380);
                }
                if (q2Ref <= 0) {
                    q2Ref += 0.0005*(6.380);
                }

                if (TimerLoop.read()>=5.0) { //(-0.002*(6.380) <= q1Ref <= 0.002*(6.380)) && (-0.002*(6.380) <= q2Ref <= 0.002*(6.380))
                    MoveHome = false;
                    SetMotorsOff();

                }

            } //End of if(movehome) statement


            if (MoveHome == false) {
                if (BUT2 == false) {
                    Switch2Demo = true;
                    TimerLoop.reset();
                }
                if ((Switch2Demo == true) && (TimerLoop.read()>=2.0)) {
                    CurrentState = Demo;
                    Switch2Demo = false;
                }
                if (BUT1 == false) {
                    Switch2OP = true;
                    TimerLoop.reset();
                }
                if ((Switch2OP == true) && (TimerLoop.read()>=2.0)) {
                    CurrentState = Operation;
                    Switch2OP = false;
                }

            } //End of else statement

            break;

        case HomingDemo:
            if (MoveHome == true) {
                if (q1Ref >= 0) {
                    q1Ref -= 0.0005*(6.380);
                }
                if (q1Ref <= 0) {
                    q1Ref += 0.0005*(6.380);
                }
                if (q2Ref >= 0) {
                    q2Ref -= 0.0005*(6.380);
                }
                if (q2Ref <= 0) {
                    q2Ref += 0.0005*(6.380);
                }

                if (TimerLoop.read()>=5.0) {
                    MoveHome = false;
                    CurrentState = Homing;
                    SetMotorsOff();

                }
            }
        break;

        case Emergency:
            LedRed = 0;
            LedGreen = 1;
            LedBlue = 1; //Red
            break;

        case EMGCal:
            LedRed = 0;
            LedGreen = 1;
            LedBlue = 0; //Pink


            if(LeftBicepsOut > MaxLeft) {
                MaxLeft = LeftBicepsOut;
                ThresholdLeft = abs(0.6000); // 60% of maximum value
                TimerLoop.reset();
            }

            if(RightBicepsOut > MaxRight) {
                MaxRight = RightBicepsOut;
                ThresholdRight = abs(0.6000); // 60% of maximum value
                TimerLoop.reset();
            }

            
            if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 5.0)) {
                TimerLoop.reset();
                CurrentState = MotorCal;
            }
            break;

        case MotorCal:
            LedRed = 1;
            LedGreen = 0;
            LedBlue = 0; //Turqoise
            if (NextMotorFlag == false) {
                if (BUT1==false) {
                    q1Ref += 0.0005*(6.2830);
                }
                if (BUTSW3 == false) {
                    q1Ref = 0;
                    Encoder1.reset();
                }
                if (BUT2 == false) {
                    q1Ref -=0.0005*(6.2830);
                }
                if (BUTSW2 == false) {
                    if (q1Ref>=-0.52*(6.2830)) {
                        q1Ref -=0.0005*(6.2830);
                    } else {
                        TimerLoop.reset();
                        NextMotorFlag = true;
                    }

                } //End of if (BUTSW2 == false)
            } //End of if (NextMotorFlag == false)

            else if ((NextMotorFlag == true) && (TimerLoop.read()>= 2)) {
                if (BUT1==false) {
                    q2Ref += 0.0005*(6.2830);
                }
                if (BUTSW3 == false) {
                    q2Ref = 0;
                    Encoder2.reset();
                }
                if (BUT2 == false) {
                    q2Ref -= 0.0005*(6.2830);
                }
                if (BUTSW2 == false) {
                    if (q2Ref<=0.733*(6.2830)) {
                        q2Ref +=0.0005*(6.2830);
                    } else {
                        CurrentState = Homing;
                        Encoder1.reset();
                        Encoder2.reset();
                        q1Ref = 0;
                        q2Ref = 0;
                        Error1 = 0;
                        Error2 = 0;
                        q1 = 0;
                        q2 = 0;
                        TimerLoop.reset();
                    }
                } // end of if (BUTSW2) statement
            }// end of else if statement

            break;

        case Operation:
            LedRed = 1;
            LedGreen = 0;
            LedBlue = 1; //Green

            if (BUTSW2 == false) {
                CurrentState = Homing;
                CurrentOperationState = OPSet;
                MoveHome = true;
                TimerLoop.reset();
            }

            switch (CurrentOperationState) {
                case OPWait:
                    if (EMGBoolLeft == true) {    //When Left Biceps are contracted, initiate chain to flip the page
                        CurrentOperationState = OPState1;
                        TimerLoop.reset();
                    }
                    break;

                case OPSet:                 // Set new homing for Operation Mode and go back to waiting mode
                    if (q2Ref  > -0.733*(6.380)) {
                        q2Ref  -= 0.0005*(6.380);
                    }
                    if (q2Ref  < -0.733*(6.380) && TimerLoop > 2) {
                        CurrentOperationState = OPWait;
                        TimerLoop.reset();
                    }
                    break;

                case OPState1:                      // First step of chain to flip the page
                    if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
                        q1Ref += 0.0005*(6.380);
                    }
                    if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
                        CurrentOperationState = OPState2;
                        TimerLoop.reset();
                    }
                    break;

                case OPState2:                      //Second step of chain to flip the page
                    if (q2Ref  > -1.133*(6.380)) {
                        q2Ref  -= 0.0005*(6.380);
                    }
                    if ((q2Ref  < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
                        CurrentOperationState = OPState3;
                        TimerLoop.reset();
                    }
                    break;

                case OPState3:                  //Third step of chain to flip the page
                    if (q1Ref > -0.15*(6.380)) {
                        q1Ref -= 0.0005*(6.380);
                    }
                    if (q2Ref  > -1.483*(6.380)) {
                        q2Ref  -= 0.0003*(6.380);
                    }
                    if ((q1Ref < -0.15*(6.380)) && (q2Ref  < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
                        CurrentOperationState = OPState4;
                        TimerLoop.reset();
                    }
                    break;

                case OPState4:                          //Fourth step of chain to flip the page
                    if (q2Ref  > -2.133*(6.380)) {
                        q2Ref  -= 0.005*(6.380);
                    }
                    if ((q2Ref  < -2.133*(6.380)) && (TimerLoop > 0.5)) {
                        TimerLoop.reset();
                        CurrentOperationState = OPHoming;
                    }
                    break;

                case OPHoming:                          //Go back to Home for Operation Mode and back to Waiting
                    if (q1Ref < 0) {
                        q1Ref += 0.003*(6.380);
                    }
                    if (q2Ref  < -0.733*(6.380)) {
                        q2Ref  += 0.001*(6.380);
                    }
                    if ((q1Ref > 0) && (q2Ref  > -0.733*(6.380)) && (TimerLoop > 3)) {
                        CurrentOperationState = OPWait;
                    }
                    break;
            }


            break;


        case Demo:
            LedRed = 1;
            LedGreen = 1;
            LedBlue = 0; //Blue

            TimerLoop.reset();
            Vdesired();
            InverseKinematics();

            if (BUTSW2 == false) {
                CurrentState = HomingDemo;
                MoveHome = true;
                TimerLoop.reset();
            }


            break;


    } //End of switch
} // End of stateMachine function

//------------------------------------------------------------------------------

void SetErrors()
{
    if ((CurrentState == Demo) || (CurrentState == HomingDemo)) {
        Error1 = -1*RatioGears*(q1Ref - qArm);
        Error2 = RatioPulleys*(q2Ref - qEndEff);
    } else {
        Error1 = q1Ref - q1;
        Error2 = q2Ref - q2;
        }
}
//------------------------------------------------------------------------------
// controller motor 1
void PID_Controller1(float Err1)
{
    float Kp = 19.8; // proportional gain
    float Ki = 3.98;//integral gain
    float Kd = 1.96;  //derivative gain
    static float ErrorIntegral = 0;
    static float ErrorPrevious = Err1;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    //Proportional part:
    float u_k = Kp * Err1;

    //Integral part:
    ErrorIntegral = ErrorIntegral + Err1*Ts;
    float u_i = Ki * ErrorIntegral;

    //Derivative part:
    float ErrorDerivative = (Err1 - ErrorPrevious)/Ts;
    float FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
    float u_d = Kd * FilteredErrorDerivative;
    ErrorPrevious = Err1;

    MotorValue1 =  u_k + u_i + u_d; //This will become the MotorValue to set motor 1
}

// controller motor 2
void PID_Controller2(float Err2)
{
    float Kp = 11.1; // proportional gain
    float Ki = 2.24;//integral gain
    float Kd = 1.1;  //derivative gain
    static float ErrorIntegral = 0;
    static float ErrorPrevious = Err2;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    //Proportional part:
    float u_k = Kp * Err2;

    //Integral part:
    ErrorIntegral = ErrorIntegral + Err2*Ts;
    float u_i = Ki * ErrorIntegral;

    //Derivative part:
    float ErrorDerivative = (Err2 - ErrorPrevious)/Ts;
    float FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
    float u_d = Kd * FilteredErrorDerivative;
    ErrorPrevious = Err2;

    MotorValue2 =  u_k + u_i + u_d; //This will become the MotorValue to set motor 2
}

// Main function for motorcontrol
void MotorControllers()
{
    PID_Controller1(Error1);
    PID_Controller2(Error2);

}
//------------------------------------------------------------------------------

//Ticker function set motorvalues
void SetMotors()
{
// Motor 1
    if (MotorValue1 >=0) { // set direction
        DirectionPin1 = 1;
    } else {
        DirectionPin1 = 0;
    }

    if (fabs(MotorValue1)>1) { // set duty cycle
        PwmPin1 = 1;
    } else {
        PwmPin1 = fabs(MotorValue1);
    }

// Motor 2
    if (MotorValue2 >=0) { // set direction
        DirectionPin2 = 1;
    } else {
        DirectionPin2 = 0;
    }

    if (fabs(MotorValue2)>1) { // set duty cycle
        PwmPin2 = 1;
    } else {
        PwmPin2 = fabs(MotorValue2);
    }

}




//-----------------------------------------------------------------------------


// Execution function
void LoopFunction()
{
    MeasureAll();
    StateMachine();
    SetErrors();
    MotorControllers();
    SetMotors();
    PrintToScreen();

}

int main()
{
    PwmPin1.period_us(60);
    PwmPin2.period_us(60);
    pc.baud(115200);
    pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
    TimerLoop.start(); // start Timer
    CurrentState = Waiting; // set initial state as Waiting
    CurrentOperationState = OPSet; //set initial state Operation mode
    bqc1.add( &bq1 ).add( &bq2 ).add( &bq5 ); //make BiQuadChain EMG left
    bqc2.add( &bq3 ).add( &bq4 ).add( &bq6 ); //make BiQuadChain EMG right
    TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz

    while (true) {
        if (PrintFlag == true) {
            pc.printf("BoolLeft = %i, BoolRight = %i, NormLeft = %f, MaxLeft = %f, NormRight = %f, MaxRight = %f ThresholdLeft = %f, ThresholdRight = %f\r",EMGBoolLeft,EMGBoolRight,NormLeft,MaxLeft,NormRight,MaxRight,ThresholdLeft,ThresholdRight);
        }
    }

}