Diff: main.cpp

Revision:

16:1dd69f935b80

Parent:

15:207d01972e0b

Child:

17:ee159658326e

--- a/main.cpp	Tue Oct 31 11:07:42 2017 +0000
+++ b/main.cpp	Tue Oct 31 15:45:03 2017 +0000
@@ -54,9 +54,9 @@
 volatile double Pos2;                        // Encoder readout of motor 2 [counts]
 volatile int count = 0;                         // Loop counts
 
-double Kp = 25.0;// you can set these constants however you like depending on trial & error
+double Kp = 260.0;// you can set these constants however you like depending on trial & error
 double Ki = 10.0;
-double Kd = 10.0;
+double Kd = 15.0;
 
 float last_error = 0;
 float error1 = 0;
@@ -98,7 +98,6 @@
 float Vy_des;          // Desired velocity end-effector in y-direction [m/s]
 float Vz_des;          // Desired velocity end-effector in z-direction [m/s]
 
-const float T = 0.002;          // Time interval of the readout of the EMG --> Check this with the ticker, etc!!!
 const float SampleTime = 0.002;     // Time interval of the mainticker
 
 // --- Booleans for the maintickerfunction ---
@@ -261,19 +260,15 @@
     V1 = Q_dot(1,1);
     W2 = Q_dot(2,1);
     W3 = Q_dot(3,1);
-
+ 
     
-    Q1 += V1*T;                     // Predicted value for q1 [m]
-    Q2 += W2*T;                     // Predicted value for q2 [rad]
-    Q3 += W3*T;                    // Predicted value for q3 [rad]
+    Q1 += V1*SampleTime;                     // Predicted value for q1 [m]
+    Q2 += W2*SampleTime;                     // Predicted value for q2 [rad]
+    Q3 += W3*SampleTime;                    // Predicted value for q3 [rad]
 
-/*
-    if (count == 100) {
-        
-        pc.printf("Velocities v1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);
-        pc.printf("After velocities Q1 = %.3f, Q2 = %.3f, Q3 = %.3f\r\n", Q1,Q2,Q3);
-    }
-*/
+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);}
 }
 
 //--- State switch function -----
@@ -319,6 +314,7 @@
     Pos1 =  motor1.getPosition()/16800*d*pi;                 // Position of link 1 [m]
     Pos2 =  motor2.getPosition()/16800*2*pi;                 // Position of link 2 [m]
 
+    v_des = 0;
     /*
     if (Pos2 >= EMG_threshold)                             //  (pot.read()>= EMG_threshold)
     {
@@ -333,13 +329,14 @@
     */
     
     if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
-          v_des = emg1.normalized;
+          v_des = 0.8;
                    
     }
     
     if (emg2.normalized >= EMG_threshold && emg1.normalized <= EMG_threshold) { // Negative direction
-             v_des = -emg2.normalized;
-        }      
+             v_des = -0.8;
+        }  
+        
 }
 
 
@@ -352,8 +349,8 @@
 
     error1 = q1 - Pos1;                             // Position error of link 1 [m]
     error2 = q2 - Pos2;                                  // Position error of link 2 [m]
-
-  
+   
+         
     changeError = (error1 - last_error)/SampleTime; // derivative term
     totalError += error1*SampleTime; //accumalate errors to find integral term
     pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
@@ -399,7 +396,8 @@
     }
 
 if (count%100 == 0){
-    //pc.printf("pidterm = %f, pidterm2 = %f\r\n", pidTerm, pidTerm2);}
+    //pc.printf("pidterm = %f, pidterm2 = %f\r\n", pidTerm, pidTerm2);
+    }
     
     last_error = error1;
     speed1 = pidTerm_scaled+0.45f;
@@ -407,6 +405,9 @@
     last_error2 = error2;
     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 ---
@@ -415,8 +416,6 @@
     setpointreadout(Vx_des);                       // Start with obtaining the position of the robot and the desired velocities
     Jacobian(Vx_des, 0, 0);                         // 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.
-    if (count%100 == 0)[
-    pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
 }
 
 void r_movevertical()
@@ -424,8 +423,7 @@
     setpointreadout(Vy_des);                       // Start with obtaining the position of the robot and the desired velocities
     Jacobian(0, Vy_des, 0);                         // 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.
-if (count%100 == 0)[
-    pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
+
 }
 
 void r_movebase()
@@ -433,8 +431,6 @@
     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
     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.
-if (count%100 == 0)[
-    pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
 }
 //--------------------------------
 
@@ -474,6 +470,7 @@
 
 int main()
 {
+    pc.printf("Hello Serotonin");
     pc.baud(115200);
     go_calibration = true;              // Setting the timeout variable
     go_switch = false;                  // Setting ticker variables
@@ -500,7 +497,7 @@
         if(count%100 == 0) {
            
            //pc.printf("MVC1 = %f, MVC2 = %f\r\n", emg1.MVC, emg2.MVC);
-            //pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
+            pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
             //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
             //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
         }