File content as of revision 16:9b7651fdf5a0:

#include "mbed.h"
#include <math.h>
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"

//set pins
DigitalIn encoder1A (D13); //Channel A van Encoder 1
DigitalIn encoder1B (D12); //Channel B van Encoder 1
DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
//DigitalOut led1 (D11); 
//DigitalOut led2 (D10);
AnalogIn potMeter1(A2);
AnalogIn potMeter2(A1);
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);
DigitalOut motor2DirectionPin(D4);
PwmOut motor2MagnitudePin(D5);
DigitalIn button1(D3);
DigitalIn button2(D9);

//library settings
Serial pc(USBTX,USBRX);
Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
//HIDScope    scope(4);

//set initial conditions
float error1_prev = 0;
float error2_prev = 0;
float IntError1 = 0;
float IntError2 = 0;
float q1 = 0;
float q2 = 0;
//set initial conditions for function references
 float q1_dot = 0.0;
 float q2_dot = 0.0;
 float motorValue1 = 0.0;
 float motorValue2 = 0.0;
 
//set constant or variable values
int counts1 = 0;
int counts2 = 0;
int counts1Prev = 0;
int counts2Prev = 0;
double DerivativeCounts;
float x0 = 1.0;
float L0 = 1.0;
float L1 = 1.0;
float dx;
float dy;
float dy_stampdown = 0.05; //5 cm movement downward to stamp

float t_sample = 0.01; //seconds
float referenceVelocity = 0;
float bqcDerivativeCounts = 0;
const float PI = 3.141592653589793;
const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
const int ccw = 1;

//set BiQuad
BiQuadChain bqc;
BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);

//set go-Ticker settings
volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
void BiQuadTicker_act(){BiQuadTicker_go=true;};
void FeedbackTicker_act(){FeedbackTicker_go=true;};
void TimeTracker_act(){TimeTracker_go=true;};
//void sampleT_act(){sampleT_go=true;};

//define encoder counts and degrees
QEI Encoder1(D12, D13, NC, 32); // turns on encoder
QEI Encoder2(D10, D11, NC, 32); // turns on encoder

const int counts_per_revolution = 4200; //counts per motor axis revolution
const int inverse_gear_ratio = 131;
//const float motor_axial_resolution = counts_per_revolution/(2*PI);
const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis

void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_dotOut, float &q2_dotOut){
    
    //get joint positions q from encoder
    float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
    float Encoder2Position = counts2/resolution;
    
    float Motor1Position = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
    float Motor2Position = Encoder2Position*inverse_gear_ratio;
      
    //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
    float biceps1 = !button1.read();
    float biceps2 = !button2.read();
    if (biceps1 > 0 && biceps2 > 0){
        //both arms activated: stamp moves down
        //led1 = 1;
        //led2 = 1;
        dx = 0;
        dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));

        /*
         wait(1);
        dy = -(dy_stampdown);  //reset vertical position
        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        */
        }
    else if (biceps1 > 0 && biceps2 <= 0){
        //arm 1 activated, move left
        //led1 = 1;
        //led2 = 0;
        dx = 1; //-biceps1;
        dy = 0;
        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        }
    else if (biceps1 <= 0 && biceps2 > 0){
        //arm 1 activated, move left
        //led1 = 0;
        //led2 = 1;
        dx = 1; //biceps2;
        dy = 0;
        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));

        }
    else{
        //led1 = 0;
        //led2 = 0;
        dx=0;
        dy=0;
        q1_dotOut = 0;
        q2_dotOut = 0;
        }
            
    //get joint angles change q_dot = Jpseudo * TwistEndEff  (Matlab)
    
    //update joint angles
    q1Out = q1Out + q1_dotOut;  //in radians
    q2Out = q2Out + q2_dotOut;
    
    pc.baud(115200);
    pc.printf("dx:          %f \r\n", dx);
    pc.printf("dy:          %f \r\n", dy);
    pc.printf("q1:          %f \r\n", q1Out);
    pc.printf("q1_dot:      %f \r\n", q1_dotOut);
    pc.printf("q2:          %f \r\n", q2Out);
    pc.printf("q2_dot:      %f \r\n", q2_dotOut);
    
    pc.printf("Counts1:     %f \r\n", counts1);
    pc.printf("Encoder1:    %f \r\n", Encoder1Position);
    pc.printf("Motor1:      %f \r\n", Motor1Position);
    pc.printf("Counts2:    %f \r\n", counts2);
    pc.printf("Encoder2:    %f \r\n", Encoder2Position);
    pc.printf("Motor2:      %f \r\n", Motor2Position);
    }

