File System Structure

To maintain an organized and neat code, we have put different aspects of the line tracing and evacuation zone into different files, which are then all controlled and called upon using a main "driver" file. This separates our code into different files which allows us to easily fix an issue within that separate file. It also allows us to work on separate aspects of line tracing at the same time, and then easily using them by simply calling the functions in the driver file.

Lines 1-4 are importations of libraries. The time library provides the current time, which is useful for determining running speeds. The cv2 library is OpenCV, which is a library used for camera related operations such as accessing frames from a camera. Numpy is a library that is commonly imported when using python and it provides many functions that aid in computational operations with arrays as well as numbers. The serial library is used for communication between the megaPi and the Raspberry pi 3.

Lines 6-12 are importations of other files. Each of the files will be described in depth later on.

Line 14 opens the video stream and cap is the reference to that video stream. Lines 16-20 set certain properties of the video stream. Line 16-17 set the dimensions of the video, 320 x 240. Line 18 sets the frame rate at 60 frames per second. Line 19 sets the brightness at 70%. By increasing the brightness, it reduces shadows.

Lastly, line 21 sets up the communication reference coms as a global variable. theSerial.serialSetup() will be discussed later.

Serial Communication

The MegaPi and the Raspberry pi 3 B+ communicate with each other through serial. There are two sets of code from each side of the communication. The code from the raspberryPi will be shown first, followed by the code from the megaPI. The serial.py file, located on the raspberry pi, contains the functions called: serialSetup(), sendInfo(), and regularMotors().

The serialSetup() function is shown below:

Line 1 imports the header. This file was mentioned above and contains all of the libraries and functions as well as the global variables for the camera and serial reference. Lines 7-8 first declares the port and rate for the serial communication. Then the function initiates communication with the megaPi by declaring coms = header.serial.Serial(port,rate). We access the serial library, by referencing the header file. After the raspberry pi initiates the communication with the megaPi, it waits to receive a signal from the megaPi, indicating that the megaPI is ready to run. While the raspberry pi is waiting, it will constantly display the current frame. This allows for the user to center the robot before running. All of the functions of camera and calibrate will be discussed later. When the raspberry pi receives a signal from the megaPI (line 20), the raspberry pi will close down the video and return the serial reference.

The sendInfo() function is shown below:

The sendInfo() function sends motor values to the megaPi. The parameters of the function are left, signL, right, signR. These parameters dictate the speed, direction, and type of turn. Because it is risky to send over negatives over serial, signL and signR are there to indicate whether the left and right are negative. signL also has another purpose, it can also tell the megapi to run encoder motors. Line 25 changes the input parameters of the functions to bytes so that the information could be sent over serial. Then on line 26, the raspberry pi sends the info to the megaPi. If the signL is 4 or 5, it indicates a encoder motor movement, so the raspberry pi will wait for a signal from the megaPi that indicates that the encoder motor movement has finished. This eliminates the chance that the code on the raspberry pi continues to run while the encoders are in the process of moving.

Encoders are a method of having precise and exact movement. As the motors run, encoder tics are counted. This way, we can turn 90 degrees exactly or go forward 10 centimeters.

Function call examples:

sendInfo(100,0,100,0): This will set both motors to forward at 100 speed each.

sendInfo(100,1,100,1): Since the signL and signR are both 1, this means that both motors will run at -100 speed. So this call will set both motors to go backwards at 100 speed.

sendInfo(100,1,100,0): Since the signL is 1, this means that the left motor will run at -100 speed. So the left motor will run backwards at 100 speed, while the right motor will run forwards at 100 speed. This will cause the robot to do a point turn to the left.

sendInfo(90,4,0,0): Since signL is 4, it indicates an encoder turn. So this will turn 90 degrees to the left.

sendInfo(90,4,1,0): This is similar to the example above, but instead, it will turn 90 degrees to the right.

sendInfo(10,5,0,0): Since sighL is 5, it indicates an encoder movement. So this will go forward 10 centimeters.

sendInfo(10,5,1,0): This is very similar to the function above, but instead of going forwards, the robot goes backwards.

