From 83ae00b88bd51f08e6ddbf3d5d215147e2cdac8a Mon Sep 17 00:00:00 2001 From: Nandan Tumu <10603428+nandantumu@users.noreply.github.com> Date: Fri, 23 Mar 2018 00:55:01 -0400 Subject: [PATCH] Modified angle calculations to take into account signage. --- src/geo_controller.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/geo_controller.py b/src/geo_controller.py index 8250678..4e63167 100755 --- a/src/geo_controller.py +++ b/src/geo_controller.py @@ -53,9 +53,12 @@ def main(): print(counter) counter += 1 print(ranges) - cr = np.sqrt(ranges[RIGHTBOT]**2 + ranges[RIGHTTOP]**2 - (2*ranges[RIGHTBOT]*ranges[RIGHTTOP]*np.cos(theta[RIGHTBOT]-theta[RIGHTTOP]))) - wr = np.arcsin((np.sin(theta[RIGHTBOT]-theta[RIGHTTOP])*ranges[RIGHTTOP])/cr) - thetar = (math.pi/2) - wr + delx = (math.sin(theta[RIGHTBOT])*ranges[RIGHTBOT]) - (math.sin(theta[RIGHTTOP])*ranges[RIGHTTOP]) + if(theta[RIGHTBOT]==(-math.pi/2) or theta[RIGHTBOT]==(-math.pi/2)): + 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 steering_input = interp(thetar, [np.radians(-20),np.radians(20)], [-100,100]) print("ThetaR:",thetar, "|Steering Angle:", steering_input) message = drive_params()