Skip to content

Commit 00029e4

Browse files
committed
refactor: Deleted MotorConfig class and MotorIO structure; add tuningMode constant in Constants
1 parent 7d4c727 commit 00029e4

13 files changed

Lines changed: 231 additions & 169 deletions

src/main/java/frc/robot/Constants.java

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ public final class Constants {
2626
public static final Mode simMode = Mode.SIM;
2727
public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
2828
public static final int PDH_ID = 1;
29+
public static final boolean tuningMode = true;
2930

3031
public static class LedConstants {
3132
public static final int kLedPort = 9;

src/main/java/frc/robot/ControllerExamples.java

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,7 @@
66
import frc.robot.util.controller.AdvancedController;
77
import frc.robot.util.controller.AdvancedController.Button;
88

9-
/**
10-
* Example class demonstrating senior-level usage of the AdvancedController.
11-
*/
9+
/** Example class demonstrating senior-level usage of the AdvancedController. */
1210
public class ControllerExamples {
1311

1412
// 1. Initialize controllers
@@ -26,15 +24,14 @@ public void configureBindings() {
2624

2725
// Double press (Fires immediately on second click, cancels single)
2826
// Also triggers a double-pulse rumble for tactile confirmation.
29-
xBinding.doublePress().onTrue(
30-
new InstantCommand(() -> System.out.println("High Gear Set"))
27+
xBinding.doublePress()
28+
.onTrue(new InstantCommand(() -> System.out.println("High Gear Set"))
3129
.alongWith(driver.rumblePulse(1.0, 0.1, 2)));
3230

3331
// --- SCENARIO 2: Chord (Simultaneous Press) ---
3432
// Goal: Emergency reset requires two hands to prevent accidental activation.
3533
driver.chord(Button.kLB, Button.kRB, 0.1)
36-
.onTrue(new PrintCommand("!! EMERGENCY RESET !!")
37-
.alongWith(driver.rumbleOnTrue(1.0, 0.5)));
34+
.onTrue(new PrintCommand("!! EMERGENCY RESET !!").alongWith(driver.rumbleOnTrue(1.0, 0.5)));
3835

3936
// --- SCENARIO 3: Modifier (Shift) Logic ---
4037
// Goal: Multiply the number of available functions using a "Shift" button.

src/main/java/frc/robot/RobotContainer.java

Lines changed: 13 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -30,11 +30,6 @@
3030
import frc.robot.subsystems.drive.ModuleIO;
3131
import frc.robot.subsystems.drive.ModuleIOSim;
3232
import frc.robot.subsystems.drive.ModuleIOSpark;
33-
import frc.robot.subsystems.vision.Vision;
34-
import frc.robot.subsystems.vision.VisionConstants;
35-
import frc.robot.subsystems.vision.VisionIO;
36-
import frc.robot.subsystems.vision.VisionIOPhotonVision;
37-
import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
3833
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
3934

4035
/**
@@ -44,7 +39,7 @@
4439
*/
4540
public class RobotContainer {
4641

47-
private final Vision vision;
42+
// private final Vision vision;
4843

4944
// Subsystems
5045
private final Drive drive;
@@ -67,29 +62,29 @@ public RobotContainer() {
6762
new ModuleIOSpark(1),
6863
new ModuleIOSpark(2),
6964
new ModuleIOSpark(3));
70-
vision = new Vision(
71-
drive::addVisionMeasurement,
72-
new VisionIOPhotonVision(VisionConstants.camera0Name, VisionConstants.robotToCamera0),
73-
new VisionIOPhotonVision(VisionConstants.camera1Name, VisionConstants.robotToCamera1));
65+
// vision = new Vision(
66+
// drive::addVisionMeasurement,
67+
// new VisionIOPhotonVision(VisionConstants.camera0Name, VisionConstants.robotToCamera0),
68+
// new VisionIOPhotonVision(VisionConstants.camera1Name, VisionConstants.robotToCamera1));
7469
break;
7570

7671
case SIM:
7772
// Sim robot, instantiate physics sim IO implementations
7873
drive = new Drive(
7974
new GyroIO() {}, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim());
80-
vision = new Vision(
81-
drive::addVisionMeasurement,
82-
new VisionIOPhotonVisionSim(
83-
VisionConstants.camera0Name, VisionConstants.robotToCamera0, drive::getPose),
84-
new VisionIOPhotonVisionSim(
85-
VisionConstants.camera1Name, VisionConstants.robotToCamera1, drive::getPose));
75+
// vision = new Vision(
76+
// drive::addVisionMeasurement,
77+
// new VisionIOPhotonVisionSim(
78+
// VisionConstants.camera0Name, VisionConstants.robotToCamera0, drive::getPose),
79+
// new VisionIOPhotonVisionSim(
80+
// VisionConstants.camera1Name, VisionConstants.robotToCamera1, drive::getPose));
8681
break;
8782

8883
default:
8984
// Replayed robot, disable IO implementations
9085
drive = new Drive(
9186
new GyroIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {});
92-
vision = new Vision(drive::addVisionMeasurement, new VisionIO() {});
87+
// vision = new Vision(drive::addVisionMeasurement, new VisionIO() {});
9388
break;
9489
}
9590

@@ -118,7 +113,7 @@ public RobotContainer() {
118113
private void configureButtonBindings() {
119114
// Default command, normal field-relative drive
120115
drive.setDefaultCommand(DriveCommands.joystickDrive(
121-
drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX()));
116+
drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRawAxis(2)));
122117

123118
// Lock to 0° when A button is held
124119
controller

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -34,23 +34,23 @@ public class DriveConstants {
3434
};
3535

3636
// Zeroed rotation values for each module, see setup instructions
37-
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(Units.degreesToRadians(106));
38-
public static final Rotation2d frontRightZeroRotation = new Rotation2d(Units.degreesToRadians(28.8));
39-
public static final Rotation2d backLeftZeroRotation = new Rotation2d(Units.degreesToRadians(29.6));
40-
public static final Rotation2d backRightZeroRotation = new Rotation2d(Units.degreesToRadians(75.6));
37+
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(-1.77);
38+
public static final Rotation2d frontRightZeroRotation = new Rotation2d(-2.7);
39+
public static final Rotation2d backLeftZeroRotation = new Rotation2d(-2.49);
40+
public static final Rotation2d backRightZeroRotation = new Rotation2d(-1.14);
4141

4242
// Device CAN IDs
4343
public static final int pigeonCanId = 31;
4444

45-
public static final int frontLeftDriveCanId = 11;
46-
public static final int backLeftDriveCanId = 13;
47-
public static final int frontRightDriveCanId = 12;
48-
public static final int backRightDriveCanId = 14;
45+
public static final int frontLeftDriveCanId = 59;
46+
public static final int backLeftDriveCanId = 14;
47+
public static final int frontRightDriveCanId = 13;
48+
public static final int backRightDriveCanId = 11;
4949

50-
public static final int frontLeftTurnCanId = 21;
51-
public static final int backLeftTurnCanId = 23;
52-
public static final int frontRightTurnCanId = 22;
53-
public static final int backRightTurnCanId = 24;
50+
public static final int frontLeftTurnCanId = 24;
51+
public static final int backLeftTurnCanId = 22;
52+
public static final int frontRightTurnCanId = 3;
53+
public static final int backRightTurnCanId = 23;
5454

5555
public static final int frontLeftTurnAbsId = 0;
5656
public static final int backLeftTurnAbsId = 2;

src/main/java/frc/robot/subsystems/exampleSubsystem.java

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77

88
public class exampleSubsystem extends SubsystemBase {
99

10+
1011
public exampleSubsystem(Led led) {
1112
led.setAnimation(
1213
new LedPattern.ColorFlow(Color.kAliceBlue, LedPattern.ColorFlow.Direction.FORWARD, 1.0), 0, 11);
Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
// Copyright (c) 2025-2026 Littleton Robotics
2+
// http://github.com/Mechanical-Advantage
3+
//
4+
// Use of this source code is governed by an MIT-style
5+
// license that can be found in the LICENSE file at
6+
// the root directory of this project.
7+
8+
package frc.robot.util;
9+
10+
import java.util.Arrays;
11+
import java.util.HashMap;
12+
import java.util.Map;
13+
import java.util.function.Consumer;
14+
import java.util.function.DoubleSupplier;
15+
import frc.robot.Constants;
16+
import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
17+
18+
/**
19+
* Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
20+
* value not in dashboard.
21+
*/
22+
@SuppressWarnings("unused")
23+
public class LoggedTunableNumber implements DoubleSupplier {
24+
private static final String tableKey = "/Tuning";
25+
26+
private final String key;
27+
private boolean hasDefault = false;
28+
private double defaultValue;
29+
private LoggedNetworkNumber dashboardNumber;
30+
private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
31+
32+
/**
33+
* Create a new LoggedTunableNumber
34+
*
35+
* @param dashboardKey Key on dashboard
36+
*/
37+
public LoggedTunableNumber(String dashboardKey) {
38+
this.key = tableKey + "/" + dashboardKey;
39+
}
40+
41+
/**
42+
* Create a new LoggedTunableNumber with the default value
43+
*
44+
* @param dashboardKey Key on dashboard
45+
* @param defaultValue Default value
46+
*/
47+
public LoggedTunableNumber(String dashboardKey, double defaultValue) {
48+
this(dashboardKey);
49+
initDefault(defaultValue);
50+
}
51+
52+
/**
53+
* Set the default value of the number. The default value can only be set once.
54+
*
55+
* @param defaultValue The default value
56+
*/
57+
public void initDefault(double defaultValue) {
58+
if (!hasDefault) {
59+
hasDefault = true;
60+
this.defaultValue = defaultValue;
61+
if (Constants.tuningMode) {
62+
dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
63+
}
64+
}
65+
}
66+
67+
/**
68+
* Get the current value, from dashboard if available and in tuning mode.
69+
*
70+
* @return The current value
71+
*/
72+
public double get() {
73+
if (!hasDefault) {
74+
return 0.0;
75+
} else {
76+
return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
77+
}
78+
}
79+
80+
/**
81+
* Checks whether the number has changed since our last check
82+
*
83+
* @param id Unique identifier for the caller to avoid conflicts when shared between multiple
84+
* objects. Recommended approach is to pass the result of "hashCode()"
85+
* @return True if the number has changed since the last time this method was called, false
86+
* otherwise.
87+
*/
88+
public boolean hasChanged(int id) {
89+
double currentValue = get();
90+
Double lastValue = lastHasChangedValues.get(id);
91+
if (lastValue == null || currentValue != lastValue) {
92+
lastHasChangedValues.put(id, currentValue);
93+
return true;
94+
}
95+
96+
return false;
97+
}
98+
99+
/**
100+
* Runs action if any of the tunableNumbers have changed
101+
*
102+
* @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
103+
* objects. Recommended approach is to pass the result of "hashCode()"
104+
* @param action Callback to run when any of the tunable numbers have changed. Access tunable
105+
* numbers in order inputted in method
106+
* @param tunableNumbers All tunable numbers to check
107+
*/
108+
public static void ifChanged(
109+
int id, Consumer<double[]> action, LoggedTunableNumber... tunableNumbers) {
110+
if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
111+
action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
112+
}
113+
}
114+
115+
/** Runs action if any of the tunableNumbers have changed */
116+
public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
117+
ifChanged(id, values -> action.run(), tunableNumbers);
118+
}
119+
120+
@Override
121+
public double getAsDouble() {
122+
return get();
123+
}
124+
}
Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,3 @@
11
package frc.robot.util.motors;
22

3-
public class MotorConfig {
4-
5-
}
3+
public class MotorConfig {}

src/main/java/frc/robot/util/Motors/MotorIO.java

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -23,16 +23,13 @@ public static class MotorIOInputs {
2323

2424
record MotorIOData(boolean isConnected, int canID, ControllerType controllerType, boolean hasError) {}
2525

26-
enum ControllerType {
26+
enum ControllerType {
2727
TALON,
2828
SPARKMAX,
2929
SPARKFLEX
3030
}
31-
32-
record Motor(ControllerType controllerType, int canID) {
3331

34-
}
32+
record Motor(int canID, ControllerType controllerType) {}
3533

3634
public default void updateInputs(MotorIOInputs inputs) {}
37-
3835
}
Lines changed: 16 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -1,65 +1,39 @@
11
package frc.robot.util.motors.controllers;
22

3-
import com.revrobotics.spark.ClosedLoopSlot;
43
import com.revrobotics.spark.SparkBase;
5-
import com.revrobotics.spark.SparkBase.PersistMode;
6-
import com.revrobotics.spark.SparkBase.ResetMode;
7-
import com.revrobotics.spark.SparkFlex;
8-
import com.revrobotics.spark.SparkLowLevel.MotorType;
9-
import com.revrobotics.spark.SparkMax;
104
import com.revrobotics.spark.config.SparkBaseConfig;
11-
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
125
import com.revrobotics.spark.config.SparkFlexConfig;
136
import com.revrobotics.spark.config.SparkMaxConfig;
14-
157
import frc.robot.util.motors.MotorIO;
168

17-
public class SparkBaseController implements MotorIO {
9+
public class SparkBaseController extends SparkBase implements MotorIO {
1810

19-
private SparkBase spark;
20-
private SparkBaseConfig sparkConfig;
11+
SparkBaseConfig sparkBaseConfig;
12+
private final ControllerType controllerType;
2113

2214
/**
2315
* Setups the motor..
2416
*
25-
* @param motor The {@link Motor} dataclass containing motor configuration
26-
* details
17+
* @param motor The {@link Motor} dataclass containing motor configuration details
2718
* @return the {@code MotorConfigurator} instance for chaining
2819
* @throws IllegalArgumentException if the motor type is unsupported
2920
*/
3021
public SparkBaseController(Motor motor) {
31-
createSparkMotor(motor.controllerType(), motor.canID());
22+
super(
23+
motor.canID(),
24+
MotorType.kBrushless,
25+
motor.controllerType() == ControllerType.SPARKMAX ? SparkModel.SparkMax : SparkModel.SparkFlex);
26+
this.controllerType = motor.controllerType();
3227
}
3328

34-
void createSparkMotor(ControllerType motorType, int canID) {
35-
switch (motorType) {
36-
case SPARKMAX:
37-
spark = new SparkMax(canID, MotorType.kBrushless);
38-
sparkConfig = new SparkMaxConfig();
39-
break;
40-
case SPARKFLEX:
41-
spark = new SparkFlex(canID, MotorType.kBrushless);
42-
sparkConfig = new SparkFlexConfig();
43-
break;
44-
default:
45-
throw new IllegalArgumentException("Invalid SparkBase Motor type");
29+
public void isInverted(boolean isInverted) {
30+
if (controllerType == ControllerType.SPARKMAX) {
31+
sparkBaseConfig = new SparkMaxConfig();
32+
} else {
33+
sparkBaseConfig = new SparkFlexConfig();
4634
}
35+
sparkBaseConfig.inverted(isInverted);
4736

37+
super.configure(sparkBaseConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
4838
}
49-
50-
public void setNeutralMode(boolean b) {
51-
sparkConfig.idleMode(b ? IdleMode.kBrake : IdleMode.kCoast);
52-
spark.configure(sparkConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
53-
}
54-
55-
public void setClosedLoopRampRate(int closedLoopRampRate) {
56-
sparkConfig.closedLoopRampRate(closedLoopRampRate);
57-
spark.configure(sparkConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
58-
}
59-
60-
public void setOpenLoopRampRate(int openLoopRampRate) {
61-
sparkConfig.openLoopRampRate(openLoopRampRate);
62-
spark.configure(sparkConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
63-
}
64-
6539
}

0 commit comments

Comments
 (0)