An easy-to-use, highly-maneuverable drive system is at the heart of a successful FIRST Technology Challenge (FTC) robot. Omnidirectional drive systems provide motion in the Y axis (forward-backward), X-axis (strafe), and Z axis (rotating about it’s center axis). Each “degree of freedom” is independent, meaning that the overall robot motion is comprised of a “mix” of motion in each of the X, Y and Z axes, control of which is easily provided with a 3-degree of freedom joystick. This resulting maneuverability is quite useful during FRC competitions to avoid other robots, pick up and place game pieces, line up for shooting to a target, etc.
Yet the driver who remains in a fixed position is now presented a new challenge: when the driving joystick is pushed forward, the robot does not necessarily move forward with respect to the field – rather it moves forward with respect to the robot. This forces the driver to develop the skill of “placing their head in the robot” and performing the angular transformation mentally. This skill can take quite awhile to develop meaning that rookie drivers face an uphill climb before they can be productive team contributors. Additionally, the mental energy involved in field-to-robot rotational transformations reduces the driver’s cognitive ability to focus other game-related tactical tasks, as evidenced by drivers who are so intently focused on driving that their response to their teammates is diminished. Moreover, when the driver does not have a clear line of sight to the robot, the “head in the robot” becomes even more challenging.
Solving this challenge is conceptually straightforward. First, the current angle (θ) of rotation between the head of the field, and the head of the robot must be measured; secondly, the joystick X/Y coordinates are transformed by θ, as shown in following pseudo-code:
double rcw = pJoystick->GetTwist(); double forwrd = pJoystick->GetY() * -1; /* Invert stick Y axis */ double strafe = pJoystick->GetX(); float pi = 3.1415926; /* Adjust Joystick X/Y inputs by navX MXP yaw angle */ double gyro_degrees = ahrs->GetYaw(); float gyro_radians = gyro_degrees * pi/180; float temp = forwrd * cos(gyro_radians) + strafe * sin(gyro_radians); strafe = -forwrd * sin(gyro_radians) + strafe * cos(gyro_radians); fwd = temp; /* At this point, Joystick X/Y (strafe/forwrd) vectors have been */ /* rotated by the gyro angle, and can be sent to drive system */