Diff: main.cpp

Revision:

17:ee159658326e

Parent:

16:1dd69f935b80

--- a/main.cpp	Tue Oct 31 15:45:03 2017 +0000
+++ b/main.cpp	Tue Oct 31 16:29:25 2017 +0000
@@ -54,9 +54,9 @@
 volatile double Pos2;                        // Encoder readout of motor 2 [counts]
 volatile int count = 0;                         // Loop counts
 
-double Kp = 260.0;// you can set these constants however you like depending on trial & error
-double Ki = 10.0;
-double Kd = 15.0;
+double Kp = 50.0;// you can set these constants however you like depending on trial & error
+double Ki = 25.0;
+double Kd = 0;
 
 float last_error = 0;
 float error1 = 0;
@@ -268,7 +268,8 @@
 
 if (count%100 == 0){
     pc.printf("\r\n Pos1 = %f, Pos2 = %f, Q1 = %f, Q2 = %f, Q3 = %f\r\n", Pos1, Pos2, Q1, Q2, Q3);
-    pc.printf("\r\n V1 = %f, W2 = %f, W3 = %f\r\n", V1, W2, W3);}
+    pc.printf("\r\n V1 = %f, W2 = %f, W3 = %f\r\n", V1, W2, W3);
+    pc.printf("\r\n encoder = %d,\r\n", motor1.getPosition());}
 }
 
 //--- State switch function -----
@@ -396,7 +397,7 @@
     }
 
 if (count%100 == 0){
-    //pc.printf("pidterm = %f, pidterm2 = %f\r\n", pidTerm, pidTerm2);
+    pc.printf("TotalError = %f, error = %f\r\n", totalError, error1);
     }
     
     last_error = error1;
@@ -406,8 +407,6 @@
     speed2 = pidTerm_scaled2+0.45f;
     //speedservo2 = speed2;
     
-     if (count%100 == 0){
-         pc.printf("error1 = %f, error2 = %f, pidTerm = %f, pidTerm2 = %f\r\n", error1, error2, pidTerm_scaled, pidTerm_scaled2);}
 }
 
 // --- Motor movements ---
@@ -429,7 +428,7 @@
 void r_movebase()
 {
     setpointreadout(Vz_des);                       // Start with obtaining the position of the robot and the desired velocities
-    Jacobian(0, 0, Vz_des);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
+    Jacobian(0, 0, 4*Vz_des);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
     PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
 }
 //--------------------------------