This testbed provides: + * + *
Motor Detection: Each motor's type (TalonFX or Spark Max) is automatically detected at
+ * startup by interrogating the CAN bus. Motors are configured appropriately based on detection
+ * results. Detection is transparent - motors just work with whatever controller is present without
+ * exposing type information to the dashboard.
+ */
public class RobotContainer implements AbstractRobotContainer {
- public static RobotConfig config =
- RobotConfig.defaultCommandBased(frc.robot.outReach.RobotContainer::new);
+ public static RobotConfig config = RobotConfig.defaultCommandBased(RobotContainer::new);
+
+ // Constants for motor control
+ private static final double JOYSTICK_DEADBAND = 0.1;
+ private static final double MAX_VOLTAGE = 12.0;
+
+ // Controllers
+ private final CommandXboxController driverController = new CommandXboxController(0);
+ private final CommandXboxController codriverController = new CommandXboxController(1);
+
+ // Subsystems - using generic swerve drive
+ private final Drive drive = SwerveBuilder.buildDefaultDrive(driverController);
+
+ // Test motors for prototype testing
+ // 4 motors for joystick control (CAN IDs 10-13)
+ private final TestMotor joystickMotor1 = new TestMotor(10, "JoystickMotor1");
+ private final TestMotor joystickMotor2 = new TestMotor(11, "JoystickMotor2");
+ private final TestMotor joystickMotor3 = new TestMotor(12, "JoystickMotor3");
+ private final TestMotor joystickMotor4 = new TestMotor(13, "JoystickMotor4");
+
+ // Paddle motors (CAN IDs 14-15)
+ private final TestMotor paddleMotor1 = new TestMotor(14, "PaddleMotor1");
+ private final TestMotor paddleMotor2 = new TestMotor(15, "PaddleMotor2");
+
+ // Button-controlled motors with dashboard settings (CAN IDs 20-23)
+ private final TestMotor buttonMotor1 = new TestMotor(20, "ButtonMotor1");
+ private final TestMotor buttonMotor2 = new TestMotor(21, "ButtonMotor2");
+ private final TestMotor buttonMotor3 = new TestMotor(22, "ButtonMotor3");
+ private final TestMotor buttonMotor4 = new TestMotor(23, "ButtonMotor4");
+
+ // Dashboard
+ private final LoggedDashboardChooser Joystick Control: The four joystick motors receive continuous voltage control (-12V
+ * to +12V) proportional to joystick deflection. A 10% deadband is applied to prevent drift.
+ *
+ * Paddle Control: The two paddle motors run synchronized - both move forward when left
+ * trigger is pressed, both move backward when right trigger is pressed. Speed is proportional to
+ * trigger pressure.
+ *
+ * Button Control: The four button motors are controlled via dashboard values. Press and
+ * hold A/B for velocity control or X/Y for position control. Release to stop. Target values are
+ * read from NetworkTables.
+ *
+ * Emergency Stops:
+ *
+ * This subsystem automatically detects the motor controller type at the given CAN ID by
+ * interrogating the CAN bus and configures itself accordingly. Supports three control modes:
+ *
+ * Motor Type Detection: Uses {@link MotorDetectUtil#detectIsTalonFX(int)} to determine if
+ * the motor is a TalonFX or Spark Max at startup. The appropriate hardware interface is then
+ * instantiated based on the detection result. This detection is transparent - the motor just works
+ * with whatever controller is present.
+ */
+public class TestMotor extends SubsystemBase {
+ // Position control constants
+ private static final double POSITION_MAX_VOLTAGE = 3.0; // Max voltage for position control
+ private static final double POSITION_TOLERANCE = 0.05; // Tolerance in rotations
+ private static final double POSITION_GAIN = 2.0; // Proportional gain for position control
+
+ private final int canId;
+ private final String name;
+ private final boolean isTalonFX;
+ private final CANBus canBus = new CANBus(); // CAN bus for TalonFX motors
+
+ // Motor controllers
+ private SparkMax sparkMotor;
+ private SparkClosedLoopController sparkPID;
+ private LoggedTalonFX talonMotor;
+ private final VoltageOut talonVoltageControl = new VoltageOut(0);
+ private final VelocityVoltage talonVelocityControl = new VelocityVoltage(0);
+
+ // Control state
+ public double targetVelocity = 0.0; // RPM for velocity control
+ public double targetPosition = 0.0; // Rotations for position control
+ private double currentOutput = 0.0; // Current voltage output
+ private ControlMode controlMode = ControlMode.STOPPED;
+
+ // Dashboard inputs for target values only
+ private final DoubleSubscriber velocityInput;
+ private final DoubleSubscriber positionInput;
+
+ public enum ControlMode {
+ STOPPED,
+ VOLTAGE,
+ VELOCITY,
+ POSITION
+ }
+
+ /**
+ * Creates a new TestMotor subsystem.
+ *
+ * @param canId The CAN ID of the motor
+ * @param name The name of the motor (for dashboard)
+ */
+ public TestMotor(int canId, String name) {
+ this.canId = canId;
+ this.name = name;
+
+ // Detect motor type
+ switch (Constants.currentMode) {
+ case REAL:
+ this.isTalonFX = MotorDetectUtil.detectIsTalonFX(canId);
+ System.out.println(
+ "[TestMotor] "
+ + name
+ + " (CAN ID "
+ + canId
+ + "): Detected as "
+ + (isTalonFX ? "TalonFX" : "Spark"));
+
+ if (isTalonFX) {
+ talonMotor = new PhoenixTalonFX(canId, canBus, name);
+ } else {
+ sparkMotor = new SparkMax(canId, MotorType.kBrushless);
+ var config = new SparkMaxConfig();
+ sparkMotor.configure(
+ config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+ sparkPID = sparkMotor.getClosedLoopController();
+ }
+ break;
+
+ case SIM:
+ this.isTalonFX = MotorDetectUtil.detectIsTalonFX(canId);
+ System.out.println(
+ "[TestMotor] "
+ + name
+ + " (CAN ID "
+ + canId
+ + "): Simulating as "
+ + (isTalonFX ? "TalonFX" : "Spark"));
+
+ if (isTalonFX) {
+ talonMotor = new SimpleMotorSim(canId, null, name, 1.0, 1.0);
+ } else {
+ // For Spark in sim, we'll just create a real Spark object since there's no sim version
+ sparkMotor = new SparkMax(canId, MotorType.kBrushless);
+ var config = new SparkMaxConfig();
+ sparkMotor.configure(
+ config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+ sparkPID = sparkMotor.getClosedLoopController();
+ }
+ break;
+
+ default:
+ this.isTalonFX = MotorDetectUtil.detectIsTalonFX(canId);
+ if (isTalonFX) {
+ talonMotor = new NoOppTalonFX(name, canId);
+ } else {
+ // For replay mode with Spark, use a real Spark in no-op mode
+ sparkMotor = new SparkMax(canId, MotorType.kBrushless);
+ var config = new SparkMaxConfig();
+ sparkMotor.configure(
+ config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+ sparkPID = sparkMotor.getClosedLoopController();
+ }
+ break;
+ }
+
+ // Set up dashboard inputs for target values only
+ // Motor type detection is internal - not exposed to dashboard
+ var table = NetworkTableInstance.getDefault().getTable("TestMotors/" + name);
+ velocityInput = table.getDoubleTopic("TargetVelocity").subscribe(0.0);
+ positionInput = table.getDoubleTopic("TargetPosition").subscribe(0.0);
+
+ // Publish default target values
+ table.getDoubleTopic("TargetVelocity").publish().set(0.0);
+ table.getDoubleTopic("TargetPosition").publish().set(0.0);
+ }
+
+ @Override
+ public void periodic() {
+ // Read target values from dashboard
+ targetVelocity = velocityInput.get();
+ targetPosition = positionInput.get();
+
+ // Update motor periodic (for TalonFX)
+ if (isTalonFX && talonMotor != null) {
+ talonMotor.periodic();
+ }
+ }
+
+ /** Sets the motor output voltage (-12 to 12 volts). */
+ public void setVoltage(double voltage) {
+ controlMode = ControlMode.VOLTAGE;
+ currentOutput = voltage;
+
+ if (isTalonFX && talonMotor != null) {
+ talonMotor.setControl(talonVoltageControl.withOutput(voltage));
+ } else if (sparkMotor != null) {
+ sparkMotor.setVoltage(voltage);
+ }
+ }
+
+ /** Sets the motor velocity in RPM. */
+ public void setVelocity(double rpm) {
+ controlMode = ControlMode.VELOCITY;
+ targetVelocity = rpm;
+
+ if (isTalonFX && talonMotor != null) {
+ // Convert RPM to rotations per second for TalonFX
+ double rps = rpm / 60.0;
+ talonMotor.setControl(talonVelocityControl.withVelocity(rps));
+ } else if (sparkMotor != null && sparkPID != null) {
+ sparkPID.setReference(rpm, ControlType.kVelocity);
+ }
+ }
+
+ /**
+ * Sets the motor position in rotations using simple control without PID.
+ *
+ * As requested, this moves the motor slowly towards the target position at a constant low
+ * speed (max 3V) without using PID control. The motor speed is proportional to the error for
+ * smooth approach, and stops when within 0.05 rotations of the target.
+ *
+ * @param rotations Target position in rotations
+ */
+ public void setPosition(double rotations) {
+ controlMode = ControlMode.POSITION;
+ targetPosition = rotations;
+
+ // Simple position control - move slowly towards target without PID
+ double currentPosition = getCurrentPosition();
+
+ double error = targetPosition - currentPosition;
+
+ // Proportional voltage based on error, capped at max voltage
+ double voltage =
+ Math.signum(error) * Math.min(Math.abs(error) * POSITION_GAIN, POSITION_MAX_VOLTAGE);
+
+ // Stop if close enough
+ if (Math.abs(error) < POSITION_TOLERANCE) {
+ voltage = 0.0;
+ }
+
+ setVoltage(voltage);
+ }
+
+ /** Gets the current motor position in rotations. */
+ private double getCurrentPosition() {
+ if (isTalonFX && talonMotor != null) {
+ return talonMotor.getPosition().in(Rotations);
+ } else if (sparkMotor != null) {
+ return sparkMotor.getEncoder().getPosition();
+ }
+ return 0.0;
+ }
+
+ /** Stops the motor. */
+ public void stop() {
+ controlMode = ControlMode.STOPPED;
+ setVoltage(0.0);
+ }
+
+ /** Returns a command that sets the motor to a specific voltage. */
+ public Command setVoltageCommand(double voltage) {
+ return runOnce(() -> setVoltage(voltage));
+ }
+
+ /** Returns a command that enables velocity control from dashboard. */
+ public Command enableVelocityControlCommand() {
+ return runOnce(() -> controlMode = ControlMode.VELOCITY);
+ }
+
+ /** Returns a command that enables position control from dashboard. */
+ public Command enablePositionControlCommand() {
+ return runOnce(() -> controlMode = ControlMode.POSITION);
+ }
+
+ /** Returns a command that stops the motor. */
+ public Command stopCommand() {
+ return runOnce(this::stop);
+ }
+
+ public String getName() {
+ return name;
+ }
+
+ public int getCanId() {
+ return canId;
+ }
+
+ public boolean isTalonFX() {
+ return isTalonFX;
+ }
+}
diff --git a/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java b/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java
new file mode 100644
index 0000000..310060c
--- /dev/null
+++ b/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java
@@ -0,0 +1,185 @@
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.testbed.util;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+/**
+ * Utility class for detecting motor types on the CAN bus.
+ *
+ * This class interrogates the CAN bus to determine if a device at a given CAN ID is a TalonFX or
+ * Spark Max motor controller by attempting to communicate with each type and checking for
+ * successful responses.
+ */
+public class MotorDetectUtil {
+
+ /**
+ * Detects if the device at the given CAN ID is a TalonFX motor controller.
+ *
+ * This method attempts to create and communicate with both TalonFX and Spark Max controllers
+ * to determine which one responds successfully. The detection works by:
+ *
+ *
+ *
+ */
+ private void configureButtonBindings() {
+ // ========================================
+ // JOYSTICK MOTORS: Direct voltage control from joysticks with deadband
+ // ========================================
+
+ joystickMotor1.setDefaultCommand(
+ createJoystickMotorCommand(joystickMotor1, codriverController::getLeftY));
+ joystickMotor2.setDefaultCommand(
+ createJoystickMotorCommand(joystickMotor2, codriverController::getLeftX));
+ joystickMotor3.setDefaultCommand(
+ createJoystickMotorCommand(joystickMotor3, codriverController::getRightY));
+ joystickMotor4.setDefaultCommand(
+ createJoystickMotorCommand(joystickMotor4, codriverController::getRightX));
+
+ // ========================================
+ // PADDLE MOTORS: Synchronized trigger control
+ // ========================================
+
+ paddleMotor1.setDefaultCommand(createPaddleMotorCommand(paddleMotor1));
+ paddleMotor2.setDefaultCommand(createPaddleMotorCommand(paddleMotor2));
+
+ // ========================================
+ // BUTTON MOTORS: Dashboard-controlled velocity and position
+ // ========================================
+
+ // A button: Velocity control for ButtonMotor1 (CAN ID 20)
+ // Hold to run at velocity from "TestMotors/ButtonMotor1/TargetVelocity" on dashboard
+ codriverController
+ .a()
+ .whileTrue(
+ Commands.run(
+ () -> {
+ buttonMotor1.setVelocity(buttonMotor1.targetVelocity);
+ },
+ buttonMotor1))
+ .onFalse(buttonMotor1.stopCommand());
+
+ // B button: Velocity control for ButtonMotor2 (CAN ID 21)
+ // Hold to run at velocity from "TestMotors/ButtonMotor2/TargetVelocity" on dashboard
+ codriverController
+ .b()
+ .whileTrue(
+ Commands.run(
+ () -> {
+ buttonMotor2.setVelocity(buttonMotor2.targetVelocity);
+ },
+ buttonMotor2))
+ .onFalse(buttonMotor2.stopCommand());
+
+ // X button: Position control for ButtonMotor3 (CAN ID 22)
+ // Hold to move slowly to position from "TestMotors/ButtonMotor3/TargetPosition" on dashboard
+ // Uses simple control without PID - moves at constant slow speed until target reached
+ codriverController
+ .x()
+ .whileTrue(
+ Commands.run(
+ () -> {
+ buttonMotor3.setPosition(buttonMotor3.targetPosition);
+ },
+ buttonMotor3))
+ .onFalse(buttonMotor3.stopCommand());
+
+ // Y button: Position control for ButtonMotor4 (CAN ID 23)
+ // Hold to move slowly to position from "TestMotors/ButtonMotor4/TargetPosition" on dashboard
+ // Uses simple control without PID - moves at constant slow speed until target reached
+ codriverController
+ .y()
+ .whileTrue(
+ Commands.run(
+ () -> {
+ buttonMotor4.setPosition(buttonMotor4.targetPosition);
+ },
+ buttonMotor4))
+ .onFalse(buttonMotor4.stopCommand());
+
+ // ========================================
+ // EMERGENCY STOP CONTROLS
+ // ========================================
+
+ // Left bumper: Stop paddle motors only
+ codriverController
+ .leftBumper()
+ .onTrue(
+ Commands.runOnce(
+ () -> {
+ paddleMotor1.stop();
+ paddleMotor2.stop();
+ }));
+
+ // Right bumper: Stop joystick motors only
+ codriverController
+ .rightBumper()
+ .onTrue(
+ Commands.runOnce(
+ () -> {
+ joystickMotor1.stop();
+ joystickMotor2.stop();
+ joystickMotor3.stop();
+ joystickMotor4.stop();
+ }));
+
+ // Back button: EMERGENCY STOP - Stop ALL test motors immediately
+ codriverController
+ .back()
+ .onTrue(
+ Commands.runOnce(
+ () -> {
+ joystickMotor1.stop();
+ joystickMotor2.stop();
+ joystickMotor3.stop();
+ joystickMotor4.stop();
+ paddleMotor1.stop();
+ paddleMotor2.stop();
+ buttonMotor1.stop();
+ buttonMotor2.stop();
+ buttonMotor3.stop();
+ buttonMotor4.stop();
+ }));
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link frc.robot.generic.Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ @Override
+ public Command getAutonomousCommand() {
+ return autoChooser.get();
+ }
}
+;
diff --git a/src/main/java/frc/robot/testbed/subsystems/TestMotor.java b/src/main/java/frc/robot/testbed/subsystems/TestMotor.java
new file mode 100644
index 0000000..42cf1e8
--- /dev/null
+++ b/src/main/java/frc/robot/testbed/subsystems/TestMotor.java
@@ -0,0 +1,292 @@
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.testbed.subsystems;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.generic.util.LoggedTalon.LoggedTalonFX;
+import frc.robot.generic.util.LoggedTalon.NoOppTalonFX;
+import frc.robot.generic.util.LoggedTalon.PhoenixTalonFX;
+import frc.robot.generic.util.LoggedTalon.SimpleMotorSim;
+import frc.robot.testbed.util.MotorDetectUtil;
+
+/**
+ * Subsystem for controlling a single test motor (either Spark Max or TalonFX).
+ *
+ *
+ *
+ *
+ * The subsystem reads target velocity and position values from NetworkTables dashboard under
+ * "TestMotors/{name}/TargetVelocity" and "TestMotors/{name}/TargetPosition". Motor type detection
+ * is performed internally and not exposed to the dashboard.
+ *
+ *
+ *
+ *
+ * @param id The CAN ID to check
+ * @return true if the device is detected as a TalonFX, false if detected as a Spark
+ */
+ public static boolean detectIsTalonFX(int id) {
+ return detectIsTalonFX(id, "");
+ }
+
+ /**
+ * Detects if the device at the given CAN ID is a TalonFX motor controller.
+ *
+ * @param id The CAN ID to check
+ * @param canbus The name of the CAN bus (empty string for default "rio")
+ * @return true if the device is detected as a TalonFX, false if detected as a Spark
+ */
+ public static boolean detectIsTalonFX(int id, String canbus) {
+ // First, try to detect as TalonFX
+ if (canDetectTalonFX(id, canbus)) {
+ return true;
+ }
+
+ // If TalonFX detection failed, try Spark
+ if (canDetectSpark(id)) {
+ return false;
+ }
+
+ // If neither detected, default to TalonFX
+ // (This is safer as TalonFX has more robust error handling)
+ System.err.println(
+ "[MotorDetectUtil] WARNING: Could not detect motor type at CAN ID "
+ + id
+ + ". Defaulting to TalonFX.");
+ return true;
+ }
+
+ /**
+ * Attempts to detect a TalonFX at the given CAN ID.
+ *
+ * @param id The CAN ID to check
+ * @param canbus The name of the CAN bus
+ * @return true if a TalonFX is detected and responds
+ */
+ private static boolean canDetectTalonFX(int id, String canbus) {
+ try {
+ // Create a temporary TalonFX instance
+ TalonFX talon = new TalonFX(id, canbus);
+
+ // Try to get the firmware version - this will fail if device doesn't exist or isn't a TalonFX
+ // We use a small timeout to avoid blocking too long
+ var versionStatus = talon.getVersion();
+
+ // Check if we got a valid response
+ boolean detected =
+ versionStatus.getStatus() == StatusCode.OK && versionStatus.getValue() != null;
+
+ // Clean up - close the TalonFX to free resources
+ talon.close();
+
+ return detected;
+ } catch (Exception e) {
+ // Any exception means it's not a TalonFX
+ return false;
+ }
+ }
+
+ /**
+ * Attempts to detect a Spark Max at the given CAN ID.
+ *
+ * @param id The CAN ID to check
+ * @return true if a Spark is detected and responds
+ */
+ private static boolean canDetectSpark(int id) {
+ try {
+ // Create a temporary Spark instance
+ // Note: We have to guess brushless vs brushed. Brushless is more common.
+ SparkMax spark = new SparkMax(id, MotorType.kBrushless);
+
+ // Try to get firmware version - this will fail if device doesn't exist or isn't a Spark
+ String firmwareVersion = spark.getFirmwareString();
+
+ // Check if we got a valid response (non-empty firmware string)
+ boolean detected = firmwareVersion != null && !firmwareVersion.isEmpty();
+
+ // Clean up - close the Spark to free resources
+ spark.close();
+
+ return detected;
+ } catch (Exception e) {
+ // Any exception means it's not a Spark
+ return false;
+ }
+ }
+
+ /**
+ * Returns a human-readable name for the motor controller type at the given CAN ID.
+ *
+ * @param id The CAN ID to check
+ * @return "TalonFX" or "Spark" based on detection
+ */
+ public static String getMotorTypeName(int id) {
+ return detectIsTalonFX(id) ? "TalonFX" : "Spark";
+ }
+
+ /**
+ * Returns a human-readable name for the motor controller type at the given CAN ID.
+ *
+ * @param id The CAN ID to check
+ * @param canbus The name of the CAN bus
+ * @return "TalonFX" or "Spark" based on detection
+ */
+ public static String getMotorTypeName(int id, String canbus) {
+ return detectIsTalonFX(id, canbus) ? "TalonFX" : "Spark";
+ }
+
+ /**
+ * Scans a range of CAN IDs and returns information about detected devices.
+ *
+ * @param startId The first CAN ID to scan (inclusive)
+ * @param endId The last CAN ID to scan (inclusive)
+ * @return A string containing information about all detected devices
+ */
+ public static String scanCANBus(int startId, int endId) {
+ StringBuilder result = new StringBuilder();
+ result.append("CAN Bus Scan Results:\n");
+ result.append("===================\n");
+
+ for (int id = startId; id <= endId; id++) {
+ String deviceType = "Not detected";
+
+ if (canDetectTalonFX(id, "")) {
+ deviceType = "TalonFX";
+ } else if (canDetectSpark(id)) {
+ deviceType = "Spark Max";
+ }
+
+ if (!deviceType.equals("Not detected")) {
+ result.append(String.format("CAN ID %2d: %s\n", id, deviceType));
+ }
+ }
+
+ return result.toString();
+ }
+}