File content as of revision 4:e7187a17c732:

#include "mbed.h"
#include "FastPWM.h"
#include "QEI.h"
#include "MODSERIAL.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 gripperButton(D2);
DigitalIn directionSwitch(D3);
DigitalIn gripperMotorButton(D14);

MODSERIAL pc(USBTX, USBRX);
DigitalOut led1(LED1); // Red led
DigitalOut led2(LED2); // Green led
DigitalOut led3(LED3); // Blue led
QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
//QEI encoder3(A4, A5), NC, 4200);
AnalogIn pot(A0); // Speed knob
AnalogIn pot2(A1);

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);
FastPWM pwmpin3(A4); //motor pwm
DigitalOut directionpin3(D8); // motor direction
FastPWM pwmpin4(A5);
DigitalOut directionpin4(D9);

double setPointX;
double setPointY;
double qRef1;
double qRef2;
double qMeas1;
double qMeas2;

double v; // Global variable for printf
double Dpulses; // Global variable for printf

double C[5][5];

double Kp = 0.1;
double Ki = 0;
double Kd = 0;

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

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
{
    double pulses1 = encoder1.getPulses();
    double pulses2 = encoder2.getPulses();
    qMeas1 = pulses1 * 2 * PI / 8400 + 840; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
    qMeas2 = pulses2 * 2 * PI / 8400 + 70;

}

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 potx = 0.218;//pot1.read()*0.546;
        double  poty = 0.328;//pot2.read()*0.4;

        double  Pe_set[3][1] {            // defining setpoint location of end effector
            {potx},             //setting xcoord to pot 1
            {poty},             // 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 k= 1;     //virtual stifness of the force
        double Fs[3][1] {                                    //force vector from end effector to setpoint
            {k*Pe_set[0][0] - k*Pe0[0][0]},
            {k*Pe_set[1][0] - k*Pe0[1][0]},
            {k*Pe_set[2][0] - k*Pe0[2][0]}
        };
        double Fx = k*potx - k*pe0x;
        double Fy = k*poty - k*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 b =1;
        //joint friction coefficent
        //double sampleTime = 1/1000;               //Time step to reach the new angle
        double w_s1 = tau_st1/b;          // angular velocity
        double w_s2 = tau_st2/b;          // angular velocity
        //checking angle boundaries
        qRef1 = qRef1 +w_s1*sampleTime;         // 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*sampleTime;        // 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
        double error1 = qRef1 - qMeas1;
        double 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:
        Kp = pot2.read() * 1;
        double u_k1 = Kp * error1;
        double u_k2 = Kp * error2;

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

        // Derivative part
        double errorDerivative1 = (error1 - errorPrev1)/sampleTime;
        double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1);
        double u_d1 = Kd * filteredErrorDerivative1;
        errorPrev1 = error1;
        double errorDerivative2 = (error2 - errorPrev2)/sampleTime;
        double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2);
        double u_d2 = Kd * 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 = -90 * 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 (gripperButton.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 (gripperMotorButton == false) {
                u3 = 0.4;
                if (directionSwitch == true) {
                    // Close gripper, so positive direction
                } else {
                    // Open gripper
                    u3 = u3 * -1;
                }
            } else { // If the button isn't pressed, turn off motor
                u3 = 0;
            }

            if (gripperButton.read() == false) {
                led2 = 1;
                led3 = 1;
                if (directionSwitch == true) { // If we close the gripper, we also want it keep giving torgue in the next state to hold on to the object
                    u3 = 0.4;
                    } else {
                        u3 = 0;
                currentState = ScrewingState;
                stateChanged = true;
            }
            if (startButton.read() == false) {
                led2 = 1;
                led3 = 1;
                if (directionSwitch == true) {
                    u3 = 0.4;
                    } else {
                        u3 = 0;
                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 (gripperMotorButton == false) {
                u4 = 0.4;
                u3 = -0.4;
            if (directionSwitch == true) {
                // Screw
            } else {
                // Unscrew
                u4 = u4 * -1;
                u3 = u3 * -1;
            }
            } else {
                u4 = 0;
                u3 = 0.4;
                }

            if (startButton.read() == false) {
                led1 = 1;
                led3 = 1;
                u3 = 0;
                u4 = 0.4;
                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();
    inverseKinematics();
}

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

int main()
{
    pc.printf("checkpoint 1\n");
    startButton.mode(PullUp);
    failureButton.mode(PullUp);
    gripperButton.mode(PullUp);
    directionSwitch.mode(PullUp);
    gripperMotorButton.mode(PullUp);
    pc.baud(115200);
    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("delta pulses = %f\n", Dpulses);
        wait(1);
    }
}