From 1ca28810f1ee37ab57703d42b27e2d7d94e194cb Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Thu, 7 Apr 2022 16:35:07 -0600 Subject: [PATCH] Implement test mode Add quick command index to command schedule when relevant Adjust command schedule dimensions --- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 188 ++++++++++-------- .../shuffleboard/CommandSchedule.java | 12 +- .../shuffleboard/SendableCANSparkMax.java | 23 +++ 4 files changed, 132 insertions(+), 93 deletions(-) create mode 100644 src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 28464f0..2b47ce6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -98,7 +98,7 @@ public class RobotContainer { private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive); private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom); - private final CommandSchedule m_commandSchedule = new CommandSchedule(13, 6, false); + private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false); // Controllers private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 32c387a..6bf9271 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -5,35 +5,26 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.wpilibj.CAN; -import edu.wpi.first.networktables.NetworkTableType; + +import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; -import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SerializerConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; +import frc4388.utility.shuffleboard.SendableCANSparkMax; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -53,33 +44,42 @@ public class RobotMap { } /* LED Subsystem */ -// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); -// void configureLEDMotorControllers() {} + // void configureLEDMotorControllers() {} -/* Swerve Subsystem */ -public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); -public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); -public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); -public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); -public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); -public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); -public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); -public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); -public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); -public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); -public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); -public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + /* Swerve Subsystem */ + public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); + public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); -public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); + public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); -public SwerveModule leftFront; -public SwerveModule leftBack; -public SwerveModule rightFront; -public SwerveModule rightBack; + public SwerveModule leftFront; + public SwerveModule leftBack; + public SwerveModule rightFront; + public SwerveModule rightBack; -void configureSwerveMotorControllers() { + void configureSwerveMotorControllers() { + + SendableRegistry.setName(leftFrontSteerMotor, "SwerveDrive", "Left Front Steer Motor"); + SendableRegistry.setName(leftFrontWheelMotor, "SwerveDrive", "Left Front Wheel Motor"); + SendableRegistry.setName(rightFrontSteerMotor, "SwerveDrive", "Right Front Steer Motor"); + SendableRegistry.setName(rightFrontWheelMotor, "SwerveDrive", "Right Front Wheel Motor"); + SendableRegistry.setName(leftBackSteerMotor, "SwerveDrive", "Left Back Steer Motor"); + SendableRegistry.setName(leftBackWheelMotor, "SwerveDrive", "Left Back Wheel Motor"); + SendableRegistry.setName(rightBackSteerMotor, "SwerveDrive", "Right Back Steer Motor"); + SendableRegistry.setName(rightBackWheelMotor, "SwerveDrive", "Right Back Wheel Motor"); leftFrontSteerMotor.configFactoryDefault(); leftFrontWheelMotor.configFactoryDefault(); @@ -91,55 +91,55 @@ void configureSwerveMotorControllers() { rightBackWheelMotor.configFactoryDefault(); leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); NeutralMode mode = NeutralMode.Coast; leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); @@ -201,6 +201,9 @@ void configureSwerveMotorControllers() { public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO private void configureClimberMotors() { + SendableRegistry.setName(elbow, "Climber", "Elbow"); + SendableRegistry.setName(leftClaw, "Climber", "Left Claw"); + SendableRegistry.setName(rightClaw, "Climber", "Right Claw"); elbow.configFactoryDefault(); elbow.setNeutralMode(NeutralMode.Brake); } @@ -223,6 +226,7 @@ public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.S void configureShooterMotorControllers() { // LEFT FALCON + SendableRegistry.setName(shooterFalconLeft, "BoomBoom", "Left Motor"); shooterFalconLeft.configFactoryDefault(); shooterFalconLeft.setNeutralMode(NeutralMode.Coast); shooterFalconLeft.setInverted(true); @@ -239,6 +243,7 @@ void configureShooterMotorControllers() { ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON + SendableRegistry.setName(shooterFalconRight, "BoomBoom", "Right Motor"); shooterFalconRight.configFactoryDefault(); shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.setInverted(false); @@ -266,41 +271,48 @@ void configureShooterMotorControllers() { angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); -} + } -/* Serializer Subsystem */ -public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); -public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); + /* Serializer Subsystem */ + public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); + public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); -/* Intake Subsystem */ -public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); -public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + /* Intake Subsystem */ + public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); + public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + + void configureIntakeMotors() { + SendableRegistry.setName(intakeMotor, "Intake", "Intake Motor"); -void configureIntakeMotors() { intakeMotor.configFactoryDefault(); intakeMotor.setInverted(false); intakeMotor.setNeutralMode(NeutralMode.Coast); intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); -} + } + + void configureExtenderMotors() { + SendableRegistry.add(new SendableCANSparkMax(extenderMotor), "Intake", "Extender Motor"); -void configureExtenderMotors() { extenderMotor.restoreFactoryDefaults(); extenderMotor.setInverted(true); extenderMotor.setIdleMode(IdleMode.kBrake); } void configureSerializerMotors() { - serializerBelt.restoreFactoryDefaults(); - } + SendableRegistry.add(new SendableCANSparkMax(serializerBelt), "Intake", "Serializer Belt"); + SendableRegistry.setName(serializerBeam, "Intake", "Serializer Beam"); - /* Storage Subsystem */ - public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - - void configureStorageMotors() { - storageMotor.restoreFactoryDefaults(); - storageMotor.setIdleMode(IdleMode.kCoast); - } - + serializerBelt.restoreFactoryDefaults(); + } + + /* Storage Subsystem */ + public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + + void configureStorageMotors() { + SendableRegistry.add(new SendableCANSparkMax(storageMotor), "Intake", "Storage Motor"); + storageMotor.restoreFactoryDefaults(); + storageMotor.setIdleMode(IdleMode.kCoast); + } } diff --git a/src/main/java/frc4388/robot/commands/shuffleboard/CommandSchedule.java b/src/main/java/frc4388/robot/commands/shuffleboard/CommandSchedule.java index 88b9a57..c066741 100644 --- a/src/main/java/frc4388/robot/commands/shuffleboard/CommandSchedule.java +++ b/src/main/java/frc4388/robot/commands/shuffleboard/CommandSchedule.java @@ -87,7 +87,7 @@ public final class CommandSchedule extends CommandBase { int size = scheduledCommands.size(); root.withProperties(Map.of("Number of columns", size, "Number of rows", 1, "Label position", "TOP")); for (Command command : scheduledCommands.keySet()) { - putCommand(command, root, size, command::isScheduled); + putCommand(command, root, size, -1, command::isScheduled); } } @@ -119,18 +119,20 @@ public final class CommandSchedule extends CommandBase { return true; } - private void putCommand(Command command, ShuffleboardContainer layout, int siblings, BooleanSupplier running) { + private void putCommand(Command command, ShuffleboardContainer layout, int siblings, int index, BooleanSupplier running) { boolean isRoot = root == layout; String n1 = command.getName(); String n2 = command.getClass().getSimpleName(); - String name = (n1.equals(n2) ? n1 : n1 + " " + n2) + "@" + Integer.toHexString(command.hashCode()); + String name = (index < 0 ? "" : index + " ") + (n1.equals(n2) ? n1 : n1 + " " + n2) + "@" + Integer.toHexString(command.hashCode()); if (command instanceof CommandGroupBase) { Collection commands = List.of(); Function nestedRunningMaker = c -> () -> !c.isFinished(); + boolean ordered = false; if (command instanceof SequentialCommandGroup) { ArrayList commandsList = Errors.log().getWithDefault(() -> (ArrayList) sequentialCommandGroupCommandsMethod.invoke(command), new ArrayList<>()); commands = commandsList; nestedRunningMaker = c -> () -> Errors.log().getWithDefault(() -> (int) sequentialCommandGroupCurrentCommandIndexMethod.invoke(command) == commandsList.indexOf(c), false); + ordered = true; } else if (command instanceof ParallelCommandGroup) { HashMap commandsMap = Errors.log().getWithDefault(() -> (HashMap) parallelCommandGroupCommandsMethod.invoke(command), new HashMap()); commands = commandsMap.keySet(); @@ -150,8 +152,10 @@ public final class CommandSchedule extends CommandBase { nestedLayout = layout.getLayout(name, BuiltInLayouts.kGrid).withProperties(Map.of("Number of columns", size, "Number of rows", 1)); if (showGroupStatus && nestedLayout.getComponents().stream().map(ShuffleboardComponent::getTitle).noneMatch("_self_"::equals)) nestedLayout.addBoolean("_self_", running); + int i = 0; for (Command nestedCommand : commands) { - putCommand(nestedCommand, nestedLayout, size, nestedRunningMaker.apply(nestedCommand)); + putCommand(nestedCommand, nestedLayout, size, ordered ? i : -1, nestedRunningMaker.apply(nestedCommand)); + i++; } } else if (command instanceof CommandBase) { ShuffleboardContainer target = isRoot ? Objects.requireNonNullElseGet(ungroupedLayout, () -> ungroupedLayout = root.getLayout("Ungrouped", BuiltInLayouts.kList)) : layout; diff --git a/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java b/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java new file mode 100644 index 0000000..f3983d2 --- /dev/null +++ b/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java @@ -0,0 +1,23 @@ +package frc4388.utility.shuffleboard; + +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public class SendableCANSparkMax implements Sendable { + private final CANSparkMax m_canSparkMax; + + public SendableCANSparkMax(CANSparkMax canSparkMax) { + m_canSparkMax = canSparkMax; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Motor Controller"); + builder.setActuator(true); + builder.setSafeState(m_canSparkMax::stopMotor); + builder.addDoubleProperty("Value", m_canSparkMax::get, m_canSparkMax::set); + } + +}