File content as of revision 3:98ea6afa0cf2:

#include "mbed.h"
#include "QEI.h"
#include "PID.h"
#include "ReceiverIR.h"
#include "Servo.h"
#include <iostream>
#include <string>
#include <math.h>

ReceiverIR ir_rx(D2);

AnalogIn PotMeter1(A0);
AnalogIn PotMeter2(A1);
InterruptIn Button(PTA4);
 
PwmOut MotorThrottle1(D5);
PwmOut MotorThrottle2(D6);
DigitalOut MotorDirection1(D4);
DigitalOut MotorDirection2(D7);
DigitalOut servo(D3);

DigitalIn EndSwitch1(D9);
DigitalIn EndSwitch2(D8);

QEI EncoderMotor1(D12,D13,NC,4200);
QEI EncoderMotor2(D10,D11,NC,4200);

Ticker ControlTicker;
Ticker GetRefTicker;

float Interval=0.02f;
float pi=3.14159265359;

PID controller1(20.0f,0.0f,0.001f, Interval);
PID controller2(20.0f,0.0f,0.002f, Interval);

//G: elsewhere given values
float Gq1 = 0;              //from homing
float Gq2 = 0;              //from homing
float GXset = 47.5;    //from EMG in cm
float GYset = 11;    //from EMG in cm 

//Constant robot parameters
const float L1 = 27.5;      //length arm 1 in cm
const float L2 = 32;        //length arm 2 in cm


RemoteIR::Format format;
uint8_t buf[4];

Serial pc(PTB17,PTB16);

float PosRef1=1.0f;
float PosRef2=1.0f;

//Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr
void Brock(float q1, float q2, float& Xcurr, float& Ycurr)
{
    //Position of end-effector in base frame when q1 = q2 = 0 to use in He0(0)
   
    float He1[9];
    
    He1[0] = cos(q1 + q2);
    He1[1] = -sin(q1 +q2);
    He1[2] = L2*cos(q1 + q2) + L1*cos(q1);
    He1[3] = sin(q1 + q2);
    He1[4] = cos(q1 + q2);
    He1[5] = L2*sin(q1 + q2) + L1*sin(q1);
    He1[6] = 0;
    He1[7] = 0;
    He1[8] = 1;
    
    // print He1 to check the matrix
    pc.printf("He1 = [\n\r");
    
    for (int i=0; i<=8; i++){

        pc.printf("%.2f  ",He1[i]);
        if (i==2){
            pc.printf("\n\r");}
            else if (i==5){
                pc.printf("\n\r");}
    }
    pc.printf("]\n\r" );
    
    //Translation from base frame to board frame  SO FROM NOW ON ALL EXPRESSED IN BOARD FRAME
    He1[2] = He1[2] - 12;       //12 = x distance from base frame to board frame
    He1[5] = He1[5] + 11;       //11 = y distance from base frame to board frame
    
    pc.printf("The old value of Xcurr was %f, old Ycurr was %f\n\r", Xcurr, Ycurr);
    Xcurr = He1[2];
    Ycurr = He1[5];
    pc.printf("The new value of Xcurr is %f, new Ycurr is %f\n\r", Xcurr, Ycurr);
    
    // print He0 to check the matrix
    pc.printf("\n\r He0 = [\n\r");
    for (int i=0; i<=8; i++){

        pc.printf("%.2f  ",He1[i]);
        if (i==2){
            pc.printf("\n\r");}
            else if (i==5){
                pc.printf("\n\r");}
    }
    pc.printf("]\n\r" );
}


