File content as of revision 12:12b72bd60fd1:

#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, MOVING, HITTING};
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 D10, ENC1B TO D11
QEI Encoder2(D12,D13,NC,32);                                                    // CONNECT ENC2A TO D12, ENC2B TO D13

// PINS ////////////////////////////////////////////////////////////////////////
DigitalOut motor1DirectionPin(D4);                                              // Value 0: CCW; 1: CW
PwmOut motor1MagnitudePin(D5);
DigitalOut motor2DirectionPin(D7);                                              // Value 0: CW or CCW?; 1: CW or 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 emg(A0);
AnalogIn potmeter1(A0);
AnalogIn potmeter2(A1);

// MOTOR CONTROL ///////////////////////////////////////////////////////////////
Ticker controllerTicker;
const double controller_Ts = 1/200.1;                                              // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
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]
double xVelocity = 0, yVelocity = 0;                                            // X and Y velocities of the end effector at the start
double velocity = 0.07;                                                          // X and Y velocity of the end effector when desired

// MOTOR 1
volatile double position1;                                                               // Position of motor 1 [rad]
volatile double reference1 = 0;                                                          // Desired rotation of motor 1 [rad]
double motor1Max = 0;                                                 // Maximum rotation of motor 1 [rad]
double motor1Min = 2*pi*-40/360;                                                           // Minimum rotation of motor 1 [rad]
// Controller gains
const double motor1_KP = 7;                                                     // Position gain []
const double motor1_KI = 9;                                                     // Integral gain []
const double motor1_KD = 1;                                                   // 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]
double motor2Max = 2*pi*30/360;                                                 // Maximum rotation of motor 2 [rad]
double motor2Min = 2*pi*-30/360;                                                // Minimum rotation of motor 2 [rad]
// Controller gains
const double motor2_KP = potmeter1*10;                                                     // Position gain []
const double motor2_KI = potmeter2*20;                                                     // Integral gain []
const double motor2_KD = 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 FILTER //////////////////////////////////////////////////////////////////
//BiQuadChain bqc;
//BiQuad bq1(3.6, 5.0, 2.0, 0.5, 30.0);                                                      // EMG filter coefficients []
//BiQuad bq2(0, 0, 0, 0, 0);                                                      // EMG filter coefficients []

Ticker sampleTicker;
const double tickerTs = 0.1;                                                           // Time step for sampling [s]


// 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_der = bq1.step(e_der);
    e_prev = e;
    // Integral
    e_int = e_int + Ts*e;                                                       // Calculate the integral of error
    // PID
    //pc.printf("%f --> %f \r\n", e_der, e_derf);
    return Kp*e + Ki*e_int + Kd*e_der;
}

// MOTOR 1 /////////////////////////////////////////////////////////////////////
void RotateMotor1(double motor1Value)
{
    if(currentState == MOVING || currentState == HITTING)                       // 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 || currentState == HITTING)                       // Check if state is MOVING
    {
        if(motor2Value >= 0) motor2DirectionPin = 1;                            // Rotate motor 1 CW
        else motor2DirectionPin = 0;                                            // Rotate motor 1 CCW
        
        if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
        else motor2MagnitudePin = fabs(motor2Value);
    }
    else motor2MagnitudePin = 0;
}

// MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
void Motor1Controller()
{
    position1 = radPerPulse * Encoder1.getPulses();
    position2 = radPerPulse * Encoder2.getPulses();
    //pc.printf("error %f \r\n", reference1 - position1);
    double motor1Value = PID_Controller(reference1 - position1, motor1_KP, 
     motor1_KI, motor1_KD, controller_Ts, 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);
    //pc.printf("motor value %f \r\n",motor1Value);
    RotateMotor1(motor1Value);
    double motor2Value = PID_Controller(reference2 - position2, motor2_KP, 
     motor2_KI, motor2_KD, controller_Ts, 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);
    RotateMotor2(motor2Value);
}

// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
/*void Motor2Controller()
{
    position2 = radPerPulse * Encoder2.getPulses();
    double motor2Value = PID_Controller(reference2 - position2, motor2_KP, 
     motor2_KI, motor2_KD, controller2_Ts, 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);
    RotateMotor2(motor2Value);
}
*/
// TURN OFF MOTORS /////////////////////////////////////////////////////////////
void TurnMotorsOff()
{
    motor1MagnitudePin = 0;
    motor2MagnitudePin = 0;
}

// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
void jointVelocities()
{
    position1 = radPerPulse * Encoder1.getPulses();
    position2 = radPerPulse * Encoder2.getPulses();
    
    if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity;
    else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity;
    else xVelocity = 0;

    if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity;
    else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity;
    else yVelocity = 0;
    
    //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
    
    // 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];                                                              // This is 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];
    
    if(reference1 + msh[0]*tickerTs > motor1Max) reference1 = motor1Max;
    else if(reference1 + msh[0]*tickerTs < motor1Min) reference1 = motor1Min;
    else reference1 = reference1 + msh[0]*tickerTs;
    
    if(reference2 + msh[1]*tickerTs > motor2Max) reference2 = motor2Max;
    else if(reference2 + msh[1]*tickerTs < motor2Min) reference2 = motor2Min;
    else reference2 = reference2 + msh[1]*tickerTs;
    
    //Motor1Controller(), Motor2Controller();
    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);
    //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
}

// EMG /////////////////////////////////////////////////////////////////////////
void EmgSample()
{
    if(currentState == MOVING)
    {
        //double emgFiltered = bqc.step(emg.read());                              // Filter the EMG signal
        /*
            If EMG signal 1 --> xVelocity / yVelocity = velocity
            Else if EMG signal 2 --> xVelocity / yVelocity = -velocity
            Else xVelocity / yVelocity = 0
            If both signal 1 and 2 --> Switch
        */
        jointVelocities();
    }
}

// STATES //////////////////////////////////////////////////////////////////////
void ProcessStateMachine()
{
    switch(currentState)
    {
        case MOTORS_OFF:
        {
            // State initialization
            if(stateChanged)
            {
                //pc.printf("Entering MOTORS_OFF \r\n"
                //"Press button 1 to enter MOVING \r\n");
                TurnMotorsOff();                                                // Turn motors off
                stateChanged = false;
            }
            
            // Home command
            if(!button1)
            {
                currentState = MOVING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        case MOVING:
        {
            // State initialization
            if(stateChanged)
            {
                //pc.printf("Entering MOVING \r\n"
                //"Press button 2 to enter HITTING \r\n");
                stateChanged = false;
            }
            
            // Hit command    
            /*if(!button2)
            {
                currentState = HITTING;
                stateChanged = true;
                break;
            }
            */
            break;
        }
        
        case HITTING:
        {
            // State initialization
            if(stateChanged)
            {
                //pc.printf("Entering HITTING \r\n");
                stateChanged = false;
                //HitBall();                                                    // Hit the ball
                currentState = MOVING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        default:
        {
            TurnMotorsOff();                                                    // Turn motors off for safety
            break;
        }
    }
}

// MAIN FUNCTION ///////////////////////////////////////////////////////////////
int main()
{
    // Serial communication
    pc.baud(115200);
    
    pc.printf("START \r\n");
    
    //bqc.add(&bq1).add(&bq2);                                                    // Make BiQuad Chain
    
    sampleTicker.attach(&EmgSample, tickerTs);                                 // Ticker to sample EMG
    controllerTicker.attach(&Motor1Controller, controller_Ts);                // Ticker to control motor 1 (PID)
    
    motor1MagnitudePin.period_ms(1);
    motor2MagnitudePin.period_ms(1);
    TurnMotorsOff();
    
    while(true)
    {
        ProcessStateMachine();                                                  // Execute states function
    }
}