File content as of revision 8:5896424eb539:

#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 gripperButton(D2);
DigitalIn directionSwitch(D3);
DigitalIn gripperMotorButton(D14);


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

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

bool stateChanged = true;

Ticker mainTicker;
Timer stateTimer;
Ticker calTimer; //Ticker that waits (to prepare the person)
Ticker EMGsampler; //Ticker that samples the EMG

const double sampleTime = 0.001;
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; // Motor output of the long link
double u2; // Motor of the short link
double u3; // Motor of the gripper
double u4; // Motor of the screwer
FastPWM pwmpin1(D5); // Motor pwm
DigitalOut directionpin1(D4); // Motor direction
FastPWM pwmpin2 (D6);
DigitalOut directionpin2 (D7);
FastPWM pwmpin3(A4);
DigitalOut directionpin3(D8);
FastPWM pwmpin4(A5);
DigitalOut directionpin4(D9);

double setPointX = 0.3;
double setPointY = 0.28;
double qRef1;
double qRef2;
double qMeas1;
double qMeas2;

double C[5][5];

double Kp1 = 10;
double Ki1 = 0;
double Kd1 = 2;
double Kp2 = 10;
double Ki2 = 0;
double Kd2 = 2;

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.00002; // Increment
bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-

volatile int waitingcounter = 0; //How many iterations of waiting have been done
int countermax = 20; //counter until when should be waited
volatile int processnow = 0; //Point in the calibration process
volatile double armresttot = 0;
volatile double armflextot = 0;
volatile double legresttot = 0;
volatile double legflextot = 0;
const int samplecal = 2500; //Amount of data points to consider in the calibration
double ratio = samplecal/samplepack;
volatile double tot = 0; //Total measured EMG of the current type.
volatile double armcal; //Calibated value for arm measurements
volatile double legcal; //Calibrated value for leg measurements

BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5 Hz 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 ()
{
    if (currentState == MovingState) {
        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;
        }

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

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

}

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 MovingState should the qRef1, qRef2 change every sampleTime

        double  Pe_set[3][1] { // Defining setpoint location of end effector
            {setPointX},  // Setting xcoord to setPointX
            {setPointY},  // Setting ycoord to setPointY
            {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= 0.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 =1;
        double b2 =1;
        // Joint friction coefficent
        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
        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);

        // Proportional part:
        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 parts
        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);
    directionpin3 = u3 > 0.0f; // Either true or false
    pwmpin3 = fabs(u3);
    directionpin4 = u4 > 0.0f; // Either true or false
    pwmpin4 = fabs(u4);
}

void EMGwaiting(){ //Flashes LED
    led3 = !led3;
    waitingcounter++;   
}
 
void EMGsampling(){ //Ticker function of EMG
    switch(processnow){
        case 0:
        case 1: 
            tot += abs(bqc.step(emg0.read()));
            break;
        case 2:
        case 3:
            tot += abs(bqc.step(emg2.read()));
            break;
    }
    counter++;
    if (counter >= samplecal){
        tot = tot/ratio;
        switch(processnow){
            case 0:
                armresttot = tot;
                pc.printf("<Result> Average area of arm signal in rest: %f.\r\n",armresttot);
                break;
            case 1:
                armflextot = tot;
                pc.printf("<Result>Average area of arm signal while flexing: %f.\r\n",armflextot);
                armcal = (armflextot + armresttot)/2.0;
                pc.printf("<Result> Calibration value for arms determined at %f.\r\n",armcal);
                break;
            case 2:
                legresttot = tot;
                pc.printf("<Result> Average area of leg signal in rest: %f.\r\n",legresttot);
                break;
            case 3:
                legflextot = tot;
                pc.printf("<Result> Average area of leg signal while flexing: %f.\r\n",legflextot);
                legcal = (legflextot + legresttot)/2;
                pc.printf("<Result> Calibration value for legs determined at %f.\r\n",legcal);
                break;
        }
        processnow++;
        tot = 0;
        counter = 0;
    }
}

