File content as of revision 4:ca797e7daaf4:

/*
* Parts of the code copied from PES lecture slides
*/

// include all necessary libraries
#include "mbed.h"
#include "QEI.h"
#include "math.h"
#include "iostream"
#include "BiQuad.h"
#include "TextLCD.h"

//intialize all pins
PwmOut motor1(D5);
PwmOut motor2(D6);
DigitalOut motor1Dir(D4); // direction of motor 1 (1 is ccw 0 is cw (looking at the shaft from the front))
DigitalOut motor2Dir(D7); // direction of motor 2 (1 is ccw 0 is cw (looking at the shaft from the front))
QEI motor1Encoder (D10,D11, NC, 624,QEI::X4_ENCODING);
QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
DigitalIn button1(D8); //button to move cw
DigitalIn button2(D9); //button to move ccw
//DigitalIn button3(D2);
//DigitalIn button4(D3);
AnalogIn bicepsEMG(A0);
AnalogIn tricepsEMG(A1);
AnalogIn carpiFlexEMG(A2);
AnalogIn palmarisEMG(A3);
DigitalOut bit1(D2);
DigitalOut bit2(D14);
DigitalOut bit3(D15);

//initialize variables
const float pi = 3.14159265358979323846; //value for pi
double positionIncrement = 20; // increment of angle when button pressed (1 is a whole rotation (360 degrees))

const double motor1KP=7.0; //Proportional gain of motor1 PI control
const double motor1KI=3.0; //Integral gain of motor1 PI control
const double motor1KD=3.0; // Differential gain of motor1 PID control

const double motor2KP=1.3; //Proportional gain of motor2 PI control
const double motor2KI=0.5; //Integral gain of motor2 PI control
const double motor2KD=1.0; // Differential gain of motor2 PID control

const double N=100; //LP filter coefficient
const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
const double controllerTickerTime=0.01; //ticker frequency

double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller
double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller
double desiredAngle1 =0; //desired position of motor1

double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
double desiredAngle2 =0; //desired position of motor2
//initialize ticker for checking and correcting the angle
Ticker myControllerTicker;

enum States {off,motorCalib, bicepsCalib, tricepsCalib, carpiCalib, palmarisCalib, demo, EMGControl};
States state=off;
bool stateChange=false;
Timer flex;

double tickerF =0.002;
double bicepsMOVAG [20];
double tricepsMOVAG [20];
double carpiFlexMOVAG [20];
double palmarisMOVAG [20];
int MOVAGLength=20;

//Notch Filter 50Hz Q of 0.7

double bicepsNotcha0 = 0.7063988100714527;
double bicepsNotcha1 = -1.1429772843080923;
double bicepsNotcha2 = 0.7063988100714527;
double bicepsNotchb1 = -1.1429772843080923;
double bicepsNotchb2 = 0.41279762014290533;

double tricepsNotcha0 = 0.7063988100714527;
double tricepsNotcha1 = -1.1429772843080923;
double tricepsNotcha2 = 0.7063988100714527;
double tricepsNotchb1 = -1.1429772843080923;
double tricepsNotchb2 = 0.41279762014290533;

double carpiFlexNotcha0 = 0.7063988100714527;
double carpiFlexNotcha1 = -1.1429772843080923;
double carpiFlexNotcha2 = 0.7063988100714527;
double carpiFlexNotchb1 = -1.1429772843080923;
double carpiFlexNotchb2 = 0.41279762014290533;

double palmarisNotcha0 = 0.7063988100714527;
double palmarisNotcha1 = -1.1429772843080923;
double palmarisNotcha2 = 0.7063988100714527;
double palmarisNotchb1 = -1.1429772843080923;
double palmarisNotchb2 = 0.41279762014290533;

//Highpass filters 20Hz cutoff Q of 0.7
double bicepsHigha0 = 0.8370879899975344;
double bicepsHigha1 = -1.6741759799950688;
double bicepsHigha2 = 0.8370879899975344;
double bicepsHighb1 = -1.6474576182593796;
double bicepsHighb2 = 0.7008943417307579;

