Team2337 / 2022-Presented-By-Noahs-Arcade

Other
1 stars 0 forks source link

Rename `OdometryHeadingCommand` to be `HeadingToTargetCommand`. Update heading calculation to use `PolarCoordinate` #62

Closed ZachOrr closed 2 years ago

ZachOrr commented 2 years ago

This comes out of some work done on https://github.com/Team2337/2022-Presented-By-Noahs-Arcade/pull/35.

1) Renames the OdometryHeadingCommand to be HeadingToTargetCommand. It's a better name, since it describes what the command is doing. Externally, we're using the odometry. Internally, we're just looking at some target and setting the robot's heading. Matches up with the naming as well for the upcoming DistanceToTarget command. 2) Convert from using a Pose2d supplier to using a Translation2d supplier. This was leftover from when we were passing a full Drivetrain subsystem to the command. Currently we're just pulling the Pose2d to get the Translation2d off of it. If we don't need the rotation, we can just use a translation supplier. 3) Use PolarCoordinate.fromFieldCoordinate to get the angle from the target. This code got added after this command got initially built, but we should be able to use it.

This code still needs testing - since PolarCoordinate.fromFieldCoordinate is different from OdometryHeadingCommand.execute for some reason.

// OdometryHeadingCommand.execute
Pose2d pose = poseSupplier.get();
double x = pose.getX() - point.getX();
double y = pose.getY() - point.getY();
double angle = Math.atan2(y, x);  // (-π, π] radians
// PolarCoordinate.fromFieldCoordinate
double x = coordinate.getY() - referencePoint.getY();
double y = coordinate.getX() - referencePoint.getX();
double angle = Math.atan2(y, x); // (-π, π] radians

I have no idea why I'm doing this in a different way - but the PolarCoordinate code has tests, and the OdometryHeadingCommand code doesn't.