diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f9d24aa..f410d8e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,16 @@ public final class Constants { public static final Mode kCurrentMode = Mode.kReal; public static final RobotName kRobot = RobotName.kBlackMamba; - public static final boolean kIsViper = kRobot.equals(RobotName.kViper); + + // static { + // final String rioSerialNumber = RobotController.getSerialNumber(); + // if (rioSerialNumber.equals(RobotName.kViper.rioSerialNumber)) kRobot = RobotName.kViper; + // else if (rioSerialNumber.equals(RobotName.kBlackMamba.rioSerialNumber)) + // kRobot = RobotName.kBlackMamba; + // else kRobot = RobotName.kBlackMamba; + // } + + public static final boolean kIsViper = false; public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim); public static final String kDrivetrainCanBus = "CANivore1"; public static final String kSuperstructureCanBus = "CANivore2"; @@ -50,7 +59,13 @@ public static enum Mode { } public static enum RobotName { - kViper, - kBlackMamba + kViper("032380FD"), + kBlackMamba("032243C9"); + + public String rioSerialNumber; + + private RobotName(String rioSerialNumber) { + this.rioSerialNumber = rioSerialNumber; + } } } diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java index 47ffb0a..e5c74cc 100644 --- a/src/main/java/frc/robot/arm/ArmIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -55,7 +55,7 @@ public ArmIOTalonFX() { final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration(); elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig; elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction; elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; elbowLeftMotorConfig.Slot0.kP = 62.122; @@ -66,6 +66,8 @@ public ArmIOTalonFX() { elbowLeftMotorConfig.Slot0.kG = 0.37766; elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5; elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; + elbowLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0; + elbowLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0; elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0; elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -6.0; m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0); @@ -73,7 +75,7 @@ public ArmIOTalonFX() { final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration(); wristLeftMotorConfig.CurrentLimits = currentLimitsConfig; wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; wristLeftMotorConfig.ClosedLoopGeneral.ContinuousWrap = true; wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction; wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; @@ -87,17 +89,20 @@ public ArmIOTalonFX() { wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0; wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0; + wristLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0; + wristLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0; + m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig, 1.0); final TalonFXConfiguration elbowRightMotorConfig = new TalonFXConfiguration(); elbowRightMotorConfig.CurrentLimits = currentLimitsConfig; - elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig, 1.0); m_elbowRightFollowerMotor.setControl(new Follower(m_elbowLeftMotor.getDeviceID(), true)); final TalonFXConfiguration wristRightMotorConfig = new TalonFXConfiguration(); wristRightMotorConfig.CurrentLimits = currentLimitsConfig; - wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig, 1.0); m_wristRightFollowerMotor.setControl(new Follower(m_wristLeftMotor.getDeviceID(), true)); diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index f89c9db..124053f 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -6,10 +6,10 @@ import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { - public static final double kWinchReduction = 1.0; + public static final double kWinchReduction = 20.0; private final ClimberIO m_io; private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged(); - private final double kTrapLimit = 200.0; + private final double kTrapLimit = 10.0; public Climber(ClimberIO climberIO) { m_io = climberIO; diff --git a/src/main/java/frc/robot/drivetrain/Module.java b/src/main/java/frc/robot/drivetrain/Module.java index 1af183d..6aa2489 100644 --- a/src/main/java/frc/robot/drivetrain/Module.java +++ b/src/main/java/frc/robot/drivetrain/Module.java @@ -4,17 +4,20 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import frc.robot.Constants; import org.littletonrobotics.junction.Logger; public class Module { // https://www.swervedrivespecialties.com/products/mk4i-swerve-module // However, our first stage is 50:13 instead of 50:14 because we have a different gear. - public static final double kDriveRatio = (50.0 / 13.0) * (16.0 / 28.0) * (45.0 / 15.0); + private static final double kDriveRatioFirstStage = + Constants.kIsViper ? (50.0 / 13.0) : (50.0 / 14.0); + public static final double kDriveRatio = kDriveRatioFirstStage * (16.0 / 28.0) * (45.0 / 15.0); public static final double kSteerRatio = 150.0 / 7.0; private final double kWheelDiameterMeters = Units.inchesToMeters(1.87 * 2); private final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0; - private final double kCouplingGearRatio = 50.0 / 13.0; + private final double kCouplingRatio = kDriveRatioFirstStage; private final ModuleIO m_io; private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); @@ -39,7 +42,7 @@ public void periodic() { for (int i = 0; i < sampleCount; i++) { double driveRotations = Units.radiansToRotations(m_inputs.odometryDrivePositionsRad[i]); Rotation2d steerAngle = m_inputs.odometryTurnPositions[i]; - driveRotations -= steerAngle.getRotations() * kCouplingGearRatio; + driveRotations -= steerAngle.getRotations() * kCouplingRatio; double driveRadians = Units.rotationsToRadians(driveRotations); double positionMeters = driveRadians * kWheelRadiusMeters; m_odometryPositions[i] = new SwerveModulePosition(positionMeters, steerAngle); @@ -58,7 +61,7 @@ public SwerveModuleState setSetpoint(SwerveModuleState state) { if (optimizedState.speedMetersPerSecond != 0.0) { double azimuthVelocityRPS = Units.radiansToRotations(m_inputs.steerVelocityRadPerSec); - double driveRateBackOut = azimuthVelocityRPS *= kCouplingGearRatio; + double driveRateBackOut = azimuthVelocityRPS *= kCouplingRatio; setpointVelocityRPS += driveRateBackOut; } diff --git a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java index 07c8027..6b1f727 100644 --- a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java @@ -61,7 +61,7 @@ public ModuleIOTalonFX(int index) { m_driveMotor = new TalonFX(4, Constants.kDrivetrainCanBus); m_steerMotor = new TalonFX(3, Constants.kDrivetrainCanBus); m_azimuthEncoder = new CANcoder(12, Constants.kDrivetrainCanBus); - m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.37353515625 : 0.047607421875; + m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.37353515625 : 0.066162109375; kDrivekS = 0.092972; kDrivekV = 0.82452; break; @@ -127,6 +127,7 @@ public ModuleIOTalonFX(int index) { steerMotorConfig.MotionMagic.MotionMagicExpo_kA = 0.1; steerMotorConfig.Slot0.kP = kSteerkP; steerMotorConfig.Slot0.kD = kSteerkD; + steerMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; m_steerMotor.getConfigurator().apply(steerMotorConfig); m_drivePositionSignal = m_driveMotor.getPosition(); diff --git a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java index 49318a9..9f7b954 100644 --- a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java @@ -5,12 +5,15 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DigitalGlitchFilter; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants; +import java.time.Duration; public class FeederIOTalonFX implements FeederIO { private final TalonFX m_feederMotor; private final DigitalInput m_topNoteSensor; + private final DigitalGlitchFilter m_glitchFilter; private final StatusSignal m_rollerPositionSignal, m_rollerVelocitySignal, @@ -20,6 +23,9 @@ public class FeederIOTalonFX implements FeederIO { public FeederIOTalonFX() { m_feederMotor = new TalonFX(19, Constants.kSuperstructureCanBus); m_topNoteSensor = new DigitalInput(0); + m_glitchFilter = new DigitalGlitchFilter(); + m_glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos()); + m_glitchFilter.add(m_topNoteSensor); final TalonFXConfiguration feederMotorConfig = new TalonFXConfiguration(); m_feederMotor.getConfigurator().apply(feederMotorConfig); diff --git a/src/main/java/frc/robot/lights/Lights.java b/src/main/java/frc/robot/lights/Lights.java index 93e4b94..caaca71 100644 --- a/src/main/java/frc/robot/lights/Lights.java +++ b/src/main/java/frc/robot/lights/Lights.java @@ -57,7 +57,7 @@ public Command startGreenFlow() { color.green, color.blue, 0, - 0.5, + 0.1, kLedCount, Direction.Forward, kCandleLedOffset))) diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 2102d3b..908d542 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -17,7 +17,7 @@ public class Shooter extends SubsystemBase { - public static final double kShooterStepUp = Constants.kIsViper ? 0.0 : 0.5; + public static final double kShooterStepUp = Constants.kIsViper ? 0.0 : 0.0; public static final double kFarShotVelocityRpm = 5600.0; private final double kTrapShot = 400.0; private final double kAmpShot = 5000.0;