From 1499621351257fd7227e0a27c843eb18d391f523 Mon Sep 17 00:00:00 2001 From: Ruthie Date: Wed, 19 Nov 2025 13:52:21 +0000 Subject: [PATCH 1/6] wheeee? --- .../subsystems/drive/DriveConstants.java | 8 +- .../subsystems/drive/ModuleIOSpark.java | 2 +- .../frc/robot/testbed/RobotContainer.java | 298 +++++++++++++++++- .../robot/testbed/subsystems/TestMotor.java | 288 +++++++++++++++++ .../robot/testbed/util/MotorDetectUtil.java | 187 +++++++++++ 5 files changed, 775 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/testbed/subsystems/TestMotor.java create mode 100644 src/main/java/frc/robot/testbed/util/MotorDetectUtil.java diff --git a/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java index 9e94410..50ac211 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java @@ -52,12 +52,12 @@ public class DriveConstants { Stand (or imagine standing) beside the robot, facing the same direction as its absolute front. - For front wheels: - Overrotated → pointing toward the robot’s right (your left) - Underrotated → pointing toward the robot’s left (your right) + Overrotated -> pointing toward the robot’s right (your left) + Underrotated -> pointing toward the robot’s left (your right) - For back wheels: - Overrotated → pointing toward the robot’s right (your left) - Underrotated → pointing toward the robot’s left (your right) + Overrotated -> pointing toward the robot’s right (your left) + Underrotated -> pointing toward the robot’s left (your right) */ public static final Rotation2d frontLeftZeroRotation = new Rotation2d(-0.7853181997882291).minus(new Rotation2d(Degrees.of(16 + 5))); diff --git a/src/main/java/frc/robot/generic/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/generic/subsystems/drive/ModuleIOSpark.java index 1a7b320..b75e367 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/ModuleIOSpark.java @@ -73,7 +73,7 @@ public class ModuleIOSpark implements ModuleIO { private final Debouncer driveConnectedDebounce = new Debouncer(0.5); private final Debouncer turnConnectedDebounce = new Debouncer(0.5); - // Cached cancoder status — updated only in resetToAbsolute() + // Cached cancoder status - updated only in resetToAbsolute() private boolean lastCancoderConnected = false; private final int module; diff --git a/src/main/java/frc/robot/testbed/RobotContainer.java b/src/main/java/frc/robot/testbed/RobotContainer.java index ab10c88..9a62233 100644 --- a/src/main/java/frc/robot/testbed/RobotContainer.java +++ b/src/main/java/frc/robot/testbed/RobotContainer.java @@ -1,9 +1,301 @@ +// 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; +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.generic.subsystems.drive.Drive; import frc.robot.generic.util.AbstractRobotContainer; import frc.robot.generic.util.RobotConfig; +import frc.robot.generic.util.SwerveBuilder; +import frc.robot.testbed.subsystems.TestMotor; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +/** + * Testbed robot container for remotely testing motors and prototypes on a swerve drivetrain + * without any programming work. + * + * This testbed provides: + * + * + * Codriver Controller Layout (Port 1): + * + * + * Dashboard Control: Button-controlled motors read target velocity (RPM) or position + * (rotations) from NetworkTables at "TestMotors/{MotorName}/TargetVelocity" and + * "TestMotors/{MotorName}/TargetPosition". Position control moves slowly without PID until the + * encoder reaches the target. + * + * 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 autoChooser; + + /** The container for the testbed robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + // Set up auto routines (using generic implementation) + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + + // Configure button bindings + configureButtonBindings(); + } + + /** + * Creates a joystick motor control command with deadband. + * + * @param motor The motor to control + * @param axisSupplier Supplier for the joystick axis value + * @return Command that controls the motor based on axis input + */ + private Command createJoystickMotorCommand(TestMotor motor, DoubleSupplier axisSupplier) { + return Commands.run( + () -> { + double value = -axisSupplier.getAsDouble(); + if (Math.abs(value) > JOYSTICK_DEADBAND) { + motor.setVoltage(value * MAX_VOLTAGE); + } else { + motor.stop(); + } + }, + motor); + } + + /** + * Creates a paddle motor control command for trigger-based control. + * + * @param motor The motor to control + * @return Command that controls the motor based on trigger inputs + */ + private Command createPaddleMotorCommand(TestMotor motor) { + return Commands.run( + () -> { + double leftTrigger = codriverController.getLeftTriggerAxis(); + double rightTrigger = codriverController.getRightTriggerAxis(); + if (leftTrigger > JOYSTICK_DEADBAND) { + motor.setVoltage(leftTrigger * MAX_VOLTAGE); + } else if (rightTrigger > JOYSTICK_DEADBAND) { + motor.setVoltage(-rightTrigger * MAX_VOLTAGE); + } else { + motor.stop(); + } + }, + motor); + } + + /** + * Configures button bindings for testbed motor control. + * + * 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: + * + */ + 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(); + } +} \ No newline at end of file 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..b072ee9 --- /dev/null +++ b/src/main/java/frc/robot/testbed/subsystems/TestMotor.java @@ -0,0 +1,288 @@ +// 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). + * + * 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: + * + * + * 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. + * + * 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..2a9ce67 --- /dev/null +++ b/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java @@ -0,0 +1,187 @@ +// 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.CANBus; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.hal.can.CANStatus; +import edu.wpi.first.wpilibj.CAN; + +/** + * 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: + *
    + *
  1. Attempting to read firmware version from a TalonFX at the CAN ID + *
  2. If successful, returns true (device is TalonFX) + *
  3. If failed, attempts to communicate with a Spark Max at the CAN ID + *
  4. If Spark responds, returns false (device is Spark) + *
  5. If neither responds, defaults to TalonFX (false positive is safer) + *
+ * + * @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(); + } +} From 46eb370e5f6389cab44fb3cd90f1ea84298236bb Mon Sep 17 00:00:00 2001 From: Ruthie Date: Wed, 19 Nov 2025 14:02:30 +0000 Subject: [PATCH 2/6] ' --- .../robot/generic/subsystems/drive/DriveConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java index 50ac211..69b71a4 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java @@ -48,16 +48,16 @@ public class DriveConstants { That means the wheel needs to rotate toward the robot’s absolute right (clockwise when viewed from above). - To decide whether a wheel is over- or underrotated: + To decide whether a wheel is over - or underrotated: Stand (or imagine standing) beside the robot, facing the same direction as its absolute front. - For front wheels: - Overrotated -> pointing toward the robot’s right (your left) - Underrotated -> pointing toward the robot’s left (your right) + Overrotated -> pointing toward the robot's right (your left) + Underrotated -> pointing toward the robot's left (your right) - For back wheels: - Overrotated -> pointing toward the robot’s right (your left) - Underrotated -> pointing toward the robot’s left (your right) + Overrotated -> pointing toward the robot's right (your left) + Underrotated -> pointing toward the robot's left (your right) */ public static final Rotation2d frontLeftZeroRotation = new Rotation2d(-0.7853181997882291).minus(new Rotation2d(Degrees.of(16 + 5))); From 7ddf65c5064b8669a4feba96312a69631b277670 Mon Sep 17 00:00:00 2001 From: Ruthie Date: Wed, 19 Nov 2025 14:04:44 +0000 Subject: [PATCH 3/6] compilied! --- .../frc/robot/outReach/RobotContainer.java | 3 +- .../outReach/subsystems/shooter/Shooter.java | 45 ++++++----- .../frc/robot/testbed/RobotContainer.java | 74 ++++++++++--------- .../robot/testbed/subsystems/TestMotor.java | 18 +++-- .../robot/testbed/util/MotorDetectUtil.java | 8 +- 5 files changed, 77 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc/robot/outReach/RobotContainer.java b/src/main/java/frc/robot/outReach/RobotContainer.java index 1cb5f33..48bbfde 100644 --- a/src/main/java/frc/robot/outReach/RobotContainer.java +++ b/src/main/java/frc/robot/outReach/RobotContainer.java @@ -31,10 +31,9 @@ import frc.robot.generic.util.LoggedTalon.NoOppTalonFX; import frc.robot.generic.util.LoggedTalon.PhoenixTalonFX; import frc.robot.generic.util.LoggedTalon.SimpleMotorSim; -import frc.robot.outReach.subsystems.turret.Turret; import frc.robot.generic.util.RobotConfig; import frc.robot.generic.util.SwerveBuilder; - +import frc.robot.outReach.subsystems.turret.Turret; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** diff --git a/src/main/java/frc/robot/outReach/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/outReach/subsystems/shooter/Shooter.java index 0e97dd4..e98a6d8 100644 --- a/src/main/java/frc/robot/outReach/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/outReach/subsystems/shooter/Shooter.java @@ -3,9 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.outReach.subsystems.shooter; -import frc.robot.generic.util.LoggedTalon.LoggedTalonFX; - -import java.util.function.DoubleSupplier; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -13,13 +10,12 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.generic.util.LoggedTalon.LoggedTalonFX; +import java.util.function.DoubleSupplier; public class Shooter extends SubsystemBase { private final LoggedTalonFX shooterMotor; @@ -28,29 +24,30 @@ public class Shooter extends SubsystemBase { /** Creates a new Torrent. */ public Shooter(LoggedTalonFX shooterMotor) { - var config = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs().withStatorCurrentLimit(30).withSupplyCurrentLimit(60)) - .withSlot0(new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0)) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(0)) - .withMotorOutput(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)); + var config = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs().withStatorCurrentLimit(30).withSupplyCurrentLimit(60)) + .withSlot0(new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0)) + .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(0)) + .withMotorOutput(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)); + + this.shooterMotor = shooterMotor.withConfig(config); + } - this.shooterMotor = shooterMotor.withConfig(config); + public Command spinToVelocityAndRotationsCommand(DoubleSupplier velocityRPS) { + return runEnd( + () -> { + shooterMotor.setControl(velocityPIDRequest.withVelocity(velocityRPS.getAsDouble())); + }, + () -> { + shooterMotor.setControl(neutralOut); + }); } -public Command spinToVelocityAndRotationsCommand(DoubleSupplier velocityRPS) { - return runEnd(()->{ - shooterMotor.setControl(velocityPIDRequest.withVelocity(velocityRPS.getAsDouble())); - }, ()-> { - shooterMotor.setControl(neutralOut); - }); -} - -@Override + @Override public void periodic() { // This method will be called once per scheduler run shooterMotor.periodic(); } } - \ No newline at end of file diff --git a/src/main/java/frc/robot/testbed/RobotContainer.java b/src/main/java/frc/robot/testbed/RobotContainer.java index 9a62233..f624ace 100644 --- a/src/main/java/frc/robot/testbed/RobotContainer.java +++ b/src/main/java/frc/robot/testbed/RobotContainer.java @@ -26,10 +26,11 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** - * Testbed robot container for remotely testing motors and prototypes on a swerve drivetrain - * without any programming work. + * Testbed robot container for remotely testing motors and prototypes on a swerve drivetrain without + * any programming work. + * + *

