Revision 14:1428f0ba1315, committed 2015-11-03

Comitter:

AeroKev

Date:

Tue Nov 03 11:59:20 2015 +0000

Parent:

13:ae8af9693111

Commit message:

I lied

Changed in this revision

diff -r ae8af9693111 -r 1428f0ba1315 inbetweencontroller.cpp
--- a/inbetweencontroller.cpp	Tue Nov 03 10:15:37 2015 +0000
+++ b/inbetweencontroller.cpp	Tue Nov 03 11:59:20 2015 +0000
@@ -3,14 +3,11 @@
 #include "inbetweencontroller.h"
 #include <algorithm>
 
-double      emg1, emg2, emg3;
 double      x_start = 0;
 double      y_start = 18;
-double      begin_a, begin_b;
-double      prev_dir, v;
+double      prev_dir;
 bool        was_pushing = false;
 bool        goingBack = false;
-double      cur_x, cur_y;
 
 void IBC_init(double& ba, double& bb) {
    Point2Angles(x_start,y_start,ba,bb);
@@ -19,6 +16,7 @@
 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 dir = emg1-emg2;
+    double cur_x, cur_y;
     
     Angles2Point(rad_a, rad_b, cur_x, cur_y);
     
@@ -28,12 +26,15 @@
         next_y = compute_max('Y', cur_x);
         was_pushing = true;
         pushing = true;
-    } else {
+    } 
+    else {
         if(was_pushing) {
             goingBack = true;
             Point2Angles(x_start, y_start, nexta, nextb);
             pushing = false;
-            if(fabs(cur_x-x_start) < 2 && fabs(cur_y-y_start) < 5) {
+            
+            // If the current x and y are close nough to the starting position, stop going back and return control to the player
+            if(fabs(cur_x-x_start) < 5 && fabs(cur_y-y_start) < 5) {
                 was_pushing = false;
                 goingBack = false;
             }
@@ -42,6 +43,8 @@
 
         next_y = y_start;
         if(dir == 0) next_x = cur_x;
+        
+        // If the robot is moving, move it to one of the waypoints
         else if(dir > 0) {
             if (cur_x >= 15) {
                 next_x = compute_max('X', next_y) - 8;
@@ -56,7 +59,8 @@
             } else {
                 next_x = -18;
             }
-        } else {
+        } 
+        else {
             if (cur_x <= -15)
                 next_x = -compute_max('X', next_y) + 8;
             else if (cur_x <= -6) {
@@ -71,12 +75,13 @@
                 next_x = 18;
             }
         }
-        
         was_pushing = false;
         pushing = false;
     }
     
     prev_dir = dir;
+    
+    // Calculate the angle the motors need to be in in order to get to the desired point.
     Point2Angles(next_x, next_y, next_a, next_b);
     nexta = next_a;
     nextb = next_b;