Signals -> master

src/main/java/frc/JoysticksBindings.java

@@ -1,5 +1,6 @@
 package frc;
 
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.joysticks.JoystickPorts;
 import frc.joysticks.SmartJoystick;
 import frc.robot.Robot;
@@ -27,6 +28,10 @@ public static void configureBindings(Robot robot) {
 
 	private static void mainJoystickButtons(Robot robot) {
 		SmartJoystick usedJoystick = MAIN_JOYSTICK;
+		usedJoystick.A.onTrue(new RunCommand(() -> robot.test.getMotor().setPower(1), robot.getTest()));
+		usedJoystick.B.onTrue(new RunCommand(() -> robot.test.getMotor().setPower(0), robot.getTest()));
+		usedJoystick.X.onTrue(new RunCommand(() -> robot.test.getMotor().setPower(0.5), robot.getTest()));
+
 		// bindings...
 	}
 

src/main/java/frc/robot/IDs.java

@@ -1,8 +1,16 @@
 package frc.robot;
 
+import com.revrobotics.spark.SparkLowLevel;
+import frc.robot.hardware.phoenix6.BusChain;
+import frc.robot.hardware.phoenix6.Phoenix6DeviceID;
+import frc.robot.hardware.rev.motors.SparkMaxDeviceID;
+
 public class IDs {
 
 	public static class TalonFXIDs {
+
+		public static final Phoenix6DeviceID test = new Phoenix6DeviceID(0, BusChain.ROBORIO);
+
 	}
 
 	public static class CANCoderIDs {
@@ -15,6 +23,9 @@ public static class CANdleIDs {
 	}
 
 	public static class SparkMAXIDs {
+
+		public static final SparkMaxDeviceID test = new SparkMaxDeviceID(0, SparkLowLevel.MotorType.kBrushless);
+
 	}
 
 	public static class DigitalInputsIDs {

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,8 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.RobotManager;
 import frc.robot.hardware.phoenix6.BusChain;
+import frc.robot.subsystems.TalonFXTestBuilder;
+import frc.robot.subsystems.Test;
 import frc.utils.auto.PathPlannerAutoWrapper;
 import frc.utils.battery.BatteryUtil;
 
@@ -18,9 +20,11 @@
 public class Robot {
 
 	public static final RobotType ROBOT_TYPE = RobotType.determineRobotType(false);
+	public final Test test;
 
 	public Robot() {
 		BatteryUtil.scheduleLimiter();
+		this.test = TalonFXTestBuilder.buildTest();
 	}
 
 	public void periodic() {
@@ -29,6 +33,10 @@ public void periodic() {
 		CommandScheduler.getInstance().run(); // Should be last
 	}
 
+	public Test getTest() {
+		return test;
+	}
+
 	public PathPlannerAutoWrapper getAutonomousCommand() {
 		return new PathPlannerAutoWrapper();
 	}

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

@@ -0,0 +1,74 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.IDs;
+import frc.robot.RobotConstants;
+import frc.robot.hardware.mechanisms.wpilib.SimpleMotorSimulation;
+import frc.robot.hardware.rev.motors.BrushlessSparkMAXMotor;
+import frc.robot.hardware.rev.motors.SparkMaxWrapper;
+import frc.utils.TimedValue;
+import frc.utils.time.TimeUtil;
+
+import java.util.function.Supplier;
+
+public class SparkMaxTestBuilder {
+
+	public static Test buildTest() {
+		String logPath = RobotConstants.SUBSYSTEM_LOGPATH_PREFIX + "/Test";
+
+		SparkMaxWrapper sparkMaxWrapper = new SparkMaxWrapper(IDs.SparkMAXIDs.test);
+
+		BrushlessSparkMAXMotor motor = new BrushlessSparkMAXMotor(
+			logPath + "/Motor",
+			sparkMaxWrapper,
+			new SimpleMotorSimulation(new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNEO(1), 0.0001, 1 / 1), DCMotor.getNEO(1))),
+			new SysIdRoutine.Config()
+		);
+
+		TimedValue<Rotation2d> positionTimedValue = new TimedValue<>(
+			Rotation2d.fromRotations(sparkMaxWrapper.getEncoder().getPosition()),
+			TimeUtil.getCurrentTimeSeconds()
+		);
+
+		TimedValue<Rotation2d> velocityTimedValue = new TimedValue<>(
+			sparkMaxWrapper.getVelocityAnglePerSecond(),
+			TimeUtil.getCurrentTimeSeconds()
+		);
+
+		TimedValue<Double> voltageTimedValue = new TimedValue<>(sparkMaxWrapper.getVoltage(), TimeUtil.getCurrentTimeSeconds());
+
+		TimedValue<Double> currentTimedValue = new TimedValue<>(sparkMaxWrapper.getOutputCurrent(), TimeUtil.getCurrentTimeSeconds());
+
+
+		Supplier<TimedValue<Rotation2d>> positionSignal = () -> {
+			positionTimedValue.setValue(Rotation2d.fromRotations(sparkMaxWrapper.getEncoder().getPosition()));
+			positionTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return positionTimedValue;
+		};
+
+		Supplier<TimedValue<Rotation2d>> velocitySignal = () -> {
+			velocityTimedValue.setValue(sparkMaxWrapper.getVelocityAnglePerSecond());
+			velocityTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return velocityTimedValue;
+		};
+
+		Supplier<TimedValue<Double>> voltageSignal = () -> {
+			voltageTimedValue.setValue(sparkMaxWrapper.getVoltage());
+			voltageTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return voltageTimedValue;
+		};
+
+		Supplier<TimedValue<Double>> currentSignal = () -> {
+			currentTimedValue.setValue(sparkMaxWrapper.getOutputCurrent());
+			currentTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return currentTimedValue;
+		};
+
+		return new Test(logPath, motor, positionSignal, velocitySignal, voltageSignal, currentSignal);
+	}
+
+}

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

@@ -0,0 +1,89 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.IDs;
+import frc.robot.RobotConstants;
+import frc.robot.hardware.mechanisms.wpilib.SimpleMotorSimulation;
+import frc.robot.hardware.phoenix6.motors.TalonFXMotor;
+import frc.utils.TimedValue;
+import frc.utils.time.TimeUtil;
+
+import java.util.function.Supplier;
+
+public class TalonFXTestBuilder {
+
+	public static Test buildTest() {
+		String logPath = RobotConstants.SUBSYSTEM_LOGPATH_PREFIX + "/Test";
+
+		TalonFXMotor motor = new TalonFXMotor(
+			logPath + "/Motor",
+			IDs.TalonFXIDs.test,
+			new SysIdRoutine.Config(),
+			new SimpleMotorSimulation(new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNEO(1), 0.0001, 1 / 1), DCMotor.getNEO(1)))
+		);
+
+		StatusSignal<Angle> rawPositionSignal = motor.getDevice().getPosition(true);
+		StatusSignal<AngularVelocity> rawVelocitySignal = motor.getDevice().getVelocity(true);
+		StatusSignal<Voltage> rawVoltageSignal = motor.getDevice().getMotorVoltage(true);
+		StatusSignal<Current> rawCurrentSignal = motor.getDevice().getStatorCurrent(true);
+
+		rawPositionSignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ);
+		rawVelocitySignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ);
+		rawVoltageSignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ);
+		rawCurrentSignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ);
+
+		TimedValue<Rotation2d> positionTimedValue = new TimedValue<>(
+			Rotation2d
+				.fromRotations(BaseStatusSignal.getLatencyCompensatedValueAsDouble(rawPositionSignal.refresh(), rawVelocitySignal.refresh())),
+			TimeUtil.getCurrentTimeSeconds()
+		);
+		TimedValue<Rotation2d> velocityTimedValue = new TimedValue<>(
+			Rotation2d.fromRotations(rawVelocitySignal.refresh().getValueAsDouble()),
+			TimeUtil.getCurrentTimeSeconds()
+		);
+		TimedValue<Double> voltageTimedValue = new TimedValue<>(rawVoltageSignal.refresh().getValueAsDouble(), TimeUtil.getCurrentTimeSeconds());
+		TimedValue<Double> currentTimedValue = new TimedValue<>(rawCurrentSignal.refresh().getValueAsDouble(), TimeUtil.getCurrentTimeSeconds());
+
+		Supplier<TimedValue<Rotation2d>> positionSignal = () -> {
+			positionTimedValue.setValue(
+				Rotation2d
+					.fromRotations(BaseStatusSignal.getLatencyCompensatedValueAsDouble(rawPositionSignal.refresh(), rawVelocitySignal.refresh()))
+			);
+			positionTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return positionTimedValue;
+		};
+
+		Supplier<TimedValue<Rotation2d>> velocitySignal = () -> {
+			velocityTimedValue.setValue(Rotation2d.fromRotations(rawVelocitySignal.refresh().getValueAsDouble()));
+			velocityTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return velocityTimedValue;
+		};
+
+		Supplier<TimedValue<Double>> voltageSignal = () -> {
+			voltageTimedValue.setValue(rawVoltageSignal.refresh().getValueAsDouble());
+			voltageTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return voltageTimedValue;
+		};
+
+		Supplier<TimedValue<Double>> currentSignal = () -> {
+			currentTimedValue.setValue(rawCurrentSignal.refresh().getValueAsDouble());
+			currentTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds());
+			return currentTimedValue;
+		};
+
+		motor.getDevice().optimizeBusUtilization();
+
+		return new Test(logPath, motor, positionSignal, velocitySignal, voltageSignal, currentSignal);
+	}
+
+}

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

