File content as of revision 18:2b6f41f39a7f:

#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include <math.h>
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
 
const double pi = 3.1415926535897;                                              // Definition of pi

// SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(4);
 
// STATES //////////////////////////////////////////////////////////////////////
enum states{MOTORS_OFF, CALIBRATING, MOVING};
states currentState = MOTORS_OFF;                                               // Start with motors off
bool stateChanged = true;                                                       // Make sure the initialization of first state is executed
 
// ENCODER /////////////////////////////////////////////////////////////////////
QEI Encoder1(D10,D11,NC,32);                                                    // CONNECT ENC1A TO D12, ENC1B TO D13
QEI Encoder2(D12,D13,NC,32);                                                    // CONNECT ENC2A TO D10, ENC2B TO D11
 
// PINS ////////////////////////////////////////////////////////////////////////
DigitalOut motor1DirectionPin(D4);                                              // Value 0: CCW; 1: CW
PwmOut motor1MagnitudePin(D5);
DigitalOut motor2DirectionPin(D7);                                              // Value 0: CW; 1: CCW
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2);                                                        // CONNECT BUT1 TO D2
InterruptIn button2(D3);                                                        // CONNECT BUT2 TO D3
InterruptIn button3(SW2);                                                       
InterruptIn button4(SW3);
AnalogIn potmeter1(A0);                                                         // CONNECT POT1 TO A0
AnalogIn potmeter2(A1);                                                         // CONNECT POT2 TO A1
DigitalOut led1(LED_RED);
DigitalOut led2(LED_BLUE);
DigitalOut led3(LED_GREEN);
DigitalOut led4(D8);                                                            // CONNECT LED1 TO D8
DigitalOut led5(D9);                                                            // CONNECT LED2 TO D9
AnalogIn emg_r(A2);                                                             // CONNECT EMG TO A2
AnalogIn emg_l(A3);                                                             // CONNECT EMG TO A3
 
// MOTOR CONTROL ///////////////////////////////////////////////////////////////
Ticker controllerTicker;                                                        // Ticker for the controller
const double controllerTs = 1/200.1;                                           // Time step for controllerTicker [s]
const double motorRatio = 131.25;                                               // Ratio of the gearbox in the motors []
const double radPerPulse = 2*pi/(32*motorRatio);                                // Amount of radians the motor rotates per encoder pulse [rad/pulse]
volatile double xVelocity = 0, yVelocity = 0;                                   // X and Y velocities of the end effector at the start
const double velocity = 0.03;                                                   // X and Y velocity of the end effector when desired
 
// MOTOR 1
volatile double position1;                                                      // Position of motor 1 [rad]
volatile double reference1 = 2*pi*-5/360;                                       // Desired rotation of motor 1 [rad]
const double motor1Max = 0;                                                     // Maximum rotation of motor 1 [rad]
const double motor1Min = 2*pi*-40/360;                                          // Minimum rotation of motor 1 [rad]
// Controller gains
const double motor1_KP = 13;                                                    // Position gain []
const double motor1_KI = 7;                                                     // Integral gain []
const double motor1_KD = 0.3;                                                   // Derivative gain []
double motor1_err_int = 0, motor1_prev_err = 0;
// Derivative filter coefficients
const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8;                             // Derivative filter coefficients []
const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8;               // Derivative filter coefficients []
// Filter variables
double motor1_f_v1 = 0, motor1_f_v2 = 0;
 
// MOTOR 2
volatile double position2;                                                      // Position of motor 2 [rad]
volatile double reference2 = 0;                                                 // Desired rotation of motor 2 [rad]
const double motor2Max = 2*pi*25/360;                                           // Maximum rotation of motor 2 [rad]
const double motor2Min = 2*pi*-28/360;                                          // Minimum rotation of motor 2 [rad]
// Controller gains
const double motor2_KP = 13;                                                    // Position gain []
const double motor2_KI = 5;                                                     // Integral gain []
const double motor2_KD = 0.1;                                                   // Derivative gain []
double motor2_err_int = 0, motor2_prev_err = 0;
// Derivative filter coefficients
const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8;                             // Derivative filter coefficients []
const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8;               // Derivative filter coefficients []
// Filter variables
double motor2_f_v1 = 0, motor2_f_v2 = 0;
 
// EMG /////////////////////////////////////////////////////////////////////////
Ticker emgLeft;                                                                 // Ticker for EMG of left arm
Ticker emgRight;                                                                // Ticker for EMG of right arm
const double emgTs = 0.491;                                                     // Time step for EMG sampling
// Filters
BiQuadChain bqc;                                                                
BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);              // High pass filter
BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);             // Notch filter
BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191);        // Low pass filter
// Right arm
volatile double emgFiltered_r;
volatile double filteredAbs_r;
volatile double emg_value_r;
volatile double onoffsignal_r;
volatile bool check_calibration_r = false;
volatile double avg_emg_r;
volatile bool active_r = false;
// Left arm 
volatile double emgFiltered_l;
volatile double filteredAbs_l;
volatile double emg_value_l;
volatile double onoffsignal_l;
volatile bool check_calibration_l = false;
volatile double avg_emg_l;
volatile bool active_l = false;