The regularMotors() motors function will be discussed along with line tracing.

Camera Calibration

Due to the location of the camera, we are forced to use a wide angled camera so we are able to see everything. We tried using a regular camera, but it was unable to see everything. One downside of having a fish eyed camera is that it distorts the frames and the video. This can be problematic in other parts of the line tracing such as intersections.

As you can see in this image, the line curves which makes the detection of intersections extremely difficult. The fish eye camera also distorts the image in a way that makes the line less extreme than it actually is. For example, if the line veered to the right suddenly, the fish eye distortion would make the line appear less extreme, thus causing a late reaction to the turn.

We solved this issue by calling the function cv2.fisheye.estimateNewCameraMatrixForUndistortRectify() and cv2,remap(). cv2.fisheye.estimateNewCameraMatrixForUndistortRectify determines a new scale to warp the image and takes in values K and D. The values of K and D are found by taking pictures of a checkerboard pattern and calling a calibrate function to determine these values. The values of K and D woudl change based on the levels of distortion of the checkerboard pattern. Once we have the new scale, we call cv2.fisheye.initUndistortRectifyMap which maps the the new scale. Then finally, the cv2.remap() function takes in the old image and remaps it based on the map found during the cv2.fisheye.initUndistortRectifyMap call. The end product is shown below.

Although the lines are straight, it would be more beneficial for a birds eye view of the image. To accomplish this we cal cv2.warpPerspective(). This function takes the original image and the transform value M. The transform value M is calculated through cv2.getPerspectiveTransform(). This function takes in four points which are the corners of the boundaries of the image and determines the transform value.

Image Processing

The image processing is all done in a single file called "camera.py." Like all the other files, it is imported through the header and can be accessed by all the files. The functions in the "camera.py" file are refresh(), getImage(), adpCOLOR2BAW(), COLOR2HSV(), COLOR2BAW(), COLOR2GREEN(), and display().

The functions, refresh(), getImage(), and display() are all utility functions.

This function refreshes the camera stream a certain number of times which is indicated through the parameter "number." The reason for this function is that after running for a certain amount of time without retrieving a frame, would cause the stream to lag out and be behind and when you access the video stream again, it would be behind in time. This refresh function allows the camera stream to catch up and give the current and correct video frame. The function cap.read() simply references the video stream reference and takes a single frame from it. Most of the time, we use refresh(50) which refreshes the camera by reading in 50 frames.

This function simply takes a frame from the camera and returns it. When put in a loop, this function can be used to display a video stream. Line 16 is a simple error check to determine if it the cap.read() function was successful in retrieving a frame. If it is not successful, it returns -1 and prints error.

The display() function displays the frame in a loop, which produces a video. We first get the frame(line 46), make a copy of it (line 47) and unwarp the image. Lines 49-50 will be discussed below. Then we display the image (line 51). Line 52 is similar to a user input stream, it constantly checks for whether the user enters a 'q', and when it does, it will stop the video. The cv2.waitKey() is not a blocking function so it continues to display the image. The main purpose of this function is for debugging purposes.

The functions adpCOLOR2BAW(), COLOR2HSV(), COLOR2BAW(), COLOR2GREEN() will be explained below:

First we retrieve a frame, then we apply a GaussianBlur on the image. This GaussainBlur is a method of noise cancellation. It smoothes the image by reducing detail, but it also removes interference from the camera. After we apply the blur, we change the color space of the image to HSV. HSV is a different color space than RGB, which is a more commonly known color space. HSV stands for hue, saturation, and brightness. HSV is especially useful for the use of cv2.inRange, a function that will be discussed later. Once we change the colorspace to HSV, we return the frame.

Although the difference between the original and blurred image are subtle, it is generally a good habit to apply a blur before you change the color space. Just in case something bad does happen, like for some reason there is an outside interference that will mess up the camera, then the blur will remove that interference.

The functions adpCOLOR2BAW(), COLOR2BAW(), COLOR2GREEN() all use the HSV color space and apply a mask to the image. This mask usually applies a cv2.inRange(). The purpose of this mask is to single out one color and ignore the rest.