//Function INPUT: q1, q2, Xset, Yset, Xcurr, Ycurr. OUTPUT: tau1, tau2
void ErrorToTau(float q1, float q2, float Xcurr, float Ycurr, float Xset, float Yset, float& tau1, float& tau2)
{
    //The parameters k and b are free controller parameters that will determine how fast the arm moves towards the setpoint.
    float k = 0.5;    //stiffness
    
    //Current position = VARIABLE       EXPRESSED IN BASE FRAME
    float rx1 = 0;                      //x coordinate of joint 1
    float ry1 = 0;                      //y coordinate of joint 1                      
    float rx2 = cos(q1)*L1;             //x coordinate of joint 2
    float ry2 = sin(q1)*L1;             //y coordinate of joint 2
    float rxe = rx2 + cos(q1+q2)*L2;    //x coordinate of end-effector
    float rye = ry2 + sin(q1+q2)*L2;    //y coordinate of end-effector
    pc.printf("Current position: rx1 = %f, ry1 = %f, rx2 = %f, ry2 = %f, rxe = %f, rye = %f\n\r", rx1, ry1, rx2, ry2, rxe, rye);
    
    //Define transposed Jacobian J_T [3x2]
    float J_T1 = 1;
    float J_T2 = ry2;
    float J_T3 = -rx2;
    float J_T4 = 1;
    float J_T5 = rye;
    float J_T6 = -rxe;

    //Define spring force
    float Fsprx = k*(Xset - Xcurr);
    float Fspry = k*(Yset - Ycurr);
    pc.printf("The new value of Fsprx is %f, new Fspry is %f\n\r", Fsprx, Fspry);
    
    //Define wrench Wspr = (tau Fx Fy) = (rxFx - rxFy Fx Fy)
    float Wspr1 = Ycurr*Fsprx - Xcurr*Fspry;
    float Wspr2 = Fsprx;
    float Wspr3 = Fspry;

    //End-effector wrench to forces and torques on the joints using the Jacobian (transposed)
    pc.printf("The old value of tau1 was %f, old tau2 was %f\n\r", tau1, tau2);
    tau1 = J_T1*Wspr1+J_T2*Wspr2+J_T3*Wspr3;
    tau2 = J_T4*Wspr1+J_T5*Wspr2+J_T6*Wspr3;
    pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2);
}


//Overall function which only needs inputs
void SuperFunction(float q1, float q2, float Xset, float Yset, float& q1set, float& q2set)
{
    //start values
    float tau1;            //previous values are irrelevant
    float tau2;            //previous values are irrelevant
    float omg1;            //previous values are irrelevant
    float omg2;            //previous values are irrelevant
    float Xcurr;           //new value is calculated, old replaced
    float Ycurr;           //new value is calculated, old replaced
    
    float T = 0.01;         //Loop period
    float b = 200;          //damping
    
    // Call function to calculate Xcurr and Ycurr from q1 and q2
    Brock(q1, q2, Xcurr, Ycurr);
        
    // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
    ErrorToTau(q1, q2, Xcurr, Ycurr, Xset, Yset, tau1, tau2); 
    
    //torque to joint velocity
    pc.printf("The old value of omg1 was %f, old omg2 was %f\n\r", omg1, omg2);
    omg1 = tau1/b;
    omg2 = tau2/b;
    pc.printf("The new value of omg1 is %f, new omg2 is %f\n\r", omg1, omg2);
    
    //joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
    q1set = q1set + omg1*T;
    q2set = q2set + omg2*T;
}


void HomingLoop(){

    EndSwitch1.mode(PullUp);
    EndSwitch2.mode(PullUp);
    
    MotorThrottle1=0.1f;
    MotorThrottle2=0.2f;
    
    MotorDirection1=1;
    MotorDirection2=1;
    
    bool dummy1=0;
    bool dummy2=0;
    
    while(EndSwitch1.read()|EndSwitch2.read()){
        if((EndSwitch1.read()==0)&&(dummy1==0)){
            MotorThrottle1=0.0f;
            dummy1=1;
        }
        if((EndSwitch2.read()==0)&&(dummy2==0)){
            MotorThrottle2=0.0f;
            dummy2=1;
        }
    }
    MotorThrottle1=0.0f;
    MotorThrottle2=0.0f;
    EncoderMotor1.reset();
    EncoderMotor2.reset();
}