double tricepsHigha0 = 0.8370879899975344;
double tricepsHigha1 = -1.6741759799950688;
double tricepsHigha2 = 0.8370879899975344;
double tricepsHighb1 = -1.6474576182593796;
double tricepsHighb2 = 0.7008943417307579;

double carpiFlexHigha0 = 0.8370879899975344;
double carpiFlexHigha1 = -1.6741759799950688;
double carpiFlexHigha2 = 0.8370879899975344;
double carpiFlexHighb1 = -1.6474576182593796;
double carpiFlexHighb2 = 0.7008943417307579;

double palmarisHigha0 = 0.8370879899975344;
double palmarisHigha1 = -1.6741759799950688;
double palmarisHigha2 = 0.8370879899975344;
double palmarisHighb1 = -1.6474576182593796;
double palmarisHighb2 = 0.7008943417307579;

BiQuad bicepsNotch (bicepsNotcha0,bicepsNotcha1,bicepsNotcha2,bicepsNotchb1,bicepsNotchb2);
BiQuad tricepsNotch (tricepsNotcha0,tricepsNotcha1,tricepsNotcha2,tricepsNotchb1,tricepsNotchb2);
BiQuad carpiFlexNotch (carpiFlexNotcha0,carpiFlexNotcha1,carpiFlexNotcha2,carpiFlexNotchb1,carpiFlexNotchb2);
BiQuad palmarisNotch (palmarisNotcha0,palmarisNotcha1,palmarisNotcha2, palmarisNotchb1,palmarisNotchb2);

BiQuad bicepsHigh (bicepsHigha0,bicepsHigha1,bicepsHigha2,bicepsHighb1,bicepsHighb2);
BiQuad tricepsHigh (tricepsHigha0,tricepsHigha1,tricepsHigha2,tricepsHighb1,tricepsHighb2);
BiQuad carpiFlexHigh (carpiFlexHigha0,carpiFlexHigha1,carpiFlexHigha2,carpiFlexHighb1,carpiFlexHighb2);
BiQuad palmarisHigh (palmarisHigha0,palmarisHigha1,palmarisHigha2, palmarisHighb1,palmarisHighb2);

double bicepsAvg;
double tricepsAvg;
double carpiFlexAvg;
double palmarisAvg;

double bicepsMax=0;
double tricepsMax=0;
double carpiFlexMax=0;
double palmarisMax=0;

double bicepsMulti=0;
double tricepsMulti=0;
double carpiFlexMulti=0;
double palmarisMulti=0;
Ticker filterTicker;

const float l1 = 460; //Length of the arm from base to joint 1 (arm1)   ENTER MANUALLY [mm]
const float l2 = 450; //length of the arm from joint 1 to the end-effector.(arm2)   ENTER MANUALLY [mm]
float x_des = l1+l2; //(initial)desired x location of the end-effector (ee)
float y_des = 0; //(initial) desired y location of the end-effector (ee)
float xe, ye, D, phi, q2, beta, alpha, q1; //other variables used in calculating the angle for the motors
float radDeg, rad2rot;
//q1 is the angle for the base motor, q2 is the angle for the elbow motor, both in [rad]


double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError)
{
    const double a1 = -4/(N*Ts+2);
    const double a2 = -(N*Ts-2)/(N*Ts+2);
    const double b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
    const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2);
    const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);

    double v = error - a1*intError - a2*DifError;
    double u = b0*v + b1*intError + b2*DifError;
    DifError = intError;
    intError = v;
    return u;
}

void sendState(States s)
{
    if(s==motorCalib) {
        bit1=1;
        bit2=0;
        bit3=0;
    } else if(s==bicepsCalib) {
        bit1=0;
        bit2=1;
        bit3=0;
    } else if(s==tricepsCalib) {
        bit1=1;
        bit2=1;
        bit3=0;
    } else if(s==carpiCalib) {
        bit1=0;
        bit2=0;
        bit3=1;
    } else if(s==palmarisCalib) {
        bit1=1;
        bit2=0;
        bit3=1;
    } else if(s==demo) {
        bit1=0;
        bit2=1;
        bit3=1;
    } else if(s==EMGControl) {
        bit1=1;
        bit2=1;
        bit3=1;
    }
}

