YogurtLib for VEX

Finish Pure Pursuit, Motion Chaining. Starting work on Boomerang Function:
I worked more on Pure Pursuit(robot driving according to a path you’ve drawn through a imaginary circle it draws around its self and targeting where the circle intersects the path). I started by making my curve() function also return MotionResult so it is consistent with everything else in the library unlike before where it had it’s own return type FollowResult. I’ve also updated my helper function that can convert the enumberation from MotionResult into a string so the user can return the value and see what the potential error is from the logics.
I’ve also continued with the math for Pure Pursuit starting with injecting extra points into the array of points that the user provides for the path that the robot has to follow to ensure a more even data type and there aren’t senarios where the gaps are so large the circle would not find any paths to target and declare the robot as MotionResult::LOST.
To do this, I take apart the original array of points that the user provides and for any two points or segment; I divide it by the minimum gap we would accept(3 inches) to get how many new segments we would have to split it into in order that every segment is shorter than 3 inches.
Then we simply linearly interpolate the points based on how many segments we have(ex. a, a + (distance/spacing) 1, a + (distance/spacing) 2…) Ect where we just multiply the gap by the number of spaces that we want.
Lastly, I worked out a bug based on DAWGMA’s Pure Pursuit paper that sometimes the path which the robot has to take might be behind the robot(ie the robot is facing away from where the circle intercepts our path).
Thus the PP is finished fixing all known bugs(untested yet) by finally organizing all of the helper function into a master function in the PurePursuit class called step(). Then the drivetrain layer(Drivetrain class) that actually owns all of the hardware calls the function, sorts the return logic for when we are done, the path is invalid, or we are chaining the motion to move forward.
Though right now it is untested, I fixed the logic that would getActualSpeed() for the both sides of the drive by updating the encoders divided by the amount of time it has been and modifying the units based on the ratio to get an accurate RPM. We update the measured velocity each loop by blending the value we measure from the previus loop via an alpha variable(to smooth the reading to be free of noise). The user can also tune this at their will but the current default value is 40%(a.k.a. 0.4).
Lastly, I decided to make our own boomerang function(an advanced motion type in VEX) where to robot will always follow a virtual carrot point and as the robot progresses, the carrot point will blend back into the desired along the path while giving the robot a swing to reach the desired end heading as well. (A type of move to pose – where pose is position for x, y, and heading). I started doing some research and testing out the code in our library and Arduino IDE to see what it would compute and these are some of the resources I used:
https://lemlib.readthedocs.io/en/v0.5.0/tutorials/6lateralmotion.html
https://www.desmos.com/calculator/sptjw5szex
The main one(as well as with some explanation from our coach): https://area-53-robotics.github.io/Intro-To-Robotics/software/advanced-concepts/boomerang/

Complete Refractor – Making everything exist in Objects to create a template Library in VEX. Made implementation of functions better from last year and fixed existing bugs. Implemented proper asymetrical slewing to wheels and worked on a better Pure Pursuit. Added a custom YogurtLib UI with LVGL and a sensor fusion that can support Up to 3 and more IMUs.