Diff: main.cpp

Revision:

3:98ea6afa0cf2

Parent:

2:684d5cf2f683

Child:

4:c45eaa904b09

--- a/main.cpp	Mon Oct 23 10:57:11 2017 +0000
+++ b/main.cpp	Thu Oct 26 19:48:03 2017 +0000
@@ -3,6 +3,9 @@
 #include "PID.h"
 #include "ReceiverIR.h"
 #include "Servo.h"
+#include <iostream>
+#include <string>
+#include <math.h>
 
 ReceiverIR ir_rx(D2);
 
@@ -19,9 +22,6 @@
 DigitalIn EndSwitch1(D9);
 DigitalIn EndSwitch2(D8);
 
-//DigitalOut Magneet(D7);
-
-
 QEI EncoderMotor1(D12,D13,NC,4200);
 QEI EncoderMotor2(D10,D11,NC,4200);
 
@@ -29,10 +29,22 @@
 Ticker GetRefTicker;
 
 float Interval=0.02f;
-float pi=3.14159268;
+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];
 
@@ -41,7 +53,130 @@
 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(){
@@ -134,19 +269,19 @@
     switch(state)
     {
         case 22: //1
-            printf("1\n\r");
+            pc.printf("1\n\r");
         break;
         case 25: //1
-            printf("2\n\r");
+            pc.printf("2\n\r");
             break;
         case 13: //1                
-            printf("3\n\r");
+            pc.printf("3\n\r");
             break;
         case 12: //1
-            printf("4\n\r");
+            pc.printf("4\n\r");
             break;
         case 24: //1
-            printf("5\n\r");
+            pc.printf("5\n\r");
             ControlTicker.detach();
             MotorThrottle1=0.0f;
             MotorThrottle2=0.0f;
@@ -154,13 +289,13 @@
             ControlTicker.attach(&ControlLoop, Interval);
             break;
         case 94: //1
-            printf("6\n\r");
+            pc.printf("6\n\r");
             break;
         case 8: //1
-            printf("7\n\r");
+            pc.printf("7\n\r");
             break;
         case 28: //1
-            printf("8\n\r");
+            pc.printf("8\n\r");
             ControlTicker.detach();
             MotorThrottle1=0.0f;
             MotorThrottle2=0.0f;
@@ -168,7 +303,7 @@
             ControlTicker.attach(&ControlLoop, Interval);
             break;
         case 90: //1
-            printf("9\n\r");
+            pc.printf("9\n\r");
             break;
         case 70: //1
             pc.printf("Boven\n\r");
@@ -221,6 +356,13 @@
     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();