diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 650a28d..d129c34 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -326,8 +326,8 @@ public final class Constants { public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; public static final double V_FOV = 45.7; - public static final double LIME_HIXELS = 920; - public static final double LIME_VIXELS = 720; + public static final double LIME_HIXELS = 640; + public static final double LIME_VIXELS = 480; public static final double TURRET_kP = 0.1; public static final double POINTS_THRESHOLD = 400; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c509da2..165550a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -218,6 +218,11 @@ public class RobotContainer { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); } }, m_robotClimber)); + + m_robotBoomBoom.setDefaultCommand( + new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45)) + ); + // autoInit(); // recordInit(); } @@ -267,8 +272,8 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); // X > Toggles extender in and out - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); // A > Spit Out Ball new JoystickButton(getOperatorController(), XboxController.Button.kA.value) @@ -281,11 +286,11 @@ public class RobotContainer { //! Test Buttons - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index 73f82d0..310b77e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 1ec7f17..7036b15 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -46,8 +46,7 @@ public class TrackTarget extends CommandBase { boolean isExecuted = false; - public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - m_swerve = swerve; + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; @@ -71,22 +70,22 @@ public class TrackTarget extends CommandBase { m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); - points = filterPoints(points); + // points = filterPoints(points); Point average = VisionOdometry.averagePoint(points); double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2; m_turret.runTurretWithInput(output); - double position = m_turret.m_boomBoomRotateEncoder.getPosition(); + // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); - if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || - Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) - m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); - else - m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), - RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), - true); + // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || + // Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) + // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); + // else + // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), + // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), + // true); double regressedDistance = getDistance(average.y); @@ -113,25 +112,25 @@ public class TrackTarget extends CommandBase { } } - public ArrayList filterPoints(ArrayList points) { - Point average = VisionOdometry.averagePoint(points); + // public ArrayList filterPoints(ArrayList points) { + // Point average = VisionOdometry.averagePoint(points); - HashMap pointDistances = new HashMap<>(); - double distanceSum = 0; + // HashMap pointDistances = new HashMap<>(); + // double distanceSum = 0; - for(Point point : points) { - Vector2D difference = new Vector2D(point); - difference.subtract(new Vector2D(average)); + // for(Point point : points) { + // Vector2D difference = new Vector2D(point); + // difference.subtract(new Vector2D(average)); - double mag = difference.magnitude(); - distanceSum += mag; + // double mag = difference.magnitude(); + // distanceSum += mag; - pointDistances.put(point, mag); - } + // pointDistances.put(point, mag); + // } - final double averageDist = distanceSum / points.size(); - return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); - } + // final double averageDist = distanceSum / points.size(); + // return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); + // } public final double getDistance(double averageY) { double y_rot = averageY / VisionConstants.LIME_VIXELS; @@ -143,7 +142,8 @@ public class TrackTarget extends CommandBase { double regressedDistance = distanceRegression(distance); regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; - + SmartDashboard.putNumber("Distance from Lime 123", distance); + SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance); return regressedDistance; }