diff --git a/src/geo_controller.py b/src/geo_controller.py index 4e63167..1f95bd9 100755 --- 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()