diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index a07f2ec..ab42034 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -709,6 +709,16 @@ public class Drive extends SubsystemBase { return meters * DriveConstants.INCHES_PER_METER; } + /** + * Converts a value in inches per second to miles per hour + * @param ips The value in inches per second to convert + * @return The value in miles per hour + */ + public double inchesPerSecondToMilesPerHour(double ips) { + double mph = ips * (3600) * (1/63360); + return mph; + } + public void setRightMotorGains(boolean isHighGear) { if (!isHighGear) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); @@ -828,8 +838,6 @@ public class Drive extends SubsystemBase { public void updateSmartDashboard() { try { - - // SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); // SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); @@ -838,6 +846,14 @@ public class Drive extends SubsystemBase { SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro); SmartDashboard.putData("Drive Train", m_driveTrain); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition()); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition()); + + SmartDashboard.putNumber("Average Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + + double avgSpeedMPH = inchesPerSecondToMilesPerHour(10 * ticksToInches(m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY))); + + SmartDashboard.putNumber("Avg Speed MPH", avgSpeedMPH); //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); @@ -852,8 +868,6 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); @@ -889,8 +903,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); //SmartDashboard.putNumber("Odometry Heading", getHeading()); - SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); if (m_currentSong != m_songChooser.getSelected()){ m_currentSong = m_songChooser.getSelected(); diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index d8bb022..c2186f0 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -91,6 +91,8 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); + SmartDashboard.putBoolean("Drum Ready", m_isDrumReady); + //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); } diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index a34d59f..6433d52 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -17,6 +17,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.ControlType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; @@ -57,6 +58,9 @@ public class ShooterAim extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Turret Angle Raw", getShooterRotatePosition()); + + SmartDashboard.putBoolean("Aim Ready", m_isAimReady); } /** diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index c177c38..1cb3b9b 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -58,6 +58,8 @@ public class ShooterHood extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); + + SmartDashboard.putNumber("Hood Angle Raw", getAnglePosition()); } /**