The main purpose of this function is to turn the color image into a black and white image, with black being white and white becoming black. The reason we want to flip these values is simply because white has a rgb value of (255,255,255) while black has a rgb value of (0,0,0). The purpose of these will become clearer later. Line 33 calls in cv2.inRange function. It takes in a hsv image and also takes in lower_black and upper_black. These 2 values are the HSV value range of black.

After applying the erosion and dilation transformations, we make a copy of the new image, and change its color space back to RGB. This way, we can draw on the colored image which can help towards debugging. Then at Line 39, we return the colored image as well as the black and white image.

This function is very similar to the COLOR2BAW function. But instead, this function is primarily for the camera in the front. The camera in the front is not enclosed in a controlled space unlike the camera under the robot. Because of this, it is subject to change in conditions. That is why we developed a way to have the front camera adaptive such that it can change the lower_black hsv in range value to fit the environment.

Line 21 takes the sum of all RGB values of a single line. The brighter the place, the higher the rgb values will generally be, and the darker the place, the lower the rgb values will generally be. Then we applied this brightness value to a formula that was created by graphing the HSV values. In order to make this chart, we realized that the high end of the range never changed and remained at 255. So we mainly focused on the lower_black V value. We used a HSV color slider that would allow us to adjust the lower black through a slider wheel. From this, we matched up brightness levels with lower black values.

Once we find the correct lower_black value for the inRange function, everything is the same as the regular COLOR2BAW function.

Lastly, the COLOR2GREEN() function is shown below:

This function is very similar to the COLOR2BAW() function. Everything is the same except that there is no need for a erosion and dilation and the lower_green and upper_green values are different. The cv2.inRange() logic is the same as the COLOR2BAW() function.

Regular Line Tracing

In the development of our line tracing, we wanted something fast and simple. We first tried using cv2.findContours, which would find the location of the black line for us. The problem with this function was that it was way too slow and it would constantly cause late reactions from the robot. So then we decided to search for something else. We thought about regular light arrays and how they worked. Then we came up with the idea of using slices or lines of the image and from those slices and lines, we would find the location of the line. Since we used a camera, we could have an infinite amount of lines, but it would result in a slower reaction to change. So first we tried 5 lines, and then switched over to 3 lines because we saw no difference in their line-tracing. The line method was fast and simple so we decided to stick with it.

As mentioned before, the robot is ran through the driver and all the files are accessed through the driver. The driver file is as follows:

Driver

First, we grab a frame from the camera. Then we process this image by using the calibration as well as the masking. We have a frame called baw. This frame will be in charge for detecting the location of the line and we have a frame called greenMask which will be used for detecting green squares. Line 19 is the function related to regular line tracing, line 22 is the function call for intersections and green square detection, line 26 is for gap detection. We will first talk about the line.getPercent() function.

Regular Line Tracing

The method for our line tracing is by finding a percent of error and then using a modified PID controller to determine the motor speeds to run. The percent of error is calculated by distance from the center, as the line goes farther away from the center, the error increases. In order to determine this error, we use a number of lines that can determine the location of the line. The findFirstEP() function determines the end points of the line.

The parameters of the findFirstEP function are line and color. The line parameter is a array of a single line of pixels from the frame. Since the frame is black and white, the array has only two values, 255 and 0. 255 indicates that the pixel is part of the line, while 0 represents no line. Since this is the very first time finding the line, there are no previous values that can be used to look for the line so we brute force finding the line. Since finding the first end points only happens once, we aren't worried about the relative inefficiency of the task. Since the line is about 50 pixels wide, we search every 40 pixels if the pixel value is 255. If the pixel value is 255, then we record the value into the array called firstOccurences. This array, firstOccurences, contains all the locations where the line is present. One problem with this is that there can be multiple lines detected and we only want the closest one to the middle. Line 16, subtracts 145 from the location of detected line and takes the absolute minimum argument. For example, if the locations of the line were 40, and 160. Once you subtract 145, and take the absolute value you get 105 and 15. Since 15, is the minimum, 160 is the closest to the center. After we determine where the line is, we need to determine the endpoints. We do this by taking the location that we previously found, and iterating until we find the end point. We determine this endpoint when the line goes from 255 to 0 because 255 indicates line present and 0 indicates no line. Once the end points are determined, we draw it onto the frame to show it easily.