void stateMachine ()
{
    switch (currentState) {
        case WaitState:
            if (stateChanged == true) {
                // Everything that needs to be done when this state is entered the first time
                led1 = 0; // Green
                led2 = 1;
                led3 = 1;
                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
                // Everything that needs to be done when leaving this state
                led1 = 1;
                currentState = MotorCalState;
                stateChanged = true;
            }

            break;
        case MotorCalState:
            if (stateChanged == true) {
                led2 = 0; // Red
                u1 = 0.3; // Motor is set to 'low' value
                u2 = -0.3;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // The robot can only go to the next state after 3s, when the motor velocities are close to 0 and the start button is pressed
            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && fabs(measureVelocity(2)) < 100 && startButton.read() == false) {
                led2 = 1;
                encoder1.reset(); // Reset encoder for the 0 position
                encoder2.reset();
                currentState = EMGCalState;
                stateChanged = true;
                u1 = 0; // Turn off motors
                u2 = 0;
            }
            break;
        case EMGCalState:
            if (stateChanged == true) {
                led3 = 0; // Blue
                stateChanged = false;

                pc.printf("<----------Now entering EMG calibration state ---------->\r\n");
                pc.printf("<Waiting> Get ready for calibration of resting arms.\r\n");
                calTimer.attach(&EMGwaiting,0.5); //Wait
                while (waitingcounter < countermax) {}
                waitingcounter = 0;
                calTimer.detach();

                pc.printf("<Measuring> Measuring arm signal at rest. Hold still.\r\n");
                EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms rest
                while (processnow == 0) {}
                EMGsampler.detach();

                pc.printf("<Waiting> Get ready for calibration of flexing arms.\r\n");
                calTimer.attach(&EMGwaiting,0.5); // Wait
                while (waitingcounter < countermax) {}
                waitingcounter = 0;
                calTimer.detach();

                pc.printf("<Measuring> Measuring arm signal while flexing.\r\n");
                EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms flexing
                while (processnow == 1) {}
                EMGsampler.detach();

                pc.printf("<Waiting> Get ready for calibration of resting legs.\r\n");
                calTimer.attach(&EMGwaiting,0.5); //Wait
                while (waitingcounter < countermax) {}
                waitingcounter = 0;
                calTimer.detach();

                pc.printf("<Measuring> Measuring leg signal at rest. Hold still.\r\n");
                EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs rest
                while (processnow == 2) {}
                EMGsampler.detach();

                pc.printf("<Waiting> Get ready for calibration of flexing legs.\r\n");
                calTimer.attach(&EMGwaiting,0.5); // Wait
                while (waitingcounter < countermax) {}
                waitingcounter = 0;
                calTimer.detach();

                pc.printf("<Measuring> Measuring leg signal while flexing.\r\n");
                EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs flexing
                while (processnow == 3) {}
                EMGsampler.detach();

                pc.printf("<Result> EMG calibrations complete.\r\n");
            }

            if (stateTimer >= 3.0f) {
                led3 = 1;
                currentState = HomingState;
                stateChanged = true;
            }
            break;
        case HomingState:
            if (stateChanged == true) {
                led1 = 0; // Yellow
                led2 = 0;
                // qRef1 and qRef2 are set, so the motors will move to that position
                qRef1 = 60 * PI / 180;
                qRef2 = -90 * PI / 180;
                stateChanged = false;
            }

            if (startButton.read() == false) { //TODO: Also add position condition
                led1 = 1;
                led2 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case MovingState:
            if (stateChanged == true) {
                led1 = 0; // White
                led2 = 0;
                led3 = 0;
                stateChanged = false;
            }

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

            break;
        case GrippingState:
            if (stateChanged == true) {
                led2 = 0; // Light blue
                led3 = 0;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            if (gripperMotorButton.read() == false) {
                // Gripper motor is activated
                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;
            }

            // Due to the lack of buttons, we use a delay, so that when we don't immediately go to the next state when we enter this state
            if (gripperButton.read() == false && stateTimer >= 2.0f) {
                led2 = 1;
                led3 = 1;
                u3 = 0;
                currentState = ScrewingState;
                stateChanged = true;
            }
            if (startButton.read() == false) {
                led2 = 1;
                led3 = 1;
                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; // Pink
                led3 = 0;
                stateChanged = false;
            }

            if (gripperMotorButton.read() == false) {
                u4 = 0.3; // Screwing motor is activated
                u3 = -0.35; // Gripper motor is activated with a slightly higher output to make sure the gripper doesn't open
                if (directionSwitch == true) {
                    // Screw
                } else {
                    // Unscrew
                    u4 = u4 * -1;
                    u3 = u3 * -1;
                }
            } else { // If the button isn't pressed, turn off motors
                u4 = 0;
                u3 = 0;
            }

            if (startButton.read() == false) {
                led1 = 1;
                led3 = 1;
                u3 = 0; // Turn off motors
                u4 = 0;
                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 blinkTimer2 = 0;
            if (blinkTimer2 >= 0.5) { // The red LED is flashing at 1 Hz
                led1 = !led1;
                blinkTimer2 = 0;
            }
            blinkTimer2 += sampleTime;
            break;
    }
}

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

void mainLoop () // All the major functions are looped every 0.001s
{
    measureAll();
    stateMachine();
    PID_controller();
    controlMotor();
}

int main()
{
    startButton.mode(PullUp); // Makes the button active
    failureButton.mode(PullUp);
    gripperButton.mode(PullUp);
    directionSwitch.mode(PullUp);
    gripperMotorButton.mode(PullUp);
    pc.baud(115200);
    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) {
    }
}