Diff: inbetweencontroller.cpp

Revision:

7:d7cf5ed50875

Parent:

6:7d8b49d91e6b

Child:

8:d3c4444d21d0

Child:

9:fdb66e7bc47a

diff -r 7d8b49d91e6b -r d7cf5ed50875 inbetweencontroller.cpp
--- a/inbetweencontroller.cpp	Tue Oct 27 11:42:44 2015 +0000
+++ b/inbetweencontroller.cpp	Wed Oct 28 13:40:06 2015 +0000
@@ -13,29 +13,71 @@
 double y_start = 18;
 double begin_a, begin_b;
 double prev_dir, v;
+bool was_pushing = false;
+double cur_x, cur_y;
 
 void IBC_init(double& ba, double& bb) {
    Point2Angles(x_start,y_start,ba,bb);
 }
 
-void newPos(double emg1, double emg2, double emg3, double rad_a, double rad_b, double& nexta, double& nextb) {
+int np_test_tick = 0;
+void newPos(double emg1, double emg2, double emg3, double rad_a, double rad_b, double& nexta, double& nextb, bool& pushing) {
     double next_a, next_b;
-    double cur_x, cur_y;
+    double dir = emg1-emg2;
+    // v = (prev_dir==dir) ? ((v==16) ? 16 : v*2) : 1;
+    // prev_dir = dir;
+    
     Angles2Point(rad_a, rad_b, cur_x, cur_y);
-    double dir = emg1-emg2;
-    v = (prev_dir==dir) ? ((v==16) ? 16 : v*2) : 1;
-    prev_dir = dir;
+    
+    double next_x, next_y;
     if(emg3 > 0.5) {
-        Point2Angles(cur_x, max('Y', cur_x), next_a, next_b);
-    } else { 
-        double next_x = cur_x+dir;
-        double max_x = max('X', cur_y)-3;
-        double min_x = -max_x;
+        // if (!was_pushing) Angles2Point(rad_a, rad_b, cur_x, cur_y);
+        next_x = cur_x;
+        next_y = max('Y', cur_x);
+        was_pushing = true;
+        pushing = true;
+    } else {
+        // if (dir != prev_dir) Angles2Point(rad_a, rad_b, cur_x, cur_y);
+
+        next_y = y_start;
         if(dir == 0) next_x = cur_x;
-        else if(dir > 0) next_x = max_x; 
-        else next_x = min_x; 
-        Point2Angles(next_x, y_start, next_a, next_b);
+        else if(dir > 0) {
+            if (cur_x > 15) {
+                next_x = max('X', next_y) - 8;
+            } else if (cur_x > 10) {
+                next_x = 20;
+            } else if (cur_x > 0) {
+                next_x = 15;
+            } else if (cur_x > -10) {
+                next_x = 5;
+            } else if (cur_x > -20) {
+                next_x = -5;
+            } else {
+                next_x = -15;
+            }
+        } else {
+            if (cur_x < -15)
+                next_x = -max('X', next_y) + 8;
+            else if (cur_x < -10) {
+                next_x = -20;
+            } else if (cur_x < 0) {
+                next_x = -15;
+            } else if (cur_x < 10) {
+                next_x = -5;
+            } else if (cur_x < 20) {
+                next_x = 5;
+            } else {
+                next_x = 15;
+            }
+        }
+        
+        was_pushing = false;
+        pushing = false;
     }
+    
+    prev_dir = dir;
+    Point2Angles(next_x, next_y, next_a, next_b);
     nexta = next_a;
     nextb = next_b;
+    pc3.printf("%f, %f, %f, %f, %f, %f, %f\r\n", emg1, emg2, emg3, next_x, next_y, next_a, next_b);
 }
\ No newline at end of file