Diff: main.cpp

Revision:

27:456e8a47f815

Parent:

26:bb7e14f59ddd

Child:

28:6d8c6fe79394

--- a/main.cpp	Wed Oct 26 09:53:35 2016 +0000
+++ b/main.cpp	Wed Oct 26 20:24:58 2016 +0000
@@ -229,13 +229,13 @@
     q2_refOutNew = PI - asin(PointVector*sin(beta)/L2);
     
     //check angle boundaries
-    if (q1_refOutNew > q1_refOutMin && q1_refOutMax < q1_refOutMax){
+    if (q1_refOutNew > q1_refOutMin && q1_refOutNew < q1_refOutMax){
         q1_refOut = q1_refOutNew;
         }
     else {
         q1_refOut = q1_refOut;
         }
-    if (q2_refOutNew > q2_refOutMin && q2_refOutMax < q2_refOutMax){
+    if (q2_refOutNew > q2_refOutMin && q2_refOutNew < q2_refOutMax){
         q2_refOut = q2_refOutNew;
         }
     else {
@@ -292,7 +292,7 @@
     //float maxKi = 0.2;
     float Ki = 0.1; //potMeter2.read();
     
-    float q1DerivativeError = (q1_error_prev + q1_error)/t_sample;  // derivative of error in radians
+    float q1DerivativeError = (q1_error - q1_error_prev)/t_sample;  // derivative of error in radians
     float q2DerivativeError = (q2_error_prev + q2_error)/t_sample;  // derivative of error in radians
     //float maxKd = 0.2;
     float Kd = 0.0; //potMeter2.read();