Diff: main.cpp

Revision:

10:91173ed1e841

Parent:

9:4f594927cff3

Child:

11:c7e27de26ac0

--- a/main.cpp	Thu Nov 01 08:30:56 2018 +0000
+++ b/main.cpp	Thu Nov 01 16:50:47 2018 +0000
@@ -12,7 +12,7 @@
 int sensor_sensitivity = 32;
 int gear_ratio = 131;
 float full_ratio  = gear_ratio*sensor_sensitivity*4;
-
+double pi = 3.141592653589793238462643383279502884;
 DigitalIn but1(D2);
 
 QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A
@@ -153,6 +153,8 @@
  
     int gain = 3;
     
+    int sign1 = 0.5;
+    int sign2 = 1;
 void do_forward(){
     
     
@@ -368,7 +370,7 @@
        threshold2 = 0.5 * max2;
        }
        
-    if (timer.read() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
+    if (timer.read() > 1 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
         current_state = operational;
         timer.reset();
         state_changed = true;
@@ -427,9 +429,9 @@
     float inv_jacobian[4];
     
     jacobian[0] = L1;
-    jacobian[1] = (L2*cos(deg_m2))+L1;
+    jacobian[1] = (L2*cos(deg_m2/180*pi))+L1;
     jacobian[2] = -L0;
-    jacobian[3] = -L0 - (L2*sin(deg_m2));
+    jacobian[3] = -L0 - (L2*sin(deg_m2/180*pi));
     
     float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2]));
     
@@ -444,8 +446,8 @@
        
        
      
-    ref_q1 = ref_q1 + 0.002*ref_v1;
-    ref_q2 = ref_q2 + 0.002*ref_v2;
+    ref_q1 = ref_q1 + 0.002*ref_v1 * sign1;
+    ref_q2 = ref_q2 + 0.002*ref_v2 * sign2;
     
     error1 = deg_m1 - ref_q1;
     error2 = deg_m2 - ref_q2;