diff --git a/src/main/java/org/team1540/bunnybotTank2023/Robot.java b/src/main/java/org/team1540/bunnybotTank2023/Robot.java index 2b4f0cf..1bba691 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Robot.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Robot.java @@ -26,6 +26,8 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; + private boolean hasRunAuto = false; + @Override public void robotInit() { System.out.println("[Init] Starting AdvantageKit"); @@ -84,9 +86,11 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { + robotContainer.setAutoDefaultCommands(); autonomousCommand = robotContainer.getAutonomousCommand(); - if (autonomousCommand != null) { + if (autonomousCommand != null && !(hasRunAuto && DriverStation.isFMSAttached())) { autonomousCommand.schedule(); + hasRunAuto = true; } } @@ -98,6 +102,7 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + robotContainer.setTeleopDefaultCommands(); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java index 8cc1ed3..c787001 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java +++ b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java @@ -11,10 +11,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes; +import org.team1540.bunnybotTank2023.commands.auto.TestAuto; import org.team1540.bunnybotTank2023.commands.drivetrain.ArcadeDriveCommand; import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain; +import org.team1540.bunnybotTank2023.commands.drivetrain.TankdriveCommand; import org.team1540.bunnybotTank2023.commands.indexer.Indexer; import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand; import org.team1540.bunnybotTank2023.commands.indexer.IndexerIdleCommand; @@ -31,11 +34,8 @@ import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim; import org.team1540.bunnybotTank2023.io.turret.TurretIOReal; import org.team1540.bunnybotTank2023.io.vision.Limelight; -import org.team1540.bunnybotTank2023.io.vision.LimelightIO; import org.team1540.bunnybotTank2023.io.vision.LimelightIOReal; -import java.util.ArrayList; - import static org.team1540.bunnybotTank2023.Constants.*; /** @@ -53,6 +53,8 @@ public class RobotContainer { final Turret turret; final Limelight limelight; + final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("AutoChooser"); + // Controllers CommandXboxController driver = new CommandXboxController(0); CommandXboxController copilot = new CommandXboxController(1); @@ -77,7 +79,8 @@ public RobotContainer() { turret = null; limelight = null; } - setDefaultCommands(); + + initAutoChooser(); configureButtonBindings(); } @@ -92,16 +95,17 @@ private void configureButtonBindings() { ); copilot.y().whileTrue(new TurretZeroSequenceCommand(turret)); - copilot.a().onTrue(new InstantCommand( () -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true))); - copilot.rightTrigger().onTrue(new ShootSequenceCommand(shooter, indexer, 3000)); + copilot.leftBumper().whileTrue(new TurretSetpointCommand(turret, Rotation2d.fromDegrees(0))); + copilot.rightTrigger().onTrue(new ShootSequenceCommand(shooter, indexer, 2500)); + copilot.leftTrigger().whileTrue(new TurretTrackTargetCommand(turret, limelight)); // copilot.x().onTrue(new InstantCommand(() -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true))); } - private void setDefaultCommands() { - drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); + public void setTeleopDefaultCommands() { + drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver)); shooter.setDefaultCommand(new StartEndCommand( - () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), + shooter::stop, () -> {}, shooter )); @@ -114,6 +118,24 @@ private void setDefaultCommands() { )); } + public void setAutoDefaultCommands() { + drivetrain.removeDefaultCommand(); + turret.removeDefaultCommand(); +// indexer.setDefaultCommand(new IndexerIdleCommand(indexer)); +// intake.setDefaultCommand(new StartEndCommand( +// () -> intake.setMotorSpeed(0.5), +// () -> {}, +// intake +// )); + indexer.removeDefaultCommand(); + intake.removeDefaultCommand(); + } + + private void initAutoChooser() { + autoChooser.addDefaultOption("ZeroTurret", new TurretZeroSequenceCommand(turret)); + autoChooser.addOption("Forward1Meter", new TestAuto(drivetrain)); + } + /** @@ -122,6 +144,6 @@ private void setDefaultCommands() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new AutoShoot5RamTotes(drivetrain); + return autoChooser.get(); } } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/drivetrain/ArcadeDriveCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/drivetrain/ArcadeDriveCommand.java index 1411375..7c9dde6 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/drivetrain/ArcadeDriveCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/drivetrain/ArcadeDriveCommand.java @@ -11,8 +11,8 @@ public class ArcadeDriveCommand extends CommandBase { private final CommandXboxController xBoxController; - private final SlewRateLimiter leftRateLimiter = new SlewRateLimiter(4); - private final SlewRateLimiter rightRateLimiter = new SlewRateLimiter(4); + private final SlewRateLimiter leftRateLimiter = new SlewRateLimiter(5); + private final SlewRateLimiter rightRateLimiter = new SlewRateLimiter(5); public ArcadeDriveCommand(Drivetrain drivetrain, CommandXboxController xBoxController) { this.drivetrain = drivetrain; @@ -24,10 +24,10 @@ public void execute() { double throttle = MathUtil.applyDeadband(-xBoxController.getLeftY(), Constants.DEADZONE_RADIUS); double turn = MathUtil.applyDeadband(xBoxController.getRightX(), Constants.DEADZONE_RADIUS); double left = leftRateLimiter.calculate( - MathUtil.clamp(throttle + turn,-1, 1) + MathUtil.clamp(throttle + turn,-0.75, 0.75) ); double right = rightRateLimiter.calculate( - MathUtil.clamp(throttle - turn, -1, 1) + MathUtil.clamp(throttle - turn, -0.75, 0.75) ); drivetrain.drive(left, right); diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java index ee0ca1b..fbeb864 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java @@ -18,6 +18,6 @@ public void initialize() { @Override public void end(boolean interrupted) { intake.setFold(true); - intake.stop(); + intake.setMotorSpeed(0.5); } } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java index 4696f4e..67b31d0 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java @@ -16,7 +16,7 @@ public class Turret extends SubsystemBase { private final TurretInputsAutoLogged inputs = new TurretInputsAutoLogged(); private final TurretIO io; - private final AverageFilter averageFilter = new AverageFilter(20); + private final AverageFilter averageFilter = new AverageFilter(10); private final LoggedTunableNumber kP = new LoggedTunableNumber("Turret/kP", TurretConstants.kP); private final LoggedTunableNumber kI = new LoggedTunableNumber("Turret/kI", TurretConstants.kI); diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java index a95e60b..a844bb2 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java @@ -17,7 +17,7 @@ public TurretManualCommand(Turret turret, CommandXboxController controller) { @Override public void execute() { - turret.setVoltage(MathUtil.applyDeadband(controller.getRightX(), Constants.DEADZONE_RADIUS) * 12.0); + turret.setVoltage(-MathUtil.applyDeadband(controller.getRightX(), Constants.DEADZONE_RADIUS) * 12.0); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java index c8a174c..75ad907 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java @@ -15,10 +15,16 @@ public TurretTrackTargetCommand(Turret turret, Limelight limelight) { addRequirements(turret); } + @Override + public void initialize() { + Rotation2d turretSetpoint = Rotation2d.fromDegrees(limelight.getTx()).plus(turret.getPosition()); + if (limelight.getTv()) turret.autoSetPosition(turretSetpoint); + } + @Override public void execute() { Rotation2d turretSetpoint = Rotation2d.fromDegrees(limelight.getTx()).plus(turret.getPosition()); - turret.autoSetPosition(turretSetpoint); + if (limelight.getTv() && turret.isAtSetpoint()) turret.autoSetPosition(turretSetpoint); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/drivetrain/DrivetrainIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/drivetrain/DrivetrainIOReal.java index 23cd35e..594d27e 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/drivetrain/DrivetrainIOReal.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/drivetrain/DrivetrainIOReal.java @@ -36,8 +36,10 @@ public DrivetrainIOReal() { backLeft.setSmartCurrentLimit(DrivetrainConstants.DRIVETRAIN_MOTOR_CURRENT_LIMIT); backRight.setSmartCurrentLimit(DrivetrainConstants.DRIVETRAIN_MOTOR_CURRENT_LIMIT); - frontLeft.enableVoltageCompensation(12); - frontRight.enableVoltageCompensation(12); + frontRight.disableVoltageCompensation(); + frontLeft.disableVoltageCompensation(); +// frontLeft.enableVoltageCompensation(12); +// frontRight.enableVoltageCompensation(12); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java b/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java index fd6a5da..a0056a7 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java @@ -10,9 +10,9 @@ public class Limelight extends SubsystemBase { private final LimelightIOInputsAutoLogged inputs = new LimelightIOInputsAutoLogged(); private double[] target; - private final double mountingAngleDegrees = 10.0; - private final double limelightLensHeightInches = 20.0;//TODO Find this - private final double goalHeightInches = 25.0;//TODO Find this + private final double mountingAngleDegrees = 21.0; + private final double limelightLensHeightInches = 29.0;//TODO Find this + private final double goalHeightInches = 49.5;//TODO Find this private static final double HORIZONTAL_FOV = Math.toRadians(63.3); private static final double VERTICAL_FOV = Math.toRadians(49.7); @@ -40,11 +40,11 @@ public double getTa() { } public double getTx() { - if(getTv()) return target[3]; + if(getTv()) return -target[3]; return 0; } public double getTx(double[] target){ - return target[3]; + return -target[3]; } public double getTy() { diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java index 11964dc..1620d51 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java @@ -12,14 +12,14 @@ public class LimelightIOReal implements LimelightIO{ LimelightHelpers.LimelightResults limelightResults; public LimelightIOReal(){ - LimelightHelpers.setPipelineIndex("limelight-rear",9); + LimelightHelpers.setPipelineIndex("limelight-front",9); LimelightHelpers.setLEDMode_PipelineControl(""); } @Override public void updateInputs(LimelightIOInputs inputs) { validTargets.clear(); - limelightResults = LimelightHelpers.getLatestResults("limelight-rear"); + limelightResults = LimelightHelpers.getLatestResults("limelight-front"); inputs.validTarget = limelightResults.targetingResults.valid; inputs.captureLatencyMs = limelightResults.targetingResults.latency_capture; inputs.pipelineLatencyMs = limelightResults.targetingResults.latency_pipeline;