Agobot

This year's competition revolved around stacking large Lego-like blocks, known as "stones," on top of a "foundation," as high as possible. 4 teams compete in 2 vs. 2 alliances in this game, with 30-second autonomous and 2-minute driver-controlled periods.

The autonomous objectives are:

• Identifying "skystones" (stones with a black sticker on them), and taking them to the foundation first
• Moving the foundation into a taped corner zone
• Parking underneath a "bridge" structure

The driver-controlled objectives are:

• Building towers of stones on the foundation, with more points for taller towers
• In the last 30 seconds:
  • Move the foundation out of the taped-off corner zone
  • Parking in the taped-off corner

Because there is an autonomous component to the game, the robot needs to know its position, to be able to act reliably by itself. What many teams do, and indeed what we did for our first 4 years, was to track the rotations of the robot's drive motors. This approach has two problems:

1. If the wheels slip, due to fast acceleration or bumping into another robot, the position is thrown off, since the wheels turn but the robot isn't moving.
2. It restricts us to going forwards and backwards and turning, since it would be too complex to do curved paths using just the encoder data. The robot uses Mecanum wheels, allowing it to move sideways as well, and so we're not taking full advantage of our hardware.

To address this, my team added odometry wheels to the robot, a popular system among veteran FTC teams in which dead wheels are used to keep track of robot position. Documentation on programming the wheels was rather sparse, so I had to derive equations myself. This was a daunting task, as the mecanum wheels that our robot used allowed for complex motions like rotating while translating linearly, and our odometry system would need to be able to keep track of the robot even through that.

My strategy to find the equations was to look at the motion of the robot on a iteration-by-iteration basis. Since motor speeds were only set once per loop iteration, we could say that the robot would move with its motor speeds being constant between loop iterations. This greatly reduced the number of paths the robot could take in that time, and so I derived equations that would check in every loop iteration to update the position of the robot.

I had a hard time finding any resources on odometry when I was competing, so to help the community out, I wrote a guide explaining the derivation of the odometry algorithm in more detail. If you're affiliated with an FTC team, or you're just interested, feel free to take a look!

While odometry was a huge step up in our robot's autonomous capabilities, there was still some room for improvement. For instance, if we wanted the robot to move in an L-shape, the robot would move from top to bottom, stop, and then move left to right. However, a human driver would be able to do the whole path in one continuous motion. This inefficiency on the robot's part costs us valuable time, especially since the autonomous period only lasts 30 seconds.

To solve this issue, I implemented a pure pursuit navigation algorithm. Normally, when we're telling the robot to go somewhere, we tell it to approach a fixed point. In pure pursuit, we tell the robot to approach a moving point, specifically a point a set distance further along the path than the robot. In other words, we're dangling out a carrot on a stick for the robot to follow.

With this algorithm, we can define a series of lines for the robot to go along, and Agobot will drive down them without stopping, and it'll even round corners (since the approach point will reach and turn the corner before Agobot does).

Like with odometry, there's not many good resources online that explain pure pursuit in an easy-to-explain manner, so I wrote up a guide. Feel free to take a look!