Modified angle calculations to take into account signage.

ret15108 · Mar 23, 2018 · 83ae00b · 83ae00b
1 parent de345ee
commit 83ae00b
Showing 1 changed file with 6 additions and 3 deletions.
diff --git 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()