This testbed provides: * - * This testbed provides: *

    *
  • Generic swerve drive controlled by driver controller (port 0) *
  • 4 motors bound to codriver joysticks for direct voltage control (CAN IDs 10-13) @@ -42,6 +43,7 @@ *
* * Codriver Controller Layout (Port 1): + * *
    *
  • Left Joystick Y -> JoystickMotor1 (CAN 10) *
  • Left Joystick X -> JoystickMotor2 (CAN 11) @@ -63,7 +65,7 @@ * "TestMotors/{MotorName}/TargetPosition". Position control moves slowly without PID until the * encoder reaches the target. * - * Motor Detection: Each motor's type (TalonFX or Spark Max) is automatically detected at + *

    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. @@ -156,18 +158,19 @@ private Command createPaddleMotorCommand(TestMotor motor) { /** * Configures button bindings for testbed motor control. * - * Joystick Control: The four joystick motors receive continuous voltage control (-12V + *

    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 + *

    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 + *

    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: + *

    Emergency Stops: + * *

      *
    • Left Bumper: Stop paddle motors only *
    • Right Bumper: Stop joystick motors only @@ -256,41 +259,46 @@ private void configureButtonBindings() { // Left bumper: Stop paddle motors only codriverController .leftBumper() - .onTrue(Commands.runOnce(() -> { - paddleMotor1.stop(); - paddleMotor2.stop(); - })); + .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(); - })); + .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(); - })); + .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. + * Use this to pass the autonomous command to the main {@link frc.robot.generic.Robot} class. * * @return the command to run in autonomous */ @@ -298,4 +306,4 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { return autoChooser.get(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/testbed/subsystems/TestMotor.java b/src/main/java/frc/robot/testbed/subsystems/TestMotor.java index b072ee9..42cf1e8 100644 --- a/src/main/java/frc/robot/testbed/subsystems/TestMotor.java +++ b/src/main/java/frc/robot/testbed/subsystems/TestMotor.java @@ -39,8 +39,9 @@ /** * Subsystem for controlling a single test motor (either Spark Max or TalonFX). * - * This subsystem automatically detects the motor controller type at the given CAN ID by + *

      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: + * *

        *
      • Voltage Control: Direct voltage output (-12V to +12V) *
      • Velocity Control: Closed-loop velocity control (RPM) @@ -52,8 +53,8 @@ * "TestMotors/{name}/TargetVelocity" and "TestMotors/{name}/TargetPosition". Motor type detection * is performed internally and not exposed to the dashboard. * - * 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 + *

        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. */ @@ -119,7 +120,8 @@ public TestMotor(int canId, String name) { } else { sparkMotor = new SparkMax(canId, MotorType.kBrushless); var config = new SparkMaxConfig(); - sparkMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + sparkMotor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); sparkPID = sparkMotor.getClosedLoopController(); } break; @@ -140,7 +142,8 @@ public TestMotor(int canId, String name) { // 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); + sparkMotor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); sparkPID = sparkMotor.getClosedLoopController(); } break; @@ -153,7 +156,8 @@ public TestMotor(int canId, String name) { // 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); + sparkMotor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); sparkPID = sparkMotor.getClosedLoopController(); } break; @@ -211,7 +215,7 @@ public void setVelocity(double rpm) { /** * 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 + *

        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. * diff --git a/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java b/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java index 2a9ce67..310060c 100644 --- a/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java +++ b/src/main/java/frc/robot/testbed/util/MotorDetectUtil.java @@ -13,18 +13,15 @@ package frc.robot.testbed.util; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import edu.wpi.first.hal.can.CANStatus; -import edu.wpi.first.wpilibj.CAN; /** * 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 + *

        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. */ @@ -33,8 +30,9 @@ 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 + *

        This method attempts to create and communicate with both TalonFX and Spark Max controllers * to determine which one responds successfully. The detection works by: + * *

          *
        1. Attempting to read firmware version from a TalonFX at the CAN ID *
        2. If successful, returns true (device is TalonFX) From 16dbf1e4416895b30113cc6a769372938976d69c Mon Sep 17 00:00:00 2001 From: Ruthie Date: Wed, 19 Nov 2025 14:07:55 +0000 Subject: [PATCH 4/6] ok... --- src/main/java/frc/robot/testbed/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/testbed/RobotContainer.java b/src/main/java/frc/robot/testbed/RobotContainer.java index f624ace..37a5842 100644 --- a/src/main/java/frc/robot/testbed/RobotContainer.java +++ b/src/main/java/frc/robot/testbed/RobotContainer.java @@ -307,3 +307,4 @@ public Command getAutonomousCommand() { return autoChooser.get(); } } +; From eb322e65f8886ac6bb538a1b261df5c7c467ff9d Mon Sep 17 00:00:00 2001 From: Ruthie Date: Wed, 19 Nov 2025 14:08:10 +0000 Subject: [PATCH 5/6] uhh --- src/main/java/frc/robot/testbed/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/testbed/RobotContainer.java b/src/main/java/frc/robot/testbed/RobotContainer.java index 37a5842..7e78944 100644 --- a/src/main/java/frc/robot/testbed/RobotContainer.java +++ b/src/main/java/frc/robot/testbed/RobotContainer.java @@ -307,4 +307,4 @@ public Command getAutonomousCommand() { return autoChooser.get(); } } -; +; \ No newline at end of file From a3b822fee6f9e179c863da3eb08cb703043b19c1 Mon Sep 17 00:00:00 2001 From: Ruthie Date: Wed, 19 Nov 2025 14:08:43 +0000 Subject: [PATCH 6/6] space --- src/main/java/frc/robot/testbed/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/testbed/RobotContainer.java b/src/main/java/frc/robot/testbed/RobotContainer.java index 7e78944..37a5842 100644 --- a/src/main/java/frc/robot/testbed/RobotContainer.java +++ b/src/main/java/frc/robot/testbed/RobotContainer.java @@ -307,4 +307,4 @@ public Command getAutonomousCommand() { return autoChooser.get(); } } -; \ No newline at end of file +;