diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 93e0d5c..fda0dd4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -180,7 +180,10 @@ public class RobotContainer { public Command getAutonomousCommand() { return new SequentialCommandGroup( - new AvancerAuto(drive).withTimeout(2), + drivetrain.applyRequest(()-> + drive.withVelocityX(0.5*MaxSpeed) + .withVelocityY(0) + .withRotationalRate(0)).withTimeout(2), new L1Requin(requin, bougie).withTimeout(2), new ExpireCorail(requin, bougie).withTimeout(2), new RainBow(bougie)); diff --git a/src/main/java/frc/robot/commands/AvancerAuto.java b/src/main/java/frc/robot/commands/AvancerAuto.java index e8f4cd0..0f8d479 100644 --- a/src/main/java/frc/robot/commands/AvancerAuto.java +++ b/src/main/java/frc/robot/commands/AvancerAuto.java @@ -11,16 +11,18 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.TunerConstants.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class AvancerAuto extends Command { + private CommandSwerveDrivetrain commandSwerveDrivetrain = TunerConstants.createDrivetrain(); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); /** Creates a new AvancerAuto. */ - public AvancerAuto(SwerveRequest.RobotCentric drive) { + public AvancerAuto(SwerveRequest.RobotCentric drive, CommandSwerveDrivetrain commandSwerveDrivetrain) { // Use addRequirements() here to declare subsystem dependencies. } @@ -31,7 +33,11 @@ public class AvancerAuto extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drive.withVelocityY(0.2*MaxSpeed); + commandSwerveDrivetrain.applyRequest(()-> + drive.withVelocityY(0.5*MaxSpeed) + .withVelocityX(0) + .withRotationalRate(0)); + } // Called once the command ends or is interrupted.