Since we now know the endpoints of the first line, we can use these end points to find the next 2 lines that are further down the image.

the getEP function is a more optimized function that does not have to brute force to find the line. It also ensures that the location of the line is the correct one. The getEP function mainly relies on the previous end points of the line. The endpoints of the line are given by the findFirstEp() function call or previous getEP() calls. We first take previous lower end point value. If this value is 0, that means that there is no line, and we have to search for the line again, which would be our end point. Anytime the pixel value switches from 0 to 255 or vice versa, we know that it is an endpoint. If the endpoint value is 255, then we search for the nearest 0 value, which would be our endpoint. Line 56 is our way of determining the error. The mult array would have numbers -144 to +144. This way, when the location of the line, determined by the endpoints, are extremely to the right, the sum will be a large positive number. When the line location is to the left, the sum will be very negative.

Both of these functions are used in the getPercent function.

The variable firstTime determines whether or not there are previous values that can be used to find the line. If there is not, then the program will find the endpoints of the line through brute force in the findFirstEP() function. If there are previous values to reference upon, then we use the getEP function. Lines 64 and 65 will always use the getEP() function because there will always be a previous value provided by either the findFirstEP or the getEP functions. Lines 66-70 are used for intersections which will be discussed later. The sums from each of the functions are kept in a variable called sum. We then divide by the maximum value that the sum can be, which is 12000. This will make the error have a range from -1 < 0 < 1. We then return the prevs so they can be used when the getPercent() function is called again and firstTime to indicate that there are previous values that can be referenced upon.

If we reference the driver file again, we see that the percent of error is stored in the error percent. This variable is then used in line 35 in the regulatMotors() function. Before we run the regular line tracing, we detect for intersections, green, and gap before running the regularMotors(). If any of these things are detected, the regularMotors() will not run, and instead a specific set of code will run that will deal with that specific obstacle or task. For now we are going to assume that none of these are detected and the robot will run the function regularMotors(). The regularMotors() function is included in theSerial file that we have mentioned before.

The first thing we do is that we clip the percent value such that it is always -1 < 0 < 1 because sometimes it exceeds the range. We do this because controller heavily relies on percentages. In this function, we have modified a regular PID controller to better suit the environment. A PID controller is a common control loop mechanism that uses the current error and previous error to make motor speeds. A PID controller has three aspects, a proportional gain, an integral gain, and a derivative gain. All of these gains can be used together or separately, which ever best suits the situation. In this controller, we use the proportional and derivative gain. The proportional gain of the controller responds directly with the error. So the bigger the error, the proportional gain would be larger to fix this error. Once issue with only the proportional gain is that the robot will sometimes overshoot the correction and end up oscillating. This is where the derivative gain comes into play. The derivative takes the current error and present error and subtracts them. This will determine the magnitude of change. If the error and previous error are the same, then the derivative gain would equal to 0, since the previous error and the current error are the same, that means that the error hasn't been fixed yet. If the previous error and current error are drastically different, it can mean that the error is almost fixed and there is no need to turn as drastically as before. This stops oscillation. Everything up to now has been pretty standard to a regular PID controller. What we added is called a speed modifier. In the PID controller, there is a base speed that is changed based on the values from the proportional gain and derivative gains (line 40-41). What we found out when running the regular PID controller is that it would not respond quick enough to the very sharp turns, so we thought that if we slowed down during these sharp turns that the robot would be able to do these turns without messing up. The changer value basically is a percentage 0 to 100%. As the error becomes larger, this percentage decreases towards 0. Basically when the robot is on a straight line with no error, the robot will go faster. When the robot comes to a turn, then it will start to slow down as the error becomes bigger. When the error becomes large enough, the changer will be 0, and the motor values are only the negative and positive values from the PID motor controller. This will result in a point turn.

