Merge pull request #121 from Open-STEM/ansel-58

Locating Nearby Object

Author: bradamiller

course/measuring_distance/locate_object.rst

@@ -5,75 +5,246 @@ Another way to utilize the ultrasonic rangefinder is to use it to locate a nearb
 
 .. admonition:: Try it out
 
-    Before reading more, brainstorm different ways to use the ultrasonic rangefinder to locate a nearby object. 
+    What's the most effective way to locate an object? Try it out!
 
-How?
-~~~~
+Turn and Detect
+~~~~~~~~~~~~~~~
 
-The easiest way to explain the intuition behind this process is sonar. 
+To first detect an object, the robot can slowly spin in a circle while continuously polling the rangefinder.
+Then, when an object is detected, it can stop spinning, and head towards the object.
 
-The robot will essentially spin in a circle and take a distance reading while it spins.
+How can we tell when an object is detected? Imagine that the robot is in the center of an empty room, and a
+random object is placed somewhere near the robot. The rangefinder would be giving large distance readings, until
+it reaches the object, at which point the distance reading would drop. It's the *change* in distance readings that
+hints that an object has been detected.
 
-Then, when an object is detected to be within a certain distance, the robot will stop spinning and go towards the object. 
+How can we find the change in distance readings over each iteration of the loop? We can store the previous distance
+reading in a variable, and compare it to the current distance reading. If this change is greater than some threshold,
+then we can assume that an object has been detected.
 
-This is a very simple way to locate an object, but it is also very effective (and will be especially helpful during your final project). 
+To code this, we can start by setting the drive motor speeds to spin in opposite directions to start spinning
+the robot in place. Then, once the change in distance reading is greater than
+the threshold, the robot can stop spinning and head towards the object.
 
-The First Step
-~~~~~~~~~~~~~~
+In the following example code, we use a change threshold of 30 cm.
 
-The first step is to spin the robot in a circle and stop when an object is detected.
+.. tab-set::
 
-To do this, utilize a while loop which instucts the robot to spin in a circle while the distance reading is greater than a certain value.
+    .. tab-item:: Python
 
-In the following example code, our "distance threshold" is 20 cm.
+        .. code-block:: python
 
-.. error:: 
+            changeThreshold = 30 # distance change in cm needed to trigger detection
 
-    TODO insert a video and code of a working example
+            # store initial value for current distance
+            currentDistance = rangefinder.distance()
 
-The Second Step
-~~~~~~~~~~~~~~~
+            # start spinning in place until an object is detected
+            drivetrain.set_speed(5, -5)
+
+            while True: # doesn't actually repeat forever. loop will be broken if an object is detected
+                
+                # update previous and current distance
+                previousDistance = currentDistance
+                currentDistance = rangefinder.distance()
+
+                # if sudden decrease in distance, then an object has been detected
+                if previousDistance - currentDistance > changeThreshold:
+                    break # break out of the while loop
+
+                time.sleep(0.1)
+
+            # stop spinning drive motors
+            drivetrain.stop()
+
+
+    .. tab-item:: Blockly
+
+        .. image:: media/detection.png
+            :width: 900
+
+Improving Accuracy
+~~~~~~~~~~~~~~~~~~
+
+Do you see any issues with this solution?
+
+When the rangefinder distance dips below the threshold, the implication isn't that the robot has found an object,
+but rather that the robot has found its *edge*. So, the robot will aim for the edge of the object, rather than the center.
+
+Instead, the robot should remember the heading it faces when detecting *both* edges. Then, the robot can aim for the center
+between those edges, and thus the center of the object. We can store each edge angle in variables, naming them :code:`firstAngle`
+and :code:`secondAngle`.
+
+We're already quite familiar with turning until an edge is detected. Now, we'll need to detect *both* edges. However, it would be
+quite unwieldy and error-prone to just copy the edge detection code, so let's make a function to generalize this. Note that the existing
+code detects a sudden *decrease* in distance, but we want to handle sudden *increases* in distances too.
+
+How can we support both behaviors in a single function? We can pass in a parameter to specify whether we want to detect an increase
+or decrease in distance! We can call this parameter :code:`isIncrease` and pass in a boolean (true or false) value.
+
+If :code:`increase` is :code:`True`, then we want to detect an increase in distance, which is when :code:`currentDistance - previousDistance > changeThreshold`.
+
+If :code:`increase` is :code:`False`, then we want to detect a decrease in distance, which is when :code:`previousDistance - currentDistance > changeThreshold`.
+
+For more flexibility, let's add a parameter for the change threshold.
+
+Here's the function definition:
+
+.. tab-set::
+
+    .. tab-item:: Python
+
+        .. code-block:: python
+
+            def turnUntilEdge(isIncrease, changeThreshold):
+
+                # store initial value for current distance
+                currentDistance = rangefinder.distance()
+
+                # start spinning in place until an object is detected
+                drivetrain.set_speed(5, -5)
+
+                while True: # doesn't actually repeat forever. loop will be broken if an object is detected
+                    
+                    # update previous and current distance
+                    previousDistance = currentDistance
+                    currentDistance = rangefinder.distance()
+
+                    if isIncrease and currentDistance - previousDistance > changeThreshold:
+                        # if sudden increase in distance, then an object has been detected
+                        break
+                    elif not isIncrease and previousDistance - currentDistance > changeThreshold:
+                        # if sudden decrease in distance, then an object has been detected
+                        break
+
+                    time.sleep(0.1)
+
+                # stop spinning drive motors
+                drivetrain.stop()
+
+
+    .. tab-item:: Blockly
+
+        .. image:: media/detectiondefinition.png
+            :width: 900
+
+Here's the equivalent function call to the turn and detection code in the previous section:
+
+.. tab-set::
+
+    .. tab-item:: Python
+
+        .. code-block:: python
+
+            turnUntilEdge(False, 30)
+
+    .. tab-item:: Blockly
+
+        .. image:: media/detectioncall.png
+            :width: 200
+
+Now, it's time to write the full program to detect both edges and turn to the center.
+
+Implementing Dual Edge Detection
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Let's walk through each step of the process in code.
+
+First, the robot should spin in place until it detects the first edge, then stop. This is simply the function call we saw earlier.
+
+.. tab-set::
+
+    .. tab-item:: Python
+
+        .. code-block:: python
+
+            turnUntilEdge(False, 30)
+
+    .. tab-item:: Blockly
+
+        .. image:: media/detectioncall.png
+            :width: 200
+
+Next, we want to record the robot's heading for this first edge, and store it to :code:`firstAngle`.
+
+.. tab-set::
+
+    .. tab-item:: Python
+
+        .. code-block:: python
+
+            firstAngle = imu.get_yaw()
+
+    .. tab-item:: Blockly
+
+        .. image:: media/firstangle.png
+            :width: 200
+
+Then, the robot should spin in place again until it detects the second edge, which is when there is a sudden increase in distance.
+
+.. tab-set::
+
+    .. tab-item:: Python
+
+        .. code-block:: python
+
+            turnUntilEdge(True, 30)
+
+    .. tab-item:: Blockly
+
+        .. image:: media/turntoedgetrue.png
+            :width: 200
 
