diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c34b71..1a5f24b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,6 +38,7 @@ import frc.robot.commands.ModeAuto.LancerAuto; import frc.robot.commands.ModeAuto.RetourMilieuDroite; import frc.robot.commands.ModeAuto.RetourMilieuGauche; import frc.robot.commands.ModeAuto.TournerVersMur; +import frc.robot.commands.ModeAuto.TournerVersReservoir; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Balayeuse; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -81,7 +82,8 @@ public class RobotContainer { NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain)); NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse)); NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse)); - NamedCommands.registerCommand("TournerA180", new TournerVersMur(drivetrain)); + NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain)); + NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain)); NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur)); NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur)); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java index 8aee71a..a2831d9 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java @@ -25,6 +25,7 @@ public class TournerVersMur extends Command { private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); Pigeon2 pigeon2; double force; + double angle; private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -44,9 +45,11 @@ public class TournerVersMur extends Command { public void execute() { if(alliance.get() == Alliance.Blue){ force = 0.5; + angle = 0; } else{ force = -0.5; + angle = 180; } if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ drivetrain.setControl(drive.withRotationalRate(force)); @@ -63,6 +66,6 @@ public class TournerVersMur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return pigeon2.getYaw().getValueAsDouble() > 165 && pigeon2.getYaw().getValueAsDouble() < 195; + return pigeon2.getYaw().getValueAsDouble() > angle - 5 && pigeon2.getYaw().getValueAsDouble() < angle + 5; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java new file mode 100644 index 0000000..eefc789 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java @@ -0,0 +1,71 @@ +// 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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +/* 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 TournerVersReservoir extends Command { + CommandSwerveDrivetrain drivetrain; + Optional alliance = DriverStation.getAlliance(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Pigeon2 pigeon2; + double force; + double angle; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new TournerAZero. */ + public TournerVersReservoir(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(alliance.get() == Alliance.Blue){ + force = 0.5; + angle = 180; + } + else{ + force = -0.5; + angle = 0; + } + if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ + drivetrain.setControl(drive.withRotationalRate(-force)); + } + else if(pigeon2.getYaw().getValueAsDouble() >180){ + drivetrain.setControl(drive.withRotationalRate(force)); + } + } + + // 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() > angle - 5 && pigeon2.getYaw().getValueAsDouble() < angle + 5; + } +}