diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d9180ac..e5a3b65 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -119,6 +119,17 @@ public class RobotContainer { private enum ClimberMode { MANUAL, AUTONOMOUS }; private ClimberMode currentClimberMode = ClimberMode.MANUAL; +/** + * SmartDash + * - Limelight cam X + * - Limit switches X + * - Shooter RPM X + * - Distance to target x + * - target locked + * - claws boolean + * - field + */ + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java index 4f4183e..972afee 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java @@ -42,7 +42,7 @@ public class RunExtender extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.out.println("RunExtender is working"); + //System.out.println("RunExtender is working"); this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0); updateError(); } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 622376d..5b9d131 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -43,7 +43,7 @@ public class TrackTarget extends CommandBase { ArrayList points = new ArrayList<>(); private boolean targetLocked = false; - private double velocityTolerance = 100.0; + private double velocityTolerance = 300.0; private double hoodTolerance = 5.0; boolean isExecuted = false; @@ -110,21 +110,21 @@ public class TrackTarget extends CommandBase { double regressedDistance = getDistance(average.y); // ! no longer a +30 lol -aarav - velocity = m_boomBoom.getVelocity(regressedDistance); - hoodPosition = m_boomBoom.getHood(regressedDistance); + velocity = m_boomBoom.getVelocity(regressedDistance + 20); + hoodPosition = m_boomBoom.getHood(regressedDistance + 20); - m_boomBoom.runDrumShooterVelocityPID(velocity + 20); - m_hood.runAngleAdjustPID(hoodPosition + 20); + m_boomBoom.runDrumShooterVelocityPID(velocity); + m_hood.runAngleAdjustPID(hoodPosition); double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); double currentHood = this.m_hood.getEncoderPosition(); - targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance); + targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2); - SmartDashboard.putNumber("Regressed Distance", regressedDistance); + SmartDashboard.putNumber("Distance to Target", regressedDistance); // SmartDashboard.putNumber("Distance", distance); - SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); - SmartDashboard.putNumber("Vel Target Track", velocity); + // SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); + // SmartDashboard.putNumber("Vel Target Track", velocity); SmartDashboard.putBoolean("Target Locked", targetLocked); } catch (Exception e){ e.printStackTrace(); @@ -195,7 +195,7 @@ public class TrackTarget extends CommandBase { @Override public boolean isFinished() { if (this.isAuto) { - if (targetLocked& !timerStarted) { + if (targetLocked && !timerStarted) { timerStarted = true; startTime = System.currentTimeMillis(); } @@ -203,9 +203,5 @@ public class TrackTarget extends CommandBase { } else { return false; } - // return isExecuted && Math.abs(output) < .1; - // } - - // return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6ef257c..51f675d 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -96,7 +96,7 @@ public class BoomBoom extends SubsystemBase { // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); // SmartDashboard.putNumber("Shooter Current", getCurrent()); // SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); - // SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Shooter RPM", (m_shooterFalconLeft.getSelectedSensorVelocity() * 10) / 2048); } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 486f5a4..5edce2a 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -39,7 +39,6 @@ public class Claws extends SubsystemBase { m_open = false; clawsOpen = false; } - /** * Sets the state of both hooks * @param open The state of the hooks diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 997d08f..43eb4f8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -180,7 +180,7 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); - // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + m_field.setRobotPose(m_odometry.getPoseMeters()); super.periodic(); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 3ebfabc..92c5d62 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -116,8 +116,8 @@ public class Turret extends SubsystemBase { SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); - SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed()); + SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed()); // limit switch annoying time thing but actually worked first try wtf leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed).