Diff: inbetweencontroller.cpp

Revision:

6:7d8b49d91e6b

Parent:

5:0210ba224025

Child:

7:d7cf5ed50875

--- a/inbetweencontroller.cpp	Mon Oct 26 12:13:30 2015 +0000
+++ b/inbetweencontroller.cpp	Tue Oct 27 11:42:44 2015 +0000
@@ -6,7 +6,7 @@
 
 using namespace std;
 
-Serial      pc3(USBTX, USBRX);
+Serial pc3(USBTX, USBRX);
 
 double emg1, emg2, emg3; // Right/left
 double x_start = 0;
@@ -28,28 +28,14 @@
     if(emg3 > 0.5) {
         Point2Angles(cur_x, max('Y', cur_x), next_a, next_b);
     } else { 
-        double next_x = cur_x+(dir*v);
+        double next_x = cur_x+dir;
         double max_x = max('X', cur_y)-3;
-        double min_x = -max_x+3;
-        if(next_x >= 0) next_x = (next_x > max_x ? max_x : next_x); 
-        else next_x = (next_x < min_x ? min_x : next_x); 
-        Point2Angles(next_x, cur_y, next_a, next_b);
-        pc3.printf("Next_x: %f\r\nMax X: %f\r\nMin X: %f",next_x,max_x,min_x);   
+        double min_x = -max_x;
+        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);
     }
     nexta = next_a;
     nextb = next_b;
-}
-
-// int main() {
-//  init();
-//  double rad_a = deg2rad(192);
-//  double rad_b = deg2rad(36);
-//  double emg1 = 5;
-//  double emg2 = 0.0;
-//  double emg3 = 0.3;
-//  double next_a, next_b;
-//  double cur_a, cur_b;
-//  newPos(emg1, emg2, emg3, rad_a, rad_b, next_a, next_b);
-//     printf("next_a, next_b = %f, %f", next_a*(180/3.14), next_b*(180/3.14));
-//  return 0;
-// }
\ No newline at end of file
+}
\ No newline at end of file