// Logging
public static final boolean USE_TELEMETRY = true;
+ // this would disable all logger calls
+ public static final boolean DISABLE_LOGGING = true;
+
public static enum Mode {
/** Running on a real robot. */
REAL,
SmartDashboard.putBoolean("Climb Calibrated", !calibrating);
SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint());
- Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint())
- * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint())
+ * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO);
+ }
updateInputs();
- Logger.processInputs("LinearClimb", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("LinearClimb", inputs);
+ }
}
/**
}
public void periodic() {
- // Report position to SmartDashboard
double inchExtension = getPosition();
- Logger.recordOutput("Intake/Setpoint", setpointInches);
- robotExtension.setLength(inchExtension);
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Intake/Setpoint", setpointInches);
+ robotExtension.setLength(inchExtension);
+ }
SmartDashboard.putNumber("Intake Extension (in)", inchExtension);
SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0);
}
updateInputs();
- Logger.processInputs("Intake", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Intake", inputs);
+ }
SmartDashboard.putBoolean("Intake Calibrated", !calibrating);
SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5);
public void periodic() {
odometryLock.lock(); // Prevents odometry updates while reading data
gyroIO.updateInputs(gyroInputs);
- Logger.processInputs("Drive/Gyro", gyroInputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+ }
for (var module : modules) {
module.periodic();
}
// Apply update
poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
}
- Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+ }
updateOdometryVision();
field.setRobotPose(getPose());
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
+import frc.robot.constants.Constants;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Voltage;
public void periodic() {
updateInputs();
- Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+ }
// Calculate positions for odometry
int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
driveDisconnectedAlert.set(!inputs.driveConnected);
turnDisconnectedAlert.set(!inputs.turnConnected);
turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
- Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
}
public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
driveMotor.set(percentOutput);
} else {
double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
- Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+ }
double feedforward = velocity * moduleConstants.getDriveV();
driveMotor.setControl(
@Override
public void periodic() {
updateInputs();
- Logger.processInputs("Hood", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Hood", inputs);
+ }
// goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees()));
// SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees());
.withFeedForward(velocityCompensation));
}
- Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
- Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO);
- Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()));
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
+ Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO);
+ Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()));
+ }
SmartDashboard.putBoolean("Hood Calibrated", !calibrating);
SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0);
shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS));
shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS));
- Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER);
- Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER);
+ Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
+ }
double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER;
inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
- Logger.processInputs("Shooter", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Shooter", inputs);
+ }
}
/**
@Override
public void periodic() {
- updateInputs();
- Logger.processInputs("Spindexer", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ updateInputs();
+ Logger.processInputs("Spindexer", inputs);
+ }
if (resetPos == null) {
motor.setPosition(0.1 * gearRatio);
public void updateInputs() {
inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio;
inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble();
- Logger.processInputs("Spindexer", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Spindexer", inputs);
+ }
}
private Double resetPos;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.constants.Constants;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@Override
public void periodic() {
updateInputs();
- Logger.processInputs("Turret", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Turret", inputs);
+ }
// Position extrapolation
double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT;
.withFeedForward(robotTurnCompensation));
}
- Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
- Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees());
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
+ Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees());
+ }
// --- Visualization ---
ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
updateInputs();
- Logger.processInputs("Turret", inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Turret", inputs);
+ }
SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad()));
SmartDashboard.putBoolean("Turret Calibrated", !calibrating);
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.Constants;
import frc.robot.constants.FieldConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.constants.swerve.DriveConstants;
inputs.connected = camera.isConnected();
inputs.results = camera.getAllUnreadResults();
- Logger.processInputs("Vision/"+camera.getName(), inputs);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.processInputs("Vision/"+camera.getName(), inputs);
+ }
// Mechanical Advantage's vision logging
// // Read new camera observations