void filterBiceps()
{
    double bicepsSignal = bicepsEMG.read();
    double bicepsFiltered = bicepsSignal;

    //Filter out 50Hz from all signals
    bicepsFiltered = bicepsNotch.step(bicepsFiltered);

    //Highpass filtering
    bicepsFiltered = bicepsHigh.step(bicepsFiltered);
    //Rectification
    bicepsFiltered = fabs(bicepsFiltered);

    //caculate moving average
    for(int i=0; i<MOVAGLength-1; i++) {
        bicepsMOVAG[i]=bicepsMOVAG[i+1];
    }

    bicepsMOVAG[MOVAGLength-1]=bicepsFiltered;

    double bicepsSum;

    for(int i=0; i<MOVAGLength; i++) {
        bicepsSum+= bicepsMOVAG[i];
    }

    bicepsAvg= bicepsSum/MOVAGLength;
}
void filterTriceps()
{
    double tricepsSignal = tricepsEMG.read();
    double tricepsFiltered = tricepsSignal;

    //Filter out 50Hz from all signals
    tricepsFiltered = tricepsNotch.step(tricepsFiltered);

    //Highpass filtering
    tricepsFiltered = tricepsHigh.step(tricepsFiltered);

    //Rectification
    tricepsFiltered = fabs(tricepsFiltered);

    //caculate moving average
    for(int i=0; i<MOVAGLength-1; i++) {
        tricepsMOVAG[i]=tricepsMOVAG[i+1];
    }

    tricepsMOVAG[MOVAGLength-1]=tricepsFiltered;

    double tricepsSum;

    for(int i=0; i<MOVAGLength; i++) {
        tricepsSum+= tricepsMOVAG[i];
    }
    tricepsAvg= tricepsSum/MOVAGLength;
}

void filterCarpiFlex()
{
    double carpiFlexSignal = carpiFlexEMG.read();
    double carpiFlexFiltered = carpiFlexSignal;

    //Filter out 50Hz from all signals
    carpiFlexFiltered = carpiFlexNotch.step(carpiFlexFiltered);

    //Highpass filtering
    carpiFlexFiltered = carpiFlexHigh.step(carpiFlexFiltered);

    //Rectification
    carpiFlexFiltered = fabs(carpiFlexFiltered);

    //caculate moving average
    for(int i=0; i<MOVAGLength-1; i++) {
        carpiFlexMOVAG[i]=carpiFlexMOVAG[i+1];
    }

    carpiFlexMOVAG[MOVAGLength-1]=carpiFlexFiltered;

    double carpiFlexSum;
    for(int i=0; i<MOVAGLength; i++) {
        carpiFlexSum+= carpiFlexMOVAG[i];
    }

    carpiFlexAvg= carpiFlexSum/MOVAGLength;
}
void filterPalmaris()
{
    double palmarisSignal = palmarisEMG.read();
    double palmarisFiltered = palmarisSignal;

    //Filter out 50Hz from all signals
    palmarisFiltered = palmarisNotch.step(palmarisFiltered);

    //Highpass filtering
    palmarisFiltered = palmarisHigh.step(palmarisFiltered);

    //Rectification
    palmarisFiltered = fabs(palmarisFiltered);

    //caculate moving average
    for(int i=0; i<MOVAGLength-1; i++) {
        palmarisMOVAG[i]=palmarisMOVAG[i+1];
    }
    palmarisMOVAG[MOVAGLength-1]=palmarisFiltered;

    double palmarisSum;
    for(int i=0; i<MOVAGLength; i++) {
        palmarisSum+= palmarisMOVAG[i];
    }
    palmarisAvg= palmarisSum/MOVAGLength;
}
void filter()
{
    filterBiceps();
    filterTriceps();
    filterCarpiFlex();
    filterPalmaris();
}

