File content as of revision 9:7443c37f0f7b:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "Matrix.h"
#include "QEI.h"
#include <math.h>
MODSERIAL pc(USBTX, USBRX);
DigitalOut motor1Direction(D7);
DigitalOut motor2Direction(D4);
DigitalOut Led(LED_GREEN);
DigitalIn button2(SW3);
FastPWM motor1PWM(D6);
FastPWM motor2PWM(D5);
AnalogIn potMeter1(A4);
AnalogIn potMeter2(A5);
QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);

// Tickers
Ticker potmeterTicker;
Ticker measurecontrolTicker;
Ticker printTicker;
//------------------------------------------------------------------------------
//Global Variables

volatile float currentPosition1 = 0.1;          // Starting position of motor 1 [rad]
volatile float currentPosition2 = -(0.5*3.14);  // Starting position of motor 2 wrt motor 1 [rad]

//Inverse Kinematics
const float L0 = 0.1;              // Horizontal length from base to first joint [m]
const float L1 = 0.3;              // Length link 1 [m]
const float L2 = 0.3;              // Length link 2 [m]
const float L3 = 0.1;              // Vertical length from base to first joint [m]
volatile float q1 = 0.1;           // Starting reference angle of first link [rad]
volatile float q2 = -(0.5*3.12);   // Starting reference angle of second link wrt first link [rad]

float t = 0.02;                     // Time interval [s]
volatile float vx_des = 0.0;       // Starting velocity in x-direction [rad/s]
volatile float vy_des = 0.0;       // Starting velocity in y-direction [rad/s]
volatile float errorIK1 = 0.0;
volatile float errorIK2 = 0.0;
volatile float J1 = 0.0;
volatile float J2 = 0.0;
volatile float J3 = 0.0;
volatile float J4 = 0.0;
volatile float V1 = 0.0;
volatile float V2 = 0.0;
volatile float Q1 = 0.0;
volatile float Q2 = 0.0;

// Motor Control
volatile float potMeterPosition1 = 0.0;
int counts1i = 0;
int counts2i = 0;
//volatile float potMeterPosition2 = 0.0;
volatile float motorValue1 = 0.01;
volatile float motorValue2 = 0.01;
volatile float Kp = 0.34;
volatile float Ki = 0.0;
volatile float Kd = 0.0;
volatile float Ts = 0.02;


//------------------------------------------------------------------------------
// Potmeter values TO DETERMINE VX_DES, VY_DES
void GetPotMeterVelocity1()  //Potmeter standard to control X-DIRECTION
{
    float potMeter1In = potMeter1.read();
    vx_des = 4.0*3.14*potMeter1In - 2.0*3.14;
}

float SwitchPotmeterVelocity1()    //Button has been pressed,
{
    vy_des = vx_des;
    return vy_des;
}

//------------------------------------------------------------------------------
// KINEMATIC FUNCTIONS

void ComputeJ(void)
{
    J1 = -sin(q1)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
    J2 = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
    J3 = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
    J4 = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
}

void ComputeV(void)
{
    V1 = vx_des;
    V2 = vy_des;
}

float IntegrateQ1()
{
    q1  = q1 + (Q1*t);    // new value for q1
    return q1;
}

float IntegrateQ2()
{
    q2 = q2 + (Q2*t);    // new value for q2
    return q2;
}

void ComputeQ_set()
{
    Q1 = (J1*V1) + (J2*V2);
    Q2 = (J3*V1) + (J4*V2);
}

float ErrorInverseKinematics1()
{
    float errorIK1 = q1 - currentPosition1;
    return errorIK1;
}

float ErrorInverseKinematics2()
{
    float errorIK2 = q2 - currentPosition2;
    return errorIK2;
}
//------------------------------------------------------------------------------
// MOTOR PART
// Failure function
void TurnMotorsOff()                                   // Turn motors off immediately
{
    motor1PWM = 0;
    motor2PWM = 0;
}

//Encoder Posities
void MeasureEncoderPosition1()        // Read current position of motor 1, returns value in [rad]
{
    counts1i = Encoder2.getPulses();
    float counts1 = counts1i*1.0f;
    currentPosition1 = (counts1/8400)*6.28; //Rotational position in radians
}

void MeasureEncoderPosition2()        // Read current postion of motor 2, returns value in [rad]
{
    counts2i = Encoder2.getPulses();
    float counts2 = counts2i*1.0f;
    currentPosition2 = (counts2/8400)*6.28; //Rotational position in radians
}

