File content as of revision 6:e67250b8b100:

#include "mbed.h"
#include "FastPWM.h"
#include "QEI.h"
#include "BiQuad.h"
#include <iostream>
#include <string>

enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
States currentState = WaitState;

DigitalIn startButton(D0);
InterruptIn failureButton(D1);
DigitalIn grippingSwitch(SW2);
DigitalIn screwingSwitch(SW3);
DigitalIn gripDirection(D2);
DigitalIn screwDirection(D3);

Serial pc(USBTX, USBRX);

DigitalOut led1(LED1); // Red led
DigitalOut led2(LED2); // Green led
DigitalOut led3(LED3); // Blue led
QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING);
QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
//QEI encoder3(A4, A5), NC, 4200);
AnalogIn pot(A4); // Speed knob
AnalogIn pot2(A5);

AnalogIn emg0(A0);
AnalogIn emg1(A1);
AnalogIn emg2(A2);
AnalogIn emg3(A3);

bool stateChanged = true;

Ticker mainTicker;
Timer stateTimer;

const double sampleTime = 0.001;
const float maxVelocity=8.4; // in rad/s
const double PI = 3.141592653589793238463;
const double  L1 = 0.328;
const double L2 = 0.218;
 double T1[3][3] {
    {0, -1, 0},
    {1, 0, 0,},
    {0, 0, 0,}
};
 double  T20[3][3] {
    {0, -1, 0},
    {1, 0, -L1,},
    {0, 0, 0,}
};
 double  H200[3][3] {
    {1, 0, L1+L2},
    {0, 1, 0,},
    {0, 0, 1,}
};
 double Pe2 [3][1] {
    {0},
    {0},
    {1}
};

double u1;
double u2; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
double u3;
double u4;
FastPWM pwmpin1(D5); //motor pwm
DigitalOut directionpin1(D4); // motor direction
FastPWM pwmpin2 (D6);
DigitalOut directionpin2 (D7);
double setPointX = 0.218;
double setPointY = 0.328;
double qRef1;
double qRef2;
double qMeas1;
double qMeas2;

double v; // Global variable for printf
double Dpulses; // Global variable for printf
double error1; // Global variable for printf
double error2; // Global variable for printf
double pulses1; // Global varaible for printf
double pulses2; // Global varaible for printf

double C[5][5];

double Kp1 = 150;
double Ki1 = 1;
double Kd1 = 10;
double Kp2 = 50;
double Ki2 = 0.1;
double Kd2 = 10;

const int samplepack = 250; //Amount of data points before one cycle completes
volatile int counter = 0; //Counts the amount of readings this cycle
volatile double total[4] = {0,0,0,0}; //Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
double refvaluebic = 10; //Minimum total value for the motor to move (biceps)
double refvaluecalf = 50; //Minimum total value for the motor to move (calfs)
double incr = 0.00005; //increment
bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-

BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5Hz High pass filter
BiQuad notch(0.9922,-1.6054,0.9922,-1.6054,0.9844); //50 Hz Notch filter
BiQuad lpf(0.9780,1.9561,0.9780,1.9556,0.9565); //250Hz Low pass filter
BiQuadChain bqc;

void switchToFailureState ()
{
    failureButton.fall(NULL); // Detaches button, so it can only be activated once
    led1 = 0;
    led2 = 1;
    led3 = 1;
    pc.printf("SYSTEM FAILED\n");
    currentState = FailureState;
    stateChanged = true;
}

