@@ -71,7 +71,7 @@ public TurnToAmpCorner(
7171 @ Override
7272 public void initialize () {
7373 pivot .setPivotGoal (45 );
74- shooter .setFlywheelRPMs (5000 , 4400 );
74+ shooter .setFlywheelRPMs (4800 , 4200 );
7575 }
7676
7777 // Called every time the scheduler runs while the command is scheduled.
@@ -85,17 +85,17 @@ public void execute() {
8585 "trans2" ,
8686 new Translation2d (
8787 FieldConstants .fieldLength - drive .getPose ().getX (),
88- FieldConstants .fieldWidth - drive .getPose ().getY ()));
88+ ( FieldConstants .fieldWidth - 2.5 ) - drive .getPose ().getY ()));
8989 targetAngle =
9090 new Rotation2d (
9191 FieldConstants .fieldLength - drive .getPose ().getX (),
92- FieldConstants .fieldWidth - drive .getPose ().getY ())
92+ ( FieldConstants .fieldWidth - 2.5 ) - drive .getPose ().getY ())
9393 .getDegrees ()
9494 + 180 ;
9595 pid .setSetpoint (
9696 new Rotation2d (
9797 FieldConstants .fieldLength - drive .getPose ().getX (),
98- FieldConstants .fieldWidth - drive .getPose ().getY ())
98+ ( FieldConstants .fieldWidth - 2.5 ) - drive .getPose ().getY ())
9999 .getDegrees ()
100100 + 180 );
101101 // pid.setSetpoint(
@@ -106,14 +106,15 @@ public void execute() {
106106 // + 180);
107107 } else {
108108 // Logger.recordOutput("trans2", new Translation2d(0, FieldConstants.fieldWidth));
109- targetAngle = new Translation2d (0 , FieldConstants .fieldWidth ).getAngle ().getDegrees ();
109+ targetAngle = new Translation2d (0 , ( FieldConstants .fieldWidth - 2.5 ) ).getAngle ().getDegrees ();
110110 // new Rotation2d(0, FieldConstants.fieldWidth).getDegrees() + 180;
111111 // pid.setSetpoint(
112112 // new Rotation2d(0, FieldConstants.fieldWidth).getDegrees()
113113 // + 180);
114114 pid .setSetpoint (
115115 new Rotation2d (
116- -drive .getPose ().getX (), FieldConstants .fieldWidth - drive .getPose ().getY ())
116+ -drive .getPose ().getX (),
117+ (FieldConstants .fieldWidth - 2.5 ) - drive .getPose ().getY ())
117118 .getDegrees ()
118119 + 180 );
119120 // pid.setSetpoint(
0 commit comments