diff --git a/src/main/java/frc/lib/math/Conversions.java b/src/main/java/frc/lib/math/Conversions.java index f0a7e6a..5238d4d 100644 --- a/src/main/java/frc/lib/math/Conversions.java +++ b/src/main/java/frc/lib/math/Conversions.java @@ -6,74 +6,77 @@ public class Conversions { /** - * Convert from motor encoder rotations to Meters - * - * @param rotations Rotations of motor encoder - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @param circumference The circumference of the Wheels - * @return Degrees of Rotation of Mechanism falconToDegrees + * @param falconRotations Falcon rotations + * @param gearRatio gear ratio between Falcon and mechanism + * @return degrees of rotation of mechanism */ - public static double rotationsToMeters(double rotations, double gearRatio, - double circumference) { - return rotations * circumference / gearRatio; + public static double falconRotationsToMechanismDegrees(double falconRotations, + double gearRatio) { + return falconRotations * 360.0 / gearRatio; } /** - * Convert from Motor Encoder rotations to degrees - * - * @param rotations Rotations of motor encoder - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Degrees of Rotation of Mechanism falconToDegrees + * @param degrees Degrees of rotation of mechanism + * @param gearRatio gear ratio between Falcon and mechanism + * @return Falcon rotations */ - public static double rotationToDegrees(double rotations, double gearRatio) { - return rotations * (360.0 / gearRatio); + public static double degreesToFalconRotations(double degrees, double gearRatio) { + return (degrees / 360.0) * gearRatio; } /** - * Convert from Degrees to rotations of the motor - * - * @param degrees Degrees - * @param gearRatio Gear Ratio between sensor and mechanism - * @return Number of rotations of the motor encoder + * @param rps Falcon rotations per second + * @param gearRatio gear ratio between Falcon and mechanism (set to 1 for Falcon RPM) + * @return RPM of mechanism */ - public static double degreesToRotation(double degrees, double gearRatio) { - return degrees / (360.0 / gearRatio); + public static double falconRPSToMechanismRPM(double falconRPS, double gearRatio) { + double motorRPM = falconRPS * 60.0; + return motorRPM / gearRatio; } /** - * Meters per Second to Motor Encoder Rotations - * - * @param velocity Current veloctiy in Meters per Second - * @param circumference Circumference of the wheels - * @param gearRatio Gear ration between Falcon and Mechanism - * @return Number of rotations + * @param RPM RPM of mechanism + * @param gearRatio Gear ratio between Falcon and mechanism (set to 1 for Falcon RPS) + * @return Falcon rotations per second */ - public static double mpsToRotations(double velocity, double circumference, double gearRatio) { - return (velocity / circumference) * gearRatio; + public static double rpmToFalconRPS(double rpm, double gearRatio) { + double motorRPM = rpm * gearRatio; + return motorRPM / 60.0; } /** - * Motor Encoder Rotations to Meters per Second - * - * @param rps Motor Encoder Rotations per Second - * @param circumference Circumference of the wheels - * @param gearRatio Gear ration between Falcon and Mechanism - * @return Velocity in Meters per Second + * @param falconRotations Falcon rotations + * @param circumference circumference of wheel + * @param gearRatio gear ratio between Falcon and mechanism + * @return linear distance traveled by wheel in meters */ - public static double rpsToMPS(double rps, double circumference, double gearRatio) { - double wheelRPS = rps / gearRatio; - double wheelMPS = (wheelRPS * circumference); - return wheelMPS; + public static double falconRotationsToMechanismMeters(double falconRotations, + double circumference, double gearRatio) { + double wheelRotations = falconRotations / gearRatio; + return (wheelRotations * circumference); } /** - * Normalize angle to between 0 to 360 - * - * @param goal initial angle - * @return normalized angle + * @param falconRPS Falcon rotations per second + * @param circumference circumference of wheel + * @param gearRatio gear ratio between Falcon and mechanism + * @return mechanism linear velocity in meters per second */ - public static double reduceTo0_360(double goal) { - return goal % 360; + public static double falconRPSToMechanismMPS(double falconRPS, double circumference, + double gearRatio) { + double wheelRPM = falconRPSToMechanismRPM(falconRPS, gearRatio); + return (wheelRPM * circumference) / 60; + } + + /** + * @param velocity velocity in meters per second + * @param circumference circumference of wheel + * @param gearRatio gear ratio between Falcon and mechanism + * @return Falcon rotations per second + */ + public static double mpsToFalconRPS(double velocity, double circumference, double gearRatio) { + double wheelRPM = ((velocity * 60) / circumference); + return rpmToFalconRPS(wheelRPM, gearRatio); } } diff --git a/src/main/java/frc/lib/util/ctre/CTREConfigs.java b/src/main/java/frc/lib/util/ctre/CTREConfigs.java deleted file mode 100644 index 55592d1..0000000 --- a/src/main/java/frc/lib/util/ctre/CTREConfigs.java +++ /dev/null @@ -1,92 +0,0 @@ -package frc.lib.util.ctre; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import frc.robot.Constants; - -/** - * CTRE config constants - */ -public final class CTREConfigs { - public TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration(); - private CurrentLimitsConfigs swerveAngleFXCurrentLimitsConfigs = new CurrentLimitsConfigs(); - public TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); - private CurrentLimitsConfigs swerveDriveFXCurrentLimitsConfigs = new CurrentLimitsConfigs(); - public CANcoderConfiguration swerveCanCoderConfig = new CANcoderConfiguration(); - private MagnetSensorConfigs swerveCanCoderMagnetSensorConfigs = new MagnetSensorConfigs(); - - /** - * CTRE config constants - */ - public CTREConfigs() { - /* Swerve Angle Motor Configurations */ - swerveAngleFXCurrentLimitsConfigs.StatorCurrentLimitEnable = - Constants.Swerve.angleEnableCurrentLimit; - swerveAngleFXCurrentLimitsConfigs.SupplyCurrentLimitEnable = - Constants.Swerve.angleEnableCurrentLimit; - swerveAngleFXCurrentLimitsConfigs.StatorCurrentLimit = - Constants.Swerve.angleContinuousCurrentLimit; - swerveAngleFXCurrentLimitsConfigs.SupplyCurrentThreshold = - Constants.Swerve.anglePeakCurrentLimit; - swerveAngleFXCurrentLimitsConfigs.SupplyTimeThreshold = - Constants.Swerve.anglePeakCurrentDuration; - swerveAngleFXCurrentLimitsConfigs.SupplyCurrentLimit = - Constants.Swerve.anglePeakCurrentLimit; - swerveAngleFXConfig.CurrentLimits = swerveAngleFXCurrentLimitsConfigs; - swerveAngleFXConfig.Feedback = new FeedbackConfigs().withFeedbackRotorOffset(0); - - Slot0Configs swerveAngleFXConfigGains = new Slot0Configs(); - swerveAngleFXConfigGains.kP = Constants.Swerve.angleKP; - swerveAngleFXConfigGains.kI = Constants.Swerve.angleKI; - swerveAngleFXConfigGains.kD = Constants.Swerve.angleKD; - - /* Swerve Drive Motor Configuration */ - - swerveDriveFXCurrentLimitsConfigs.StatorCurrentLimitEnable = - Constants.Swerve.driveEnableCurrentLimit; - swerveDriveFXCurrentLimitsConfigs.SupplyCurrentLimitEnable = - Constants.Swerve.driveEnableCurrentLimit; - swerveDriveFXCurrentLimitsConfigs.StatorCurrentLimit = - Constants.Swerve.driveContinuousCurrentLimit; - swerveDriveFXCurrentLimitsConfigs.SupplyCurrentThreshold = - Constants.Swerve.drivePeakCurrentLimit; - swerveDriveFXCurrentLimitsConfigs.SupplyTimeThreshold = - Constants.Swerve.drivePeakCurrentDuration; - swerveDriveFXCurrentLimitsConfigs.SupplyCurrentLimit = - Constants.Swerve.drivePeakCurrentLimit; - swerveDriveFXConfig.CurrentLimits = swerveDriveFXCurrentLimitsConfigs; - swerveDriveFXConfig.Feedback = new FeedbackConfigs().withFeedbackRotorOffset(0); - - swerveDriveFXConfig.OpenLoopRamps = - new OpenLoopRampsConfigs().withVoltageOpenLoopRampPeriod(Constants.Swerve.openLoopRamp); - swerveDriveFXConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs() - .withTorqueClosedLoopRampPeriod(Constants.Swerve.closedLoopRamp); - - Slot0Configs swerveDriveFXConfigGains = new Slot0Configs(); - swerveDriveFXConfigGains.kP = Constants.Swerve.driveKP; - swerveDriveFXConfigGains.kI = Constants.Swerve.driveKI; - swerveDriveFXConfigGains.kD = Constants.Swerve.driveKD; - - - - /* Swerve CANCoder Configuration */ - swerveCanCoderMagnetSensorConfigs.AbsoluteSensorRange = - AbsoluteSensorRangeValue.Unsigned_0To1; - swerveCanCoderMagnetSensorConfigs.SensorDirection = SensorDirectionValue.Clockwise_Positive; - swerveCanCoderConfig.MagnetSensor = swerveCanCoderMagnetSensorConfigs; - - // swerveCanCoderConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; - // swerveCanCoderConfig.initializationStrategy = - // SensorInitializationStrategy.BootToAbsolutePosition; - // swerveCanCoderConfig.sensorTimeBase = SensorTimeBase.PerSecond; - } - -} diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 170df3c..7f7f87e 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,22 +1,20 @@ package frc.lib.util.swerve; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.lib.math.Conversions; import frc.lib.util.ctre.CTREModuleState; import frc.robot.Constants; -import frc.robot.Robot; /** * Base Swerve Module Class. Creates an instance of the swerve module @@ -28,11 +26,15 @@ public class SwerveModule { private TalonFX driveMotor; private CANcoder angleEncoder; private double lastAngle; - SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, - Constants.Swerve.driveKV, Constants.Swerve.driveKA); + private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration(); + private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); + private CANcoderConfiguration swerveCanCoderConfig = new CANcoderConfiguration(); private DutyCycleOut drivePower = new DutyCycleOut(0); - private VelocityVoltage driveSpeed = new VelocityVoltage(0); - private PositionDutyCycle angleMotorPosition = new PositionDutyCycle(0); + private VelocityDutyCycle driveSpeed = new VelocityDutyCycle(0).withSlot(0); + private PositionDutyCycle angleMotorPosition = new PositionDutyCycle(0).withSlot(0); + // Use Voltage to turn Voltage Compensation on + // private VelocityVoltage driveSpeed = new VelocityVoltage(0).withSlot(0); + // private PositionVoltage angleMotorPosition = new PositionVoltage(0).withSlot(0); /** * Creates an instance of a Swerve Module @@ -45,15 +47,15 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants angleOffset = constants.angleOffset; /* Angle Encoder Config */ - angleEncoder = new CANcoder(constants.cancoderID); + angleEncoder = new CANcoder(constants.cancoderID, Constants.Swerve.canBus); configAngleEncoder(); /* Angle Motor Config */ - angleMotor = new TalonFX(constants.angleMotorID); + angleMotor = new TalonFX(constants.angleMotorID, Constants.Swerve.canBus); configAngleMotor(); /* Drive Motor Config */ - driveMotor = new TalonFX(constants.driveMotorID); + driveMotor = new TalonFX(constants.driveMotorID, Constants.Swerve.canBus); configDriveMotor(); @@ -69,34 +71,27 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { desiredState = CTREModuleState.optimize(desiredState, getState().angle); - // Custom optimize - // command, since - // default WPILib - // optimize assumes - // continuous - // controller which - // CTRE is - // not + // Custom optimize command, since default WPILib optimize assumes + // continuous controller which CTRE is not if (isOpenLoop) { double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; driveMotor.setControl(drivePower.withOutput(percentOutput)); } else { - double velocity = Conversions.mpsToRotations(desiredState.speedMetersPerSecond, - Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio); - driveMotor.setControl(driveSpeed.withVelocity(velocity) - .withFeedForward(feedforward.calculate(desiredState.speedMetersPerSecond))); - + double velocity = Conversions.mpsToFalconRPS(desiredState.speedMetersPerSecond, + Constants.Swerve.wheelCircumference, 1); + driveMotor.setControl(driveSpeed.withVelocity(velocity)); } + // Prevent rotating module if speed is less then 1%. Prevents Jittering. double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) ? lastAngle - : desiredState.angle.getDegrees(); // Prevent rotating module if speed is - // less then 1%. Prevents - // Jittering. - angleMotor.setControl(angleMotorPosition - .withPosition(Conversions.degreesToRotation(angle, Constants.Swerve.angleGearRatio))); + : desiredState.angle.getDegrees(); + + // See Line 163, swerveAngleFXConfig.Feedback.SensorToMechanismRatio + angleMotor.setControl( + angleMotorPosition.withPosition(Conversions.degreesToFalconRotations(angle, 1))); lastAngle = angle; } @@ -105,9 +100,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) */ private void resetToAbsolute() { double absolutePosition = getCanCoderRaw() / Constants.Swerve.angleGearRatio; - FeedbackConfigs feedbackConfigs = Robot.ctreConfigs.swerveAngleFXConfig.Feedback - .withFeedbackRotorOffset(absolutePosition); - angleMotor.getConfigurator().apply(feedbackConfigs); + swerveAngleFXConfig.Feedback.FeedbackRotorOffset = absolutePosition; + angleMotor.getConfigurator().apply(swerveAngleFXConfig); } /** @@ -116,15 +110,11 @@ private void resetToAbsolute() { * @return Swerve module position */ public SwerveModulePosition getPosition() { + double position = Conversions.falconRotationsToMechanismMeters( + driveMotor.getPosition().getValueAsDouble(), Constants.Swerve.wheelCircumference, 1); - - - double position = - Conversions.rotationsToMeters(driveMotor.getRotorPosition().getValueAsDouble(), - Constants.Swerve.driveGearRatio, Constants.Swerve.wheelCircumference); - - Rotation2d angle = Rotation2d.fromDegrees(Conversions.rotationToDegrees( - angleMotor.getRotorPosition().getValueAsDouble(), Constants.Swerve.angleGearRatio)); + Rotation2d angle = Rotation2d.fromDegrees(Conversions + .falconRotationsToMechanismDegrees(angleMotor.getPosition().getValueAsDouble(), 1)); return new SwerveModulePosition(position, angle); } @@ -133,10 +123,13 @@ public SwerveModulePosition getPosition() { */ private void configAngleEncoder() { angleEncoder.getConfigurator().apply(new CANcoderConfiguration()); - MagnetSensorConfigs canCoderConfiguration = - Robot.ctreConfigs.swerveCanCoderConfig.MagnetSensor; - canCoderConfiguration.MagnetOffset = angleOffset; - angleEncoder.getConfigurator().apply(canCoderConfiguration); + swerveCanCoderConfig.MagnetSensor.SensorDirection = + Constants.Swerve.canCoderInvert ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + swerveCanCoderConfig.MagnetSensor.AbsoluteSensorRange = + AbsoluteSensorRangeValue.Unsigned_0To1; + swerveCanCoderConfig.MagnetSensor.MagnetOffset = angleOffset; + angleEncoder.getConfigurator().apply(swerveCanCoderConfig); } /** @@ -144,7 +137,23 @@ private void configAngleEncoder() { */ private void configAngleMotor() { angleMotor.getConfigurator().apply(new TalonFXConfiguration()); - angleMotor.getConfigurator().apply(Robot.ctreConfigs.swerveAngleFXConfig); + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.angleEnableCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.anglePeakCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.anglePeakCurrentDuration; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = + Constants.Swerve.angleContinuousCurrentLimit; + swerveAngleFXConfig.Feedback.FeedbackRotorOffset = 0; + swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP; + swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI; + swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD; + // swerveAngleFXConfig.Slot0.kS = 0; + // swerveAngleFXConfig.Slot0.kV = 0; + swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; + // swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; + angleMotor.getConfigurator().apply(swerveAngleFXConfig); angleMotor.setInverted(Constants.Swerve.angleMotorInvert); angleMotor.setNeutralMode(Constants.Swerve.angleNeutralMode); resetToAbsolute(); @@ -155,7 +164,26 @@ private void configAngleMotor() { */ private void configDriveMotor() { driveMotor.getConfigurator().apply(new TalonFXConfiguration()); - driveMotor.getConfigurator().apply(Robot.ctreConfigs.swerveDriveFXConfig); + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.driveEnableCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.drivePeakCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.drivePeakCurrentDuration; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = + Constants.Swerve.driveContinuousCurrentLimit; + swerveDriveFXConfig.Feedback.FeedbackRotorOffset = 0; + swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; + swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; + swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; + swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; + swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; + swerveDriveFXConfig.Slot0.kS = Constants.Swerve.driveKS; + swerveDriveFXConfig.Slot0.kV = Constants.Swerve.driveKV; + swerveDriveFXConfig.Slot0.kA = Constants.Swerve.driveKA; + swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; + driveMotor.getConfigurator().apply(swerveDriveFXConfig); driveMotor.setInverted(Constants.Swerve.driveMotorInvert); driveMotor.setNeutralMode(Constants.Swerve.driveNeutralMode); } @@ -175,8 +203,8 @@ public double getCanCoderRaw() { * @return 2d rotation of the module */ public Rotation2d getCanCoder() { - return Rotation2d.fromDegrees( - Conversions.rotationToDegrees(getCanCoderRaw(), Constants.Swerve.angleGearRatio)); + return Rotation2d.fromDegrees(Conversions + .falconRotationsToMechanismDegrees(getCanCoderRaw(), Constants.Swerve.angleGearRatio)); } /** @@ -185,10 +213,10 @@ public Rotation2d getCanCoder() { * @return Swerve module state */ public SwerveModuleState getState() { - double velocity = Conversions.rpsToMPS(driveMotor.getRotorVelocity().getValueAsDouble(), - Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio); - Rotation2d angle = Rotation2d.fromDegrees(Conversions.rotationToDegrees( - angleMotor.getRotorPosition().getValueAsDouble(), Constants.Swerve.angleGearRatio)); + double velocity = Conversions.falconRPSToMechanismMPS( + driveMotor.getVelocity().getValueAsDouble(), Constants.Swerve.wheelCircumference, 1); + Rotation2d angle = Rotation2d.fromDegrees(Conversions + .falconRotationsToMechanismDegrees(angleMotor.getPosition().getValueAsDouble(), 1)); return new SwerveModuleState(velocity, angle); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 56e4e7e..d51947c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,6 +57,7 @@ public static final class Swerve { public static final edu.wpi.first.wpilibj.SPI.Port navXID = edu.wpi.first.wpilibj.SPI.Port.kMXP; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- + public static final String canBus = "rio"; /* Drivetrain Constants */ public static final double trackWidth = Units.inchesToMeters(14); @@ -71,7 +72,7 @@ public static final class Swerve { public static final double closedLoopRamp = 0.4; public static final double driveGearRatio = (8.14 / 1.0); // 8.14:1 - public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1 + public static final double angleGearRatio = 1 / ((14.0 / 50.0) * (10.0 / 60.0)); // 150/7:1 public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(new Translation2d(wheelBase / 2.0, trackWidth / 2.0), @@ -117,8 +118,8 @@ public static final class Swerve { public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake; /* Motor Inverts */ - public static final boolean driveMotorInvert = false; - public static final boolean angleMotorInvert = false; + public static final boolean driveMotorInvert = true; + public static final boolean angleMotorInvert = true; /* Angle Encoder Invert */ public static final boolean canCoderInvert = false; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3e7d82e..9f71594 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.lib.util.ctre.CTREConfigs; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -16,7 +15,6 @@ * project. */ public class Robot extends TimedRobot { - public static CTREConfigs ctreConfigs; private Command m_autonomousCommand; @@ -29,7 +27,6 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - ctreConfigs = new CTREConfigs(); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index f658f52..7bdce28 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,20 +1,20 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.1.2", + "version": "v2024.1.4", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", "jniDependencies": [], "cppDependencies": [ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.1.2", + "version": "v2024.1.4", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.1.2", + "version": "v2024.1.4", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.1.2" + "version": "v2024.1.4" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.1.2" + "version": "v2024.1.4" } ] } \ No newline at end of file