diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e457019..6f683d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,16 +78,8 @@ public class RobotContainer { public Command getAutonomousCommand() { - return - new - ParallelCommandGroup( - autoChooser - .getSelected(), - new - RainBow - ( - bougie - ) - ); + return new ParallelCommandGroup( + autoChooser.getSelected(), + new RainBow(bougie)); } } diff --git a/src/main/java/frc/robot/commands/AprilTag3.java b/src/main/java/frc/robot/commands/AprilTag3.java index b0613e2..3a0282f 100644 --- a/src/main/java/frc/robot/commands/AprilTag3.java +++ b/src/main/java/frc/robot/commands/AprilTag3.java @@ -3,18 +3,32 @@ // 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.Limelight3; -import frc.robot.subsystems.Limelight3G; /* 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 AprilTag3 extends Command { private Limelight3 limelight3; + + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors /** Creates a new AprilTag3G. */ public AprilTag3(Limelight3 limelight3) { this.limelight3 = limelight3; addRequirements(limelight3); + // Use addRequirements() here to declare subsystem dependencies. } @@ -27,17 +41,20 @@ public class AprilTag3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(limelight3.getV() == true){ - + drive.withRotationalRate(limelight3.getX()/48); } else{ - + drive.withRotationalRate(0); } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + drive.withRotationalRate(0); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/AprilTag3G.java b/src/main/java/frc/robot/commands/AprilTag3G.java index 47a0db4..88245b8 100644 --- a/src/main/java/frc/robot/commands/AprilTag3G.java +++ b/src/main/java/frc/robot/commands/AprilTag3G.java @@ -3,13 +3,26 @@ // 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.Limelight3G; /* 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 AprilTag3G extends Command { private Limelight3G limelight3g; + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + /** Creates a new AprilTag3G. */ /** Creates a new AprilTag3G. */ public AprilTag3G(Limelight3G limelight3g) { this.limelight3g = limelight3g; @@ -25,17 +38,17 @@ public class AprilTag3G extends Command { @Override public void execute() { if(limelight3g.getV() == true){ - + drive.withRotationalRate(limelight3g.getX()/48); } else{ - + drive.withRotationalRate(0); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + drive.withRotationalRate(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Forme3.java b/src/main/java/frc/robot/commands/Forme3.java index c6e7676..53f1c0c 100644 --- a/src/main/java/frc/robot/commands/Forme3.java +++ b/src/main/java/frc/robot/commands/Forme3.java @@ -3,17 +3,28 @@ // 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.Limelight3; -import frc.robot.subsystems.Limelight3G; /* 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 Forme3 extends Command { private Limelight3 limelight3; + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors /** Creates a new Forme3. */ public Forme3(Limelight3 limelight3) { this.limelight3 = limelight3; + addRequirements(limelight3); // Use addRequirements() here to declare subsystem dependencies. } @@ -28,16 +39,18 @@ public class Forme3 extends Command { @Override public void execute() { if(limelight3.getV() == true){ - + drive.withRotationalRate(limelight3.getX()/48); } else{ - + drive.withRotationalRate(0); } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + drive.withRotationalRate(0); + } // Returns true when the command should end. @Override