main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "PID.h"
00004 #include "ReceiverIR.h"
00005 #include "Servo.h"
00006 #include <iostream>
00007 #include <string>
00008 #include <math.h>
00009 
00010 ReceiverIR ir_rx(D2);
00011 
00012 AnalogIn PotMeter1(A0);
00013 AnalogIn PotMeter2(A1);
00014 InterruptIn Button(PTA4);
00015  
00016 PwmOut MotorThrottle1(D5);
00017 PwmOut MotorThrottle2(D6);
00018 DigitalOut MotorDirection1(D4);
00019 DigitalOut MotorDirection2(D7);
00020 DigitalOut servo(D3);
00021 
00022 DigitalIn EndSwitch1(D9);
00023 DigitalIn EndSwitch2(D8);
00024 
00025 QEI EncoderMotor1(D12,D13,NC,4200);
00026 QEI EncoderMotor2(D10,D11,NC,4200);
00027 
00028 Ticker ControlTicker;
00029 Ticker GetRefTicker;
00030 
00031 float Interval=0.02f;
00032 float pi=3.14159265359;
00033 
00034 PID controller1(100.0f,0.0f,0.001f, Interval);
00035 PID controller2(100.0f,0.0f,0.002f, Interval);
00036 
00037 //G: elsewhere given values
00038 float GXset = 40.5;    //from EMG in cm
00039 float GYset = 11;    //from EMG in cm 
00040 
00041 //Constant robot parameters
00042 const float L1 = 27.5;      //length arm 1 in cm
00043 const float L2 = 32;        //length arm 2 in cm
00044 
00045 //Motor angles in radialen
00046 float q1set = 0.25f*pi;
00047 float q2set = -0.5f*pi;
00048 
00049 RemoteIR::Format format;
00050 uint8_t buf[4];
00051 
00052 RawSerial pc(PTB17,PTB16);
00053 
00054 //Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr
00055 void Brock(float q1, float q2, float& Xcurr, float& Ycurr)
00056 {
00057     //Position of end-effector in base frame when q1 = q2 = 0 to use in He0(0)
00058    
00059     float He1[9];
00060     
00061     He1[0] = cos(q1 + q2);
00062     He1[1] = -sin(q1 +q2);
00063     He1[2] = L2*cos(q1 + q2) + L1*cos(q1);
00064     He1[3] = sin(q1 + q2);
00065     He1[4] = cos(q1 + q2);
00066     He1[5] = L2*sin(q1 + q2) + L1*sin(q1);
00067     He1[6] = 0;
00068     He1[7] = 0;
00069     He1[8] = 1;
00070     
00071     // print He1 to check the matrix
00072     /*
00073     pc.printf("He1 = [\n\r");
00074     
00075     for (int i=0; i<=8; i++){
00076 
00077         pc.printf("%.2f  ",He1[i]);
00078         if (i==2){
00079             pc.printf("\n\r");}
00080             else if (i==5){
00081                 pc.printf("\n\r");}
00082     }
00083     pc.printf("]\n\r" );
00084     */
00085     //Translation from base frame to board frame  SO FROM NOW ON ALL EXPRESSED IN BOARD FRAME
00086     He1[2] = He1[2] - 14.2;       //12 = x distance from base frame to board frame
00087     He1[5] = He1[5] + 11.9;       //11 = y distance from base frame to board frame
00088     
00089     //pc.printf("The old value of Xcurr was %f, old Ycurr was %f\n\r", Xcurr, Ycurr);
00090     Xcurr = He1[2];
00091     Ycurr = He1[5];
00092     //pc.printf("The new value of Xcurr is %f, new Ycurr is %f\n\r", Xcurr, Ycurr);
00093     /*
00094     // print He0 to check the matrix
00095     pc.printf("\n\r He0 = [\n\r");
00096     for (int i=0; i<=8; i++){
00097 
00098         pc.printf("%.2f  ",He1[i]);
00099         if (i==2){
00100             pc.printf("\n\r");}
00101             else if (i==5){
00102                 pc.printf("\n\r");}
00103     }
00104     pc.printf("]\n\r" );
00105     */
00106 }
00107 
00108 
00109 //Function INPUT: q1, q2, Xset, Yset, Xcurr, Ycurr. OUTPUT: tau1, tau2
00110 void ErrorToTau(float q1, float q2, float Xcurr, float Ycurr, float Xset, float Yset, float& tau1, float& tau2)
00111 {
00112     //The parameters k and b are free controller parameters that will determine how fast the arm moves towards the setpoint.
00113     float k = 0.2;    //stiffness
00114     
00115     //Current position = VARIABLE       EXPRESSED IN BASE FRAME
00116     float rx1 = -14.2;                      //x coordinate of joint 1
00117     float ry1 = 11.9;                      //y coordinate of joint 1                      
00118     float rx2 = cos(q1)*L1-14.2;             //x coordinate of joint 2
00119     float ry2 = sin(q1)*L1+11.9;             //y coordinate of joint 2
00120     float rxe = rx2 + cos(q1+q2)*L2;    //x coordinate of end-effector
00121     float rye = ry2 + sin(q1+q2)*L2;    //y coordinate of end-effector
00122     //pc.printf("Current position: rx1 = %f, ry1 = %f, rx2 = %f, ry2 = %f, rxe = %f, rye = %f\n\r", rx1, ry1, rx2, ry2, rxe, rye);
00123     //pc.printf("Posx , Posy: %f %f \r\n",rxe,rye);
00124     //Define transposed Jacobian J_T [3x2]
00125     float J_T1 = 1;
00126     float J_T2 = ry1;
00127     float J_T3 = rx1;
00128     float J_T4 = 1;
00129     float J_T5 = ry2;
00130     float J_T6 = -rx2;
00131 
00132     //Define spring force
00133     float Fsprx = k*(Xset - Xcurr);
00134     float Fspry = k*(Yset - Ycurr);
00135     //pc.printf("The new value of Fsprx is %f, new Fspry is %f\n\r", Fsprx, Fspry);
00136     
00137     //Define wrench Wspr = (tau Fx Fy) = (rxFx - rxFy Fx Fy)
00138     float Wspr1 = -Ycurr*Fsprx + Xcurr*Fspry;
00139     float Wspr2 = Fsprx;
00140     float Wspr3 = Fspry;
00141     //pc.printf("Fx , Fy: %f %f \r\n",Fsprx,Fspry);
00142     
00143     //End-effector wrench to forces and torques on the joints using the Jacobian (transposed)
00144     //pc.printf("The old value of tau1 was %f, old tau2 was %f\n\r", tau1, tau2);
00145     tau1 = J_T1*Wspr1+J_T2*Wspr2+J_T3*Wspr3;
00146     tau2 = J_T4*Wspr1+J_T5*Wspr2+J_T6*Wspr3;
00147     //pc.printf("tau Rene: %f %f\r\n",tau1,tau2);
00148     tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1));
00149     tau2 = Fspry*L2*cos(q1+q2)              - Fsprx*L2*sin(q1+q2);
00150     //pc.printf("tau Bram: %f %f\r\n",tau1,tau2);
00151     //pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2);
00152     //pc.printf("Tau1, Tau2: %f %f \r\n",tau1,tau2);
00153 }
00154 
00155 void ControlFunction(float AngleRef1,float AngleRef2){
00156     
00157     float Angle1 = (EncoderMotor1.getPulses()/4200.0f)*(2.0f*pi)/3.0f; //Motorpos. in radians
00158     float Angle2 = (EncoderMotor2.getPulses()/4200.0f)*(2.0f*pi)/1.8f; //Motorpos. in radians
00159     
00160     // PID input
00161     controller1.setSetPoint(AngleRef1);
00162     controller2.setSetPoint(AngleRef2);
00163 
00164     //pc.printf("ANGLEREF: %f %f \r\n",AngleRef1,AngleRef2);
00165     //pc.printf("ANGLE   : %f %f \r\n\n",Angle1,Angle2);
00166 
00167     controller1.setProcessValue(Angle1);
00168     controller2.setProcessValue(Angle2);
00169     
00170     // PID output
00171     float OutPut1=controller1.compute();
00172     float OutPut2=controller2.compute();
00173 
00174     // Direction handling
00175     float Direction1=0;
00176     float Direction2=0;
00177     
00178     if(OutPut1<0){
00179         Direction1=1;
00180     }
00181     if(OutPut2<0){
00182         Direction2=1;
00183     }
00184     
00185     OutPut1=fabs(OutPut1);
00186     OutPut2=fabs(OutPut2);
00187     
00188     // Output schrijven
00189     MotorThrottle1.write(OutPut1);
00190     MotorThrottle2.write(OutPut2);
00191     MotorDirection1=Direction1;
00192     MotorDirection2=Direction2;
00193 }
00194     float omg1;            //previous values are irrelevant
00195     float omg2;            //previous values are irrelevant
00196 //Overall function which only needs inputs
00197 void LoopFunction()
00198 {
00199     float q1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f; //Motorpos. in radians
00200     float q2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f; //Motorpos. in radians
00201     
00202     //start values
00203     float tau1;            //previous values are irrelevant
00204     float tau2;            //previous values are irrelevant
00205 
00206     float Xcurr;           //new value is calculated, old replaced
00207     float Ycurr;           //new value is calculated, old replaced
00208     
00209     float b = 200;          //damping
00210     
00211     // Call function to calculate Xcurr and Ycurr from q1 and q2
00212     Brock(q1, q2, Xcurr, Ycurr);
00213         
00214     // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
00215     ErrorToTau(q1, q2, Xcurr, Ycurr, GXset, GYset, tau1, tau2); 
00216     
00217     //torque to joint velocity
00218     //pc.printf("The old value of omg1 was %f, old omg2 was %f\n\r", omg1, omg2);
00219     omg1 = tau1/b;
00220     omg2 = tau2/b;
00221     //pc.printf("The new value of omg1 is %f, new omg2 is %f\n\r", omg1, omg2);
00222     
00223     //joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
00224     q1set = q1set + omg1*Interval;
00225     q2set = q2set + omg2*Interval;
00226     
00227     //pc.printf("q1set , q2set: %f %f \r\n",q1set,q2set);
00228     //pc.printf("q1    , q2   : %f %f \r\n",q1,q2);
00229     
00230     ControlFunction(q1set,q2set);
00231 }
00232 
00233 
00234 void HomingLoop(){
00235 
00236     EndSwitch1.mode(PullUp);
00237     EndSwitch2.mode(PullUp);
00238     
00239     MotorThrottle1=0.2f;
00240     MotorThrottle2=0.1f;
00241     
00242     MotorDirection1=1;
00243     MotorDirection2=1;
00244     
00245     bool dummy1=0;
00246     bool dummy2=0;
00247     
00248     while(EndSwitch1.read()|EndSwitch2.read()){
00249         if((EndSwitch1.read()==0)&&(dummy1==0)){
00250             MotorThrottle1=0.0f;
00251             dummy1=1;
00252         }
00253         if((EndSwitch2.read()==0)&&(dummy2==0)){
00254             MotorThrottle2=0.0f;
00255             dummy2=1;
00256         }
00257     }
00258     MotorThrottle1=0.0f;
00259     MotorThrottle2=0.0f;
00260     EncoderMotor1.reset(0.29/2/pi*4200*3.0f);
00261     EncoderMotor2.reset((3.3715-2*pi)/2/pi*4200*1.8f);
00262 }
00263 
00264 
00265 
00266 
00267 void PickUp(){
00268 
00269 }
00270     
00271 void LayDown(){
00272     
00273     
00274 }
00275 
00276 
00277 void RemoteController(){
00278     
00279    int bitcount;
00280 
00281 
00282    bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
00283     int state = buf[2];
00284     switch(state)
00285     {
00286         case 22: //1
00287             pc.printf("1\n\r");
00288         break;
00289         case 25: //1
00290             pc.printf("2\n\r");
00291             break;
00292         case 13: //1                
00293             pc.printf("3\n\r");
00294             break;
00295         case 12: //1
00296             pc.printf("4\n\r");
00297             break;
00298         case 24: //1
00299             pc.printf("5\n\r");
00300             ControlTicker.detach();
00301             MotorThrottle1=0.0f;
00302             MotorThrottle2=0.0f;
00303             PickUp();
00304             //ControlTicker.attach(&ControlLoop, Interval);
00305             break;
00306         case 94: //1
00307             pc.printf("6\n\r");
00308             break;
00309         case 8: //1
00310             pc.printf("7\n\r");
00311             break;
00312         case 28: //1
00313             pc.printf("8\n\r");
00314             ControlTicker.detach();
00315             MotorThrottle1=0.0f;
00316             MotorThrottle2=0.0f;
00317             LayDown();
00318             //ControlTicker.attach(&ControlLoop, Interval);
00319             break;
00320         case 90: //1
00321             pc.printf("9\n\r");
00322             break;
00323         case 70: //1
00324             pc.printf("Boven\n\r");
00325             //PosRef2=PosRef2-0.1f;
00326             /*
00327             GYset = GYset + 1;
00328             */
00329             //pc.printf("%f\n\r", PosRef2);
00330             break;
00331         case 21: //1
00332             pc.printf("Onder\n\r");
00333             //PosRef2=PosRef2+0.1f;  
00334             /*
00335             GYset = GYset - 1;
00336             */
00337             //pc.printf("%f\n\r", PosRef2);
00338             break;
00339         case 68: //1
00340             pc.printf("Links\n\r");
00341             //PosRef1=PosRef1+0.1f;
00342             /*
00343             GXset = GXset + 1;
00344             */
00345             //pc.printf("%f\n\r", PosRef1);
00346             break;
00347         case 67: //1
00348             pc.printf("Rechts\n\r");
00349             //PosRef1=PosRef1-0.1f;
00350             /*
00351             GXset = GXset - 1;
00352             */
00353             //pc.printf("%f\n\r", PosRef1);
00354             break;
00355         case 64: //1
00356             pc.printf("OK\n\r");
00357             //ControlTicker.detach();
00358             //MotorThrottle1=0.0f;
00359             //MotorThrottle2=0.0f;
00360             //HomingLoop();
00361             //ControlTicker.attach(&ControlLoop, Interval);
00362             
00363             break;
00364         default:
00365             break;
00366     }
00367 }
00368 
00369 
00370 
00371 // Give Reference Position
00372 void DeterminePosRef(){
00373     GXset=40*PotMeter1.read(); // Reference in Rad
00374     GYset=40*PotMeter2.read(); // Reference in Rad
00375     pc.printf("RefX , RefY: %f %f\r\n",GXset,GYset);
00376     float posx=0;
00377     float posy=0;
00378     Brock(EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f,posx,posy);
00379     pc.printf("posx posy  : %f %f\r\n",posx,posy);
00380     pc.printf("q1set,q2set: %f %f\r\n",q1set,q2set);
00381     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);
00382     pc.printf("w1   ,w2   : %f %f\r\n",omg1,omg2);
00383     pc.printf("\n");
00384     //ControlFunction(GXset,GYset);
00385 }
00386 
00387 int main() {
00388     pc.baud(115200);
00389     pc.printf("startup\r\n");
00390     
00391     controller1.setInputLimits(-2.0f*pi,2.0f*pi);
00392     controller2.setInputLimits(-2.0f*pi,2.0f*pi);
00393     controller1.setOutputLimits(-0.15f,0.15f);
00394     controller2.setOutputLimits(-0.5f,0.5f);  
00395   
00396     pc.printf("Homing \r\n");
00397     HomingLoop();
00398     pc.printf("Starting Tickers \r\n");
00399     ControlTicker.attach(&LoopFunction,Interval);
00400     GetRefTicker.attach(&DeterminePosRef,0.5f);
00401 
00402     
00403     while(1)
00404     { 
00405         if (ir_rx.getState() == ReceiverIR::Received)
00406         {
00407             pc.printf("received");
00408             
00409             
00410             RemoteController();
00411             wait(0.1);
00412         }
00413     }       
00414     
00415     
00416     while(1){}
00417 
00418 
00419 }