Diff: main.cpp

Revision:

25:596255732b65

Parent:

24:393da1cc1fa8

Child:

26:bb7e14f59ddd

--- a/main.cpp	Wed Oct 26 09:24:38 2016 +0000
+++ b/main.cpp	Wed Oct 26 09:38:42 2016 +0000
@@ -90,6 +90,8 @@
 
 float q1_error_prev = 0;
 float q2_error_prev = 0;
+float DerTotalError1 = 0;
+float DerTotalError2 = 0;
 float q1IntError = 0;
 float q2IntError = 0;
 float TotalError1= 0;
@@ -313,26 +315,30 @@
         TotalError2=TotalError2;
         }
     
+    /*
+    DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
+    DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
+    motorValue1Out = DerTotalError1/MotorGain;       
+    motorValue2Out = DerTotalError2/MotorGain;       
+    */
+    
     motorValue1Out = TotalError1/MotorGain;       
     motorValue2Out = TotalError2/MotorGain;       
     
-    //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
-    //pc.printf("Counts encoder1: %i rad \r\n", counts1);
-    //pc.printf("Kp: %f \r\n", Kp);
-    //pc.printf("MotorValue: %f \r\n", motorValue1);
-    
     pc.printf("error1: %f \r\n", q1_error);
     pc.printf("IntError1: %f \r\n", q1IntError);
     pc.printf("DerError1: %f \r\n", q1DerivativeError);
     pc.printf("error2: %f \r\n", q2_error);
     pc.printf("IntError2: %f \r\n", q2IntError);
     pc.printf("DerError2: %f \r\n", q2DerivativeError);
-        
+    
+    /*    
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
-    //float biceps_l = !button1.read();
-    //float biceps_r = !button2.read();
-       
+    TotalError1_prev = TotalError1;
+    TotalError1_prev = TotalError1;
+    */
+     
     /*
     scope.set(0,q1);
     scope.set(1,q2);