//Code for motor angles as a function of the length and positions of the arms, for a double revolutional joint arm in 2D plane
void motorAngle()
{
//Function for making sure the arm does not exceed its maximum reach
//if it tries to go beyond its max. reach
//it will try to reach a point within reach in the same direction as desired.
//Works for all 4 quadrants of the (unit) circle
    xe = x_des;
    ye = y_des;

    //while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)){ //when attempted to go to a point out of reach
//       if (y_des == 0){    //make sure you do not divide by 0 if ye == 0
//           xe = x_des - 1;
//       } else {
//           xe = x_des - (x_des/y_des)/10; //go to a smaller xe point on the same line
//       }
//       if (x_des == 0){    //make sure you do not divide by 0 if xe == 0
//           ye = y_des - 1;
//       } else {
//           ye = y_des - (y_des/x_des)/10; //go to a smaller ye point on the same line
//       }
//       x_des = xe;
//       y_des = ye;
//   }


    // while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
//        if (xe > 0) {            //right hand plane
//            if (ye > 0) {        //positive x & y            QUADRANT 1
//                xe = x_des - (x_des/y_des);   //go to a smaller xe point on the same line
//                //(x_des/y_des) can be multiplied or divided to make bigger or smaller steps back if you're trying to exceed max. reach
//                ye = y_des - (y_des/x_des);   //go to a smaller ye point on the same line
//            } else if (ye < 0) { //positive x, negative y    QUADRANT 2
//                xe = x_des - (x_des/y_des);   //smaller xe
//                ye = y_des + (y_des/x_des);   //greater ye
//            } else if (ye == 0) { //positive x, y == 0
//                xe = x_des - 1;                  //stay on the x-axis but at a smaller value (within reach)
//            }
//        } else if (xe < 0) {     //left hand plane
//            if (ye > 0) {        //negative x, positive y    QUADRANT 4
//                xe = x_des + (x_des/y_des);   //greater xe
//                ye = y_des - (y_des/x_des);   //smaller ye
//            } else if (ye < 0) { //negative x & y            QUADRANT 3
//                xe = x_des + (x_des/y_des);   //greater xe
//                ye = y_des + (y_des/x_des);   //greater ye
//            } else if (ye ==0) { //negative x, y == 0
//                xe = x_des + 1;                  //stay on the x-axis but at a greater value (within reach)
//            }
//
//        } else if (xe == 0) {    //on the y-axis
//            if (ye > 0) {        //x == 0, positive y
//                ye = y_des - 1;                  //stay on the y-axis but at a smaller value (within reach)
//            } else if (ye < 0) { //x == 0, negative y
//                ye = y_des + 1;                  //stay on the y-axis but at a greater value (within reach)
//                //x == 0 & y == 0 is a state that is unable to be reached, no need to cover that case
//            }
//            x_des = xe;
//            y_des = ye;
//        }
//    }


//from here on is the code for setting the angles for the motors
    D = ((pow(l1,2)+pow(l2,2))-pow(xe,2)-pow(ye,2))/(2*l1*l2); //D = cos(phi)
    phi = atan2(sqrt(1 - pow(D, 2)), D); //angle between arm1 and arm2, from arm1 to arm2 [rad]
    //Use atan2(sqrt(1 - pow(D, 2)),D) for "elbow down" position (like your right arm)
    q2 = pi - phi; //angle of arm2 with respect to the orientation of arm1, motor2 [rad]
    if (-pi/2 > q2) {       //Make sure the angle of motor2 doesn’t wreck our setup (max -90 or 90 degrees w.r.t. arm1)
        q2 = 0;
    } else if ( q2 > pi/2) {
        q2 = pi/2;
    }
    beta = atan2(ye, xe); //angle between "line from origin to ee" and x-axis [rad]
    alpha = atan2(l2*sin(q2), l1+l2*cos(q2)); //angle between "line from origin to ee" and arm1 [rad]
    q1 = beta - alpha; //angle of arm 1 with respect to the x-axis, motor1 [rad]
    radDeg = 180/pi;
    rad2rot = radDeg/360;
    desiredAngle1 = q1 * rad2rot;
    desiredAngle2 = q2 * rad2rot;
}

void motorButtonController()
{
    double angle1= encoderToMotor*motor1Encoder.getPulses();
    double angleError1 = (desiredAngle1 - angle1)*1.25;

    //change direction based on error sign
    if(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
        motor1Dir=0;
    } else {
        motor1Dir =1;
    }
    //set motor speed based on PI controller error
    motor1 = fabs(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));

    double angle2= encoderToMotor*motor2Encoder.getPulses();
    double angleError2 = desiredAngle2 - angle2;

    //change direction based on error sign
    if(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
        motor2Dir=0;
    } else {
        motor2Dir =1;
    }
    //set motor speed based on PI controller error
    motor2 = fabs(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));

}

