File content as of revision 3:97cac1d5ba8a:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "MovingAverage.h"
#define NSAMPLE 200

// Inverse dingen
//InverseJacobian11 = (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
//InverseJacobian12 = -(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
//InverseJacobian21 = -(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
//InverseJacobian22 = (L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));



//states
enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo};

// Global variables
States CurrentState;
Ticker TickerLoop;
Timer TimerLoop;

//Communication
MODSERIAL pc(USBTX,USBRX);
QEI Encoder1(D10,D11,NC,32);    // Encoder motor q1 (arm)
QEI Encoder2(D12,D13,NC,32);    // Encoder motor q2 (end-effector)

//EMG settings
AnalogIn emg0(A0); //Biceps Left
AnalogIn emg1(A1); //Biceps Right
MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above 
MovingAverage <double>Movag_RB(NSAMPLE,0.0);
    
    // filters
//Make Biquad filters Left(a0, a1, a2, b1, b2) 
BiQuadChain bqc1; //chain voor High Pass en Notch
BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
//Make Biquad filters Right
BiQuadChain bqc2; //chain voor High Pass en Notch
BiQuad bq3(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter


//Global pin variables Motors 1
PwmOut PwmPin1(D5);
DigitalOut DirectionPin1(D4);

//Global pin variables Motors 2
PwmOut PwmPin2(D6);
DigitalOut DirectionPin2(D7);

//Output LED
DigitalOut LedRed (LED_RED);
DigitalOut LedGreen (LED_GREEN);
DigitalOut LedBlue (LED_BLUE);

// Buttons
DigitalIn ButtonHoming(SW2); // Button to go to Homing state
DigitalIn BUTSW3(SW3);
DigitalIn BUT1(D8);
DigitalIn BUT2(D9);

//Constant variables
const float L0 = 0.1; // Distance between origin frame and joint 1 [m[
const float L1 = 0.326; // Length link 1 (shaft to shaft) [m] 
const float L2 = 0.209; // Length link 2 (end-effector length) [m]

 
// Volatile variables
//EMG
volatile bool EMGBoolLeft; // Boolean EMG 1 (left)
volatile bool EMGBoolRight; // Boolean EMG 2 (right)
volatile double LeftBicepsOut; // Final value left of processed EMG
volatile double RightBicepsOut; // Final value right of processed EMG
volatile double ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1)
volatile double ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1)


//Motors
volatile float q1; // Current angle of motor 1 (arm)
volatile float q2; // Current angle of motor 2 (end-effector)
volatile float MotorValue1; // Inputvalue to set motor 1 
volatile float MotorValue2; // Inputvalue to set motor 2

//Inverse kinematics
volatile float VdesX; // Desired velocity in x direction
volatile float VdesY; // Desired velocity in y direction
volatile float Error1; // Difference in reference angle and current angle motor 1
volatile float Error2; // Difference in reference angle and current angle motor 2



//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

// Function for processing EMG
    void ProcessingEMG()
    {
    //Filter Left Biceps
    double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
    double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal 
    Movag_LB.Insert(LB_Rectify); //Moving Average
    LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
    
    //Filter Right Biceps
    double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
    double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
    Movag_RB.Insert(RB_Rectify); //Moving Average 
    RightBicepsOut = Movag_RB.GetAverage();  //Get final value
    
    if (LeftBicepsOut > ThresholdLeft) 
        {
            EMGBoolLeft = true;
        }
    else 
        {
            EMGBoolLeft = false;
        }
    if (RightBicepsOut > ThresholdRight) 
        {
            EMGBoolRight = true;
        }
    else 
        {
            EMGBoolRight = false;
        }
    
    } 

void MeasureAll()
{
    //Processing and reading EMG
    ProcessingEMG();

    //Measure current motor angles
    q1 = Encoder1.getPulses()/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
    q2 = Encoder2.getPulses()/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
    
}

//State machine
void StateMachine()
{
    switch (CurrentState)
    {
        case Waiting:
            LedRed = 0;
            LedGreen = 0;
            LedBlue = 1; //Yellow
        break;
        
        case Homing:
            LedRed = 0;
            LedGreen = 0;
            LedBlue = 0; //White
        break;  
        
        case Emergency:
            LedRed = 0;
            LedGreen = 1;
            LedBlue = 1; //Red
        break;
              
        case EMGCal:
            LedRed = 0;
            LedGreen = 1;
            LedBlue = 0; //Pink
        break;
        
        case MotorCal:
            LedRed = 1;
            LedGreen = 0;
            LedBlue = 0; //Turqoise
        break;
              
        case Operation:
            LedRed = 1;
            LedGreen = 0;
            LedBlue = 1; //Green
        break;
        
        case Demo:
            LedRed = 1;
            LedGreen = 1;
            LedBlue = 0; //Blue       
        break;
        

    }
}

