diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java b/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java deleted file mode 100644 index b19d21f..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java +++ /dev/null @@ -1,56 +0,0 @@ -// 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.ModeAuto; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import static edu.wpi.first.units.Units.*; - -/* 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 TournerA180 extends Command { - CommandSwerveDrivetrain drivetrain; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - Pigeon2 pigeon2; - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /** Creates a new TournerAZero. */ - public TournerA180(CommandSwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - addRequirements(drivetrain); - // 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() { - if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); - } - else if(pigeon2.getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return pigeon2.getYaw().getValueAsDouble() > 165 && pigeon2.getYaw().getValueAsDouble() < 195; - } -} diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java b/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java deleted file mode 100644 index 9e45d08..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java +++ /dev/null @@ -1,56 +0,0 @@ -// 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.ModeAuto; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import static edu.wpi.first.units.Units.*; - -/* 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 TournerAZero extends Command { - CommandSwerveDrivetrain drivetrain; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - Pigeon2 pigeon2; - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /** Creates a new TournerAZero. */ - public TournerAZero(CommandSwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - addRequirements(drivetrain); - // 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() { - if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); - } - else if(pigeon2.getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return pigeon2.getYaw().getValueAsDouble() > 345 || pigeon2.getYaw().getValueAsDouble() < 15; - } -}