2727
2828public class PathPlanner {
2929 private CommandSwerveDrive drivetrain ;
30- private ChassisSpeeds lastSpeeds ;
3130 private boolean trackingTranslation = false ;
3231 private boolean trackingRotation = false ;
3332 private Map <String , PathPlannerPath > mirroredPaths = new HashMap <>();
@@ -43,21 +42,6 @@ public PathPlanner(CommandSwerveDrive drivetrain) {
4342 ChassisSpeeds appliedSpeeds =
4443 ChassisSpeeds .fromRobotRelativeSpeeds (speeds , new Rotation2d (drivetrain .getYaw ()));
4544
46- if (lastSpeeds != null ) {
47- appliedSpeeds .vxMetersPerSecond +=
48- (appliedSpeeds .vxMetersPerSecond - lastSpeeds .vxMetersPerSecond )
49- / 0.02
50- * drivetrain .getConstants ().Driving .AutoLinearAccelerationScalar ;
51- appliedSpeeds .vyMetersPerSecond +=
52- (appliedSpeeds .vyMetersPerSecond - lastSpeeds .vyMetersPerSecond )
53- / 0.02
54- * drivetrain .getConstants ().Driving .AutoLinearAccelerationScalar ;
55- appliedSpeeds .omegaRadiansPerSecond +=
56- (appliedSpeeds .omegaRadiansPerSecond - lastSpeeds .omegaRadiansPerSecond )
57- / 0.02
58- * drivetrain .getConstants ().Driving .AutoAngularAccelerationScalar ;
59- }
60-
6145 if (!trackingTranslation ) {
6246 appliedSpeeds .vxMetersPerSecond = 0 ;
6347 appliedSpeeds .vyMetersPerSecond = 0 ;
@@ -149,7 +133,6 @@ private Command followPath(
149133 public void initialize () {
150134 trackingRotation = trackRotation ;
151135 trackingTranslation = trackTranslation ;
152- lastSpeeds = null ;
153136 pathPlannerCommand .initialize ();
154137 }
155138
@@ -163,7 +146,6 @@ public void end(boolean interrupted) {
163146 pathPlannerCommand .end (interrupted );
164147 trackingRotation = false ;
165148 trackingTranslation = false ;
166- lastSpeeds = null ;
167149 }
168150
169151 @ Override
0 commit comments