@@ -80,9 +80,12 @@ public void initialize() {
8080 logger .info ("Starting auto pickup going to {}" , endPose );
8181 omegaAutoDriveController .reset (driveSubsystem .getPoseMeters ().getRotation ().getRadians ());
8282 xAutoDriveController .reset (
83- driveSubsystem .getPoseMeters ().getX (), robotStateSubsystem .isBlueAlliance () ? driveSubsystem .getFieldRelSpeed ().vxMetersPerSecond : -driveSubsystem .getFieldRelSpeed ().vxMetersPerSecond );
83+ driveSubsystem .getPoseMeters ().getX (),
84+ robotStateSubsystem .isBlueAlliance ()
85+ ? driveSubsystem .getFieldRelSpeed ().vxMetersPerSecond
86+ : -driveSubsystem .getFieldRelSpeed ().vxMetersPerSecond );
8487 yAutoDriveController .reset (
85- driveSubsystem .getPoseMeters ().getY (), robotStateSubsystem . isBlueAlliance () ? driveSubsystem . getFieldRelSpeed (). vyMetersPerSecond : - driveSubsystem .getFieldRelSpeed ().vyMetersPerSecond );
88+ driveSubsystem .getPoseMeters ().getY (),driveSubsystem .getFieldRelSpeed ().vyMetersPerSecond );
8689 }
8790
8891 @ Override
@@ -96,12 +99,11 @@ public void execute() {
9699 MathUtil .angleModulus (driveSubsystem .getGyroRotation2d ().getRadians ()),
97100 robotStateSubsystem .getAllianceColor () == Alliance .Blue ? 0.0 : Math .PI );
98101 if (robotStateSubsystem .isBlueAlliance ()) {
99- if (driveSubsystem .getPoseMeters ().getX () <= endPose .getX ()) {
100- xCalc = Math .abs (xCalc );
101- }
102- } else
103- if (driveSubsystem .getPoseMeters ().getX () >= endPose .getX ())
104- xCalc = -1 * Math .abs (xCalc );
102+ if (driveSubsystem .getPoseMeters ().getX () <= endPose .getX ()) {
103+ xCalc = Math .abs (xCalc );
104+ }
105+ } else if (driveSubsystem .getPoseMeters ().getX () >= endPose .getX ())
106+ xCalc = -1 * Math .abs (xCalc );
105107 logger .info (
106108 "Moving X : {} | Moving Y : {} | \n Current Pose {}" ,
107109 xCalc ,
0 commit comments