Diff: main.cpp

Revision:

6:e67250b8b100

Parent:

5:a1843b698d0d

Child:

7:1906d404cd1e

--- a/main.cpp	Thu Nov 01 12:35:45 2018 +0000
+++ b/main.cpp	Thu Nov 01 19:35:15 2018 +0000
@@ -20,7 +20,7 @@
 DigitalOut led1(LED1); // Red led
 DigitalOut led2(LED2); // Green led
 DigitalOut led3(LED3); // Blue led
-QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
+QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING);
 QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
 //QEI encoder3(A4, A5), NC, 4200);
 AnalogIn pot(A4); // Speed knob
@@ -81,15 +81,17 @@
 double Dpulses; // Global variable for printf
 double error1; // Global variable for printf
 double error2; // Global variable for printf
+double pulses1; // Global varaible for printf
+double pulses2; // Global varaible for printf
 
 double C[5][5];
 
-double Kp1 = 7;
-double Ki1 = 0.3;
-double Kd1 = 0.4;
-double Kp2 = 5;
-double Ki2 = 0.3;
-double Kd2 = 0.4;
+double Kp1 = 150;
+double Ki1 = 1;
+double Kd1 = 10;
+double Kp2 = 50;
+double Ki2 = 0.1;
+double Kd2 = 10;
 
 const int samplepack = 250; //Amount of data points before one cycle completes
 volatile int counter = 0; //Counts the amount of readings this cycle
@@ -139,12 +141,14 @@
         if (total[3] >= refvaluecalf){
             moving[3] = 1;
         }
-        pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
+        //pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
+        pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
         counter = 0; //Reset for next cycle
         for (int i=0;i<4;i++){ //clear 'total' array
             total[i] = 0;
         }
     }
+
     if(moving[0]){
         setPointX += incr;
     }
@@ -157,7 +161,7 @@
     if (moving[3]){
         setPointY -= incr;
     }
-    //pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
+   
 }
 
 double measureVelocity (int motor)
@@ -194,10 +198,10 @@
 
 void measurePosition() // Measure actual angle position with the encoder
 {
-    double pulses1 = encoder1.getPulses();
-    double pulses2 = encoder2.getPulses();
-    qMeas1 = (pulses1) * 2 * PI / 8400 + 140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
-    qMeas2 = -qMeas1 +3*PI/180 ;
+     pulses1 = encoder2.getPulses();   // motor 1 is on M2 and ENC2 of biorobotics shield
+     pulses2 = encoder1.getPulses();   // motor 2 is on M1 and ENC1 of biorobotics shield
+    qMeas1 = (pulses1) * 2 * PI / 8400  +140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
+    qMeas2 = (pulses2) * 2 * PI / 8400  - 93*PI/180 ;
 
 }
 
@@ -236,12 +240,10 @@
 void inverseKinematics ()
 {
     if (currentState == MovingState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
-        double potx = 0.55;//pot1.read()*0.546;
-        double  poty = 0.01;//pot2.read()*0.4;
-
+        
         double  Pe_set[3][1] {            // defining setpoint location of end effector
-            {potx},             //setting xcoord to pot 1
-            {poty},             // setting ycoord to pot 2
+            {setPointX},             //setting xcoord to pot 1
+            {setPointY},             // setting ycoord to pot 2
             {1}
         };
 
@@ -304,14 +306,14 @@
 
 //Determing 'Pulling" force to setpoint
 
-        double k= 0.005;     //virtual stifness of the force
+        double kspring= 1;     //virtual stifness of the force
         double Fs[3][1] {                                    //force vector from end effector to setpoint
-            {k*Pe_set[0][0] - k*Pe0[0][0]},
-            {k*Pe_set[1][0] - k*Pe0[1][0]},
-            {k*Pe_set[2][0] - k*Pe0[2][0]}
+            {kspring*Pe_set[0][0] - kspring*Pe0[0][0]},
+            {kspring*Pe_set[1][0] - kspring*Pe0[1][0]},
+            {kspring*Pe_set[2][0] - kspring*Pe0[2][0]}
         };
-        double Fx = k*potx - k*pe0x;
-        double Fy = k*poty - k*pe0y;
+        double Fx = kspring*setPointX - kspring*pe0x;
+        double Fy = kspring*setPointY - kspring*pe0y;
         double W0t[3][1] {
             {pe0x*Fy - pe0y*Fx},
             {Fx},
@@ -329,7 +331,7 @@
 
 //Calculating joint behaviour
 
-        double b1 =1;
+        double b1 =10;
         double b2 =1;
         //joint friction coefficent
         //double sampleTime = 1/1000;               //Time step to reach the new angle
@@ -477,7 +479,7 @@
                 led2 = 0; // EmisampleTime yellow together
                 //TODO: Set qRef1 and qRef2
                 qRef1 = 90 * PI / 180;
-                qRef2 = -qRef1 +0 *PI/180;
+                qRef2 = -qRef1 + 0 *PI/180;
                 stateChanged = false;
             }
 
@@ -606,7 +608,7 @@
         //int pulses = encoder1.getPulses();
         //pc.printf("pulses = %i\n", pulses);
        // pc.printf("v = %f\n", v);
-        //pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n", error1,error2,qRef1,qRef2);
+        pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n qMeas1 = %f qMeas2 = %f \n, Pulses1 = %f Pulses2 = %f \n", error1,error2,qRef1,qRef2,qMeas1,qMeas2, pulses1, pulses2);
         wait(2);
     }
 }
\ No newline at end of file