// PROCESS EMG SIGNALS ///////////////////////////////////////////////////////// 
Ticker processTicker;                                                           // Ticker for processing of EMG
const double processTs = 0.1;                                                   // Time step for processing of EMG

volatile bool xdir = true, ydir = false;                                        // Direction the EMG signal moves the end effector
volatile int count = 0;                                                         // Counter to change direction
 
// FUNCTIONS ///////////////////////////////////////////////////////////////////
// BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
double biquad(double u, double &v1, double &v2, const double a1,
 const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1; v1 = v;
    return y;
}
 
// PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
double PID_Controller(double e, const double Kp, const double Ki,
 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
 const double f_b1, const double f_b2)
{
    // Derivative
    double e_der = (e - e_prev)/Ts;                                             // Calculate the derivative of error
    e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);            // Filter the derivative of error
    e_prev = e;
    // Integral
    e_int = e_int + Ts*e;                                                       // Calculate the integral of error
    // PID
    return Kp*e + Ki*e_int + Kd*e_der;                                          // Calculate motor value
}
 
// MOTOR 1 /////////////////////////////////////////////////////////////////////
void RotateMotor1(double motor1Value)
{
    if(currentState == MOVING)                                                  // Check if state is MOVING
    {
        if(motor1Value >= 0) motor1DirectionPin = 0;                            // Rotate motor 1 CW
        else motor1DirectionPin = 1;                                            // Rotate motor 1 CCW
        
        if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motor1Value);
    }
    else motor1MagnitudePin = 0;
}
 
// MOTOR 2 /////////////////////////////////////////////////////////////////////
void RotateMotor2(double motor2Value)
{
    if(currentState == MOVING)                                                  // Check if state is MOVING
    {
        if(motor2Value >= 0) motor2DirectionPin = 1;                            // Rotate motor 2 CW
        else motor2DirectionPin = 0;                                            // Rotate motor 2 CCW
        
        if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
        else motor2MagnitudePin = fabs(motor2Value);
    }
    else motor2MagnitudePin = 0;
}
 
// MOTOR PID-CONTROLLER //////////////////////////////////////////////////////
void MotorController()
{
    if(currentState == MOVING)
    {
        position1 = radPerPulse * Encoder1.getPulses();                         // Get current position of motor 1
        position2 = radPerPulse * Encoder2.getPulses();                         // Get current position of motor 2
        
        double motor1Value = PID_Controller(reference1 - position1, motor1_KP,  // Calculate motor value motor 1
         motor1_KI, motor1_KD, controllerTs, motor1_err_int, motor1_prev_err,
         motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
         motor1_f_b1, motor1_f_b2);
        
        double motor2Value = PID_Controller(reference2 - position2, motor2_KP,  // Calculate motor value motor 2
         motor2_KI, motor2_KD, controllerTs, motor2_err_int, motor2_prev_err,
         motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
         motor2_f_b1, motor2_f_b2);
        
        RotateMotor1(motor1Value);                                              // Rotate motor 1
        RotateMotor2(motor2Value);                                              // Rotate motor 2
    }
}

// TURN OFF MOTORS /////////////////////////////////////////////////////////////
void TurnMotorsOff()
{
    motor1MagnitudePin = 0;
    motor2MagnitudePin = 0;
}
 
// EMG /////////////////////////////////////////////////////////////////////////
// Filter EMG signal of right arm
void filter_r()
{                                      
    if(check_calibration_r == 1)
    {
        emg_value_r = emg_r.read();                                             // Get EMG signal
        emgFiltered_r = bqc.step(emg_value_r);                                  // Filter EMG signal using BiQuad Chain
        filteredAbs_r = abs(emgFiltered_r);                                     // Takes absolute value
        
        if (avg_emg_r != 0)
        {
            onoffsignal_r = filteredAbs_r/avg_emg_r;                            // Divide the emg_r signal by the max emg_r to calibrate the signal per person
        }
    }
}

// Filter EMG signal of left arm 
void filter_l()
{                                      
    if(check_calibration_l == 1)
    {
        emg_value_l = emg_l.read();
        emgFiltered_l = bqc.step(emg_value_l);
        filteredAbs_l = abs( emgFiltered_l );
        if (avg_emg_l != 0)
        {
            onoffsignal_l = filteredAbs_l/avg_emg_l;                            
        }
    }
}

