File content as of revision 13:acd120520e80:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "math.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "EMG_filter.h"
#include "Matrix.h"
#include "MatrixMath.h"

const double pi = 3.14159265359;            // Defining the constant pi

MODSERIAL pc(USBTX,USBRX);                                  // Enabling communication with the pc.

enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};           // Defining states of the robot
r_states state;

// ---- EMG parameters ----
//HIDScope scope (2);
EMG_filter emg1(A0);
EMG_filter emg2(A1);
float EMG_threshold = 0.2;                                  // Threshold for the EMG signal
// ------------------------

// ---- Motor input and outputs ----
PwmOut speed1(D5);
PwmOut speed2(D6);
PwmOut speedservo1(D11);
//PwmOut speedservo2(D8);
DigitalOut dir1(D4);
DigitalOut dir2(D7);
DigitalIn press(PTA4);
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);
DigitalOut ledstateswitch(D3);
DigitalOut ledstatedef(D2);
AnalogIn pot(A2);
AnalogIn pot2(A3);
Encoder motor1(PTD0,PTC4);
Encoder motor2(D12,D13);
// ----------------------------------

// --- Define Ticker and Timeout ---
Ticker mainticker;          //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
Timeout calibrationgo;      // Timeout that will determine the duration of the calibration.
// ---------------------------------


float PwmPeriod = 0.0001f;

volatile double Pos1;                        // Encoder readout of motor 1 [counts]
volatile double Pos2;                        // Encoder readout of motor 2 [counts]
volatile int count = 0;                         // Loop counts 

double Kp = 250;// you can set these constants however you like depending on trial & error
double Ki = 100;
double Kd = 0;

float last_error = 0;
float error1 = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;

float last_error2 = 0;
float error2 = 0;
float changeError2 = 0;
float totalError2 = 0;
float pidTerm2 = 0;
float pidTerm_scaled2 = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|

volatile double potvalue = 0.0;
volatile double potvalue2 = 0.0;
volatile double position = 0.0;
volatile double position2 = 0.0;

// --- Kinematics parameter ---
float L1 = 0.38;                // length of link 1 [m]
float L5 = 0.25;                // length of link 5 [m]
float d = 0.03;                 // diameter of the pulley [m]

volatile float xe;              // x-coordinate of end-effector
volatile float ye;              // y-coordinate of end-effector
volatile float ze;              // z-coordinate of end-effector

volatile float Q1;              // Position of joint 1 [m]
volatile float Q2;              // Position of joint 2 [rad]
volatile float Q3;              // Position of joint 3 [rad]

volatile float V1;              // Desired velocity of joint 1 [m/s];  --> or normalize it?
volatile float W2;              // Desired velocity of joint 2 [rad/s];
volatile float W3;              // Desired velocity of joint 3 [rad/s];

volatile float Vx_des;          // Desired velocity end-effector in x-direction [m/s]
volatile float Vy_des;          // Desired velocity end-effector in y-direction [m/s]
volatile float Vz_des;          // Desired velocity end-effector in z-direction [m/s]

const float T = 0.002;          // Time interval of the readout of the EMG --> Check this with the ticker, etc!!!


// --- Booleans for the maintickerfunction ---
//bool readoutsetpoint = true;
bool go_EMG;                    // Boolean to put on/off the EMG readout
bool go_calibration;            // Boolean to put on/off the calibration of the EMG
bool go_switch;                  // Boolean to go to different state
bool go_PID;                    // Boolean to calculate PID and motor aansturing --> probably replaced by go_move
bool go_move;                   //Boolean to move the robot arm and base
bool go_kinematics;               // Boolean which will determine whether a new position will be calculated or not --> replace by go_move?
// -------------------------------------------

// --- calibrate EMG signal ----

void calibrationGO()                   // Function for go or no go of calibration
{
    go_calibration = false;

}

/*
Calibration of the robot works as follows:
EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
During the calibration the user should exert maximum force.
*/
void calibrationEMG()                   // Function for calibrating the EMG signal
{
    if (go_calibration) {

        emg1.calibration();                 // Using the calibration function of the EMG_filter class
        emg2.calibration();
    }
}

// ------------------------------
//Function that reads out the raw EMG and filters the signal
void processEMG ()
{


    emg1.filter();                      //filter the incoming emg signal
    emg2.filter();
    //emg3.filter();

    /*  scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope
      scope.set(1, emg1.emg0);
      scope.send();*/

}

// --- Kinematics functions ---

