diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e577f4a7..ef24a116 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,16 +103,15 @@ public RobotContainer() { break; } - // Set up named commands for PathPlanner + // Set up auto routines NamedCommands.registerCommand( "Run Flywheel", Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); - - // Set up auto routines + () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel) + .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - // Set up FF characterization routines + // Set up feedforward characterization autoChooser.addOption( "Drive FF Characterization", new FeedForwardCharacterization( @@ -120,7 +119,7 @@ public RobotContainer() { autoChooser.addOption( "Flywheel FF Characterization", new FeedForwardCharacterization( - flywheel, flywheel::runCharacterizationVolts, flywheel::getCharacterizationVelocity)); + flywheel, flywheel::runVolts, flywheel::getCharacterizationVelocity)); // Configure the button bindings configureButtonBindings(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 22713232..f36eb6ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -28,6 +28,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; @@ -72,6 +73,9 @@ public Drive( this::runVelocity, new HolonomicPathFollowerConfig( MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index bfcbc4d7..5b94ef0b 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -27,7 +27,7 @@ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2(20); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZ(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); public GyroIOPigeon2(boolean phoenixDrive) { pigeon.getConfigurator().apply(new Pigeon2Configuration()); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 4d886309..6c79134b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -13,10 +13,10 @@ package frc.robot.subsystems.drive; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index d60bbab0..33a44481 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -14,9 +14,9 @@ package frc.robot.subsystems.drive; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import com.ctre.phoenix6.unmanaged.Unmanaged; import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -59,7 +59,7 @@ public Queue registerSignal(ParentDevice device, StatusSignal si signalsLock.lock(); Drive.odometryLock.lock(); try { - isCANFD = Unmanaged.isNetworkFD(device.getNetwork()); + isCANFD = CANBus.isNetworkFD(device.getNetwork()); BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; System.arraycopy(signals, 0, newSignals, 0, signals.length); newSignals[signals.length] = signal; diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index 1de6bf46..e2b15ca3 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -53,6 +53,11 @@ public void periodic() { Logger.processInputs("Flywheel", inputs); } + /** Run open loop at the specified voltage. */ + public void runVolts(double volts) { + io.setVoltage(volts); + } + /** Run closed loop at the specified velocity. */ public void runVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); @@ -73,12 +78,7 @@ public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); } - /** Runs forwards at the commanded voltage. */ - public void runCharacterizationVolts(double volts) { - io.setVoltage(volts); - } - - /** Returns the average drive velocity in radians/sec. */ + /** Returns the current velocity in radians per second. */ public double getCharacterizationVelocity() { return inputs.velocityRadPerSec; } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java index 9f557949..787f3693 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java @@ -13,21 +13,25 @@ package frc.robot.subsystems.flywheel; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.SparkMaxPIDController.ArbFFUnits; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.util.Units; +/** + * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with + * "CANSparkFlex". + */ public class FlywheelIOSparkMax implements FlywheelIO { private static final double GEAR_RATIO = 1.5; private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkMaxPIDController pid = leader.getPIDController(); + private final SparkPIDController pid = leader.getPIDController(); public FlywheelIOSparkMax() { leader.restoreFactoryDefaults();