File content as of revision 0:550f6e86da32:

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

// Tickers
Ticker startMotor;
Ticker printTicker;

//------------------------------------------------------------------------------
//Inverse Kinematics
const double L0 = 0.1;              // Horizontal length from base to first joint [m]   NAMETEN!!
const double L1 = 0.3;              // Length link 1 [m]                                NAMETEN!!
const double L2 = 0.3;              // Length link 2 [m]                                NAMETEN!!
const double L3 = 0.1;              // Vertical length from base to first joint [m]     NAMETEN!
volatile double q1 = 0.0;           // Starting reference angle of first link [rad]
volatile double q2 = -(0.5*3.14);   // Starting reference angle of second link wrt first link [rad]
double t = 2.0;                     // Time interval [s]                                SHOULD BE REPLACED          SAMPLING TIJD!!
Matrix Q_set(2,1);                  // Setting a matrix for storing the angular velocities [rad/sec]
Matrix J(2,2);                      // Setting a matrix for the Jacobian
Matrix V(2,1);                      // Setting a matrix for storing the EMG-measured velocities
 
// Motor Control
volatile double potMeterPosition1 = 0.0;
volatile double potMeterPosition2 = 0.0;
volatile double motorValue1 = 0.01;
volatile double motorValue2 = 0.01;
volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
volatile double Kd = 0.0;
volatile double Ts = 0.01;                      //                                                          SAMPLING TIJD!!
 

//------------------------------------------------------------------------------
// Potmeter values TO DETERMINE VX_DES, VY_DES
double GetPotMeterPosition1()      // Measure the current Potmeter1 value
{
    double potMeter1In = potMeter1.read();
    potMeterPosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
    return potMeterPosition1;
}
 
double GetPotMeterPosition2()      // Measure the current Potmeter2 value
{
    double potMeter2In = potMeter2.read();
    potMeterPosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
    return potMeterPosition2;
}

// Drive the motor
double FeedbackControl1(double Error1)      //                                  Dit moet zo geschreven worden dat het zowel met EMG als met potmeters gebruikt kan worden
{
    static double Error_integral1 = 0;
    static double Error_prev1 = Error1;
    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
    // Proportional part:
    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
    double u_k1 = Kp * Error1;
    // Integral part:
    Error_integral1 = Error_integral1 + Error1 * Ts;
    double u_i1 = Ki * Error_integral1;
    // Derivative part
    double Error_derivative1 = (Error1 - Error_prev1)/Ts;
    double u_d1 = Kd * Error_derivative1;
    Error_prev1 = Error1;
    // Sum all parts and return it
    return u_k1 + u_i1 + u_d1; //motorValue
}
 
double FeedbackControl2(double Error2)
{
    static double Error_integral2 = 0;
    static double Error_prev2 = Error2;
    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
    // Proportional part:
    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
    double u_k2 = Kp * Error2;
    // Integral part:
    Error_integral2 = Error_integral2 + Error2 * Ts;
    double u_i2 = Ki * Error_integral2;
    // Derivative part
    double Error_derivative2 = (Error2 - Error_prev2)/Ts;
    double u_d2 = Kd * Error_derivative2;
    Error_prev2 = Error2;
    // Sum all parts and return it
    return u_k2 + u_i2 + u_d2; //motorValue
}
 
void SetMotor1(double 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(double 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 MeasureAndControl1(void)
{
    // This function determines the desired velocity, measures the
    // actual velocity, and controls the motor with
    // a simple Feedback controller. Call this from a Ticker.
    potMeterPosition1 = q1;
    currentPosition1 = MeasureEncoderPosition1();
    motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
    SetMotor1(motorValue1);
}
 
void MeasureAndControl2(void)
{
    // This function determines the desired velocity, measures the
    // actual velocity, and controls the motor with
    // a simple Feedback controller. Call this from a Ticker.
    potMeterPosition2 = q2;
    currentPosition2 = MeasureEncoderPosition2();
    motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
    SetMotor2(motorValue2);
    
//------------------------------------------------------------------------------
// Kinematic functions

Matrix ComputeJ(void){
    double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
    double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
    double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
    double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
    J << a << b
      << c << d;
    return J;
}

Matrix ComputeV(void){
    V.add(1,1,vx_des); // Add desired x-velocity in V
    V.add(2,1,vy_des); // Add desired y-velocity in V
    return V;
}

double IntegrateQ1(){
    q1  = q1 + (Q_set(1,1))*t;    // new value for q1
    return q1;
}

double IntegrateQ2(){
    q2 = q2 + (Q_set(2,1))*t;    // new value for q2
    return q2;
}

Matrix ComputeQ_set(void){
    float a = J(1,1);
    float b = J(1,2);
    float c = J(2,1);
    float d = J(2,2);
    float e = V(1,1);
    float f = V(2,1);
    float first_row = a*e + b*f;
    float second_row = c*e + d*f;
    Q_set.add(1,1,first_row);
    Q_set.add(2,1,second_row);
    return Q_set;
}

int main(){
    //led = 0;
    pc.baud(115200);
    pc.printf("\r\nDoet het script het?\r\n");
    // Compute Jacobian
    J = ComputeJ();
    J.print();
    pc.printf("\r\n");
    // Compute velocities
    V = ComputeV();
    V.print();
    pc.printf("\r\n");
    // Multiplying matrix J and V and storing in array Q_set
    Q_set = ComputeQ_set();

    // velocity to position
    q1 = IntegrateQ1();
    q2 = IntegrateQ2();

    /*if (q1 > ... && < ...)  {
        TurnMotorsOff;
        /go to failure mode
    }
    if (q2 > ... && < ...)  {
        TurnMotorsOff/go to failure mode
    }*/

    pc.printf("New position q1: %f, New Position q2: %f", q1, q2);

}

int main() {
    currentPosition1 = MeasureEncoderPosition1();   // You want to know the current angle of the motors to get the right Jacobian
    currentPosition2 = MeasureEncoderPosition2();   // You want to know the current angle of the motors to get the right Jacobian
    J = ComputeJ();                                 // Compute matrix J (inverse Jacobian, and Twist is already left out)
    V = ComputeV();                                 // Compute matrix V (stores the desired velocities obtained from the EMG signal)
    Q_set = ComputeQ_set();                         // Compute Q_set (stores Q1 and Q2)
    IntegrateQ1();                                  // Compute required angle to go to desired position of end-effector
    IntegrateQ2();                                  // Compute required angle to go to desired position of end-effector
    if (q1 < 1.047) {                               // q1 can only be smaller than 1.047 rad
        q1 = q1;
    } else {                                        // If value of q1 is greater than 1.047 rad, then stay at maximum angle
         q1 = 1.047;
    }
    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
    double errorIK1 = ErrorInverseKinematics1();       // Determine difference between current angle motor 1 and desired angle
    double errorIK2 = ErrorInverseKinematics2();        // Determine difference between current angle motor 2 and desired angle
    // Determine motorValues
    motorValue1 = FeedbackControl1(errorIK1);                       // Calculate motorValue1
    motorValue2 = FeedbackControl2(errorIK2);                       // Calculate motorValue2
    // Make Motor move
    SetMotor1(motorValue1);
    SetMotor2(motorValue2);
                
    while (true){}
}