Diff: inbetweencontroller.cpp

Revision:

12:6dbed55bcf43

Parent:

11:3914c3153607

Child:

13:ae8af9693111

--- a/inbetweencontroller.cpp	Fri Oct 30 09:49:05 2015 +0000
+++ b/inbetweencontroller.cpp	Fri Oct 30 10:41:22 2015 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "compute.h" 
 #include "inbetweencontroller.h"
+#include <algorithm>
 #include <stdlib.h>
 #include <stdio.h>
 
@@ -33,7 +34,7 @@
     if(emg3 > 0.5 && !goingBack) {
         // if (!was_pushing) Angles2Point(rad_a, rad_b, cur_x, cur_y);
         next_x = cur_x;
-        next_y = max('Y', cur_x);
+        next_y = compute_max('Y', cur_x);
         was_pushing = true;
         pushing = true;
     } else {
@@ -41,7 +42,7 @@
             goingBack = true;
             Point2Angles(x_start, y_start, nexta, nextb);
             pushing = false;
-            if(fabs(cur_x-x_start) < 5 && fabs(cur_y-y_start) < 5) {
+            if(fabs(cur_x-x_start) < 2 && fabs(cur_y-y_start) < 5) {
                 was_pushing = false;
                 goingBack = false;
             }
@@ -50,10 +51,11 @@
         // if (dir != prev_dir) Angles2Point(rad_a, rad_b, cur_x, cur_y);
 
         next_y = y_start;
+        pc3.printf("NEX X: %f | CUR: %f\r\n",next_x, cur_x);
         if(dir == 0) next_x = cur_x;
         else if(dir > 0) {
             if (cur_x >= 15) {
-                next_x = max('X', next_y) - 8;
+                next_x = compute_max('X', next_y) - 8;
             } else if (cur_x >= 6) {
                 next_x = 18;
             } else if (cur_x >= -3) {
@@ -67,7 +69,7 @@
             }
         } else {
             if (cur_x <= -15)
-                next_x = -max('X', next_y) + 8;
+                next_x = -compute_max('X', next_y) + 8;
             else if (cur_x <= -6) {
                 next_x = -18;
             } else if (cur_x <= 3) {
@@ -85,6 +87,8 @@
         pushing = false;
     }
     
+    pc3.printf("NEX X: %f\r\n", next_x);
+    
     prev_dir = dir;
     Point2Angles(next_x, next_y, next_a, next_b);
     nexta = next_a;