After calculating the motor change values, we need to process the information to send over to the megaPi to run. Since it is risky to send over negatives over serial, we test if the number is negative and change the sign if it is. We then set the marker if the sign is negative. For example, if the motorLeft was -50, then it would be changed to 50 and signL = 1. Then we clip the motor speeds between 0 and 255. This is another way to make sure we do not have any negatives, and also that we don't go over the 255, which is the maximum value that can be sent over serial.

Intersections

We detect intersections through the width of the lines. Normally the line would be around 50 pixels long, but when there is an intersection, the line width becomes more than 100.

Lines 66-70 create 5 additional lines that search for intersections. They are farther away from the line tracing lines because if they were too close to the line detection lines, the detection of the intersection would be too late. All of these lines puts the endpoints in the array prevs. This prevs array is returned to the driver.py script and then passed into the functions in the inter.py script.

The previous endpoints array are passed through the function from the driver.py script. This array of previous endpoints contains all of the endpoints of the lines that are on the image. We compare the width of the intersection test lines to the width of the regular line tracing lines and we determine if it is greater than 90, than there exists a intersection. To confirm that there is an intersection, we check the next line if the difference between the intersection line and the line tracing line is greater than 80.

After we detect that there is an intersection, we want to make sure that it isn't an intersection where you ignore the turn and go forward (shown above,going from B to C or C to B). We detect if there is a fake intersection by moving 2 centimeters forward. Then we refresh the camera and check if we can find a line at the very beginning of the frame. We do this by taking the previous endpoints and estimating where the resulting line should be. If there exists a line where the estimated line should be, we know to ignore the turn and instead go forward.

If you look at the image from Regular Intersection, you will notice that you cannot see the intersection anymore and there is no way of determining whether it is a left or right turn. So we need to find the line again. We do this by backing up, until we find the line again. We also noticed that sometimes if we did not back up, the turn would be very inconsistent. By backing up, it establishes a reference point for the turn that would stay constant for every turn. This ensures the consistency of the turn, because the turn always starts at the same location.

After we repositioned our robot, we need to determine whether it is a right turn or a left turn. We do this by taking the absolute difference of the right endpoint of the regular line tracing and the intersection detection line.

After determining the direction of the turn, we turn 70 degrees using encoders. We only turn 70 degrees because we let our center function take care of the rest. Sometimes, the 90 degrees turns aren't perfectly lined up, so we only turn 70 degrees and have our center function center the robot. This way the 90 degrees turns are robust and consistent. The centering function is as followed:

The parameters of the function are low, high, and where. Low and high are the range of where the robot wants to be (145 is the center). Where is the point of turning where the robot will decide where to center. First we get an image and mask it such that it is black and white. Then we find the end points of the line and calculate the middle using the mid point formula. After that, we see if the middle is within the accepted range provided by the parameters, if its not, we turn based on the location of the middle.

After the center function ends, the intersection returns and continues line tracing.

Green Squares

When looking for green squares, we want to use masking and the COLOR2GREEN function to tell whether there is a green square or not. But this can slow down the program and result in late responses. So we found out that we can apply the same concept as we did to intersection to know when to check for green square.

As you can see, the green square is picked up by the black line masking. We can use this information similarly to how we detect an intersection. We compare the lengths of the green detection line to the lengths of the regular line. Since the length of a green square is smaller than the length of a 90 degree turn, we have to modify values. We check if there is a difference in length of 10 pixels. In the detection image, we compare the length of the red lines to the length of the blue lines. From this, we can determine that there is a possibility that there is a green square because the difference between the lengths is greater than 10. By detecting when there is a possibility of a green line, we drastically reduce the number of times we check for a green square, thus making our code more efficient. Once we think that there is a green square, we set a green square flag to be true. After we break out of the for loop, then we proceed to call checkGreen(), which is the function in charge of checking the green squares.

The first thing we do is that we detect whether or not there is a green square in the image. We do this by grabbing a frame and masking it.

