diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f9bba13..5bda228 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -206,7 +206,6 @@ public class RobotContainer { //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); - } /** diff --git a/src/main/java/frc4388/robot/commands/auto/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java index 9280310..2b06ae3 100644 --- a/src/main/java/frc4388/robot/commands/auto/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -46,11 +46,11 @@ public class Wait extends CommandBase { public void execute() { if (counter == 0) { - SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); + //SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); } m_currentTime = System.currentTimeMillis(); - SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); + //SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); counter ++; } diff --git a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java index f72a223..a2be80e 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java @@ -61,7 +61,7 @@ public class DriveStraightToPositionMM extends CommandBase { //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); - SmartDashboard.putBoolean("MM Run", true); + //SmartDashboard.putBoolean("MM Run", true); i++; } @@ -74,7 +74,7 @@ public class DriveStraightToPositionMM extends CommandBase { @Override public boolean isFinished() { if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){ - SmartDashboard.putBoolean("MM Run", false); + //SmartDashboard.putBoolean("MM Run", false); return true; } else { if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) { diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java index 3a958a9..6444141 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java @@ -59,9 +59,9 @@ public class DriveWithJoystickAuxPID extends CommandBase { m_drive.driveWithInputAux(moveOutput, m_targetGyro); - System.err.println("Target: " + m_targetGyro); - System.err.println("Current: " + currentGyro); - System.err.println(); + //System.err.println("Target: " + m_targetGyro); + //System.err.println("Current: " + currentGyro); + //System.err.println(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java index 345b160..56d2f96 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java @@ -106,12 +106,12 @@ public class DriveWithJoystickDriveStraight extends CommandBase { steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier; } m_drive.driveWithInput(move, steerOutput); - System.out.println("Driving With Input"); + //System.out.println("Driving With Input"); } private void runDriveStraight(double move) { m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - System.out.println("Driving Straight with Target: " + m_targetGyro); + //System.out.println("Driving Straight with Target: " + m_targetGyro); } /** diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java index 3b2ef73..13523af 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java @@ -132,12 +132,12 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } m_drive.driveWithInput(move, steerOutput); - System.out.println("Driving With Input"); + //System.out.println("Driving With Input"); } private void runDriveStraight(double move) { m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - System.out.println("Driving Straight with Target: " + m_targetGyro); + //System.out.println("Driving Straight with Target: " + m_targetGyro); } private void runStoppedTurn(double steer) { @@ -158,7 +158,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } else { m_drive.driveWithInputAux(0, m_targetGyro); } - System.out.println("Turning with Target: " + m_targetGyro); + //System.out.println("Turning with Target: " + m_targetGyro); } /** diff --git a/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java index b9f7d35..e0999e6 100644 --- a/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java @@ -50,8 +50,8 @@ public class TurnDegrees extends CommandBase { m_drive.runTurningPID(m_targetAngleTicksOut); - SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); - SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); + //SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); + //SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); i++; } diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 980c9ee..405530d 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -33,8 +33,8 @@ public class HoodPositionPID extends CommandBase { /*double slope = ShooterConstants.HOOD_CONVERT_SLOPE; double b = ShooterConstants.HOOD_CONVERT_B; firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ - SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); - SmartDashboard.putNumber("Fire Angle", firingAngle); + //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); + //SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooter.runAngleAdjustPID(firingAngle); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 8411aeb..f806eb4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -36,8 +36,8 @@ public class ShooterVelocityControlPID extends CommandBase { public void execute() { m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); - SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); - SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); + //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); + //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 300359d..f040c4f 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -733,16 +733,16 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); - SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); + //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); + //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor RPM", leftRPM); - SmartDashboard.putNumber("Right Motor RPM", rightRPM); + //SmartDashboard.putNumber("Left Motor RPM", leftRPM); + //SmartDashboard.putNumber("Right Motor RPM", rightRPM); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -751,8 +751,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + //SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + //SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); @@ -760,23 +760,23 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); */ - SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); - SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); + //SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); + //SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); - SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); - SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 6ef70e5..cbad21c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -50,6 +50,6 @@ public class LED extends SubsystemBase { @Override public void periodic(){ - SmartDashboard.putNumber("LED", currentLED); + //SmartDashboard.putNumber("LED", currentLED); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index b4f1403..e16cabe 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -78,15 +78,15 @@ public class Shooter extends SubsystemBase { m_shooterTable = new ShooterTables(); - SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); - SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); - SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); - SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); + //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); + //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); + //SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); - SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); - SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); - SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); - SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); + //SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); + //SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); + //SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); + //SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); @@ -106,8 +106,8 @@ public class Shooter extends SubsystemBase { try{ SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Fire Velocity From CSV", m_fireVel); - SmartDashboard.putNumber("Fire Angle From CSV", m_fireAngle); + SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); + SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index a014544..67d515d 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -77,8 +77,8 @@ public class Storage extends SubsystemBase { m_storagePIDController.setFF(storageGains.m_kF); m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); - SmartDashboard.putNumber("Storage Position PID Target", targetPos); - SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); + //SmartDashboard.putNumber("Storage Position PID Target", targetPos); + //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index dc7fa83..85244ce 100644 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -85,12 +85,12 @@ public class ShooterTables { } catch (IOException e) { e.printStackTrace(); } - SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); - SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); - SmartDashboard.putNumber("m_distanceLength", m_distanceLength); - SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); - SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); - SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); + //SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); + //SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); + //SmartDashboard.putNumber("m_distanceLength", m_distanceLength); + //SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); + //SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); + //SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); } public double getHood(double distance) { //Rotations of motor