feat: workingish targeting and testing changes

Co-authored-by: DaviTheDinosaur <[email protected]>
flamingchickens1540 · Dec 16, 2023 · c0cbd53 · c0cbd53
1 parent 8485533
commit c0cbd53
Show file tree

Hide file tree

Showing 10 changed files with 63 additions and 28 deletions.
diff --git 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
@@ -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<Command> 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;