void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
    //float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
    //float Position1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
    
    // linear feedback control
    float error1 = q1_dot; //referencePosition1 - Position1;             // proportional error in radians
    float error2 = q2_dot; //referencePosition1 - Position1;             // proportional error in radians
    float Kp = 1; //potMeter2.read();

    float IntError1 = IntError1 + error1*t_sample;             // integrated error in radians
    float IntError2 = IntError2 + error2*t_sample;             // integrated error in radians
    //float maxKi = 0.2;
    float Ki = 0.1; //potMeter2.read();
    
    float DerivativeError1 = (error1_prev + error1)/t_sample;  // derivative of error in radians
    float DerivativeError2 = (error2_prev + error2)/t_sample;  // derivative of error in radians
    //float maxKd = 0.2;
    float Kd = 0.0; //potMeter2.read();
    
    //scope.set(0,referencePosition1);
    //scope.set(1,Position1);
    //scope.set(2,Ki);
    //scope.send();
    
    motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd;    //total controller output = motor input
    motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd;    //total controller output = motor input
    //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
    //pc.printf("Counts encoder1: %i rad \r\n", counts1);
    //pc.printf("Kp: %f \r\n", Kp);
    //pc.printf("MotorValue: %f \r\n", motorValue1);
    
    pc.printf("error1: %f \r\n", error1);
    pc.printf("IntError1: %f \r\n", IntError1);
    pc.printf("DerError1: %f \r\n", DerivativeError1);
    pc.printf("error2: %f \r\n", error2);
    pc.printf("IntError2: %f \r\n", IntError2);
    pc.printf("DerError2: %f \r\n", DerivativeError2);
        
    error1_prev = error1;
    error2_prev = error1;
    float biceps1 = !button1.read();
    float biceps2 = !button2.read();
       
    /*
    scope.set(0,q1);
    scope.set(1,q2);
    scope.set(2,biceps1);
    scope.set(3,biceps2);
    scope.send();
    */
}

void SetMotor1(float motorValue1, 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
    //control motor 1
    if (motorValue1 >=0) //clockwise rotation
        {motor1DirectionPin=ccw;        //inverted due to opposite (to other motor) build-up in tower
        //led1=1;
        //led2=0;
        }
    else    //counterclockwise rotation 
        {motor1DirectionPin=cw;         //inverted due to opposite (to other motor) build-up in tower
        //led1=0;
        //led2=1;
        }
    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = 0.1*fabs(motorValue1);  //fabs(motorValue1);
    //control motor 2
    if (motorValue2 >=0)  //clockwise rotation
        {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
        //led1=1;
        //led2=0;
        }
    else    //counterclockwise rotation 
        {motor2DirectionPin=cw;    //action is ccw, due to faulty motor2DirectionPin (inverted)
        //led1=0;
        //led2=1;
        }
    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
        else motor2MagnitudePin = 0.1*fabs(motorValue2);  //fabs(motorValue1);
}

void MeasureAndControl()
{
    // This function measures the EMG of both arms, calculates via IK what
    // the joint speeds should be, and controls the motor with 
    // a Feedforward controller. This is called from a Ticker.
    GetReferenceKinematics1(q1, q2, q1_dot, q2_dot);
    FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2);
    SetMotor1(motorValue1, motorValue2);
}

void TimeTrackerF(){
     //wait(1);   
     //float Potmeter1 = potMeter1.read();
     //float referencePosition1 = GetReferencePosition();
     //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
     //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
     //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
     //pc.printf("TTCounts: %i \r\n", counts1);
}

/*
void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
    //double in=DerivativeCounts();
    bqcDerivativeCounts=bqc.step(DerivativeCounts);
    //return(bqcDerivativeCounts);
    }
*/  

int main()
{
 //Initialize
 //int led1val = led1.read();
 //int led2val = led2.read();
  pc.baud(115200);
 pc.printf("Test putty IK");
 MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
 bqc.add(&bq1).add(&bq2);
 
 while(1)
    {
        if (MeasureTicker_go){
            MeasureTicker_go=false;
            MeasureAndControl();
            counts1 = Encoder1.getPulses();           // gives position of encoder 
            counts2 = Encoder2.getPulses();           // gives position of encoder 
            }
/*
        if (BiQuadTicker_go){
            BiQuadTicker_go=false;
            BiQuadFilter();
        }
  */  
    }
}