float FeedbackControl1(float Error1)
{
    static float Error_integral1 = 0;
    static float Error_prev1 = Error1;
    // Proportional part:
    float u_k1 = Kp * Error1;
    // Integral part:
    Error_integral1 = Error_integral1 + Error1 * Ts;
    float u_i1 = Ki * Error_integral1;
    // Derivative part:
    float Error_derivative1 = (Error1 - Error_prev1)/Ts;
    float u_d1 = Kd * Error_derivative1;
    Error_prev1 = Error1;
    // Sum all parts and return it
    return u_k1 + u_i1 + u_d1; //motorValue
}

float FeedbackControl2(float Error2)
{
    static float Error_integral2 = 0;
    static float Error_prev2 = Error2;
    // Proportional part:
    float u_k2 = Kp * Error2;
    // Integral part:
    Error_integral2 = Error_integral2 + Error2 * Ts;
    float u_i2 = Ki * Error_integral2;
    // Derivative part:
    float Error_derivative2 = (Error2 - Error_prev2)/Ts;
    float u_d2 = Kd * Error_derivative2;
    Error_prev2 = Error2;
    // Sum all parts and return it
    return u_k2 + u_i2 + u_d2; //motorValue
}

void SetMotor1(float motorValue1)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    // 0 = clockwise motor direction
    // 1 = counterclockwise motor direction
    if (motorValue1 >=0) {
        motor1Direction=0;
    } else {
        motor1Direction=1;
    }
    if (fabs(motorValue1)>1) {
        motor1PWM = 1;
    } else {
        motor1PWM = fabs(motorValue1);
    }
}

void SetMotor2(float motorValue2)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    // 0 = counterclockwise motor direction
    // 1 = clockwise motor direction
    if (motorValue2 >=0) {
        motor2Direction=1;
    } else {
        motor2Direction=0;
    }
    if (fabs(motorValue2)>1) {
        motor2PWM = 1;
    } else {
        motor2PWM = fabs(motorValue2);
    }
}

void MeasureAndInverseK()
{
    MeasureEncoderPosition1();   // You want to know the current angle of the motors to get the right Jacobian
    MeasureEncoderPosition2();   // You want to know the current angle of the motors to get the right Jacobian
    ComputeJ();                                 // Compute matrix J (inverse Jacobian, and Twist is already left out)
    ComputeV();                                 // Compute matrix V (stores the desired velocities obtained from the EMG signal)
    ComputeQ_set();                         // Compute Q_set (stores Q1 and Q2)
    q1 = IntegrateQ1();                                  // Compute required angle to go to desired position of end-effector
    q2 = IntegrateQ2();                                  // Compute required angle to go to desired position of end-effector
    if (q > 0.0) {                      // q1 cannot be greater than 0
        q = 0.0;
    } else if (q < - 1.047) {           // q1 cannot be smaller than 60 degrees
        q1 = -1.047;
    } else {                            // q1 can be computed q1 between intervals
        q1 = q1;
    }
    if (q2 > -0.61) {                   // q2 cannot be smaller than 0.61 rad
        q2 = -0.61;                     // Stay at mimimum angle
    } else if (q2 < -1.57) {            // q2 cannot be greater than 1.57 rad
        q2 = -1.57;                     // Stay at maximum angle
    } else {                            // If q2 is in the right range, let calculated q2 be q2
        q2 = q2;
// Determine error and add PID control
        errorIK1 = ErrorInverseKinematics1();       // Determine difference between current angle motor 1 and desired angle
        errorIK2 = ErrorInverseKinematics2();       // Determine difference between current angle motor 2 and desired angle
// Determine motorValues
        motorValue1 = FeedbackControl1(errorIK1);                       // Calculate motorValue1
        motorValue2 = FeedbackControl2(errorIK2);                       // Calculate motorValue2
        if (errorIK1<0.1) {                // When error is small enough, stop turning
            if (errorIK2<0.1) {
                TurnMotorsOff();
            } else {
                motor1PWM = 0.0;
                SetMotor2();
            }
        } else if (errorIK2<0.1) {
            if (errorIK1<0.1) {
                TurnMotorsOff();
            } else {
                motor2PWM = 0.0;
                SetMotor1();
            }
        }

// Make Motor move
        SetMotor1(motorValue1);
        SetMotor2(motorValue2);

// Press button to switch velocity direction
        if (!button2.read()) { //keep pressing button2
            vy_des=vx_des;
            vx_des=0.0;
        } else {
            vy_des=0.0;
        }
    }

    /*void PrintValues()
    {
        pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
        pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
        pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
        pc.printf("q1  Qset = %f \t q2 Qset = %f \r\n", Q1, Q2);
    }*/
//------------------------------------------------------------------------------
// MAIN
    int main() {
        pc.baud(115200);
        potmeterTicker.attach(GetPotMeterVelocity1, 0.02);
        measurecontrolTicker.attach(MeasureAndInverseK, 0.01);
        /*printTicker.attach(printen, 0.5);*/

        while (true) {
            Led = !Led;
            wait(0.5);
        }
    }