Start adding other methods for tracking.

tracking/algorithms.py

@@ -65,6 +65,31 @@ def convert_lidar_to_cartesian(new_point):
     return (x, y)
 
 
+def convert_cartesian_to_lidar(x, y):
+    """
+    Converts a point on the grid (with car as the origin) to a lidar tuple (distance, angle)
+
+    Parameters
+    ----------
+    x
+        Horizontal component of point to convert.
+
+    y
+        Vertical component of point to convert.
+
+    Returns
+    -------
+    converted
+        A tuple (distance, angle) that represents the point. Angle is in degrees.
+    """
+    # Angle depends on x/y position.
+    # if x is positive and y is positive, then angle = 90 - tan-1(y/x)
+    # if x is positive and y is negative, then angle = 90 + tan-1(y/x)
+    # if x is negative and y is positive, then angle = 270 + tan-1(y/x)
+    # if x is negative and y is negative, then angle = 270 - tan-1(y/x)
+    return (math.sqrt(x ** 2 + y ** 2), math.degrees(math.atan(y/x) + (180 if ))
+
+
 def calc_groups(scan):
     """
     Calculates groups of points from a lidar scan. The scan should
@@ -131,8 +156,29 @@ def assign_groups(prev_groups, new_groups):
 
 def updateCarVelocity(oldGroup, newGroup):
     """
-    Return a vector indicating how the tracked group has changed, which can
+    Return a tuple (DistanceChange, AngleChange) indicating how the tracked groups have changed, which can
     be used to then update the steering/throttle of the car (or other vehicle that
     may be used)
     """
     pass
+
+
+def dualServoChange(newCentre, changeTuple):
+    """
+    Gets a tuple (throttleChange, steeringChange) indicating the change that should be applied to the current
+    throttle/steering of an rc car that uses dual servos.
+
+    Parameters
+    ---------
+    newCentre
+        Tuple (distance,angle) of the new centre of the tracked group.
+
+    changeTuple
+        Tuple (distanceChange, angleChange) from the old centre to the new centre.
+
+    Returns
+    -------
+    tuple
+        Tuple of (throttleChange, steeringChange) to apply to the 2 servos.
+    """
+    return ((changeTuple[0] / 3) - (newCentre[0] / 4) + 1, 0)