Field Oriented Control

By Abhi

Task: Implement a drive system depending on field perspective

We are always looking for ways to make it easier to drive. One way to do that is to modify our code such that no matter where the front of the robot is, moving the joystick in a certain direction will move the entire robot in that direction. This allows our drivers to only think about the field and align with the cryptobox easier. I knew that some FRC teams used libraries developed by WPLIB to implement this sort of drive. Reading their code, I figured out how to implement field-oriented drive in our codebase.

The code began by getting the joystick axis readings. This data then needed to be processed to account for the heading of the robot. This needed a special method depicted below.

Some math needed to be done for the angle. This is no easy feat so I will explain it in case if any other teams want to use this code. The first thing we need to do is the find the sine and cosine of the heading. This allows us to find the power to the x-axis and the y-axis respective to the angle.

Now that the trig is done, we needed to apply these values to the joystick axes. In this method, x represented the forward direction and y represented the strafing direction. That is why, when we look at out[0] which would tell the output forward direction, it considers the joystick's y direction and modifies it with the x-direction so that the joysticks get converted to their respective axes. This applies to the strafing direction as well.

Going back to the original method, the directions output from the method are applied to the actual powers of the motors. Before this happens, in case if any dimensions are over 1.0 (the max speed), they need to be scaled down to to 1. This is what the normalize and clampMotors methods do. Therefore, in the end, the code allows drivers to control the bot respective to the field.

Next Steps:

Now the drive team just needs to test the code out and see what happens.