void measureEMG ()
{
    total[0] += abs(bqc.step(emg0.read()));
    total[1] += abs(bqc.step(emg1.read()));
    total[2] += abs(bqc.step(emg2.read()));
    total[3] += abs(bqc.step(emg3.read()));
    counter++;
    if (counter >= samplepack){
        moving[0] = 0;
        moving[1] = 0;
        moving[2] = 0;
        moving[3] = 0;
        if (total[0] >= refvaluebic){
            moving[0] = 1;
        }
        if (total[1] >= refvaluebic){
            moving[1] = 1;
        }
        if (total[2] >= refvaluecalf){
            moving[2] = 1;
        }
        if (total[3] >= refvaluecalf){
            moving[3] = 1;
        }
        //pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
        pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
        counter = 0; //Reset for next cycle
        for (int i=0;i<4;i++){ //clear 'total' array
            total[i] = 0;
        }
    }

    if(moving[0]){
        setPointX += incr;
    }
    if (moving[1]){
        setPointX -= incr;
    }
    if (moving[2]){
        setPointY += incr;
    }
    if (moving[3]){
        setPointY -= incr;
    }
   
}

double measureVelocity (int motor)
{
    static double lastPulses = 0;
    double currentPulses;
    static double velocity = 0;

    static int i = 0;
    if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
        switch (motor) { // Check which motor to measure
            case 1:
                currentPulses = encoder1.getPulses();
                break;
            case 2:
                //currentPulses = encoder2.getPulses();
                break;
            case 3:
                //currentPulses = encoder3.getPulses();
                break;
        }

        double deltaPulses = currentPulses - lastPulses;
        Dpulses = deltaPulses;
        velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
        lastPulses = currentPulses;
        i = 0;
    } else {
        i += 1;
    }
    v = velocity;
    return velocity;
}

void measurePosition() // Measure actual angle position with the encoder
{
     pulses1 = encoder2.getPulses();   // motor 1 is on M2 and ENC2 of biorobotics shield
     pulses2 = encoder1.getPulses();   // motor 2 is on M1 and ENC1 of biorobotics shield
    qMeas1 = (pulses1) * 2 * PI / 8400  +140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
    qMeas2 = (pulses2) * 2 * PI / 8400  - 93*PI/180 ;

}

void getMotorControlSignal ()   // Milestone 1 code, not relevant anymore
{
    double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
    //pc.printf("motor control signal = %f\n", posampleTimeignal);
    u1 = potSignal;
    u2 = potSignal;
}

template<std::size_t N, std::size_t M, std::size_t P>
void mult(double A[N][M], double B[M][P])
{

    for( int n =0; n < 5; n++) {
        for(int p =0; p < 5; p++) {
            C[n][p] =0;
        }
    }
    for (int n = 0; n < N; n++) {
        for (int p = 0; p < P; p++) {
            double num = 0;
            for (int m = 0; m < M; m++) {
                num += A[n][m] * B[m][p];

            }

            C[n][p] = num;

        }
    }

}