void ControlLoop(){
    
    float Pos1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
    float Pos2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
    float error1 = PosRef1 - Pos1;
    float error2 = PosRef2 - Pos2;
    
    // PID input
    controller1.setSetPoint(PosRef1);
    controller2.setSetPoint(PosRef2);
    
    controller1.setProcessValue(Pos1);
    controller2.setProcessValue(Pos2);
    
    // PID output
    float OutPut1=controller1.compute();
    float OutPut2=controller2.compute();

    // Direction handling
    float Direction1=0;
    float Direction2=0;
    
    if(OutPut1<0){
        Direction1=1;
    }
    if(OutPut2<0){
        Direction2=1;
    }
    
    OutPut1=fabs(OutPut1);
    OutPut2=fabs(OutPut2);
    
    // Output schrijven
    MotorThrottle1.write(OutPut1);
    MotorThrottle2.write(OutPut2);
    MotorDirection1=Direction1;
    MotorDirection2=Direction2;
}


void PickUp(){

}
    
void LayDown(){
    
    
    }


void RemoteController(){
    
   int bitcount;


   bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
    int state = buf[2];
    switch(state)
    {
        case 22: //1
            pc.printf("1\n\r");
        break;
        case 25: //1
            pc.printf("2\n\r");
            break;
        case 13: //1                
            pc.printf("3\n\r");
            break;
        case 12: //1
            pc.printf("4\n\r");
            break;
        case 24: //1
            pc.printf("5\n\r");
            ControlTicker.detach();
            MotorThrottle1=0.0f;
            MotorThrottle2=0.0f;
            PickUp();
            ControlTicker.attach(&ControlLoop, Interval);
            break;
        case 94: //1
            pc.printf("6\n\r");
            break;
        case 8: //1
            pc.printf("7\n\r");
            break;
        case 28: //1
            pc.printf("8\n\r");
            ControlTicker.detach();
            MotorThrottle1=0.0f;
            MotorThrottle2=0.0f;
            LayDown();
            ControlTicker.attach(&ControlLoop, Interval);
            break;
        case 90: //1
            pc.printf("9\n\r");
            break;
        case 70: //1
            pc.printf("Boven\n\r");
            PosRef2=PosRef2-0.1f;
            pc.printf("%f\n\r", PosRef2);
            break;
        case 21: //1
            pc.printf("Onder\n\r");
            PosRef2=PosRef2+0.1f;  
            pc.printf("%f\n\r", PosRef2);
            break;
        case 68: //1
            pc.printf("Links\n\r");
            PosRef1=PosRef1+0.1f;
            pc.printf("%f\n\r", PosRef1);
            break;
        case 67: //1
            pc.printf("Rechts\n\r");
            PosRef1=PosRef1-0.1f;
            pc.printf("%f\n\r", PosRef1);
            break;
        case 64: //1
            pc.printf("OK\n\r");
            ControlTicker.detach();
            MotorThrottle1=0.0f;
            MotorThrottle2=0.0f;
            HomingLoop();
            ControlTicker.attach(&ControlLoop, Interval);
            
            break;
        default:
            break;
    }
}




void DeterminePosRef(){
    PosRef1=2*pi*PotMeter1.read(); // Reference in Rad
    PosRef2=2*pi*PotMeter2.read(); // Reference in Rad
}

int main() {
    pc.baud(115200);
    pc.printf("startup");
    
    controller1.setInputLimits(0.5f,2.0f*pi);
    controller2.setInputLimits(0.5f,2.0f*pi);
    controller1.setOutputLimits(-0.15f,0.15f);
    controller2.setOutputLimits(-0.5f,0.5f);  
    
    float q1set;    //becomes new value after calling function again
    float q2set;    //becomes new value after calling function again
    SuperFunction(Gq1, Gq2, GXset, GYset, q1set, q2set);
    pc.printf("The new value of q1set is %f, the new q2set is %f\n\r", q1set, q2set);
    SuperFunction(Gq1, Gq2, GXset, GYset, q1set, q2set);
    pc.printf("The new value of q1set is %f, the new q2set is %f\n\r", q1set, q2set);
    
    
    HomingLoop();
    
    
    ControlTicker.attach(&ControlLoop, Interval);
    //GetRefTicker.attach(&DeterminePosRef, 0.1f);
    

    
    while(1)
    { 
        if (ir_rx.getState() == ReceiverIR::Received)
        {

            pc.printf("received");
            RemoteController();
            wait(0.01);
        }
    }       
    
    
    while(1){}


}