File content as of revision 4:c45eaa904b09:

#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(100.0f,0.0f,0.001f, Interval);
PID controller2(100.0f,0.0f,0.002f, Interval);

//G: elsewhere given values
float GXset = 40.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

//Motor angles in radialen
float q1set = 0.25f*pi;
float q2set = -0.5f*pi;

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

RawSerial pc(PTB17,PTB16);

//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] - 14.2;       //12 = x distance from base frame to board frame
    He1[5] = He1[5] + 11.9;       //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.2;    //stiffness
    
    //Current position = VARIABLE       EXPRESSED IN BASE FRAME
    float rx1 = -14.2;                      //x coordinate of joint 1
    float ry1 = 11.9;                      //y coordinate of joint 1                      
    float rx2 = cos(q1)*L1-14.2;             //x coordinate of joint 2
    float ry2 = sin(q1)*L1+11.9;             //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);
    //pc.printf("Posx , Posy: %f %f \r\n",rxe,rye);
    //Define transposed Jacobian J_T [3x2]
    float J_T1 = 1;
    float J_T2 = ry1;
    float J_T3 = rx1;
    float J_T4 = 1;
    float J_T5 = ry2;
    float J_T6 = -rx2;

    //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;
    //pc.printf("Fx , Fy: %f %f \r\n",Fsprx,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("tau Rene: %f %f\r\n",tau1,tau2);
    tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1));
    tau2 = Fspry*L2*cos(q1+q2)              - Fsprx*L2*sin(q1+q2);
    //pc.printf("tau Bram: %f %f\r\n",tau1,tau2);
    //pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2);
    //pc.printf("Tau1, Tau2: %f %f \r\n",tau1,tau2);
}

void ControlFunction(float AngleRef1,float AngleRef2){
    
    float Angle1 = (EncoderMotor1.getPulses()/4200.0f)*(2.0f*pi)/3.0f; //Motorpos. in radians
    float Angle2 = (EncoderMotor2.getPulses()/4200.0f)*(2.0f*pi)/1.8f; //Motorpos. in radians
    
    // PID input
    controller1.setSetPoint(AngleRef1);
    controller2.setSetPoint(AngleRef2);

    //pc.printf("ANGLEREF: %f %f \r\n",AngleRef1,AngleRef2);
    //pc.printf("ANGLE   : %f %f \r\n\n",Angle1,Angle2);

    controller1.setProcessValue(Angle1);
    controller2.setProcessValue(Angle2);
    
    // 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;
}
    float omg1;            //previous values are irrelevant
    float omg2;            //previous values are irrelevant
//Overall function which only needs inputs
void LoopFunction()
{
    float q1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f; //Motorpos. in radians
    float q2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f; //Motorpos. in radians
    
    //start values
    float tau1;            //previous values are irrelevant
    float tau2;            //previous values are irrelevant

    float Xcurr;           //new value is calculated, old replaced
    float Ycurr;           //new value is calculated, old replaced
    
    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, GXset, GYset, 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*Interval;
    q2set = q2set + omg2*Interval;
    
    //pc.printf("q1set , q2set: %f %f \r\n",q1set,q2set);
    //pc.printf("q1    , q2   : %f %f \r\n",q1,q2);
    
    ControlFunction(q1set,q2set);
}


void HomingLoop(){

    EndSwitch1.mode(PullUp);
    EndSwitch2.mode(PullUp);
    
    MotorThrottle1=0.2f;
    MotorThrottle2=0.1f;
    
    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(0.29/2/pi*4200*3.0f);
    EncoderMotor2.reset((3.3715-2*pi)/2/pi*4200*1.8f);
}




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;
            /*
            GYset = GYset + 1;
            */
            //pc.printf("%f\n\r", PosRef2);
            break;
        case 21: //1
            pc.printf("Onder\n\r");
            //PosRef2=PosRef2+0.1f;  
            /*
            GYset = GYset - 1;
            */
            //pc.printf("%f\n\r", PosRef2);
            break;
        case 68: //1
            pc.printf("Links\n\r");
            //PosRef1=PosRef1+0.1f;
            /*
            GXset = GXset + 1;
            */
            //pc.printf("%f\n\r", PosRef1);
            break;
        case 67: //1
            pc.printf("Rechts\n\r");
            //PosRef1=PosRef1-0.1f;
            /*
            GXset = GXset - 1;
            */
            //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;
    }
}



// Give Reference Position
void DeterminePosRef(){
    GXset=40*PotMeter1.read(); // Reference in Rad
    GYset=40*PotMeter2.read(); // Reference in Rad
    pc.printf("RefX , RefY: %f %f\r\n",GXset,GYset);
    float posx=0;
    float posy=0;
    Brock(EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f,posx,posy);
    pc.printf("posx posy  : %f %f\r\n",posx,posy);
    pc.printf("q1set,q2set: %f %f\r\n",q1set,q2set);
    pc.printf("q1   ,q2   : %f %f\r\n",EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f);
    pc.printf("w1   ,w2   : %f %f\r\n",omg1,omg2);
    pc.printf("\n");
    //ControlFunction(GXset,GYset);
}

int main() {
    pc.baud(115200);
    pc.printf("startup\r\n");
    
    controller1.setInputLimits(-2.0f*pi,2.0f*pi);
    controller2.setInputLimits(-2.0f*pi,2.0f*pi);
    controller1.setOutputLimits(-0.15f,0.15f);
    controller2.setOutputLimits(-0.5f,0.5f);  
  
    pc.printf("Homing \r\n");
    HomingLoop();
    pc.printf("Starting Tickers \r\n");
    ControlTicker.attach(&LoopFunction,Interval);
    GetRefTicker.attach(&DeterminePosRef,0.5f);

    
    while(1)
    { 
        if (ir_rx.getState() == ReceiverIR::Received)
        {
            pc.printf("received");
            
            
            RemoteController();
            wait(0.1);
        }
    }       
    
    
    while(1){}


}