Path Finding with Camera - quasics/quasics-frc-sw-2015 GitHub Wiki
This is for the Path Pickup in the 2021 Season of Infinite Recharge. Use this to find Pathing, and to use to have a reference if using this with vision & for the robot to pick the correct path.
Current path finding is being done, but in the Galactic Search section, the robot has to distinguish between two paths. Using vision would be the easiest, and most of the code seems to be done on the vision side. Using trig, or using how big the ball appears are the two options.
Pseudocode can be found underneath the image.
Messing around with some ways to show certain data points, in Python, can be found under Vision.py in the Quasics2021Code
Logic (how we perceive it):
To find which path to use
1A and 1B would be red alliance, 2A and 2B would be blue alliance.
DECIDING IF THE PATHING GROUP IS A OR B
Finding which pathing Group to use would be interesting. If the robot sees 3 balls, then they are in Pathing Group A.
If the robot sees two balls, then we are in Pathing Group B.
GENERAL REASON FOR HOW THESE CRITERIA WILL BE MET
If placing the robot next to the closest ball to pick up, we can use logic to determine the ball that we see.
For example, if the robot was in Pathing Group A, the balls and the number of them would give reason to know that they are in Pathing Group A, since there are 3 balls visible in Pathing group A based on the closest point to the ball. In Group B, from the closest position, it would only show 2 visible balls on the field.
SPECIFIC PATH FINDING BASED ON ALLIANCE
If decided to be in Pathing Group A, then it will see at least 3 balls, with all of the objects on the path visible.
If this ball pattern occurs in order of largest to smallest, then we will be on path 1A in Pathing Group A: center, right, left
If this ball pattern occurs in order of largest to smallest, then we will be on path 2A in Pathing Group A: center, left, left.
If decided to be in Pathing Group B, then we will only be seeing at least 2 balls, with the third being hidden behind the largest.
If this ball pattern occurs in order of largest to smallest, then we will be on path 1B in Pathing group B: center, right
If this ball pattern occurs in order of largest to smallest, then we will be on path 2B in Pathing group B: center, left
** ALTERNATIVE: ONLY IF STARTING IN SAME PLACE DURING ALL RUNS **
Degree Range can be changed in the future, but for example purposes, it would be 0 - 10 degrees to determine it to be the center.
If a ball is seen in the center, or within a certain degrees from the center, 0 - 10 degrees from center, then we can determine that we are on Pathing Group A, and then have to determine if it is 1A or 2A.
If a ball is not seen in the center, or in the 0 - 10 degrees surrounding it, then it can be said that we are in Pathing Group B, and will either pick 1B or 2B.
Path A:
If you are standing in the middle of the starting zone, the center ball will either be the largest or the smallest. If the ball is the largest, then it would be Path 1A, if not, the path would be Path 2A.
If the order from largest to smallest goes like this: center, right, left, then it would be Path 1A.
If the order from largest to smallest goes like this: right, left, center, then it would be Path 2A.
Path B:
If you are standing in the middle of the starting zone, there is no true center ball, but there are sides. If the ball is the largest on the left, then it is Path 1B. If the path is largest on the right, then it is Path 2B.
If the ball on the right is the second largest, then it would be Path 1B.
If the ball on the left is the second largest, then it would be Path 2B.
double Ball Number = n
bool PathGroupA = true
bool Path1A = true
bool Path2A = true
bool Path1B = true
bool Path2B = true
double dist1
double dist2
double dist3
double centerx1
double centerx2
double centerx3
If n = 3
PathGroupA = true
If n = 2
PathGroupA = false
If PathGroupA = true
dist1 < dist2
dist2 < dist3
centerx3 < -0.1
if centerx2 > 0.1
Path1A = true
Path2A = false
else
Path1A = false
Path2A = true
If PathGroupA = false
dist3 = 0
dist1 > dist 2
centerx3 = 0
if centerx2 > 0.1
Path1B = true
Path2B = false
else
Path1B = false
Path2B = true