void Brockett(float q1, float q2, float q3)                    // Determine position of end-effector using Brockett
{

    Matrix E100b(4,4);           // Complete matrix form of 1st joint
    E100b = MatrixMath::Eye(4);
    E100b(3,4) = q1;


    Matrix E210b(4,4);              // Complete matrix form of 2nd joint
    E210b = MatrixMath::RotZ(q2);


    Matrix E320b(4,4);                  // Complete matrix form of 3rd joint
    E320b = MatrixMath::RotZ(q3-q2);
    E320b(1,4) = L1 - L1*cos(q3-q2);
    E320b(2,4) = -L1*sin(q3-q2);


    Matrix HE0ref(4,4);                 // H-matrix of the robot in reference position
    HE0ref = MatrixMath::Eye(4);
    HE0ref(1,4) = L1;
    HE0ref(2,4) = -L5;


    Matrix HE0(4,4);                    // H-matrix to find the position of the end-effector, expressed in frame 0.
    HE0 = E100b * E210b * E320b * HE0ref;       // This H-matrix is not really correct. Check with matlab!!!


    // Determine new position of end effector
    xe = HE0(1,4);                      // New x-coordinate of the end-effector
    ye = HE0(2,4);                      // New y-coordinate of the end-effector
    ze = HE0(3,4);                      // New z-coordinate of the end-effector


}

void Jacobian(float vx_des, float vy_des, float vz_des)                                 // Function to determine the velocities with desired velocity as input
{
    //kinematics_go = false;        // putting boolean to false

// Finding joint positions
    Q1 = Pos1;                            // Position of joint 1 [m]
    Q2 = Pos2;                                // Position of joint 2 [rad]
    Q3 = 0;                                                             // Position of joint 3 [rad]      --> need this from Servo!

    //pc.printf("\r\nq1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1, q2, q3);

    Brockett(Q1,Q2,Q3);                          // Start with conducting Brockett to obtain the end-effector position

    Matrix T_des(3,1);                  // Twist of desired linear velocities
    T_des(1,1) = vx_des;
    T_des(2,1) = vy_des;
    T_des(3,1) = vz_des;


    Matrix T100j(1,4);                  // Unit twist of the first frame in current configuration
    T100j(1,4) = 1;

    Matrix T210j(1,4);                  // Unit twist of the second frame in current configuration
    T210j(1,1) = 1;

    Matrix T320j(1,4);                  // Unit twist of the third frame in current configuration
    T320j(1,1) = 1;
    T320j(1,2) = -L1*sin(Q2);
    T320j(1,3) = -L1 * cos(Q2);

    Matrix J(4,3);                      // Jacobian matrix

    for (int i = 1 ; i < 5 ; i++) {
        J(i,1) = T100j(1,i);
    }

    for (int i = 1 ; i < 5 ; i++) {
        J(i,2) = T210j(1,i);
    }

    for (int i = 1 ; i < 5 ; i++) {
        J(i,3) = T320j(1,i);
    }


    Matrix H0f(4,4);                        // H-matrix to transform Jacobian from frame 0 to frame f, which is attached to the end-effector, but not rotating with it.
    H0f = MatrixMath::Eye(4);
    H0f(1,4) = -xe;
    H0f(2,4) = -ye;
    H0f(3,4) = -ze;

    Matrix AdH0f(4,4);                      // Adjoint of H0f, necessary for twist transformations
    AdH0f = MatrixMath::Eye(4);
    AdH0f(2,1) = H0f(2,4);
    AdH0f(3,1) = -H0f(1,4);

    Matrix Jprime(4,3);                     // Transformed Jacobian
    Jprime =  AdH0f*J;

    Matrix Jprimeprime(3,3);                // Truncated Jacobian
    for (int i = 1 ; i <4 ; i++) {
        for (int j = 1 ; j < 4 ; j++) {
            Jprimeprime(i,j) = Jprime(i+1,j);
        }
    }

    Matrix JprimeprimeT(3,3);                   // The transpose of the truncated Jacobian
    JprimeprimeT = MatrixMath::Transpose(Jprimeprime);

    Matrix PseudoInverse(3,3);                  // The Pseudo-inverse of the truncated Jacobian
    PseudoInverse = MatrixMath::Inv(JprimeprimeT*Jprimeprime)*JprimeprimeT;

    Matrix Q_dot(1,3);                          // Desired joint velocities
    Q_dot = PseudoInverse * T_des;

    V1 = Q_dot(1,1);
    W2 = Q_dot(2,1);
    W3 = Q_dot(3,1);

    // pc.printf("\r\nv1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);

// Eva - not sure if we need this... --> I think for the setpoint of the PID controller
    Q1 += V1*T;                     // Predicted value for q1 [m]
    Q2 += W2*T;                     // Predicted value for q2 [rad]
    Q3 += W3*T;                    // Predicted value for q3 [rad]

    // pc.printf("\r\n q1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1,q2,q3);

}

//--- State switch function -----
void r_processStateSwitch()
{
    go_switch = false;
    switch(state) {
        case R_HORIZONTAL:
            state = R_VERTICAL;
            ledblue = 1;
            ledred = 1;
            ledgreen = 0;
            pc.printf("state is vertical");
            break;
        case R_VERTICAL:
            state = R_BASE;
            ledgreen = 1;
            ledblue = 1;
            ledred = 0;
            pc.printf("state is base");
            break;
        case R_BASE:
            state = R_HORIZONTAL;
            ledgreen = 1;
            ledred = 1;
            ledblue = 0;
            pc.printf("state is horizontal");
            break;
    }
    wait(1.0f);                             // waits 1 second to continue with the main function. I think ticker does run, but main is on hold.
    ledstatedef = 1;
    ledstateswitch = 0;
    go_move = true;
}