// Check threshold right arm
void check_emg_r()
{           
    double filteredAbs_temp_r;
          
    if((check_calibration_l == 1) && (check_calibration_r == 1))
    { 
        for(int i = 0; i<250; i++)
        {
            filter_r();
            filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
            wait(0.0004);
        }    
        filteredAbs_temp_r = filteredAbs_temp_r/250;                                 
        if(filteredAbs_temp_r<=0.3)                                             //if signal is lower then 0.5 the blue light goes on
        {                                   
            led1.write(1);                                                      //led 1 is rood en uit
            active_r = false;
        }
        else if(filteredAbs_temp_r > 0.3)                                       //if signal does not pass threshold value, blue light goes on
        {                
            led1.write(0);
            active_r = true;
        }
    }
}

// Check threshold left arm
void check_emg_l()
{        
    double filteredAbs_temp_l;
          
    if((check_calibration_l == 1) && (check_calibration_r == 1))
    { 
        for( int i = 0; i<250; i++)
        {
            filter_l();
            filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
            wait(0.0004);
        }    
        filteredAbs_temp_l = filteredAbs_temp_l/250;                                 
        if(filteredAbs_temp_l <= 0.3)                                           //if signal is lower then 0.5 the blue light goes on
        {
            led2.write(1);          
            active_l = false;
        }
        else if(filteredAbs_temp_l > 0.3)                                       //if signal does not pass threshold value, blue light goes on
        {
            led2.write(0);
            active_l = true;
        }
    }
}
 
// Calibrate right arm
void calibration_r()
{
    led3.write(0);
       
    double signal_collection_r = 0;
    for(int n =0; n < 5000; n++)                                                //read for 5000 samples as calibration
    {   
        filter_r();      
        emg_value_r = emg_r.read();
        emgFiltered_r = bqc.step( emg_value_r );
        filteredAbs_r = abs(emgFiltered_r);
         
        signal_collection_r = signal_collection_r + filteredAbs_r ;
        wait(0.0004);
             
        if (n == 4999)
        {
            avg_emg_r = signal_collection_r / n;
        }
    }  
    led3.write(1);
    check_calibration_r = 1;
}
 
// Calibrate left arm
void calibration_l()
{
    led3.write(0);
       
    double signal_collection_l = 0;
    for(int n = 0; n < 5000; n++)                                               //read for 5000 samples as calibration
    {   
        filter_l();
        emg_value_l = emg_l.read();
        emgFiltered_l = bqc.step(emg_value_l);
        filteredAbs_l = abs(emgFiltered_l);
        signal_collection_l = signal_collection_l + filteredAbs_l ;
        wait(0.0004);
             
        if (n == 4999)
        {
            avg_emg_l = signal_collection_l / n;       
        }
    }  
    led3.write(1);
    wait(1);
    check_calibration_l = 1;
}

// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
void JointVelocities()
{
    if(currentState == MOVING)
    {
        //position1 = radPerPulse * Encoder1.getPulses();
        //position2 = radPerPulse * Encoder2.getPulses();
        
        if(active_l && active_r)                                                // If both left and right EMG are active for 1 second the direction of control changes
            {
                count += 1;
                if(count == 20)
                {
                    active_l = false;
                    active_r = false;
                    xdir = !xdir;
                    ydir = !ydir;
                    led4 = !led4;
                    led5 = !led5;
                    xVelocity = 0;
                    yVelocity = 0;
                }
            }
            else count = 0;
        
        if(xdir)                                                                // Control in x-direction
        {
            if(active_r && count == 0 &&                                        // Checks whether EMG is active, changing direction and max rotation of motors
             reference1 > motor1Min && reference2 < motor2Max) 
            {
                xVelocity = velocity;                                           // Give velocity to end effector
            }
            else if(active_l && count == 0 &&
             reference1 < motor1Max && reference2 > motor2Min)
            {
                xVelocity = -velocity;
            }
            else xVelocity = 0;
        }
        else if(ydir)                                                           // Control in y-direction
        {
            if(active_r && count == 0 &&
             reference2 < motor2Max ) //&& reference1 > motor2Min)
            {
                yVelocity = velocity;
            }
            else if(active_l && count == 0
             && reference2 > motor2Min ) //&& reference1 > motor2Min)
            {
                yVelocity = -velocity;
            }
            else yVelocity = 0;
        }
        
        // Construct Jacobian
        double q[4];
        q[0] = position1, q[1] = -position1;
        q[2] = position2, q[3] = -position2;
        
        double T2[3];                                                           // Second column of the jacobian
        double T3[3];                                                           // Third column of the jacobian
        double T4[3];                                                           // Fourth column of the jacobian
        double T1[6];
        static const signed char b_T1[3] = { 1, 0, 0 };
        double J_data[6];
        
        T2[0] = 1.0;
        T2[1] = 0.365 * cos(q[0]);
        T2[2] = 0.365 * sin(q[0]);
        T3[0] = 1.0;
        T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * 
         sin((0.21406068356382149 + q[0]) + q[1]);
        T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 *
         cos((0.21406068356382149 + q[0]) + q[1]);
        T4[0] = 1.0;
        T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 *
         sin((0.21406068356382149 + q[0]) + q[1])) +
         0.265 * sin((q[0] + q[1]) + q[2]);
        T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 *
         cos((0.21406068356382149 + q[0]) + q[1])) - 0.265 *
         cos((q[0] + q[1]) + q[2]);
         
        for (int i = 0; i < 3; i++)
        {
            T1[i] = (double)b_T1[i] - T2[i];
            T1[3 + i] = T3[i] - T4[i];
        }
        
        for (int i = 0; i < 2; i++) 
        {
            for (int j = 0; j < 3; j++)
            {
                J_data[j + 3 * i] = T1[j + 3 * i];
            }
        }
        
        // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
        // Note: the matrices from now on will we constructed rowwise
        double Jvelocity[4];
        Jvelocity[0] = J_data[1];
        Jvelocity[1] = J_data[4];
        Jvelocity[2] = J_data[2];
        Jvelocity[3] = J_data[5];
        
        // Creating the inverse Jacobian
        double Jvelocity_inv[4];                                                // The inverse matrix of the jacobian
        double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2];    // The determinant of the matrix
        Jvelocity_inv[0] = Jvelocity[3]/determ;
        Jvelocity_inv[1] = -Jvelocity[1]/determ;
        Jvelocity_inv[2] = -Jvelocity[2]/determ;
        Jvelocity_inv[3] = Jvelocity[0]/determ;
        
        // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
        double msh[2];                                                          // The velocity the joints have to have
        msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
        msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
        
        // Determine reference position of motor 1
        if(reference1 + msh[0]*processTs > motor1Max) reference1 = motor1Max;
        else if(reference1 + msh[0]*processTs < motor1Min) reference1 = motor1Min;
        else reference1 = reference1 + msh[0]*processTs;
        
        // Determine reference position of motor 2
        if(reference2 + msh[1]*processTs > motor2Max) reference2 = motor2Max;
        else if(reference2 + msh[1]*processTs < motor2Min) reference2 = motor2Min;
        else reference2 = reference2 + msh[1]*processTs;
        
        /*scope.set(0,reference1);
        scope.set(1,position1);
        scope.set(2,reference2);
        scope.set(3,position2);
        scope.send();*/
        
        pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
        pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
    }
}
 
// STATES //////////////////////////////////////////////////////////////////////
void ProcessStateMachine()
{
    switch(currentState)
    {
        case MOTORS_OFF:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOTORS_OFF \r\n"
                "Press button 1 to enter CALIBRATING \r\n");
                TurnMotorsOff();                                                // Turn motors off
                stateChanged = false;
            }
            
            // Continue button
            if(!button1)
            {
                currentState = CALIBRATING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        case CALIBRATING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering CALIBRATING \r\n"
                "Tighten muscles until green LED is off \r\n");
                stateChanged = false;
                calibration_r();                                                // Calibrate right arm
                calibration_l();                                                // Calibrate left arm
                currentState = MOVING;
                stateChanged = true;
            }
            break;
        }
        
        case MOVING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOVING \r\n");
                stateChanged = false;
            }
            break;
        }
        
        default:
        {
            TurnMotorsOff();                                                    // Turn motors off for safety
            break;
        }
    }
}
 
// MAIN FUNCTION ///////////////////////////////////////////////////////////////
int main()
{
    // Serial communication
    pc.baud(115200);
    
    // Start values of LEDs
    led1.write(1);
    led2.write(1);
    led3.write(1);
    led4.write(1);
    led5.write(0);
    
    pc.printf("START \r\n");
    
    bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch );                        // Make BiQuad Chain
    
    processTicker.attach(&JointVelocities, processTs);                          // Ticker to process EMG
    controllerTicker.attach(&MotorController, controllerTs);                   // Ticker to control motor 1 (PID)
    emgRight.attach(&check_emg_r, emgTs);                                       // Ticker to sample EMG of right arm
    emgLeft.attach(&check_emg_l, emgTs);                                        // Ticker to sample EMG of left arm
    
    motor1MagnitudePin.period_ms(1);                                            // PWM frequency of motor 1 (Should actually be 5 - 10 kHz)
    motor2MagnitudePin.period_ms(1);                                            // PWM frequency of motor 2 (Should actually be 5 - 10 kHz)
    
    while(true)
    {
        ProcessStateMachine();                                                  // Execute states function
    }
}