int main()
{
    wait(2);
    xe = x_des;
    ye = y_des;
    myControllerTicker.attach(&motorButtonController, controllerTickerTime);
    bit1=0;
    bit2=0;
    bit3=0;
    wait(5);
    for(int i=0; i<MOVAGLength; i++) {
        bicepsMOVAG[i]=0;
        tricepsMOVAG[i]=0;
        carpiFlexMOVAG[i]=0;
        palmarisMOVAG[i]=0;
    }
    filterTicker.attach(filter,tickerF);

    //start up
    state=bicepsCalib;
    stateChange=true;

    while(true) {
        if(palmarisAvg*palmarisMax > 0.9) {
            y_des -= positionIncrement;
            motorAngle();
            wait(0.3f);
        }

        if(carpiFlexAvg*carpiFlexMax > 0.9) {
            y_des += positionIncrement;
            motorAngle();
            wait(0.3f);
        }
        if(bicepsAvg*bicepsMulti > 0.9) {
            x_des -= positionIncrement;
            motorAngle();
            wait(0.3f);
        }
        if(tricepsAvg*tricepsMulti > 0.9) {
            x_des += positionIncrement;
            motorAngle();
            wait(0.3f);
        }

        if(stateChange) {
            switch(state) {
                case motorCalib  :
                    stateChange=false;
                    sendState(motorCalib);
                    break;
                case bicepsCalib  :
                    stateChange=false;
                    sendState(bicepsCalib);
                    wait(5);
                    flex.reset();
                    flex.start();
                    while(flex.read()<2) {
                        if(bicepsAvg>bicepsMax) {
                            bicepsMax=bicepsAvg;
                        }
                    }
                    bit1 = 0;
                    bit2 = 0;
                    bit3 = 0;
                    wait(5);
                    state=tricepsCalib;
                    stateChange=true;
                    break;
                case tricepsCalib  :
                    stateChange=false;
                    sendState(tricepsCalib);
                    wait(5);
                    flex.reset();
                    flex.start();
                    while(flex.read()<2) {
                        if(tricepsAvg>tricepsMax) {
                            tricepsMax=tricepsAvg;
                        }
                    }
                    bit1 = 0;
                    bit2 = 0;
                    bit3 = 0;
                    wait(5);
                    state=carpiCalib;
                    stateChange=true;
                    break;
                case carpiCalib  :
                    stateChange=false;
                    sendState(carpiCalib);
                    wait(5);
                    flex.reset();
                    flex.start();
                    while(flex.read()<2) {
                        if(carpiFlexAvg>carpiFlexMax) {
                            carpiFlexMax=carpiFlexAvg;
                        }
                    }
                    bit1 = 0;
                    bit2 = 0;
                    bit3 = 0;
                    wait(5);
                    state=palmarisCalib;
                    stateChange=true;
                    break;
                case palmarisCalib  :
                    stateChange=false;
                    sendState(palmarisCalib);
                    wait(5);
                    flex.reset();
                    flex.start();
                    while(flex.read()<2) {
                        if(palmarisAvg>palmarisMax) {
                            palmarisMax=palmarisAvg;
                        }
                    }
                    bit1 = 0;
                    bit2 = 0;
                    bit3 = 0;
                    wait(5);
                    state=EMGControl;
                    stateChange=true;
                    break;
                case demo  :
                    stateChange=false;
                    sendState(demo);
                    break;
                case EMGControl  :
                    stateChange=false;
                    sendState(EMGControl);
                    if(bicepsMax != 0) {
                        bicepsMulti=1/bicepsMax;
                    }
                    if(tricepsMax != 0) {
                        tricepsMulti=1/tricepsMax;
                    }
                    if(carpiFlexMax != 0) {
                        carpiFlexMulti=1/carpiFlexMax;
                    }
                    if(palmarisMax != 0) {
                        palmarisMulti=1/palmarisMax;
                    }
                    break;
            }
        }
    }
}