Pulled the code from the v2 heading controller in to one branch so I could test all of it together (since I was already testing all of it together).
The Big Things
Added Drivetrain.getHeading which returns some heading value on the robot (represented as a Rotation2d) that converts the Pigeon yaw value in to some (-180, 180) range. Also takes care of flipping values so CW rotations = positive values and CCW rotations = negative values. Ex: A 90 degree CCW rotation coming from the Pigeon will return a -90 degree rotation.
Added Utilities.relativeRotationFromAbsoluteRotation to do (-360, 360) degree -> (-180, 180) math so it can be used inside the Heading subsystem to sanitize inputs.
Modifies the Heading controller to give us a rotation joystick value, clamped at some max speed.
Modifies the Heading controller to work in our (-180, 180) range by sanitizing inputs.
The Medium Things
Renamed actualRotationSupplier to currentHeadingSupplier as part of the next bullet...
Renamed currentHeading to be maintainHeading in the Heading subsystem. The current notation is pretty confusing.
Added setNextHeadingToMaintainHeading to replace HeadingCommand. This was to alleviate the weirdness of having to get the next heading and then set the maintain heading from the next heading, with a side effect of clearing the next heading after retrieving it's value. setNextHeadingToMaintainHeading does all of those things in one call with a much nicer name.
Removed HeadingCommand - the naming was kind of confusing and it really didn't do what the name would suggest it did. Use setNextHeadingToMaintainHeading instead.
The Small Things
Fixed Heading instantiation to be after Drivetrain (since we need values from the Drivetrain) in RobotContainer
Wrote a bunch of comments for the Heading subsystem.
Pulled the code from the v2 heading controller in to one branch so I could test all of it together (since I was already testing all of it together).
The Big Things
Drivetrain.getHeadingwhich returns some heading value on the robot (represented as aRotation2d) that converts the Pigeon yaw value in to some(-180, 180)range. Also takes care of flipping values so CW rotations = positive values and CCW rotations = negative values. Ex: A 90 degree CCW rotation coming from the Pigeon will return a -90 degree rotation.Utilities.relativeRotationFromAbsoluteRotationto do(-360, 360)degree ->(-180, 180)math so it can be used inside the Heading subsystem to sanitize inputs.Headingcontroller to give us a rotation joystick value, clamped at some max speed.Headingcontroller to work in our(-180, 180)range by sanitizing inputs.The Medium Things
actualRotationSuppliertocurrentHeadingSupplieras part of the next bullet...currentHeadingto bemaintainHeadingin theHeadingsubsystem. Thecurrentnotation is pretty confusing.setNextHeadingToMaintainHeadingto replaceHeadingCommand. This was to alleviate the weirdness of having to get the next heading and then set the maintain heading from the next heading, with a side effect of clearing the next heading after retrieving it's value.setNextHeadingToMaintainHeadingdoes all of those things in one call with a much nicer name.HeadingCommand- the naming was kind of confusing and it really didn't do what the name would suggest it did. UsesetNextHeadingToMaintainHeadinginstead.The Small Things
Headinginstantiation to be afterDrivetrain(since we need values from theDrivetrain) inRobotContainer