diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..7b5ef1f --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1,10 @@ +{ + "download": { + "deleteAfter": false, + "localDir": "C:\\Users\\Joshua Moran\\Documents\\logs\\mrt", + "serverTeam": "10.59.85.2" + }, + "output": { + "outputFolder": "C:\\Users\\Joshua Moran\\Documents\\logs\\mrt" + } +} diff --git a/src/main/java/frc/robot/Config.java b/src/main/java/frc/robot/Config.java index b9550c7..b29e001 100644 --- a/src/main/java/frc/robot/Config.java +++ b/src/main/java/frc/robot/Config.java @@ -102,16 +102,21 @@ public class Config { * * This value should be set to 1 at a comp. This is for linear tuning, which can easily be done by anyone. For illinear tuning (i.e its off by different amounts at different points go to Adam, don't mess with the equation.) * */ - public static final double kLimelightShooterSpeedModiferPercentage = 1;//1.010375; + public static final double kLimelightShooterSpeedModiferPercentage = 1.65;//1.010375; + public static final double kLimelightShooterCTerm = 500; public static final double kLimelightTuningOffset = 0; //y = 3077.128 + (1786 - 3077.128)/(1 + (x/55.07565)^6.788735) - public static final double kLimelightATerm = 1722.884; - public static final double kLimelightBTerm = 5.779675; - public static final double kLimelightCTerm = 603.8564; - public static final double kLimelightDTerm = 445984300; + public static final double kLimelightATerm = 1684.135; + public static final double kLimelightBTerm = 5.531879; + public static final double kLimelightCTerm = 599.2879; + public static final double kLimelightDTerm = 256575100; public static final double kLimelightAcceptableAngle =3.5; + //Diagnostics + + public static final double kLowPressureWarn = 20; //PSI + } diff --git a/src/main/java/frc/robot/DriverInterface.java b/src/main/java/frc/robot/DriverInterface.java index 2e8bb6c..ab15270 100644 --- a/src/main/java/frc/robot/DriverInterface.java +++ b/src/main/java/frc/robot/DriverInterface.java @@ -5,6 +5,9 @@ package frc.robot; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -25,6 +28,34 @@ public class DriverInterface { private final SendableChooser verboseOutputChooser = new SendableChooser<>(); + private DoubleLogEntry batteryVoltage; + private DoubleLogEntry totalCurrent; + private DoubleLogEntry channel0Current; + private DoubleLogEntry channel1Current; + private DoubleLogEntry channel2Current; + private DoubleLogEntry channel3Current; + private DoubleLogEntry channel4Current; + private DoubleLogEntry channel5Current; + private DoubleLogEntry channel6Current; + private DoubleLogEntry channel7Current; + private DoubleLogEntry channel8Current; + private DoubleLogEntry channel9Current; + private DoubleLogEntry channel10Current; + private DoubleLogEntry channel11Current; + private DoubleLogEntry channel12Current; + private DoubleLogEntry channel13Current; + private DoubleLogEntry channel14Current; + private DoubleLogEntry channel15Current; + private DoubleLogEntry channel16Current; + private DoubleLogEntry channel17Current; + private DoubleLogEntry channel18Current; + private DoubleLogEntry channel19Current; + private DoubleLogEntry channel20Current; + private DoubleLogEntry channel21Current; + private DoubleLogEntry channel22Current; + private DoubleLogEntry channel23Current; + private StringLogEntry consoleErrorOutput; + public DriverInterface() { @@ -163,12 +194,15 @@ public void consoleOutput(MessageType messageType, String message) { break; case CAUTION: System.out.println("CAUTION " + message); + consoleErrorOutput.append("CAUTION " + message, 0); break; case WARNING: System.out.println("WARNING " + message); + consoleErrorOutput.append("WARNING " + message, 0); break; case CRITICAL: System.out.println("⚠ CRITICAL " + message + " ⚠ "); + consoleErrorOutput.append("⚠ CRITICAL " + message + " ⚠ ", 0); break; } } @@ -603,6 +637,10 @@ public void initSmartDashboard() { SmartDashboard.putBoolean("Foward direction", getRobotFowardDirection() == RobotFowardDirection.FRONT); SmartDashboard.putNumber("Shooter target", 2000); SmartDashboard.putNumber("Vision TUNING", 5); + + SmartDashboard.putNumber("Shooter Offset Multiplier", Config.kLimelightShooterSpeedModiferPercentage); + SmartDashboard.putNumber("Shooter C Term", Config.kLimelightShooterCTerm); + try { SmartDashboard.putNumber("Pressure", Pneumatics.getInstance().getPressure()); } catch (Exception e) { @@ -630,6 +668,14 @@ public double getShooterRatioDenomonatorField() { return SmartDashboard.getNumber("Shooter Ratio Denomonator", 1); } + public double getShooterOffsetMultiplier() { + return SmartDashboard.getNumber("Shooter Offset Multiplier", Config.kLimelightShooterSpeedModiferPercentage); + } + + public double getShooterCTerm() { + return SmartDashboard.getNumber("Shooter C Term", Config.kLimelightShooterCTerm); + } + //xbox controlller y // x b @@ -683,6 +729,72 @@ public void rumble() { } } + public void initLogging(DataLog aLog) { + + batteryVoltage = new DoubleLogEntry(aLog, "Battery Voltage"); + totalCurrent = new DoubleLogEntry(aLog, "Total Current"); + channel0Current = new DoubleLogEntry(aLog, "Channel 0 current"); + channel1Current = new DoubleLogEntry(aLog, "Channel 1 current"); + channel2Current = new DoubleLogEntry(aLog, "Channel 2 current"); + channel3Current = new DoubleLogEntry(aLog, "Channel 3 current"); + channel4Current = new DoubleLogEntry(aLog, "Channel 4 current"); + channel5Current = new DoubleLogEntry(aLog, "Channel 5 current"); + channel6Current = new DoubleLogEntry(aLog, "Channel 6 current"); + channel7Current = new DoubleLogEntry(aLog, "Channel 7 current"); + channel8Current = new DoubleLogEntry(aLog, "Channel 8 current"); + channel9Current = new DoubleLogEntry(aLog, "Channel 9 current"); + channel10Current = new DoubleLogEntry(aLog, "Channel 10 current"); + channel11Current = new DoubleLogEntry(aLog, "Channel 11 current"); + channel12Current = new DoubleLogEntry(aLog, "Channel 12 current"); + channel13Current = new DoubleLogEntry(aLog, "Channel 13 current"); + channel14Current = new DoubleLogEntry(aLog, "Channel 14 current"); + channel15Current = new DoubleLogEntry(aLog, "Channel 15 current"); + channel16Current = new DoubleLogEntry(aLog, "Channel 16 current"); + channel17Current = new DoubleLogEntry(aLog, "Channel 17 current"); + channel18Current = new DoubleLogEntry(aLog, "Channel 18 current"); + channel19Current = new DoubleLogEntry(aLog, "Channel 19 current"); + channel20Current = new DoubleLogEntry(aLog, "Channel 20 current"); + channel21Current = new DoubleLogEntry(aLog, "Channel 21 current"); + channel22Current = new DoubleLogEntry(aLog, "Channel 22 current"); + channel23Current = new DoubleLogEntry(aLog, "Channel 23 current"); + + consoleErrorOutput = new StringLogEntry(aLog, "Error log"); + consoleErrorOutput.append("Logging start", 0); + + } + + public void updateLogging(long aTime) { + batteryVoltage.append(RobotMap.getPDH().getVoltage(), aTime); + totalCurrent.append(RobotMap.getPDH().getTotalCurrent(), aTime); + channel0Current.append(RobotMap.getPDH().getCurrent(0), aTime); + channel1Current.append(RobotMap.getPDH().getCurrent(1), aTime); + channel2Current.append(RobotMap.getPDH().getCurrent(2), aTime); + channel3Current.append(RobotMap.getPDH().getCurrent(3), aTime); + channel4Current.append(RobotMap.getPDH().getCurrent(4), aTime); + channel5Current.append(RobotMap.getPDH().getCurrent(5), aTime); + channel6Current.append(RobotMap.getPDH().getCurrent(6), aTime); + channel7Current.append(RobotMap.getPDH().getCurrent(7), aTime); + channel8Current.append(RobotMap.getPDH().getCurrent(8), aTime); + channel9Current.append(RobotMap.getPDH().getCurrent(9), aTime); + channel10Current.append(RobotMap.getPDH().getCurrent(10), aTime); + channel11Current.append(RobotMap.getPDH().getCurrent(11), aTime); + channel12Current.append(RobotMap.getPDH().getCurrent(12), aTime); + channel13Current.append(RobotMap.getPDH().getCurrent(13), aTime); + channel14Current.append(RobotMap.getPDH().getCurrent(14), aTime); + channel15Current.append(RobotMap.getPDH().getCurrent(15), aTime); + channel16Current.append(RobotMap.getPDH().getCurrent(16), aTime); + channel17Current.append(RobotMap.getPDH().getCurrent(17), aTime); + channel18Current.append(RobotMap.getPDH().getCurrent(18), aTime); + channel19Current.append(RobotMap.getPDH().getCurrent(19), aTime); + channel20Current.append(RobotMap.getPDH().getCurrent(20), aTime); + channel21Current.append(RobotMap.getPDH().getCurrent(21), aTime); + channel22Current.append(RobotMap.getPDH().getCurrent(22), aTime); + channel23Current.append(RobotMap.getPDH().getCurrent(23), aTime); + + + + } + diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1aeabba..d040d3b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -93,6 +93,7 @@ public void robotInit() { FrontIntake.getInstance().initLogging(log); Pneumatics.getInstance().initLogging(log); Shooter.getInstance().initLogging(log); + VisionTrack.getInstance().initLogging(log); } @@ -116,6 +117,7 @@ public void robotPeriodic() { FrontIntake.getInstance().updateLogging(logTime); Pneumatics.getInstance().updateLogging(logTime); Shooter.getInstance().updateLogging(logTime); + VisionTrack.getInstance().updateLogging(logTime); Shuffleboard.update(); diff --git a/src/main/java/frc/robot/autonomous/CompletedSequences.java b/src/main/java/frc/robot/autonomous/CompletedSequences.java index 332d4ed..c958ad8 100644 --- a/src/main/java/frc/robot/autonomous/CompletedSequences.java +++ b/src/main/java/frc/robot/autonomous/CompletedSequences.java @@ -100,7 +100,7 @@ private static Sequence createPos1Complex() t1.setAngle(-84.75); autoDrive d1 = new autoDrive(); d1.setAngle(-84.75); - d1.setDist(-.82); + d1.setDist(-.6); d1.setSpeed(0.85); d1.setAccFwdLimit(0.15); d1.setAccRevLimit(0.17); @@ -136,7 +136,7 @@ private static Sequence createPos1Complex() t3.setAngle(0); autoDrive d4 = new autoDrive(); d4.setAngle(0); - d4.setDist(-1.8875);//NICE + d4.setDist(-1.15); d4.setSpeed(0.9); d4.setAccFwdLimit(0.15); d4.setAccRevLimit(0.22); @@ -273,7 +273,7 @@ private static Sequence createPos4Normal() d3.setDist(1.5); d3.setSpeed(0.3); autoTurn t4 = new autoTurn(); - t4.setAngle(210); + t4.setAngle(180); autoBallDetectionLimelight ball2 = new autoBallDetectionLimelight(); ball2.setNumBalls(5); autoEjector eject2 = new autoEjector(); diff --git a/src/main/java/frc/robot/subsystems/BackIntake.java b/src/main/java/frc/robot/subsystems/BackIntake.java index 7001441..9344656 100644 --- a/src/main/java/frc/robot/subsystems/BackIntake.java +++ b/src/main/java/frc/robot/subsystems/BackIntake.java @@ -7,7 +7,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.util.datalog.DataLog; -import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.*; import frc.robot.Config; import frc.robot.RobotMap; @@ -28,6 +28,9 @@ public static enum BackIntakeStates { // DataLog variables private DoubleLogEntry logRearInSpeed; private DoubleLogEntry logRearInStatorCurrent; + private StringLogEntry logRearInCurrentState; + private StringLogEntry logRearInDesiredState; + public BackIntake() { @@ -108,6 +111,13 @@ public BackIntakeStates getCurrentState() { return currentState; } + /** + * @return The desired state of the intakes + */ + public BackIntakeStates getDesiredState() { + return desiredState; + } + /** * @param State to become the intake desired state. */ @@ -139,12 +149,18 @@ public void initLogging(DataLog aLog) { logRearInSpeed = new DoubleLogEntry(aLog, "Rear Intake Speed"); logRearInStatorCurrent = new DoubleLogEntry(aLog, "Rear Intake Stator Current"); + logRearInCurrentState = new StringLogEntry(aLog, "Rear Intake Current State"); + logRearInDesiredState = new StringLogEntry(aLog, "Rear Intake Desired State"); + } public void updateLogging(long aTime) { logRearInSpeed.append(BackIntake.getInstance().getSpeed(), aTime); logRearInStatorCurrent.append(BackIntake.getInstance().getStatorCurrent(), aTime); + logRearInCurrentState.append(getCurrentState().toString(), aTime); + logRearInDesiredState.append(getDesiredState().toString(), aTime); + } } diff --git a/src/main/java/frc/robot/subsystems/Diagnostics.java b/src/main/java/frc/robot/subsystems/Diagnostics.java index ef4f69b..54453f4 100644 --- a/src/main/java/frc/robot/subsystems/Diagnostics.java +++ b/src/main/java/frc/robot/subsystems/Diagnostics.java @@ -4,7 +4,13 @@ package frc.robot.subsystems; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Config; +import frc.robot.DriverInterface; +import frc.robot.RobotMap; +import frc.robot.DriverInterface.MessageType; /** Add your docs here. */ public class Diagnostics { @@ -18,22 +24,93 @@ public Diagnostics getInstance() { return m_instance; } + private StringLogEntry pneumaticsFault; + private PneumaticsError pneumaticsError = PneumaticsError.NONE; + public enum PneumaticsError { NONE, COMMS_FAULT, VENT_OPEN, - LOW_PRESSURE_STARTUP + LOW_PRESSURE + + } + + public enum FalconError { + NONE, + COMMS_FAULT, + } + + public enum PowerDistrubutinError { + NONE, + CHANNEL_TRIP, + LOW_INPUT, + OVERCURRENT, + OVERTEMP, + + } + + + + public PneumaticsError pneumaticsDiagnostics() { + if(phCanCheck()) { + return PneumaticsError.COMMS_FAULT; + } else if(DriverStation.isEnabled()) { + if(Pneumatics.getInstance().getPressure() <= Config.kLowPressureWarn && Pneumatics.getInstance().getPressure() >= 5) { + return PneumaticsError.LOW_PRESSURE; + } else if(Pneumatics.getInstance().getPressure() <= Config.kLowPressureWarn) { + return PneumaticsError.VENT_OPEN; + } else { + return PneumaticsError.NONE; + } + } else { + return PneumaticsError.NONE; + } } - // public PneumaticsError pneumaticsDiagnostics() { - // if(DriverStation.) { + private boolean phCanCheck() { + try { + RobotMap.getCompressor().getAnalogVoltage(); + return false; + } catch (Exception e) { + return true; + } + } + + private boolean pdhCanCheck() { + try { + RobotMap.getPDH().getModule(); + return false; + } catch (Exception e) { + return true; + } + } + + public void updateDiagnostics() { + pneumaticsError = pneumaticsDiagnostics(); + switch(pneumaticsError) { + case COMMS_FAULT: + DriverInterface.getInstance().consoleOutput(MessageType.CRITICAL, "Pneumatics COMMS fault"); + break; + case LOW_PRESSURE: + DriverInterface.getInstance().consoleOutput(MessageType.CAUTION, "Low Pressure"); + break; + case VENT_OPEN: + DriverInterface.getInstance().consoleOutput(MessageType.WARNING, "Vent Open"); + break; + default: + break; - // } else { - // return PneumaticsError.NONE; - // } + } + } - // } + public void initLogging(DataLog aLog) { + pneumaticsFault = new StringLogEntry(aLog, "Pneumatics Fault Code"); + } + public void updateLogging(long aTime) { + pneumaticsFault.append(pneumaticsError.toString(), aTime); + } + } diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index bc5ea74..0ec7e8f 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -14,6 +14,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -31,6 +33,31 @@ public class Drive extends Subsystems { RelativeEncoder mRightEnc; public static AHRS _imu; + //Log entries + private DoubleLogEntry leftDriveAPosition; + private DoubleLogEntry leftDriveASpeed; + private DoubleLogEntry leftDriveACurrent; + private StringLogEntry leftDriveAFaults; + + private DoubleLogEntry leftDriveBPosition; + private DoubleLogEntry leftDriveBSpeed; + private DoubleLogEntry leftDriveBCurrent; + private StringLogEntry leftDriveBFaults; + + private DoubleLogEntry rightDriveAPosition; + private DoubleLogEntry rightDriveASpeed; + private DoubleLogEntry rightDriveACurrent; + private StringLogEntry rightDriveAFaults; + + private DoubleLogEntry rightDriveBPosition; + private DoubleLogEntry rightDriveBSpeed; + private DoubleLogEntry rightDriveBCurrent; + private StringLogEntry rightDriveBFaults; + + private DoubleLogEntry gyroPitch; + private DoubleLogEntry gyroYaw; + private DoubleLogEntry gyroRoll; + /** * Drive motor brake status. Drive brakes are disables upon powering up the robot. */ @@ -399,6 +426,14 @@ public void resetSensors() { _imu.reset(); } + /** + * + * @return linear distance traveled in m + */ + // public double getLinearDistance() { + // return (((RobotM)) + // } + /** * Set the motor controllers' brakes on or off. * @param brake True to enable brake mode, false to set to coast. @@ -469,14 +504,57 @@ public void clearFaults() { RobotMap.getRightDriveC().clearStickyFaults(); } - public void initLogging(DataLog aLog) - { - + public void initLogging(DataLog aLog) { + leftDriveAPosition = new DoubleLogEntry(aLog, "Left Drive A Encoder Position"); + leftDriveASpeed = new DoubleLogEntry(aLog, "Left Drive A Encoder Speed"); + leftDriveACurrent = new DoubleLogEntry(aLog, "Left Drive A Stator Current"); + leftDriveAFaults = new StringLogEntry(aLog, "Left Drive A Faults"); + + leftDriveBPosition = new DoubleLogEntry(aLog, "Left Drive B Encoder Position"); + leftDriveBSpeed = new DoubleLogEntry(aLog, "Left Drive B Encoder Speed"); + leftDriveBCurrent = new DoubleLogEntry(aLog, "Left Drive B Stator Current"); + leftDriveBFaults = new StringLogEntry(aLog, "Left Drive B Faults"); + + rightDriveAPosition = new DoubleLogEntry(aLog, "Right Drive A Encoder Position"); + rightDriveASpeed = new DoubleLogEntry(aLog, "Right Drive A Encoder Speed"); + rightDriveACurrent = new DoubleLogEntry(aLog, "Right Drive A Stator Current"); + rightDriveAFaults = new StringLogEntry(aLog, "Right Drive A Faults"); + + rightDriveBPosition = new DoubleLogEntry(aLog, "Right Drive B Encoder Position"); + rightDriveBSpeed = new DoubleLogEntry(aLog, "Right Drive B Encoder Speed"); + rightDriveBCurrent = new DoubleLogEntry(aLog, "Right Drive B Stator Current"); + rightDriveBFaults = new StringLogEntry(aLog, "Right Drive B Faults"); + + gyroPitch = new DoubleLogEntry(aLog, "Gyro Pitch value"); + gyroYaw = new DoubleLogEntry(aLog, "Gyro Yaw value"); + gyroRoll = new DoubleLogEntry(aLog, "Gyro Roll value"); } - public void updateLogging(long aTime) - { - + public void updateLogging(long aTime) { + leftDriveAPosition.append(RobotMap.getLeftDriveA().getSelectedSensorPosition(), aTime); + leftDriveASpeed.append(RobotMap.getLeftDriveA().getSelectedSensorVelocity(), aTime); + leftDriveACurrent.append(RobotMap.getLeftDriveA().getSupplyCurrent(), aTime); + leftDriveAFaults.append(RobotMap.getLeftDriveA().getLastError().toString(), aTime); + + leftDriveBPosition.append(RobotMap.getLeftDriveB().getSelectedSensorPosition(), aTime); + leftDriveBSpeed.append(RobotMap.getLeftDriveB().getSelectedSensorVelocity(), aTime); + leftDriveBCurrent.append(RobotMap.getLeftDriveB().getSupplyCurrent(), aTime); + leftDriveBFaults.append(RobotMap.getLeftDriveB().getLastError().toString(), aTime); + + rightDriveAPosition.append(RobotMap.getLeftDriveA().getSelectedSensorPosition(), aTime); + rightDriveASpeed.append(RobotMap.getLeftDriveA().getSelectedSensorVelocity(), aTime); + rightDriveACurrent.append(RobotMap.getLeftDriveA().getSupplyCurrent(), aTime); + rightDriveAFaults.append(RobotMap.getLeftDriveA().getLastError().toString(), aTime); + + rightDriveBPosition.append(RobotMap.getLeftDriveB().getSelectedSensorPosition(), aTime); + rightDriveBSpeed.append(RobotMap.getLeftDriveB().getSelectedSensorVelocity(), aTime); + rightDriveBCurrent.append(RobotMap.getLeftDriveB().getSupplyCurrent(), aTime); + rightDriveBFaults.append(RobotMap.getLeftDriveB().getLastError().toString(), aTime); + + gyroPitch.append(_imu.getPitch(), aTime); + gyroYaw.append(_imu.getYaw(), aTime); + gyroRoll.append(_imu.getYaw(), aTime); + } public boolean getDriveZeroInput() { diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 54899fe..dacde73 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -44,6 +44,12 @@ public double getAngleToTarget() { return tx; } + public double getErrorY() { + //setPipeline(0); + double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0); + return ty; + } + public void disableVision() { setPipeline(0); } @@ -52,6 +58,10 @@ private void setPipeline(int pipelineId) { NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); } + public double getPipeline() { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").getDouble(0); + } + /** * * @return distance in metres to target diff --git a/src/main/java/frc/robot/subsystems/Pneumatics.java b/src/main/java/frc/robot/subsystems/Pneumatics.java index 44de7d5..d8cae75 100644 --- a/src/main/java/frc/robot/subsystems/Pneumatics.java +++ b/src/main/java/frc/robot/subsystems/Pneumatics.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; import frc.robot.Config; import frc.robot.RobotMap; /** @@ -15,6 +16,10 @@ public class Pneumatics extends Subsystems{ boolean compressor = true; private static Pneumatics m_instance; + private DoubleLogEntry pressure; + private DoubleLogEntry compressorCurrent; + + @Override public void update() { @@ -71,14 +76,19 @@ public static Pneumatics getInstance() { public void clearFaults() { } - public void initLogging(DataLog aLog) - { - + public void initLogging(DataLog aLog) { + + pressure = new DoubleLogEntry(aLog, "Pressure"); + compressorCurrent = new DoubleLogEntry(aLog, "Compressor Current"); + } - public void updateLogging(long aTime) - { + public void updateLogging(long aTime) { + pressure.append(getPressure(), aTime); + compressorCurrent.append(RobotMap.getCompressor().getCurrent(), aTime); + + } public double getPressure() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index b637fb3..9962eb3 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -12,11 +12,13 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Config; import frc.robot.Constants; import frc.robot.DriverInterface; import frc.robot.RobotMap; +import frc.robot.DriverInterface.MessageType; /** * Put docs here // TODO */ @@ -31,6 +33,7 @@ public class Shooter extends Subsystems{ private DoubleLogEntry logShootBottomFB; private DoubleLogEntry logShootTopRef; private DoubleLogEntry logShootBottomRef; + private StringLogEntry shooterState; public enum ShooterSpeedSlot { @@ -102,7 +105,7 @@ public void update() { } } catch(Exception e) { - System.out.println("CRITICAL ERROR,,, ERROR CHECKING JUST STOPPED A MAJOR CRASH!!!!!! PLEASE COME TO ADAM OR JOSH WITH THIS"); + DriverInterface.getInstance().consoleOutput(MessageType.WARNING, "ERROR CHECKING JUST STOPPED A MAJOR CRASH!!!!!! PLEASE COME TO ADAM OR JOSH WITH THIS"); } if (waitCounts > 0) { @@ -399,6 +402,7 @@ public void initLogging(DataLog aLog) logShootBottomFB = new DoubleLogEntry(aLog, "Shooter Bottom Fb"); logShootTopRef = new DoubleLogEntry(aLog, "Shooter Top Ref"); logShootBottomRef = new DoubleLogEntry(aLog, "Shooter Bottom Ref"); + shooterState = new StringLogEntry(aLog, "Shooter state"); } public void updateLogging(long aTime) @@ -407,6 +411,7 @@ public void updateLogging(long aTime) logShootBottomFB.append(getShooterBottomRPM(), aTime); logShootTopRef.append(topOutput * 600 / 2048, aTime); logShootBottomRef.append(botOutput * 600 / 2048, aTime); + shooterState.append(currentState.toString(), aTime); } } diff --git a/src/main/java/frc/robot/subsystems/Subsystems.java b/src/main/java/frc/robot/subsystems/Subsystems.java index ee4d8b1..8760227 100644 --- a/src/main/java/frc/robot/subsystems/Subsystems.java +++ b/src/main/java/frc/robot/subsystems/Subsystems.java @@ -15,7 +15,7 @@ public static enum diagnosticState{ OK, //All systems are good CAUTION, //Non-critical issue to be noted. WARNING, //Higher impact error. Notes somewhat failure in a mechanism - CRITICAL, //Notes a critical failure, crippling the mechanism + CRITICAL, //Denotes a critical failure, crippling the mechanism FAILURE, //Mechanism is unusable/completly failed. } diff --git a/src/main/java/frc/robot/subsystems/VisionTrack.java b/src/main/java/frc/robot/subsystems/VisionTrack.java index 8a6bb6a..56680e3 100644 --- a/src/main/java/frc/robot/subsystems/VisionTrack.java +++ b/src/main/java/frc/robot/subsystems/VisionTrack.java @@ -6,12 +6,14 @@ import frc.robot.subsystems.Shooter.ShooterSpeedSlot; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DriverStation; /** * Put docs here // TODO */ -public class VisionTrack { +public class VisionTrack extends Subsystems{ private static VisionTrack mInstance; private static Limelight m_lime; private static Shooter m_Shooter = Shooter.getInstance(); @@ -24,6 +26,13 @@ public class VisionTrack { private double speed; private double tx; private boolean turnStatus; + + private BooleanLogEntry targetLocked; + private DoubleLogEntry limelightPipeline; + private DoubleLogEntry limelightErrorX; + private DoubleLogEntry limelightErrorY; + private DoubleLogEntry limelightDistanceToTarget; + public static VisionTrack getInstance() { if (mInstance == null) { mInstance = new VisionTrack(); @@ -276,7 +285,7 @@ public double returnShooterSpeedLimelight(){ try{ double distance = Limelight.getInstance().getDistanceToTarget(); double aspeed = Config.kLimelightDTerm + (Config.kLimelightATerm - Config.kLimelightDTerm)/(1 + Math.pow((distance/Config.kLimelightCTerm),Config.kLimelightBTerm)); - aspeed = aspeed * Config.kLimelightShooterSpeedModiferPercentage; + aspeed = (aspeed * DriverInterface.getInstance().getShooterOffsetMultiplier()) + DriverInterface.getInstance().getShooterCTerm(); return aspeed; } catch(Exception e){ @@ -292,4 +301,59 @@ public boolean getLimelightLock() { public double getLimelightDistance() { return m_lime.getDistanceToTarget(); } + +@Override +public void resetSensors() { + // TODO Auto-generated method stub + +} + +@Override +public boolean initMechanism() { + // TODO Auto-generated method stub + return false; +} + +@Override +public diagnosticState getDiagnosticState() { + // TODO Auto-generated method stub + return null; +} + +@Override +public void initMotorControllers() { + // TODO Auto-generated method stub + +} + +@Override +public diagnosticState test() { + // TODO Auto-generated method stub + return null; +} + +@Override +public void clearFaults() { + // TODO Auto-generated method stub + +} + +@Override +public void initLogging(DataLog aLog) { + targetLocked = new BooleanLogEntry(aLog, "Limelight target lock"); + limelightPipeline = new DoubleLogEntry(aLog, "Limelight Pipeline ID"); + limelightErrorX = new DoubleLogEntry(aLog, "Limelight X Error"); + limelightErrorY = new DoubleLogEntry(aLog, "Limelight Y Error"); + limelightDistanceToTarget = new DoubleLogEntry(aLog, "Limelight distance to target"); + +} + +@Override +public void updateLogging(long aTime) { + targetLocked.append(m_lime.getTargetAcquired(), aTime); + limelightPipeline.append(m_lime.getPipeline(), aTime); + limelightErrorX.append(m_lime.getAngleToTarget(), aTime); + limelightErrorY.append(m_lime.getErrorY(), aTime); + limelightDistanceToTarget.append(m_lime.getDistanceToTarget(), aTime); +} } \ No newline at end of file