void inverseKinematics ()
{
    if (currentState == MovingState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
        
        double  Pe_set[3][1] {            // defining setpoint location of end effector
            {setPointX},             //setting xcoord to pot 1
            {setPointY},             // setting ycoord to pot 2
            {1}
        };

//Calculating new H matrix
        double T1e[3][3] {
            {cos(qRef1), -sin(qRef1), 0},
            {sin(qRef1), cos(qRef1), 0},
            {0, 0, 1}
        };
        double  T20e[3][3] {
            {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)},
            {sin(qRef2), cos(qRef2), -L1*sin(qRef2)},
            {0, 0, 1}
        };


        mult<3,3,3>(T1e,T20e);  // matrix multiplication
        double H201[3][3] {
            {C[0][0],C[0][1],C[0][2]},
            {C[1][0],C[1][1],C[1][2]},
            {C[2][0],C[2][1],C[2][2]}
        };

        mult<3,3,3>(H201,H200);   // matrix multiplication
        double H20 [3][3] {
            {C[0][0],C[0][1],C[0][2]},
            {C[1][0],C[1][1],C[1][2]},
            {C[2][0],C[2][1],C[2][2]}
        };

        mult<3,3,1>(H20,Pe2);   // matrix multiplication
        double Pe0[3][1] {
            {C[0][0]},
            {C[1][0]},
            {C[2][0]}
        };

        double pe0x = Pe0[0][0];                 // seperating coordinates of end effector location
        double pe0y = Pe0[1][0];

        // Determing the jacobian

        double T_1[3][1] {
            {1},
            {T1[0][2]},
            {T1[1][2]}
        };

        double T_2[3][1] {
            {1},
            {L1*sin(qRef1)},
            {-L1*cos(qRef1)}
        };

        double J[3][2] {
            {T_1[0][0], T_2[0][0]},
            {T_1[1][0], T_2[1][0]},
            {T_1[2][0], T_2[2][0]}
        };

//Determing 'Pulling" force to setpoint

        double kspring= 1;     //virtual stifness of the force
        double Fs[3][1] {                                    //force vector from end effector to setpoint
            {kspring*Pe_set[0][0] - kspring*Pe0[0][0]},
            {kspring*Pe_set[1][0] - kspring*Pe0[1][0]},
            {kspring*Pe_set[2][0] - kspring*Pe0[2][0]}
        };
        double Fx = kspring*setPointX - kspring*pe0x;
        double Fy = kspring*setPointY - kspring*pe0y;
        double W0t[3][1] {
            {pe0x*Fy - pe0y*Fx},
            {Fx},
            {Fy}
        };

        double Jt[2][3] {                                    // transposing jacobian matrix
            {J[0][0], J[1][0], J[2][0]},
            {T_2[0][0], T_2[1][0], T_2[2][0]}
        };

        mult<2,3,1>(Jt,W0t);
        double tau_st1 = C[0][0];
        double tau_st2 = C[1][0];

//Calculating joint behaviour

        double b1 =10;
        double b2 =1;
        //joint friction coefficent
        //double sampleTime = 1/1000;               //Time step to reach the new angle
        double w_s1 = tau_st1/b1;          // angular velocity
        double w_s2 = tau_st2/b2;          // angular velocity
        //checking angle boundaries
        qRef1 = qRef1 +w_s1*1;         // calculating new angle of qRef1 in time step sampleTime
        if (qRef1 > 2*PI/3) {
            qRef1 = 2*PI/3;
        } else if (qRef1 < PI/6) {
            qRef1= PI/6;
        }

        qRef2 = qRef2 + w_s2*1;        // calculating new angle of qRef2 in time step sampleTime
        if (qRef2 > -PI/4) {
            qRef2 = -PI/4;
        } else if (qRef2 < -PI/2) {
            qRef2= -PI/2;
        }
    }
}
void PID_controller() // Put the error trough PID control to make output 'u'

{
     if (currentState >= HomingState && currentState < FailureState){
     // Should only work when we move the robot to a defined position
         error1 = qRef1 - qMeas1;
        error2 = qRef2 - qMeas2;

        static double errorIntegral1 = 0;
        static double errorIntegral2 = 0;
        static double errorPrev1 = error1;
        static double errorPrev2 = error2;
        static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
        //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant

        // Proportional part:
        double Kp = pot2.read() * 0.001;
        double u_k1 = Kp1 * error1;
        double u_k2 = Kp2 * error2;

        //Integral part:
        errorIntegral1 = errorIntegral1 + error1 * sampleTime;
        double u_i1 = Ki1 * errorIntegral1;
        errorIntegral2 = errorIntegral2 + error2 * sampleTime;
        double u_i2 = Ki2 * errorIntegral2;

        // Derivative part
        double errorDerivative1 = (error1 - errorPrev1)/sampleTime;
        double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1);
        double u_d1 = Kd1 * filteredErrorDerivative1;
        errorPrev1 = error1;
        double errorDerivative2 = (error2 - errorPrev2)/sampleTime;
        double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2);
        double u_d2 = Kd2 * filteredErrorDerivative2;
        errorPrev2 = error2;

        // Sum all parsampleTime
        u1 = u_k1 + u_i1 + u_d1;
        u2 = u_k2 + u_i2 + u_d2;
        
    
    }
}

void controlMotor ()   // Control direction and speed
{
    directionpin1 = u1 > 0.0f; // Either true or false
    pwmpin1 = fabs(u1);
    directionpin2 = u2 > 0.0f; // Either true or false
    pwmpin2 = fabs(u2);
}

void stateMachine ()
{
    switch (currentState) {
        case WaitState:
            if (stateChanged == true) {
                led1 = 0;
                led2 = 1;
                led3 = 1;
                // Entry action: all the things you do once in this state
                u1 = 0; // Turn off all motors
                u2 = 0;
                u3 = 0;
                u4 = 0;
                stateChanged = false;
            }

            if (startButton.read() == false) { // When button is pressed, value is false
                //pc.printf("Switching to motor calibration");
                led1 = 1;
                currentState = MotorCalState;
                stateChanged = true;
            }

            break;
        case MotorCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                led2 = 0;
                // Set motorpwm to 'low' value
                //u1 = 0.6; //TODO: Check if direction is right
                //u2 = 0.6;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop
            getMotorControlSignal();

            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f
                //TODO: Add reset of encoder2
                led2 = 1;
                encoder1.reset(); // Reset encoder for the 0 position
                currentState = EMGCalState;
                stateChanged = true;
                u1 = 0; // Turn off motors
                u2 = 0;
            }
            break;
        case EMGCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led3 = 0;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop

            if (stateTimer >= 3.0f) {
                //pc.printf("Starting homing...\n");
                led3 = 1;
                currentState = HomingState;
                stateChanged = true;
            }
            break;
        case HomingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led2 = 0; // EmisampleTime yellow together
                //TODO: Set qRef1 and qRef2
                qRef1 = 90 * PI / 180;
                qRef2 = -qRef1 + 0 *PI/180;
                stateChanged = false;
            }

            // Nothing extra happens till robot reaches starting position and button is pressed

            if (startButton.read() == false) { //TODO: Also add position condition
                led1 = 1;
                led2 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case MovingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led2 = 0;
                led3 = 0; // EmisampleTime white together
                stateChanged = false;
            }

            if (grippingSwitch.read() == false) {
                led1 = 1;
                led2 = 1;
                led3 = 1;
                currentState = GrippingState;
                stateChanged = true;
            }

            break;
        case GrippingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led2 = 0;
                led3 = 0; // EmisampleTime light blue together
                stateChanged = false;
            }

            if (gripDirection == true) {
                // Close gripper
            } else {
                // Open gripper
            }

            if (screwingSwitch.read() == false) {
                led2 = 1;
                led3 = 1;
                currentState = ScrewingState;
                stateChanged = true;
            }
            if (startButton.read() == false) {
                led2 = 1;
                led3 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case ScrewingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led3 = 0; // EmisampleTime pink together
                stateChanged = false;
            }

            if (screwDirection == true) {
                // Screw
            } else {
                // Unscrew
            }

            if (startButton.read() == false) {
                led1 = 1;
                led3 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case FailureState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                u1 = 0; // Turn off all motors
                u2 = 0;
                u3 = 0;
                u4 = 0;
                stateChanged = false;
            }

            static double blinkTimer = 0;
            if (blinkTimer >= 0.5) {
                led1 = !led1;
                blinkTimer = 0;
            }
            blinkTimer += sampleTime;

            break;
    }
}

void measureAll ()
{
    measurePosition();
    measureEMG();
    inverseKinematics();
}

void mainLoop ()
{
    // Add measure, motor controller and output function    
    measureAll();
    stateMachine();
    PID_controller();
    controlMotor();
}

int main()
{
    pc.baud(115200);
    pc.printf("checkpoint 1\n");
    bqc.add(&hpf).add(&notch).add(&lpf); //Add bqfilters to bqc
    mainTicker.attach(mainLoop, sampleTime);
    failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated

    while (true) {
        //pc.printf("State = %i\n", currentState);
        //int pulses = encoder1.getPulses();
        //pc.printf("pulses = %i\n", pulses);
       // pc.printf("v = %f\n", v);
        pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n qMeas1 = %f qMeas2 = %f \n, Pulses1 = %f Pulses2 = %f \n", error1,error2,qRef1,qRef2,qMeas1,qMeas2, pulses1, pulses2);
        wait(2);
    }
}