@@ -0,0 +1,51 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.hardware.interfaces.ControllableMotor;
+import frc.utils.TimedValue;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.function.Supplier;
+
+public class Test extends GBSubsystem {
+
+	ControllableMotor motor;
+
+	Supplier<TimedValue<Rotation2d>> positionSignal;
+	Supplier<TimedValue<Rotation2d>> velocitySignal;
+	Supplier<TimedValue<Double>> voltageSignal;
+	Supplier<TimedValue<Double>> currentSignal;
+
+	public Test(
+		String logPath,
+		ControllableMotor motor,
+		Supplier<TimedValue<Rotation2d>> positionSignal,
+		Supplier<TimedValue<Rotation2d>> velocitySignal,
+		Supplier<TimedValue<Double>> voltageSignal,
+		Supplier<TimedValue<Double>> currentSignal
+	) {
+		super(logPath);
+
+		this.motor = motor;
+
+		this.positionSignal = positionSignal;
+		this.voltageSignal = voltageSignal;
+		this.velocitySignal = velocitySignal;
+		this.currentSignal = currentSignal;
+	}
+
+	public ControllableMotor getMotor() {
+		return motor;
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+		motor.updateSimulation();
+
+		Logger.recordOutput(getLogPath() + "/Position", positionSignal.get().getValue());
+		Logger.recordOutput(getLogPath() + "/Velocity", velocitySignal.get().getValue());
+		Logger.recordOutput(getLogPath() + "/Voltage", voltageSignal.get().getValue());
+		Logger.recordOutput(getLogPath() + "/Current", currentSignal.get().getValue());
+	}
+
+}