Fixed calculations

ret15108 · Mar 23, 2018 · dcc1171 · dcc1171
1 parent 83ae00b
commit dcc1171
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/geo_controller.py b/src/geo_controller.py
@@ -58,7 +58,7 @@ def main():
             dely = (math.sin(theta[RIGHTTOP])*ranges[RIGHTTOP])
         else:
             dely = (math.sin(theta[RIGHTTOP])*ranges[RIGHTTOP]) - (math.sin(theta[RIGHTBOT])*ranges[RIGHTBOT])
-        thetar = -1* np.arctan(dely/delx) - np.pi/2
+        thetar = -1* np.arctan(dely/delx) + np.pi/2
         steering_input = interp(thetar, [np.radians(-20),np.radians(20)], [-100,100])
         print("ThetaR:",thetar, "|Steering Angle:", steering_input)
         message = drive_params()