//------------------------------------------------------------------------------
// Inverse Kinematics
void InverKinematics()
{

}


//------------------------------------------------------------------------------
// controller motor 1
 void PID_Controller1()
 {
   double Ts = 0.002; //Sampling time 500 Hz
   double Kp = 5.34; // proportional gain
   double Ki = 2.0;//integral gain
   double Kd = 6.9;  //derivative gain
   static double ErrorIntegral = 0;
   static double ErrorPrevious = Error1;
   static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
   
   //Proportional part:
   double u_k = Kp * Error1;
   
   //Integral part:
   ErrorIntegral = ErrorIntegral + Error1*Ts;
   double u_i = Ki * ErrorIntegral;
   
   //Derivative part:
   double ErrorDerivative = (Error1 - ErrorPrevious)/Ts;
   double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
   double u_d = Kd * FilteredErrorDerivative;
   ErrorPrevious = Error1;
   
   MotorValue1 =  u_k + u_i + u_d; //This will become the MotorValue to set motor 1
 }
   
// controller motor 2
 void PID_Controller2()
 {
   double Ts = 0.002; //Sampling time 500 Hz
   double Kp = 5.34; // proportional gain
   double Ki = 2.0;//integral gain
   double Kd = 6.9;  //derivative gain
   static double ErrorIntegral = 0;
   static double ErrorPrevious = Error2;
   static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
   
   //Proportional part:
   double u_k = Kp * Error2;
   
   //Integral part:
   ErrorIntegral = ErrorIntegral + Error2*Ts;
   double u_i = Ki * ErrorIntegral;
   
   //Derivative part:
   double ErrorDerivative = (Error2 - ErrorPrevious)/Ts;
   double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
   double u_d = Kd * FilteredErrorDerivative;
   ErrorPrevious = Error2;
   
   MotorValue2 =  u_k + u_i + u_d; //This will become the MotorValue to set motor 2
 }

// Main function for motorcontrol
void MotorControllers()
{   
    PID_Controller1();
    PID_Controller2();
    
}
//------------------------------------------------------------------------------

//Ticker function set motorvalues
void SetMotors()
{
// Motor 1
    if (MotorValue1 >=0) // set direction
    {
        DirectionPin1 = 1;
    }
    else
    {
        DirectionPin1 = 0;
    }
    
    if (fabs(MotorValue1)>1) // set duty cycle
    {
        PwmPin1 = 1; 
    }
    else
    {
        PwmPin1 = fabs(MotorValue1);
    }
    
// Motor 2
    if (MotorValue2 >=0) // set direction
    {
        DirectionPin2 = 1;
    }
    else
    {
        DirectionPin2 = 0;
    }
    
    if (fabs(MotorValue2)>1) // set duty cycle
    {
        PwmPin2 = 1; 
    }
    else
    {
        PwmPin2 = fabs(MotorValue2);
    }

}

void SetMotorsOff()
{
    // Turn motors off
    PwmPin1 = 0; 
    PwmPin2 = 0;
}

//-----------------------------------------------------------------------------


// Execution function
void LoopFunction()
{
    // buttons
   // if (Button1.read() == false) // if pressed
  //  {CurrentState = Operation; TimerLoop.reset();}
    
   // if (Button2.read() == false) // if pressed 
   // {CurrentState = Demo; TimerLoop.reset();}
    
   // if (ButtonHome.read() == false) // if pressed
  //  {CurrentState = Homing; TimerLoop.reset();}
    //functions
  //  StateMachine(); //determine states
  //  if (CurrentState >= 4)
   // {MeasureAndControl(); PrintToScreen();}
   // else 
   // {SetMotorOff();}
}

int main()
{
    pc.baud(115200);
    pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
    TimerLoop.start(); // start Timer
    CurrentState = Waiting; // set initial state as Waiting
    bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain EMG left
    bqc2.add( &bq3 ).add( &bq4 ); //make BiQuadChain EMG right
    TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
    
    while (true)
    {
    
    }
    
}