Diff: main.cpp

Revision:

4:c45eaa904b09

Parent:

3:98ea6afa0cf2

--- a/main.cpp	Thu Oct 26 19:48:03 2017 +0000
+++ b/main.cpp	Fri Oct 27 11:45:13 2017 +0000
@@ -31,27 +31,25 @@
 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);
+PID controller1(100.0f,0.0f,0.001f, Interval);
+PID controller2(100.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 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];
 
-Serial pc(PTB17,PTB16);
-
-float PosRef1=1.0f;
-float PosRef2=1.0f;
+RawSerial pc(PTB17,PTB16);
 
 //Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr
 void Brock(float q1, float q2, float& Xcurr, float& Ycurr)
@@ -71,6 +69,7 @@
     He1[8] = 1;
     
     // print He1 to check the matrix
+    /*
     pc.printf("He1 = [\n\r");
     
     for (int i=0; i<=8; i++){
@@ -82,16 +81,16 @@
                 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
+    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);
+    //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);
-    
+    //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++){
@@ -103,6 +102,7 @@
                 pc.printf("\n\r");}
     }
     pc.printf("]\n\r" );
+    */
 }
 
 
@@ -110,118 +110,62 @@
 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
+    float k = 0.2;    //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 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("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 = ry2;
-    float J_T3 = -rx2;
+    float J_T2 = ry1;
+    float J_T3 = rx1;
     float J_T4 = 1;
-    float J_T5 = rye;
-    float J_T6 = -rxe;
+    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);
+    //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 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);
+    //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);
+    //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);
 }
 
-
-//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);
+void ControlFunction(float AngleRef1,float AngleRef2){
     
-    //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;
+    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(PosRef1);
-    controller2.setSetPoint(PosRef2);
-    
-    controller1.setProcessValue(Pos1);
-    controller2.setProcessValue(Pos2);
+    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();
@@ -247,6 +191,77 @@
     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(){
@@ -256,7 +271,7 @@
 void LayDown(){
     
     
-    }
+}
 
 
 void RemoteController(){
@@ -286,7 +301,7 @@
             MotorThrottle1=0.0f;
             MotorThrottle2=0.0f;
             PickUp();
-            ControlTicker.attach(&ControlLoop, Interval);
+            //ControlTicker.attach(&ControlLoop, Interval);
             break;
         case 94: //1
             pc.printf("6\n\r");
@@ -300,38 +315,50 @@
             MotorThrottle1=0.0f;
             MotorThrottle2=0.0f;
             LayDown();
-            ControlTicker.attach(&ControlLoop, Interval);
+            //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);
+            //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;  
-            pc.printf("%f\n\r", PosRef2);
+            //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;
-            pc.printf("%f\n\r", PosRef1);
+            //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;
-            pc.printf("%f\n\r", PosRef1);
+            //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);
+            //ControlTicker.detach();
+            //MotorThrottle1=0.0f;
+            //MotorThrottle2=0.0f;
+            //HomingLoop();
+            //ControlTicker.attach(&ControlLoop, Interval);
             
             break;
         default:
@@ -341,45 +368,47 @@
 
 
 
-
+// Give Reference Position
 void DeterminePosRef(){
-    PosRef1=2*pi*PotMeter1.read(); // Reference in Rad
-    PosRef2=2*pi*PotMeter2.read(); // Reference in Rad
+    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");
+    pc.printf("startup\r\n");
     
-    controller1.setInputLimits(0.5f,2.0f*pi);
-    controller2.setInputLimits(0.5f,2.0f*pi);
+    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);  
-    
-    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);
-    
-    
+  
+    pc.printf("Homing \r\n");
     HomingLoop();
-    
-    
-    ControlTicker.attach(&ControlLoop, Interval);
-    //GetRefTicker.attach(&DeterminePosRef, 0.1f);
-    
+    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.01);
+            wait(0.1);
         }
     }