To the human eye, we can see that there is clearly a green square there, but for a computer, it isn't that obvious. So in order for the robot to tell, we take lines of pixels of the image. One line of pixels from the after masking image will contain either 0 or 255. 0 is where there is black while 255 is where there is white. If we do a bitwise operation on the array, we can change the array of pixels to simply 0s and 1s. Since 0 in binary is 00000000 and 255 in binary is 11111111, when we do a bitwise operation of and 1 which is 00000001 in binary, we get 00000000 and 00000001. This makes it easier for the robot to tell where the green is. 0 for no green and 1 for green. So when we take a line of pixels and we add all of the values of the pixels in the array, we get the total sum of the amount of green in the array. So if there was no green square in the line of pixels present, then this sum value would be 0, but if there was a green square, then the sum would be greater than 30. Once we know that the summation of the line is greater than 30, then we know that there is a green square and we have to determine the location of the green square.

One problem that occurs is that after we tell the robot to stop, the momentum of the robot forces the robot to overshoot a little bit. This causes for when we detect the green square, for the robot to move forward and lose track of the green square. To fix this issue, we made the robot retrack its steps and try to find the green squares as the same time. While trying to also retrack our steps, we are able to differentiate which green squares are relevant, and which green squares should be ignored.

For all of these cases, assume the robot starts at position A. The green squares between the intersections of lines C and D as well as B and C should be ignored. The way we determine if the green square should be ignored, is that we check if there is a line above the green square. First we will evaluate case number 1 and we will assign numbers to the green squares.

After we find the correct green squares, we need to determine which direction to turn.

After we determine the type of turn, we make the turn very similarly to the intersection 90s. We first turn 70 degrees and then line ourselves back up with the line using the center function. After we center ourselves, we then continue to do regular line tracing.

Gap

Since the camera is underneath the robot, the field of view of the camera is limited. This can be an issue because if the robot is not perfectly lined up when detecting the gap, then the robot may not pick up the line again. Because of this, once we detect the gap, we need to align ourselves using some basic trigonometry.

The way we detect a gap is by taking the sum of a line of pixels. If there is a gap, the sum of the line of pixels should be 0, since 0 indicates no line. If the line of pixels contains more than 5 pixels that are considered a part of the line, then we return that there is no gap. We set it greater than 5 pixels so that there is some room for error just in case there is noise or outside interference. Once we have determined that there is a gap, we then move backwards. We want to use the line to center the robot, but since the momentum of the robot carries the robot forward a little bit after it stops, we need to find the line again. Once we move backwards we call the function angleCorrection().

First we get the frame from the image and we convert the image to black and white through masking. We then get the endpoints of the line at two separate locations, at y = 10 and y =40. From these endpoints, we determine the middle of the endpoints using the midpoint formula. From this we can use basic trigonometry to determine the angle of the line.

After aligning the robot, we keep moving forward until we see the line. We do this by taking a line of pixels and taking the sum of the pixels. Once the line of pixels contains the line, the sum will be greater than 50. If the line of pixels hasn't seen the line yet, the sum will be 0. Once we do find the line, we stop and return to the driver where the regular line tracing will continue.

Obstacle

One key thing that we wanted to focus on about the obstacle is that sometimes, the obstacle may be close to the wall, so turning a certain direction could cause the robot to crash into the wall. We wanted to eliminate this possibility by attaching ultra sonic senors on the side that could detect which way to turn.

We first need to detect whether or not there is an obstacle in front of the robot. We use the TOF sensors that are used for the ball detection to detect the obstacle. We retrieve the value from the sensors by calling the sendInfo function and reading them in through the coms.read(1) function. If this distance is greater than 5, there is no obstacle in front, and we return false. If the distance is less than 5, then we know there is an obstacle. We first straighten ourselves out through the gap.angleCorrection() function to ensure reliability and consistency. Then we retrieve the distance from the left and right ultra sonic sensors. We do this to ensure that we don't turn the wrong way and cause the robot to crash into the wall.

If the distance on the side left ultra sonic sensor is left than the distance on the side right ultra sonic sensor, we turn to the right. If it is not, then we turn to the left.