// --- Determine the setpoints of the joint velocities
void setpointreadout(float v_des)
{
    /*
    potvalue = pot.read();
    setpoint = emg1.normalized;

    potvalue2 = pot2.read();
    setpoint2 = emg2.normalized;
    */

    Pos1 = motor1.getPosition()/16800*d*pi;                 // Position of link 1 [m]
    Pos2 = motor2.getPosition()/16800*2*pi;                 // Position of link 2 [m]

    if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
        v_des = emg1.normalized;
    }

    if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Negative direction
        v_des = -emg2.normalized;
    }

//pc.printf("\r\nv_des in setpoint function= %f\r\n", v_des);
}
void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
{

    //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
    //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);


    error1 = q1 - Pos1;                             // Position error of link 1 [m]
    error2 = q2 - Pos2;                                  // Position error of link 2 [m]

    changeError = (error1 - last_error)/0.001f; // derivative term
    totalError += error1*0.001f; //accumalate errors to find integral term
    pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
    pidTerm = pidTerm;
    if (pidTerm >= 1000) {
        pidTerm = 1000;
    } else if (pidTerm <= -1000) {
        pidTerm = -1000;
    } else {
        pidTerm = pidTerm;
    }
    //constraining to appropriate value
    if (pidTerm >= 0) {
        dir1 = 0;// Forward motion
    } else {
        dir1 = 1;//Reverse motion
    }
    pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
    if(pidTerm_scaled >= 0.55f) {
        pidTerm_scaled = 0.55f;
    }

    changeError2 = (error2 - last_error2)/0.001f; // derivative term
    totalError2 += error2*0.001f; //accumalate errors to find integral term
    pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
    pidTerm2 = pidTerm2;
    if (pidTerm2 >= 1000) {
        pidTerm2 = 1000;
    } else if (pidTerm2 <= -1000) {
        pidTerm2 = -1000;
    } else {
        pidTerm2 = pidTerm2;
    }
    //constraining to appropriate value
    if (pidTerm2 >= 0) {
        dir2 = 1;// Forward motion
    } else {
        dir2 = 0;//Reverse motion
    }
    pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
    if(pidTerm_scaled2 >= 0.55f) {
        pidTerm_scaled2 = 0.55f;
    }

    last_error = error1;
    speed1 = pidTerm_scaled+0.45f;
    //speedservo1 = speed1;
    last_error2 = error2;
    speed2 = pidTerm_scaled2+0.45f;
    //speedservo2 = speed2;
}

// --- Motor movements ---
void r_movehorizontal()
{
    setpointreadout(Vx_des);                       // Start with obtaining the position of the robot and the desired velocities
    //pc.printf("\r\n Vx_des outside setpoint function = %f\r\n", Vx_des);
    Jacobian(Vx_des, 0, 0);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
    PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.

}

void r_movevertical()
{
    setpointreadout(Vy_des);                       // Start with obtaining the position of the robot and the desired velocities
    //pc.printf("\r\n Vy_des outside setpoint function = %f\r\n", Vy_des);
    Jacobian(0, Vy_des, 0);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
    PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.

}

void r_movebase()
{

    setpointreadout(Vz_des);                       // Start with obtaining the position of the robot and the desired velocities
    //pc.printf("\r\n Vz_des outside setpoint function = %f\r\n", Vz_des);
    Jacobian(0, 0, Vz_des);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
    PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.

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


void maintickerfunction()
{
    count++;
    if (go_switch) {
        r_processStateSwitch();
    }

    if(go_EMG && count%2 == 0) {
        processEMG();
    }
    
    if (go_move) {
            switch(state) {
                case R_HORIZONTAL:
                    r_movehorizontal();
                    break;
                case R_VERTICAL:
                    r_movevertical();
                    break;
                case R_BASE:
                    r_movebase();
                    break;
            }
        }

    /*if(emg1.normalized >=0.2 && emg2.normalized >= 0.2) // PIDcalculation should not happen.
    {
        go_PID = false;
    }
    else{go_PID = true;}*/


}

int main()
{
    pc.baud(9600);
    go_EMG = true;                      // Setting ticker variables
    go_calibration = true;              // Setting the timeout variable
    go_PID = true;
    go_switch = false;
    go_move = true;
    speed1.period(PwmPeriod);
    speed2.period(PwmPeriod);

    calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
    mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
    wait(5.0f);
    mainticker.attach(&maintickerfunction,0.001f);

    while(true) {

        ledstatedef = 1;
        if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
            ledstateswitch = 1;
            ledstatedef = 0;
            go_switch = true;
            go_move = false;

        }

        

        if(count == 100) {
            count = 0;
            pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
            //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
            //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
        }


        wait(0.001f);
    }

}