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()