Ball Detection Idea

In the beginning, we planned on using the camera to detect the silver ball. We tried various methods such as houghCircles and CannyEdge detection. Unfortunately, these didn't work too well since the silver would blend in with the white background. The black ball detection with these functions worked pretty well at close ranges, but when the ball was far away, it was very hard for the raspberry pi to detect the black ball. After these failures, we looked for something else to detect the balls.

We first thought about using ultrasonic sensors to detect the ball, but we realized that we would have a lot of issues because the ultrasonic sensor would be unable to tell the difference between the wall and the ball. So we came up with the idea that if we had two ultrasonic sensors, one that would detect the ball, and one that would be above the ball, and detect the distance of the wall. If there was no ball in front of the robot, the ultra sonic sensor values should be roughly the same. If there was a ball in front of the robot, there should be some difference between them. When we actually tried this idea, it didn't work. We assumed that because of the wide angle of the ultra sonic sensors, it interfered with each other and was unable to see far distances. We still liked the concept of having two distance sensors to locate the ball, so we went out looking for better hardware. We then came across the time of flight sensors that wouldn't interfere with each other, and had a pretty good range. Since these sensors used lasers, they were extremely accurate and pinpointed. The lasers also allow the robot to detect if there is a ball across the room.

First we want to detect if there is a ball in front of the robot. We do this by retrieving the values of the top and bottom time of flight sensors. Once we get these values, we subtract the bottom sensor value from the top sensor value. If there is a ball in front of the robot, then the bottom sensor will have a shorter distance than the top sensor. Our threshold is about 5 cm or 50 mm since the diameter of the ball is around 5 cm. In other words, once the top TOF sensor reads in a value that is 50 mm bigger than the value from the bottom sensor, then we know that there is a ball.

After we detect that there is a ball, we then need to drive up to the ball. While going forward, we constantly check if the ball is still in front of the robot. If the ball is no longer in front of the robot, then we try to find the ball again. We do this by turning side to side and comparing the distances between the top sensor value and the bottom sensor value. If we don't find the ball again, we turn 360 degrees until we find the ball. Once we are less than 15 cm away from the ball, we stop and pick up the ball.

Evacuation Point Detection idea

We knew that we could use the camera to detect the evacuation point because the evacuation point is a very big black box, that would stand out to the camera. We first tried simply taking the largest shape in the image and driving up to it assuming that it was the evacuation point. But that came we a lot of problems, because if the evacuation point was very far away, and there was a ball close to the robot, then the ball would seem bigger than the evacuation point. This would mislead the robot into thinking that, since the ball was the largest shape, that the ball was the evacuation point. To combat this, we looked at the dimensions of the shape. No matter how far away evacuation point or ball is, the ratios between their width and height stay some what constant. Balls typically have a ratio of 1:1 between their height and width, while the evacuation point would have a ratio of at least 3:1 between the height and the width.

We first take in a frame from the camera so that we can find the contours. After we take the image in, we mask for black by using the adapCOLOR2BAW function. Since there is no light that can control the environment, we have to use an adaptive masking. After we change it to black and white, we can then find the contours. The contour is the outline of elements in the black and white frame. The cv2.findContour function returns an array of contours. If there are no contours found, then this array would be None. If we also find no contours, we keep on turning until we do see contours. Once we see the contours, we have to tell whether this contour is the evacuation point or a ball. We determine this through the dimensions. We first need to find the dimensions, so we draw a rectangle around the contour. From this function, we get the width and height. We divide the width by the height to get the ratio between these two values. Balls would typically have a ratio of 1:1, while the evacuation point would have a ratio of at least 3:1. Also, if we are not centered up with the evacuation point, we would only see some of the evacuation point. The height would stay the same, but the width would be shorter. This would cause for the robot to think that the partially shown evacuation point is a ball, and it would continue to turn until the evacuation point fully comes into frame. Once it does, this means that the robot is centered and we move forward. We stop when the front time of flight sensor reads less than 4 cm, and then we drop the ball into the evacuation point.