// Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. package frc.robot.commands; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; 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, CommandSwerveDrivetrain commandSwerveDrivetrain) { // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { commandSwerveDrivetrain.applyRequest(()-> drive.withVelocityY(0.5*MaxSpeed) .withVelocityX(0) .withRotationalRate(0)); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { drive.withVelocityY(0); } // Returns true when the command should end. @Override public boolean isFinished() { return false; } }