-The second step is to go towards the object and stop when the object is within a certain distance.
+Once the robot has detected the second edge, it should record its heading and store it to :code:`secondAngle`. Now, need to figure
+out how much the robot needs to backtrack to aim for the center of the object. We can do this by finding the difference between
+the two angles, and dividing by two. This is half the angle between the two edges, and if the robot backtracks by this amount,
+it will be facing the center of the object. Let's store this in a variable called :code:`angleToTurn`.
 
-To do this, integrate the code from the previous example and add a while loop which instructs the robot to move forward while the distance reading is greater than a certain value.
+.. tab-set::
 
-In the following example code, our "distance threshold" is 5 cm.
+    .. tab-item:: Python
 
-.. error:: 
+        .. code-block:: python
 
-    TODO insert a video and code of a working example
+            secondAngle = imu.get_yaw()
+            angleToTurn = (firstAngle - secondAngle) / 2
 
-The Problem with this Method
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+    .. tab-item:: Blockly
 
-The problem with this method is that it is not very accurate.
+        .. image:: media/angletoturn.png
+            :width: 400
 
-The reason for this is that our robot is currently acting on the "edge" of the object that it sees. 
+Finally, the robot can turn this much to face the center of the object, and head towards it.
 
-This means that the robot will not be able to accurately locate the center of the object.
+Here's the full code. Note that half-second pauses are added to make the robot's actions more visible:
 
-The Solution
-~~~~~~~~~~~~
+.. tab-set::
 
-The solution to this problem is to use "flags" to keep track of the edges of an object. 
+    .. tab-item:: Python
 
-In this case, we can use a "first edge" flag and a "second edge" flag.
+        .. code-block:: python
 
-The first edge flag will be set to true when the robot first detects a sudden decrease in distance measurements (i.e. the robot detects the first edge of an object)
+            # turn to first edge
+            turnUntilEdge(False, 30)
 
-When the first edge is set to be true, the robot will take note of that angle, let's call it "firstAngle"
+            # store angle at first edge
+            firstAngle = imu.get_yaw()
 
-The second edge flag will be set to true when the robot detects a sudden increase in distance measurements (i.e. the robot detects the second edge of an object)
+            time.sleep(0.5)
 
-Now that the second edge is also set to be true, the robot can then take note of that angle, let's call it "secondAngle".
+            # turn to second edge
+            turnUntilEdge(True, 30)
 
-We then know that the center of the object is at the angle halfway between firstAngle and secondAngle.
+            # store angle at second edge and calculate angle to turn
+            secondAngle = imu.get_yaw()
+            angleToTurn = (firstAngle - secondAngle) / 2
 
-To find that angle, we can take the average of firstAngle and secondAngle; and then save that angle as "centerAngle".
+            time.sleep(0.5)
 
-This method will allow the robot to accurately locate the center of an object.
+            # turn to center of object
+            drivetrain.turn(angleToTurn)
 
-.. error:: 
+    .. tab-item:: Blockly
 
-    TODO insert a video and code of a working example
+        .. image:: media/fulldualedge.png
+            :width: 400