From 24429a5dc5edb6483e4341a047b57156d70ff2f1 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 16 Nov 2025 10:54:15 -0500 Subject: [PATCH 01/25] feat: Create the base motor template classes --- src/FRC3484_Lib/motor_templates/angular_pos_motor.py | 3 +++ src/FRC3484_Lib/motor_templates/linear_pos_motor.py | 3 +++ src/FRC3484_Lib/motor_templates/power_motor.py | 3 +++ src/FRC3484_Lib/motor_templates/velocity_motor.py | 3 +++ 4 files changed, 12 insertions(+) create mode 100644 src/FRC3484_Lib/motor_templates/angular_pos_motor.py create mode 100644 src/FRC3484_Lib/motor_templates/linear_pos_motor.py create mode 100644 src/FRC3484_Lib/motor_templates/power_motor.py create mode 100644 src/FRC3484_Lib/motor_templates/velocity_motor.py diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py new file mode 100644 index 0000000..8502f9b --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -0,0 +1,3 @@ +class AngularPositionMotor: + def __init__(self) -> None: + pass \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/linear_pos_motor.py b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py new file mode 100644 index 0000000..5126572 --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py @@ -0,0 +1,3 @@ +class LinearPositionMotor: + def __init__(self) -> None: + pass \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py new file mode 100644 index 0000000..2635ec3 --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -0,0 +1,3 @@ +class PowerMotor: + def __init__(self) -> None: + pass \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py new file mode 100644 index 0000000..9f922c8 --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -0,0 +1,3 @@ +class VelocityMotor: + def __init__(self) -> None: + pass \ No newline at end of file From 894ceaadd4c83e9a6f7a86101f607face23e902c Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 16 Nov 2025 11:36:52 -0500 Subject: [PATCH 02/25] feat: Create datatypes for the motor template classes --- src/FRC3484_Lib/SC_Datatypes.py | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index e567358..b84eaff 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -1,3 +1,4 @@ +from phoenix6.signals import NeutralModeValue from wpimath.units import \ seconds, \ inches, \ @@ -70,6 +71,9 @@ class SC_MotorConfig: current_time: seconds = 0.1 current_limit: amperes = 20 + starting_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST + motor_type: str = "falcon" # falcon or minion + ''' Swerve Drive Datatypes ''' @@ -132,4 +136,21 @@ class SC_CameraConfig: class SC_CameraResults: vision_measurement: Pose2d timestamp: seconds - standard_deviation: tuple[float, float, float] \ No newline at end of file + standard_deviation: tuple[float, float, float] + +''' +Motor Template Classes Datatypes +''' + +# TODO: This is just a copy of SC_SwerveCurrentConfig. Since the naming of the existing one is exclusive to swerve, I thought I should make a new one, but maybe we just create a unified one? +class SC_TemplateMotorCurrentConfig: + drive_current_threshold: amperes = 60 + drive_current_time: seconds = 0.1 + drive_current_limit: amperes = 35 + drive_open_loop_ramp: seconds = 0.25 + + steer_current_threshold: amperes = 40 + steer_current_time: seconds = 0.1 + steer_current_limit: amperes = 25 + + current_limit_enabled: bool = True \ No newline at end of file From 8ba44733e3465123251cc55e2b0dc13f6fb1ee03 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 16 Nov 2025 12:00:03 -0500 Subject: [PATCH 03/25] feat: Implement PowerMotor --- src/FRC3484_Lib/SC_Datatypes.py | 2 +- .../motor_templates/power_motor.py | 72 ++++++++++++++++++- 2 files changed, 71 insertions(+), 3 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index b84eaff..8d83207 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -71,7 +71,7 @@ class SC_MotorConfig: current_time: seconds = 0.1 current_limit: amperes = 20 - starting_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST + neutral_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST motor_type: str = "falcon" # falcon or minion ''' diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index 2635ec3..348635a 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -1,3 +1,71 @@ +from operator import inv +import phoenix6 +from phoenix6.hardware import TalonFX, TalonFXS +from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration +from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue +from src.FRC3484_Lib.SC_Datatypes import SC_MotorConfig, SC_TemplateMotorCurrentConfig + class PowerMotor: - def __init__(self) -> None: - pass \ No newline at end of file + ''' + Creates a motor template class that can be used to create a base motor that simply powers forwards or backwards at a given power + + Parameters: + - motor_config (SC_MotorConfig): The configuration for the motor + - current_config (SC_TemplateMotorCurrentConfig): Current limit settings for the motor + ''' + def __init__(self, motor_config: SC_MotorConfig, current_config: SC_TemplateMotorCurrentConfig) -> None: + self._motor: TalonFX | TalonFXS + self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + + # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation + # There is no communtation for the falcon, so use a talon FX controller instead + if motor_config.motor_type == "minion": + self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXSConfiguration() + + self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST + + elif motor_config.motor_type == "falcon": + self._motor = phoenix6.hardware.TalonFX(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXConfiguration() + else: + raise ValueError(f"Invalid motor type: {motor_config.motor_type}") + + # TODO: Do these need to be swapped? + if motor_config.inverted: + self._motor_config.motor_output.inverted = InvertedValue.COUNTER_CLOCKWISE_POSITIVE + else: + self._motor_config.motor_output.inverted = InvertedValue.CLOCKWISE_POSITIVE + + self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode + + self._motor_config.current_limits = CurrentLimitsConfigs() \ + .with_supply_current_limit_enable(current_config.current_limit_enabled) \ + .with_supply_current_limit(current_config.drive_current_limit) \ + .with_supply_current_lower_limit(current_config.drive_current_threshold) \ + .with_supply_current_lower_time(current_config.drive_current_time) + + def set_power(self, power: float) -> None: + ''' + Sets the power of the motor + + Parameters: + - power (float): The power to set the motor to + ''' + self._motor.set(power) + + def set_brake_mode(self) -> None: + ''' + Sets the motor to brake mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE + _ = self._motor.configurator.apply(self._motor_config) + + def set_coast_mode(self) -> None: + ''' + Sets the motor to coast mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST + _ = self._motor.configurator.apply(self._motor_config) \ No newline at end of file From 8bfddff2bea9a643f29866f41df6adb5d55a212c Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 17 Nov 2025 14:00:00 -0500 Subject: [PATCH 04/25] fix: Use a seperate SC_TemplateMotorConfig datatype --- src/FRC3484_Lib/SC_Datatypes.py | 18 ++++++++++++++---- src/FRC3484_Lib/motor_templates/power_motor.py | 4 ++-- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index 8d83207..543f81f 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -71,9 +71,6 @@ class SC_MotorConfig: current_time: seconds = 0.1 current_limit: amperes = 20 - neutral_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST - motor_type: str = "falcon" # falcon or minion - ''' Swerve Drive Datatypes ''' @@ -141,8 +138,21 @@ class SC_CameraResults: ''' Motor Template Classes Datatypes ''' +@dataclass(frozen=True) +class SC_TemplateMotorConfig: + can_id: int + inverted: bool = False + can_bus_name: str = "rio" + + current_limit_enabled: bool = True + current_threshold: amperes = 50 + current_time: seconds = 0.1 + current_limit: amperes = 20 -# TODO: This is just a copy of SC_SwerveCurrentConfig. Since the naming of the existing one is exclusive to swerve, I thought I should make a new one, but maybe we just create a unified one? + neutral_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST + motor_type: str = "falcon" # falcon or minion + +@dataclass(frozen=True) class SC_TemplateMotorCurrentConfig: drive_current_threshold: amperes = 60 drive_current_time: seconds = 0.1 diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index 348635a..b025310 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -3,7 +3,7 @@ from phoenix6.hardware import TalonFX, TalonFXS from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue -from src.FRC3484_Lib.SC_Datatypes import SC_MotorConfig, SC_TemplateMotorCurrentConfig +from src.FRC3484_Lib.SC_Datatypes import SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig class PowerMotor: ''' @@ -13,7 +13,7 @@ class PowerMotor: - motor_config (SC_MotorConfig): The configuration for the motor - current_config (SC_TemplateMotorCurrentConfig): Current limit settings for the motor ''' - def __init__(self, motor_config: SC_MotorConfig, current_config: SC_TemplateMotorCurrentConfig) -> None: + def __init__(self, motor_config: SC_TemplateMotorConfig, current_config: SC_TemplateMotorCurrentConfig) -> None: self._motor: TalonFX | TalonFXS self._motor_config: TalonFXConfiguration | TalonFXSConfiguration From 6b924d224d5bfa39dd601ac1310b62fb32eb20cb Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 17 Nov 2025 14:02:30 -0500 Subject: [PATCH 05/25] fix: Set the motor rotation direction without using an if statement --- src/FRC3484_Lib/motor_templates/power_motor.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index b025310..f92f03f 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -33,11 +33,7 @@ def __init__(self, motor_config: SC_TemplateMotorConfig, current_config: SC_Temp else: raise ValueError(f"Invalid motor type: {motor_config.motor_type}") - # TODO: Do these need to be swapped? - if motor_config.inverted: - self._motor_config.motor_output.inverted = InvertedValue.COUNTER_CLOCKWISE_POSITIVE - else: - self._motor_config.motor_output.inverted = InvertedValue.CLOCKWISE_POSITIVE + self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode From 1a7e69f39b9bd9ed4034214835c00d542eec7d30 Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 17 Nov 2025 14:09:00 -0500 Subject: [PATCH 06/25] fix: Fix imports in PowerMotor --- src/FRC3484_Lib/motor_templates/power_motor.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index f92f03f..c6c2a94 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -1,8 +1,7 @@ -from operator import inv -import phoenix6 from phoenix6.hardware import TalonFX, TalonFXS from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue + from src.FRC3484_Lib.SC_Datatypes import SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig class PowerMotor: @@ -27,7 +26,7 @@ def __init__(self, motor_config: SC_TemplateMotorConfig, current_config: SC_Temp self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST elif motor_config.motor_type == "falcon": - self._motor = phoenix6.hardware.TalonFX(motor_config.can_id, motor_config.can_bus_name) + self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) self._motor_config = TalonFXConfiguration() else: From 690b5a5a26ce06cf5b5f3d468242e8e4d51aac92 Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 17 Nov 2025 18:20:18 -0500 Subject: [PATCH 07/25] fix: Forgot to apply the configs to PowerMotor --- src/FRC3484_Lib/motor_templates/power_motor.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index c6c2a94..fbc524c 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -6,7 +6,8 @@ class PowerMotor: ''' - Creates a motor template class that can be used to create a base motor that simply powers forwards or backwards at a given power + Creates a motor template class that can be used to create a + base motor that simply powers forwards or backwards at a given power Parameters: - motor_config (SC_MotorConfig): The configuration for the motor @@ -42,6 +43,8 @@ def __init__(self, motor_config: SC_TemplateMotorConfig, current_config: SC_Temp .with_supply_current_lower_limit(current_config.drive_current_threshold) \ .with_supply_current_lower_time(current_config.drive_current_time) + _ = self._motor.configurator.apply(self._motor_config) + def set_power(self, power: float) -> None: ''' Sets the power of the motor From e27ca80166453e305dde31d1588277058068574f Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 17 Nov 2025 18:20:32 -0500 Subject: [PATCH 08/25] feat: Impement most of VelocityMotor --- .../motor_templates/velocity_motor.py | 113 +++++++++++++++++- 1 file changed, 111 insertions(+), 2 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 9f922c8..517566e 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -1,3 +1,112 @@ +from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration +from phoenix6.hardware import TalonFX, TalonFXS +from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue +from wpimath.controller import PIDController +from wpimath.units import revolutions_per_minute +from FRC3484_Lib.SC_Datatypes import SC_MotorConfig, SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig + + class VelocityMotor: - def __init__(self) -> None: - pass \ No newline at end of file + ''' + Creates a motor template class that represents a motor that can be set to a target speed + + Parameters: + - motor_config (SC_MotorConfig): The configuration for the motor + - current_config (SC_TemplateMotorCurrentConfig): Current limit settings for the motor + - pid_config (SC_PIDConfig): The configuration for the PID controller + - gear_ratio (float): The gear ratio of the motor + - tolerance (float): The tolerance for the target speed to consider it reached + ''' + def __init__( + self, + motor_config: SC_TemplateMotorConfig, + current_config: SC_TemplateMotorCurrentConfig, + pid_config: SC_PIDConfig, + gear_ratio: float, + tolerance: float + ) -> None: + self.motor: TalonFX | TalonFXS + self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) + + self._target_speed: revolutions_per_minute = 0.0 + + self._tolerance: float = tolerance + self._gear_ratio: float = gear_ratio + + # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation + # There is no communtation for the falcon, so use a talon FX controller instead + if motor_config.motor_type == "minion": + self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXSConfiguration() + + self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST + + elif motor_config.motor_type == "falcon": + self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXConfiguration() + else: + raise ValueError(f"Invalid motor type: {motor_config.motor_type}") + + self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) + + self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode + + self._motor_config.current_limits = CurrentLimitsConfigs() \ + .with_supply_current_limit_enable(current_config.current_limit_enabled) \ + .with_supply_current_limit(current_config.drive_current_limit) \ + .with_supply_current_lower_limit(current_config.drive_current_threshold) \ + .with_supply_current_lower_time(current_config.drive_current_time) + + _ = self._motor.configurator.apply(self._motor_config) + + def set_speed(self, speed: revolutions_per_minute) -> None: + ''' + Sets the target speed for the motor + + Parameters: + - speed (revolutions_per_minute): The speed to set the motor to in RPMs + ''' + self._target_speed = speed * self._gear_ratio + + def at_target_speed(self) -> bool: + ''' + Checks if the motor is at the target speed + + Returns: + - bool: True if the motor is at the target speed, False otherwise + ''' + if self._target_speed == 0.0: + return True + + # TODO: Is this correct? I wasn't quite sure how this function worked on Agent Smith + # Convert RPS to RPM, then subtract the target speed and compare to the tolerance + return abs((self._motor.get_velocity().value * 60) - self._target_speed) < self._tolerance + + def set_brake_mode(self) -> None: + ''' + Sets the motor to brake mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE + _ = self._motor.configurator.apply(self._motor_config) + + def set_coast_mode(self) -> None: + ''' + Sets the motor to coast mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST + _ = self._motor.configurator.apply(self._motor_config) + + def set_power(self, power: float) -> None: + ''' + Sets the power of the motor for testing purposes + + Parameters: + - power (float): The power to set the motor to + ''' + # TODO: Have a boolean for testing mode to disable PID and feed forward + self._motor.set(power) + + \ No newline at end of file From ba5ed67cfdbd59fd149f586857fbc37f1432b9a6 Mon Sep 17 00:00:00 2001 From: nfoert Date: Wed, 19 Nov 2025 14:14:49 -0500 Subject: [PATCH 09/25] fix: Use SC_TemplateMotorVelocityControl instead of a simple float for velocity target speed --- src/FRC3484_Lib/SC_Datatypes.py | 7 ++++++- src/FRC3484_Lib/motor_templates/velocity_motor.py | 13 ++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index 543f81f..5624276 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -163,4 +163,9 @@ class SC_TemplateMotorCurrentConfig: steer_current_time: seconds = 0.1 steer_current_limit: amperes = 25 - current_limit_enabled: bool = True \ No newline at end of file + current_limit_enabled: bool = True + +@dataclass(frozen=True) +class SC_TemplateMotorVelocityControl: + power: float + speed: revolutions_per_minute \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 517566e..12b56d3 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -2,8 +2,7 @@ from phoenix6.hardware import TalonFX, TalonFXS from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue from wpimath.controller import PIDController -from wpimath.units import revolutions_per_minute -from FRC3484_Lib.SC_Datatypes import SC_MotorConfig, SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig +from FRC3484_Lib.SC_Datatypes import SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorVelocityControl class VelocityMotor: @@ -29,7 +28,7 @@ def __init__( self._motor_config: TalonFXConfiguration | TalonFXSConfiguration self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) - self._target_speed: revolutions_per_minute = 0.0 + self._target_speed: SC_TemplateMotorVelocityControl = SC_TemplateMotorVelocityControl(0.0, 0.0) self._tolerance: float = tolerance self._gear_ratio: float = gear_ratio @@ -62,14 +61,14 @@ def __init__( _ = self._motor.configurator.apply(self._motor_config) - def set_speed(self, speed: revolutions_per_minute) -> None: + def set_speed(self, speed: SC_TemplateMotorVelocityControl) -> None: ''' Sets the target speed for the motor Parameters: - - speed (revolutions_per_minute): The speed to set the motor to in RPMs + - speed (SC_TemplateMotorVelocityControl): The speed and power to set the motor to ''' - self._target_speed = speed * self._gear_ratio + self._target_speed = SC_TemplateMotorVelocityControl(speed.speed * self._gear_ratio, speed.power) def at_target_speed(self) -> bool: ''' @@ -107,6 +106,6 @@ def set_power(self, power: float) -> None: - power (float): The power to set the motor to ''' # TODO: Have a boolean for testing mode to disable PID and feed forward - self._motor.set(power) + self._target_speed = SC_TemplateMotorVelocityControl(0.0, power) \ No newline at end of file From 92ca74835d60976f02100535906c7bdd8c7b2d5b Mon Sep 17 00:00:00 2001 From: nfoert Date: Wed, 19 Nov 2025 15:06:09 -0500 Subject: [PATCH 10/25] feat: Add PID control to VelocityMotor --- src/FRC3484_Lib/SC_Datatypes.py | 1 + .../motor_templates/velocity_motor.py | 50 ++++++++++++++++--- 2 files changed, 44 insertions(+), 7 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index 5624276..64b3276 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -140,6 +140,7 @@ class SC_CameraResults: ''' @dataclass(frozen=True) class SC_TemplateMotorConfig: + motor_name: str = "Motor" # Should be changed. Used for Smart Dashboard diagnostics can_id: int inverted: bool = False can_bus_name: str = "rio" diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 12b56d3..bce9fa2 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -1,11 +1,17 @@ +from typing import override + +from wpilib import SmartDashboard +from commands2.subsystem import Subsystem + from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration from phoenix6.hardware import TalonFX, TalonFXS from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue -from wpimath.controller import PIDController +from phoenix6.controls import DutyCycleOut, VelocityVoltage + from FRC3484_Lib.SC_Datatypes import SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorVelocityControl -class VelocityMotor: +class VelocityMotor(Subsystem): ''' Creates a motor template class that represents a motor that can be set to a target speed @@ -24,14 +30,16 @@ def __init__( gear_ratio: float, tolerance: float ) -> None: - self.motor: TalonFX | TalonFXS + super().__init__() + + self._motor: TalonFX | TalonFXS self._motor_config: TalonFXConfiguration | TalonFXSConfiguration - self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) self._target_speed: SC_TemplateMotorVelocityControl = SC_TemplateMotorVelocityControl(0.0, 0.0) self._tolerance: float = tolerance self._gear_ratio: float = gear_ratio + self._motor_name: str = motor_config.motor_name # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation # There is no communtation for the falcon, so use a talon FX controller instead @@ -59,7 +67,33 @@ def __init__( .with_supply_current_lower_limit(current_config.drive_current_threshold) \ .with_supply_current_lower_time(current_config.drive_current_time) + # TODO: The CTRE examples set Ks, Kv, and Kp, but not Ki or Kd. Do we need to specify more in SC_PIDConfig? + _ = self._motor_config.slot0 \ + .with_k_p(pid_config.Kp) \ + .with_k_i(pid_config.Ki) \ + .with_k_d(pid_config.Kd) + _ = self._motor.configurator.apply(self._motor_config) + + @override + def periodic(self) -> None: + ''' + Handles Smart Dashboard diagnostic information and actually controlling the motors + ''' + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) + _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) + + if not SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + if self._target_speed.power == 0.0 and self._target_speed.speed == 0.0: + _ = self._motor.set_control(DutyCycleOut(0)) + + elif self._target_speed.power != 0.0: + _ = self._motor.set_control(DutyCycleOut(self._target_speed.power)) + + else: + _ = self._motor.set_control(VelocityVoltage(self._target_speed.speed)) + def set_speed(self, speed: SC_TemplateMotorVelocityControl) -> None: ''' @@ -77,12 +111,14 @@ def at_target_speed(self) -> bool: Returns: - bool: True if the motor is at the target speed, False otherwise ''' - if self._target_speed == 0.0: + if self._target_speed.power == 0.0 and self._target_speed.speed == 0.0: return True - # TODO: Is this correct? I wasn't quite sure how this function worked on Agent Smith + elif self._target_speed.power != 0.0: + return (self._motor.get() - self._target_speed.speed) * (1 if self._target_speed.speed >= 0 else -1) > 0 + # Convert RPS to RPM, then subtract the target speed and compare to the tolerance - return abs((self._motor.get_velocity().value * 60) - self._target_speed) < self._tolerance + return abs((self._motor.get_velocity().value * 60) - self._target_speed.speed) < self._tolerance def set_brake_mode(self) -> None: ''' From f9d62310cd2bb7cf40b42ada6838530caeed82ba Mon Sep 17 00:00:00 2001 From: nfoert Date: Fri, 21 Nov 2025 07:38:29 -0500 Subject: [PATCH 11/25] fix: Fix PID for VelocityMotor --- .../motor_templates/velocity_motor.py | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index bce9fa2..7114476 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -7,8 +7,10 @@ from phoenix6.hardware import TalonFX, TalonFXS from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue from phoenix6.controls import DutyCycleOut, VelocityVoltage +from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters +from wpimath.units import volts -from FRC3484_Lib.SC_Datatypes import SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorVelocityControl +from FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorVelocityControl class VelocityMotor(Subsystem): @@ -27,6 +29,7 @@ def __init__( motor_config: SC_TemplateMotorConfig, current_config: SC_TemplateMotorCurrentConfig, pid_config: SC_PIDConfig, + feed_forward_config: SC_LinearFeedForwardConfig, gear_ratio: float, tolerance: float ) -> None: @@ -34,6 +37,8 @@ def __init__( self._motor: TalonFX | TalonFXS self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) + self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) self._target_speed: SC_TemplateMotorVelocityControl = SC_TemplateMotorVelocityControl(0.0, 0.0) @@ -67,12 +72,6 @@ def __init__( .with_supply_current_lower_limit(current_config.drive_current_threshold) \ .with_supply_current_lower_time(current_config.drive_current_time) - # TODO: The CTRE examples set Ks, Kv, and Kp, but not Ki or Kd. Do we need to specify more in SC_PIDConfig? - _ = self._motor_config.slot0 \ - .with_k_p(pid_config.Kp) \ - .with_k_i(pid_config.Ki) \ - .with_k_d(pid_config.Kd) - _ = self._motor.configurator.apply(self._motor_config) @override @@ -86,13 +85,16 @@ def periodic(self) -> None: if not SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): if self._target_speed.power == 0.0 and self._target_speed.speed == 0.0: - _ = self._motor.set_control(DutyCycleOut(0)) + self._pid_controller.reset(0) + self._motor.setVoltage(0) elif self._target_speed.power != 0.0: - _ = self._motor.set_control(DutyCycleOut(self._target_speed.power)) + self._motor.set(self._target_speed.power) else: - _ = self._motor.set_control(VelocityVoltage(self._target_speed.speed)) + pid: volts = self._pid_controller.calculate(self._motor.get_velocity().value, self._target_speed.speed) + feed_forward: volts = self._feed_forward_controller.calculate(self._motor.get_velocity().value, self._target_speed.speed) + self._motor.setVoltage(pid + feed_forward) def set_speed(self, speed: SC_TemplateMotorVelocityControl) -> None: From 8c78b7da85b846d3cc535f9439005655fc4ef6c2 Mon Sep 17 00:00:00 2001 From: Purgso Date: Fri, 21 Nov 2025 22:41:38 -0500 Subject: [PATCH 12/25] feat: angular position template --- .../motor_templates/angular_pos_motor.py | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 8502f9b..6c71488 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -1,3 +1,24 @@ class AngularPositionMotor: def __init__(self) -> None: - pass \ No newline at end of file + pass + +''' +Create an enum for state - power or position control +This is purely so we don't have pid and power control running at the same time +If the most recent input was power, set to power control +If the most recent input was position, set to position control + +__init__ should get motor config, current config, pid config, feed forward config, trapezoid config (max vel and accel), angle tolerance, and stall limit (maybe add this to motor config with a default value of 0.75?) + init should also have optional parameters for gear ratio and external encoder (CTRE Cancoder). If no external encoder is provided, use the internal encoder +Create public functions for set target angle, set power, brake mode, coast mode, at target angle, set angle, and get angle + +set angle shouldn't update the trapezoid if the new target is the same as the old one (this won't matter when we switch to motion magic) + +All motors should also have functions for get stall percentage and get stalled. +Stall percentage is the percentage of stall current being drawn by the motor. +It can be calculated as (supply current / motor stall current) * (supply voltage / 12). +The motor controller has functions for all of these values. +Get stalled should return true if the stall percentage is above the stall limit given in the config + +All motors should have a function for printing diagnostics to smart dashboard +''' \ No newline at end of file From a8924e6547b559358254e55b997db53792502ab1 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 22 Nov 2025 10:27:05 -0500 Subject: [PATCH 13/25] feat: Implement AngularPositionMotor --- src/FRC3484_Lib/SC_Datatypes.py | 20 +- .../motor_templates/angular_pos_motor.py | 245 ++++++++++++++++-- 2 files changed, 239 insertions(+), 26 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index 64b3276..350b74a 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -1,4 +1,5 @@ from phoenix6.signals import NeutralModeValue +from phoenix6.units import degree from wpimath.units import \ seconds, \ inches, \ @@ -11,7 +12,11 @@ volt_seconds_per_meter, \ volt_seconds_squared_per_meter, \ volt_seconds_per_radian, \ - volt_seconds_squared_per_radian + volt_seconds_squared_per_radian, \ + feet_per_second_squared, \ + feet_per_second, \ + degrees_per_second, \ + degrees_per_second_squared from wpilib import PneumaticsModuleType from wpimath.geometry import Transform3d, Pose2d @@ -149,6 +154,7 @@ class SC_TemplateMotorConfig: current_threshold: amperes = 50 current_time: seconds = 0.1 current_limit: amperes = 20 + stall_limit: float = 0.75 neutral_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST motor_type: str = "falcon" # falcon or minion @@ -169,4 +175,14 @@ class SC_TemplateMotorCurrentConfig: @dataclass(frozen=True) class SC_TemplateMotorVelocityControl: power: float - speed: revolutions_per_minute \ No newline at end of file + speed: revolutions_per_minute + +@dataclass +class SC_TemplateMotorPositionControl: + position: float + speed: float + +@dataclass(frozen=True) +class SC_TemplateMotorTrapezoidConfig: + max_velocity: feet_per_second | degrees_per_second + max_acceleration: feet_per_second_squared | degrees_per_second_squared diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 6c71488..e7c3958 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -1,24 +1,221 @@ -class AngularPositionMotor: - def __init__(self) -> None: - pass - -''' -Create an enum for state - power or position control -This is purely so we don't have pid and power control running at the same time -If the most recent input was power, set to power control -If the most recent input was position, set to position control - -__init__ should get motor config, current config, pid config, feed forward config, trapezoid config (max vel and accel), angle tolerance, and stall limit (maybe add this to motor config with a default value of 0.75?) - init should also have optional parameters for gear ratio and external encoder (CTRE Cancoder). If no external encoder is provided, use the internal encoder -Create public functions for set target angle, set power, brake mode, coast mode, at target angle, set angle, and get angle - -set angle shouldn't update the trapezoid if the new target is the same as the old one (this won't matter when we switch to motion magic) - -All motors should also have functions for get stall percentage and get stalled. -Stall percentage is the percentage of stall current being drawn by the motor. -It can be calculated as (supply current / motor stall current) * (supply voltage / 12). -The motor controller has functions for all of these values. -Get stalled should return true if the stall percentage is above the stall limit given in the config - -All motors should have a function for printing diagnostics to smart dashboard -''' \ No newline at end of file +from enum import Enum +from typing import override + +from commands2 import Subsystem +from wpilib import Timer, SmartDashboard +from wpimath.units import degrees, degrees_per_second +from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters +from wpimath.trajectory import TrapezoidProfile + +from phoenix6.hardware import TalonFX, TalonFXS, CANcoder +from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration +from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue + +from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorTrapezoidConfig + +class State(Enum): + POWER = 0 + POSITION = 1 + +class AngularPositionMotor(Subsystem): + ''' + Defines a base motor class for angular position control + + Parameters: + motor_config: SC_TemplateMotorConfig + current_config: SC_TemplateMotorCurrentConfig + pid_config: SC_PIDConfig + feed_forward_config: SC_LinearFeedForwardConfig + trapezoid_config: SC_TemplateMotorTrapezoidConfig + angle_tolerance: degrees + gear_ratio: float = 1.0 + external_encoder: CANcoder | None = None + ''' + def __init__( + self, + motor_config: SC_TemplateMotorConfig, + current_config: SC_TemplateMotorCurrentConfig, + pid_config: SC_PIDConfig, + feed_forward_config: SC_LinearFeedForwardConfig, + trapezoid_config: SC_TemplateMotorTrapezoidConfig, + angle_tolerance: degrees, + gear_ratio: float = 1.0, + external_encoder: CANcoder | None = None + ) -> None: + super().__init__() + + # Set up variables + self._state: State = State.POWER + + self._motor: TalonFX | TalonFXS + self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) + self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) + self._motor_name: str = motor_config.motor_name + self._stall_limit: float = motor_config.stall_limit + + self._trapezoid: TrapezoidProfile = TrapezoidProfile(TrapezoidProfile.Constraints(trapezoid_config.max_velocity, trapezoid_config.max_acceleration)) + self._trapezoid_timer: Timer = Timer() + + self._encoder: CANcoder | None = external_encoder + + self._initial_state: TrapezoidProfile.State = TrapezoidProfile.State(0.0, 0.0) + self._target_state: TrapezoidProfile.State = TrapezoidProfile.State(0.0, 0.0) + + self._previous_velocity: degrees_per_second = 0 + + self._gear_ratio: float = gear_ratio + self._angle_tolerance: degrees = angle_tolerance + + # Set up motor + + # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation + # There is no communtation for the falcon, so use a talon FX controller instead + if motor_config.motor_type == "minion": + self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXSConfiguration() + + self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST + + elif motor_config.motor_type == "falcon": + self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXConfiguration() + else: + raise ValueError(f"Invalid motor type: {motor_config.motor_type}") + + self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) + + self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode + + self._motor_config.current_limits = CurrentLimitsConfigs() \ + .with_supply_current_limit_enable(current_config.current_limit_enabled) \ + .with_supply_current_limit(current_config.drive_current_limit) \ + .with_supply_current_lower_limit(current_config.drive_current_threshold) \ + .with_supply_current_lower_time(current_config.drive_current_time) + + _ = self._motor.configurator.apply(self._motor_config) + + self._trapezoid_timer.start() + + @override + def periodic(self) -> None: + ''' + Handles controlling the motors in position mode and printing diagnostics + ''' + # TODO: Should this check for a home position so it can know to + # stop powering even if we're not using a homing state? + # TODO: What was the encoder going to be used for? + if self._state == State.POSITION: + current_state = self._trapezoid.calculate(self._trapezoid_timer.get(), self._initial_state, self._target_state) + feed_forward = self._feed_forward_controller.calculate(self._previous_velocity, current_state.velocity) + pid = self._pid_controller.calculate(self.get_angle(), current_state.position) + + self._motor.setVoltage(pid + feed_forward) + self._previous_velocity = current_state.velocity + + self.print_diagnostics() + + def at_target_angle(self) -> bool: + ''' + Returns whether the motor is at the target angle or not + + Returns: + - bool: True if the motor is at the target angle, False otherwise + ''' + return abs(self._target_state.position - self.get_angle()) < self._angle_tolerance + + def get_angle(self) -> degrees: + ''' + Returns the current angle of the motor + + Returns: + - degrees: The current angle of the motor + ''' + return self._motor.get_position().value / self._gear_ratio + + def get_velocity(self) -> degrees_per_second: + ''' + Returns the current velocity of the motor + + Returns: + - degrees_per_second: The current velocity of the motor + ''' + return self._motor.get_velocity().value / self._gear_ratio + + def set_power(self, power: float) -> None: + ''' + Sets the power of the motor + + Parameters: + - power (float): The power to set the motor to + ''' + self._state = State.POWER + self._motor.set(power) + + def set_target_angle(self, angle: degrees) -> None: + ''' + Sets the target angle of the motor + + Parameters: + - angle (degrees): The angle to set the motor to + ''' + self._state = State.POSITION + + if angle != self._target_state.position: + # TODO: I can't get the linter to be happy with these, because .position + # and .velocity are a @property and not a variable. Does this matter? + self._target_state.position = angle + self._target_state.velocity = 0 + + self._initial_state.position = self.get_angle() + self._initial_state.velocity = self.get_velocity() + + self._trapezoid_timer.reset() + + def set_brake_mode(self) -> None: + ''' + Sets the motor to brake mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE + _ = self._motor.configurator.apply(self._motor_config) + + def set_coast_mode(self) -> None: + ''' + Sets the motor to coast mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST + _ = self._motor.configurator.apply(self._motor_config) + + def get_stall_percentage(self) -> float: + ''' + Returns the percentage of stall current being drawn by the motor + + Returns: + - float: The percentage of stall current being drawn by the motor + ''' + if abs(self._motor.get()) > self._stall_limit: + return (self._motor.get_supply_current().value / self._motor.get_motor_stall_current().value * abs(self._motor.get())) + else: + return 0 + + def get_stalled(self) -> bool: + ''' + Returns whether the motor is stalled or not + + Returns: + - bool: True if the motor is stalled, False otherwise + ''' + return self.get_stall_percentage() > self._stall_limit + + def print_diagnostics(self) -> None: + ''' + Prints diagnostic information to Smart Dashboard + ''' + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + _ = SmartDashboard.putNumber(f"{self._motor_name} Angle (degrees)", self.get_angle()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) \ No newline at end of file From d453daed0df0d483104ef0873e0712dfcf0ebdbe Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 22 Nov 2025 10:29:47 -0500 Subject: [PATCH 14/25] feat: Add diagnostic printing and stall detection to PowerMotor --- .../motor_templates/power_motor.py | 55 ++++++++++++++++++- 1 file changed, 52 insertions(+), 3 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index fbc524c..5525055 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -1,10 +1,14 @@ +from typing import override +from commands2 import Subsystem + from phoenix6.hardware import TalonFX, TalonFXS from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue +from wpilib import SmartDashboard from src.FRC3484_Lib.SC_Datatypes import SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig -class PowerMotor: +class PowerMotor(Subsystem): ''' Creates a motor template class that can be used to create a base motor that simply powers forwards or backwards at a given power @@ -13,10 +17,19 @@ class PowerMotor: - motor_config (SC_MotorConfig): The configuration for the motor - current_config (SC_TemplateMotorCurrentConfig): Current limit settings for the motor ''' - def __init__(self, motor_config: SC_TemplateMotorConfig, current_config: SC_TemplateMotorCurrentConfig) -> None: + def __init__( + self, + motor_config: SC_TemplateMotorConfig, + current_config: SC_TemplateMotorCurrentConfig + ) -> None: + super().__init__() + self._motor: TalonFX | TalonFXS self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + self._motor_name: str = motor_config.motor_name + self.stall_limit: float = motor_config.stall_limit + # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation # There is no communtation for the falcon, so use a talon FX controller instead if motor_config.motor_type == "minion": @@ -45,6 +58,12 @@ def __init__(self, motor_config: SC_TemplateMotorConfig, current_config: SC_Temp _ = self._motor.configurator.apply(self._motor_config) + def periodic(self) -> None: + ''' + Handles printing diagnostic information to Smart Dashboard + ''' + self.print_diagnostics() + def set_power(self, power: float) -> None: ''' Sets the power of the motor @@ -66,4 +85,34 @@ def set_coast_mode(self) -> None: Sets the motor to coast mode ''' self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST - _ = self._motor.configurator.apply(self._motor_config) \ No newline at end of file + _ = self._motor.configurator.apply(self._motor_config) + + def get_stall_percentage(self) -> float: + ''' + Returns the percentage of stall current being drawn by the motor + + Returns: + - float: The percentage of stall current being drawn by the motor + ''' + if abs(self._motor.get()) > self.stall_limit: + return (self._motor.get_supply_current().value / self._motor.get_motor_stall_current().value * abs(self._motor.get())) + else: + return 0 + + def get_stalled(self) -> bool: + ''' + Returns whether the motor is stalled or not + + Returns: + - bool: True if the motor is stalled, False otherwise + ''' + return self.get_stall_percentage() > self.stall_limit + + def print_diagnostics(self) -> None: + ''' + Prints diagnostic information to Smart Dashboard + ''' + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) \ No newline at end of file From 9f00e9a9459ad4cd99a0250749e8577913724798 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 22 Nov 2025 10:34:06 -0500 Subject: [PATCH 15/25] feat: Add stall checking and diagnostic printing to VelocityMotor --- .../motor_templates/velocity_motor.py | 40 ++++++++++++++++--- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 7114476..a2f5340 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -45,6 +45,7 @@ def __init__( self._tolerance: float = tolerance self._gear_ratio: float = gear_ratio self._motor_name: str = motor_config.motor_name + self.stall_limit: float = motor_config.stall_limit # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation # There is no communtation for the falcon, so use a talon FX controller instead @@ -79,10 +80,6 @@ def periodic(self) -> None: ''' Handles Smart Dashboard diagnostic information and actually controlling the motors ''' - if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): - _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) - _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) - if not SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): if self._target_speed.power == 0.0 and self._target_speed.speed == 0.0: self._pid_controller.reset(0) @@ -95,7 +92,8 @@ def periodic(self) -> None: pid: volts = self._pid_controller.calculate(self._motor.get_velocity().value, self._target_speed.speed) feed_forward: volts = self._feed_forward_controller.calculate(self._motor.get_velocity().value, self._target_speed.speed) self._motor.setVoltage(pid + feed_forward) - + + self.print_diagnostics() def set_speed(self, speed: SC_TemplateMotorVelocityControl) -> None: ''' @@ -146,4 +144,36 @@ def set_power(self, power: float) -> None: # TODO: Have a boolean for testing mode to disable PID and feed forward self._target_speed = SC_TemplateMotorVelocityControl(0.0, power) + def get_stall_percentage(self) -> float: + ''' + Returns the percentage of stall current being drawn by the motor + + Returns: + - float: The percentage of stall current being drawn by the motor + ''' + if abs(self._motor.get()) > self.stall_limit: + return (self._motor.get_supply_current().value / self._motor.get_motor_stall_current().value * abs(self._motor.get())) + else: + return 0 + + def get_stalled(self) -> bool: + ''' + Returns whether the motor is stalled or not + + Returns: + - bool: True if the motor is stalled, False otherwise + ''' + return self.get_stall_percentage() > self.stall_limit + + def print_diagnostics(self) -> None: + ''' + Prints diagnostic information to Smart Dashboard + ''' + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) + _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) + \ No newline at end of file From 402d781dbce07e7596f9f4b3aa2686e66f64265b Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 22 Nov 2025 10:37:03 -0500 Subject: [PATCH 16/25] fix: Don't set the motor position unless its not in testing mode --- src/FRC3484_Lib/motor_templates/angular_pos_motor.py | 5 ++++- src/FRC3484_Lib/motor_templates/power_motor.py | 2 ++ src/FRC3484_Lib/motor_templates/velocity_motor.py | 5 ++++- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index e7c3958..7c657ad 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -107,7 +107,7 @@ def periodic(self) -> None: # TODO: Should this check for a home position so it can know to # stop powering even if we're not using a homing state? # TODO: What was the encoder going to be used for? - if self._state == State.POSITION: + if self._state == State.POSITION and not SmartDashboard.getBoolean(f"{self._motor_name} Test Mode", False): current_state = self._trapezoid.calculate(self._trapezoid_timer.get(), self._initial_state, self._target_state) feed_forward = self._feed_forward_controller.calculate(self._previous_velocity, current_state.velocity) pid = self._pid_controller.calculate(self.get_angle(), current_state.position) @@ -213,6 +213,9 @@ def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Test Mode", False) + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): _ = SmartDashboard.putNumber(f"{self._motor_name} Angle (degrees)", self.get_angle()) _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index 5525055..72198fa 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -112,6 +112,8 @@ def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index a2f5340..9cb2811 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -80,7 +80,7 @@ def periodic(self) -> None: ''' Handles Smart Dashboard diagnostic information and actually controlling the motors ''' - if not SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + if not SmartDashboard.getBoolean(f"{self._motor_name} Test Mode", False): if self._target_speed.power == 0.0 and self._target_speed.speed == 0.0: self._pid_controller.reset(0) self._motor.setVoltage(0) @@ -169,6 +169,9 @@ def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Test Mode", False) + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) From fe4cfc2386d8396ef7081bf8a67b14eb5d284cb5 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 23 Nov 2025 14:35:01 -0500 Subject: [PATCH 17/25] fix: Rename motor template configs to not be exclusive to the template motors --- src/FRC3484_Lib/SC_Datatypes.py | 84 ++++++++----------- .../motor_templates/angular_pos_motor.py | 8 +- .../motor_templates/power_motor.py | 6 +- .../motor_templates/velocity_motor.py | 14 ++-- 4 files changed, 49 insertions(+), 63 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index 350b74a..ed29a76 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -68,13 +68,46 @@ class SC_LinearFeedForwardConfig: @dataclass(frozen=True) class SC_MotorConfig: can_id: int - inverted: bool = False can_bus_name: str = "rio" + + motor_name: str = "Motor" # Should be changed. Used for Smart Dashboard diagnostics + motor_type: str = "falcon" # falcon or minion + inverted: bool = False + neutral_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST current_limit_enabled: bool = True current_threshold: amperes = 50 current_time: seconds = 0.1 current_limit: amperes = 20 + stall_limit: float = 0.75 + +@dataclass(frozen=True) +class SC_CurrentConfig: + drive_current_threshold: amperes = 60 + drive_current_time: seconds = 0.1 + drive_current_limit: amperes = 35 + drive_open_loop_ramp: seconds = 0.25 + + steer_current_threshold: amperes = 40 + steer_current_time: seconds = 0.1 + steer_current_limit: amperes = 25 + + current_limit_enabled: bool = True + +@dataclass(frozen=True) +class SC_VelocityControl: + speed: revolutions_per_minute + power: float + +@dataclass +class SC_PositionControl: + speed: float + position: inches + +@dataclass(frozen=True) +class SC_TrapezoidConfig: + max_velocity: feet_per_second | degrees_per_second + max_acceleration: feet_per_second_squared | degrees_per_second_squared ''' Swerve Drive Datatypes @@ -138,51 +171,4 @@ class SC_CameraConfig: class SC_CameraResults: vision_measurement: Pose2d timestamp: seconds - standard_deviation: tuple[float, float, float] - -''' -Motor Template Classes Datatypes -''' -@dataclass(frozen=True) -class SC_TemplateMotorConfig: - motor_name: str = "Motor" # Should be changed. Used for Smart Dashboard diagnostics - can_id: int - inverted: bool = False - can_bus_name: str = "rio" - - current_limit_enabled: bool = True - current_threshold: amperes = 50 - current_time: seconds = 0.1 - current_limit: amperes = 20 - stall_limit: float = 0.75 - - neutral_mode: NeutralModeValue = NeutralModeValue.BRAKE # BRAKE or COAST - motor_type: str = "falcon" # falcon or minion - -@dataclass(frozen=True) -class SC_TemplateMotorCurrentConfig: - drive_current_threshold: amperes = 60 - drive_current_time: seconds = 0.1 - drive_current_limit: amperes = 35 - drive_open_loop_ramp: seconds = 0.25 - - steer_current_threshold: amperes = 40 - steer_current_time: seconds = 0.1 - steer_current_limit: amperes = 25 - - current_limit_enabled: bool = True - -@dataclass(frozen=True) -class SC_TemplateMotorVelocityControl: - power: float - speed: revolutions_per_minute - -@dataclass -class SC_TemplateMotorPositionControl: - position: float - speed: float - -@dataclass(frozen=True) -class SC_TemplateMotorTrapezoidConfig: - max_velocity: feet_per_second | degrees_per_second - max_acceleration: feet_per_second_squared | degrees_per_second_squared + standard_deviation: tuple[float, float, float] \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 7c657ad..21950e8 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -11,7 +11,7 @@ from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue -from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorTrapezoidConfig +from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig class State(Enum): POWER = 0 @@ -33,11 +33,11 @@ class AngularPositionMotor(Subsystem): ''' def __init__( self, - motor_config: SC_TemplateMotorConfig, - current_config: SC_TemplateMotorCurrentConfig, + motor_config: SC_MotorConfig, + current_config: SC_CurrentConfig, pid_config: SC_PIDConfig, feed_forward_config: SC_LinearFeedForwardConfig, - trapezoid_config: SC_TemplateMotorTrapezoidConfig, + trapezoid_config: SC_TrapezoidConfig, angle_tolerance: degrees, gear_ratio: float = 1.0, external_encoder: CANcoder | None = None diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index 72198fa..43364ee 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -6,7 +6,7 @@ from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue from wpilib import SmartDashboard -from src.FRC3484_Lib.SC_Datatypes import SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig +from src.FRC3484_Lib.SC_Datatypes import SC_MotorConfig, SC_CurrentConfig class PowerMotor(Subsystem): ''' @@ -19,8 +19,8 @@ class PowerMotor(Subsystem): ''' def __init__( self, - motor_config: SC_TemplateMotorConfig, - current_config: SC_TemplateMotorCurrentConfig + motor_config: SC_MotorConfig, + current_config: SC_CurrentConfig ) -> None: super().__init__() diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 9cb2811..135c8e5 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -10,7 +10,7 @@ from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters from wpimath.units import volts -from FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_TemplateMotorConfig, SC_TemplateMotorCurrentConfig, SC_TemplateMotorVelocityControl +from FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_VelocityControl class VelocityMotor(Subsystem): @@ -26,8 +26,8 @@ class VelocityMotor(Subsystem): ''' def __init__( self, - motor_config: SC_TemplateMotorConfig, - current_config: SC_TemplateMotorCurrentConfig, + motor_config: SC_MotorConfig, + current_config: SC_CurrentConfig, pid_config: SC_PIDConfig, feed_forward_config: SC_LinearFeedForwardConfig, gear_ratio: float, @@ -40,7 +40,7 @@ def __init__( self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) - self._target_speed: SC_TemplateMotorVelocityControl = SC_TemplateMotorVelocityControl(0.0, 0.0) + self._target_speed: SC_VelocityControl = SC_VelocityControl(0.0, 0.0) self._tolerance: float = tolerance self._gear_ratio: float = gear_ratio @@ -95,14 +95,14 @@ def periodic(self) -> None: self.print_diagnostics() - def set_speed(self, speed: SC_TemplateMotorVelocityControl) -> None: + def set_speed(self, speed: SC_VelocityControl) -> None: ''' Sets the target speed for the motor Parameters: - speed (SC_TemplateMotorVelocityControl): The speed and power to set the motor to ''' - self._target_speed = SC_TemplateMotorVelocityControl(speed.speed * self._gear_ratio, speed.power) + self._target_speed = SC_VelocityControl(speed.speed * self._gear_ratio, speed.power) def at_target_speed(self) -> bool: ''' @@ -142,7 +142,7 @@ def set_power(self, power: float) -> None: - power (float): The power to set the motor to ''' # TODO: Have a boolean for testing mode to disable PID and feed forward - self._target_speed = SC_TemplateMotorVelocityControl(0.0, power) + self._target_speed = SC_VelocityControl(0.0, power) def get_stall_percentage(self) -> float: ''' From e4bfabb24310de8356eddec12f8bc78f113a5133 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 23 Nov 2025 14:38:55 -0500 Subject: [PATCH 18/25] fix: Create a new state instead of trying to set the existing one --- src/FRC3484_Lib/motor_templates/angular_pos_motor.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 21950e8..b87dc44 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -164,13 +164,8 @@ def set_target_angle(self, angle: degrees) -> None: self._state = State.POSITION if angle != self._target_state.position: - # TODO: I can't get the linter to be happy with these, because .position - # and .velocity are a @property and not a variable. Does this matter? - self._target_state.position = angle - self._target_state.velocity = 0 - - self._initial_state.position = self.get_angle() - self._initial_state.velocity = self.get_velocity() + self._target_state = TrapezoidProfile.State(angle, 0) + self._initial_state = TrapezoidProfile.State(self.get_angle(), self.get_velocity()) self._trapezoid_timer.reset() From 26f9fffc1a137831448649a728184de4c49ea4d4 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 23 Nov 2025 14:44:21 -0500 Subject: [PATCH 19/25] fix: Remove test mode, and fix smart dashboard diagnostics logic --- .../motor_templates/angular_pos_motor.py | 22 +++++++++---------- .../motor_templates/power_motor.py | 14 ++++++------ .../motor_templates/velocity_motor.py | 19 ++++++++-------- 3 files changed, 26 insertions(+), 29 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index b87dc44..69bfa25 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -98,6 +98,7 @@ def __init__( _ = self._motor.configurator.apply(self._motor_config) self._trapezoid_timer.start() + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) @override def periodic(self) -> None: @@ -107,15 +108,16 @@ def periodic(self) -> None: # TODO: Should this check for a home position so it can know to # stop powering even if we're not using a homing state? # TODO: What was the encoder going to be used for? - if self._state == State.POSITION and not SmartDashboard.getBoolean(f"{self._motor_name} Test Mode", False): + if self._state == State.POSITION: current_state = self._trapezoid.calculate(self._trapezoid_timer.get(), self._initial_state, self._target_state) feed_forward = self._feed_forward_controller.calculate(self._previous_velocity, current_state.velocity) pid = self._pid_controller.calculate(self.get_angle(), current_state.position) self._motor.setVoltage(pid + feed_forward) self._previous_velocity = current_state.velocity - - self.print_diagnostics() + + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", defaultValue=False): + self.print_diagnostics() def at_target_angle(self) -> bool: ''' @@ -208,12 +210,8 @@ def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' - _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Test Mode", False) - - if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): - _ = SmartDashboard.putNumber(f"{self._motor_name} Angle (degrees)", self.get_angle()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) \ No newline at end of file + _ = SmartDashboard.putNumber(f"{self._motor_name} Angle (degrees)", self.get_angle()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index 43364ee..2f94760 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -58,11 +58,14 @@ def __init__( _ = self._motor.configurator.apply(self._motor_config) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) + def periodic(self) -> None: ''' Handles printing diagnostic information to Smart Dashboard ''' - self.print_diagnostics() + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + self.print_diagnostics() def set_power(self, power: float) -> None: ''' @@ -112,9 +115,6 @@ def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' - _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) - - if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): - _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) - _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) \ No newline at end of file + _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 135c8e5..e094d7c 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -75,6 +75,8 @@ def __init__( _ = self._motor.configurator.apply(self._motor_config) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) + @override def periodic(self) -> None: ''' @@ -93,7 +95,8 @@ def periodic(self) -> None: feed_forward: volts = self._feed_forward_controller.calculate(self._motor.get_velocity().value, self._target_speed.speed) self._motor.setVoltage(pid + feed_forward) - self.print_diagnostics() + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + self.print_diagnostics() def set_speed(self, speed: SC_VelocityControl) -> None: ''' @@ -169,14 +172,10 @@ def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' - _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Test Mode", False) - - if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): - _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) - _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) - _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) + _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) \ No newline at end of file From e2c7736b2b37ebb81cd59f1c5fe248c3063f2eec Mon Sep 17 00:00:00 2001 From: nfoert Date: Sun, 23 Nov 2025 14:54:22 -0500 Subject: [PATCH 20/25] feat: If an external encoder is provided, assign it to the motor --- .../motor_templates/angular_pos_motor.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 69bfa25..5d9bae1 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -8,8 +8,8 @@ from wpimath.trajectory import TrapezoidProfile from phoenix6.hardware import TalonFX, TalonFXS, CANcoder -from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration -from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue +from phoenix6.configs import CurrentLimitsConfigs, ExternalFeedbackConfigs, FeedbackConfigs, TalonFXConfiguration, TalonFXSConfiguration +from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue, InvertedValue, MotorArrangementValue, NeutralModeValue from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig @@ -78,10 +78,20 @@ def __init__( self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST + if self._encoder is not None: + self._motor_config.external_feedback = ExternalFeedbackConfigs() \ + .with_feedback_remote_sensor_id(self._encoder.device_id) \ + .with_external_feedback_sensor_source(ExternalFeedbackSensorSourceValue.REMOTE_CANCODER) + elif motor_config.motor_type == "falcon": self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) self._motor_config = TalonFXConfiguration() + + if self._encoder is not None: + self._motor_config.feedback = FeedbackConfigs() \ + .with_feedback_remote_sensor_id(self._encoder.device_id) \ + .with_feedback_sensor_source(FeedbackSensorSourceValue.REMOTE_CANCODER) else: raise ValueError(f"Invalid motor type: {motor_config.motor_type}") @@ -115,7 +125,7 @@ def periodic(self) -> None: self._motor.setVoltage(pid + feed_forward) self._previous_velocity = current_state.velocity - + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", defaultValue=False): self.print_diagnostics() From 73482b7dc548cdb0b5855bdb826eceb49ba83538 Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 24 Nov 2025 19:22:54 -0500 Subject: [PATCH 21/25] fix: Fix stall current calculation --- src/FRC3484_Lib/SC_Datatypes.py | 1 - .../motor_templates/angular_pos_motor.py | 13 ++++++------- src/FRC3484_Lib/motor_templates/power_motor.py | 10 ++++++---- src/FRC3484_Lib/motor_templates/velocity_motor.py | 10 ++++++---- 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index ed29a76..14aca56 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -79,7 +79,6 @@ class SC_MotorConfig: current_threshold: amperes = 50 current_time: seconds = 0.1 current_limit: amperes = 20 - stall_limit: float = 0.75 @dataclass(frozen=True) class SC_CurrentConfig: diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 5d9bae1..3b4554a 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -31,6 +31,9 @@ class AngularPositionMotor(Subsystem): gear_ratio: float = 1.0 external_encoder: CANcoder | None = None ''' + STALL_LIMIT: float = 0.75 + STALL_THRESHOLD: float = 0.1 + def __init__( self, motor_config: SC_MotorConfig, @@ -52,7 +55,6 @@ def __init__( self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) self._motor_name: str = motor_config.motor_name - self._stall_limit: float = motor_config.stall_limit self._trapezoid: TrapezoidProfile = TrapezoidProfile(TrapezoidProfile.Constraints(trapezoid_config.max_velocity, trapezoid_config.max_acceleration)) self._trapezoid_timer: Timer = Timer() @@ -115,9 +117,6 @@ def periodic(self) -> None: ''' Handles controlling the motors in position mode and printing diagnostics ''' - # TODO: Should this check for a home position so it can know to - # stop powering even if we're not using a homing state? - # TODO: What was the encoder going to be used for? if self._state == State.POSITION: current_state = self._trapezoid.calculate(self._trapezoid_timer.get(), self._initial_state, self._target_state) feed_forward = self._feed_forward_controller.calculate(self._previous_velocity, current_state.velocity) @@ -202,8 +201,8 @@ def get_stall_percentage(self) -> float: Returns: - float: The percentage of stall current being drawn by the motor ''' - if abs(self._motor.get()) > self._stall_limit: - return (self._motor.get_supply_current().value / self._motor.get_motor_stall_current().value * abs(self._motor.get())) + if abs(self._motor.get()) > self.STALL_THRESHOLD: + return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) else: return 0 @@ -214,7 +213,7 @@ def get_stalled(self) -> bool: Returns: - bool: True if the motor is stalled, False otherwise ''' - return self.get_stall_percentage() > self._stall_limit + return self.get_stall_percentage() > self.STALL_LIMIT def print_diagnostics(self) -> None: ''' diff --git a/src/FRC3484_Lib/motor_templates/power_motor.py b/src/FRC3484_Lib/motor_templates/power_motor.py index 2f94760..effd53b 100644 --- a/src/FRC3484_Lib/motor_templates/power_motor.py +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -17,6 +17,9 @@ class PowerMotor(Subsystem): - motor_config (SC_MotorConfig): The configuration for the motor - current_config (SC_TemplateMotorCurrentConfig): Current limit settings for the motor ''' + STALL_LIMIT: float = 0.75 + STALL_THRESHOLD: float = 0.1 + def __init__( self, motor_config: SC_MotorConfig, @@ -28,7 +31,6 @@ def __init__( self._motor_config: TalonFXConfiguration | TalonFXSConfiguration self._motor_name: str = motor_config.motor_name - self.stall_limit: float = motor_config.stall_limit # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation # There is no communtation for the falcon, so use a talon FX controller instead @@ -97,8 +99,8 @@ def get_stall_percentage(self) -> float: Returns: - float: The percentage of stall current being drawn by the motor ''' - if abs(self._motor.get()) > self.stall_limit: - return (self._motor.get_supply_current().value / self._motor.get_motor_stall_current().value * abs(self._motor.get())) + if abs(self._motor.get()) > self.STALL_THRESHOLD: + return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) else: return 0 @@ -109,7 +111,7 @@ def get_stalled(self) -> bool: Returns: - bool: True if the motor is stalled, False otherwise ''' - return self.get_stall_percentage() > self.stall_limit + return self.get_stall_percentage() > self.STALL_LIMIT def print_diagnostics(self) -> None: ''' diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index e094d7c..7ba1eb0 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -24,6 +24,9 @@ class VelocityMotor(Subsystem): - gear_ratio (float): The gear ratio of the motor - tolerance (float): The tolerance for the target speed to consider it reached ''' + STALL_LIMIT: float = 0.75 + STALL_THRESHOLD: float = 0.1 + def __init__( self, motor_config: SC_MotorConfig, @@ -45,7 +48,6 @@ def __init__( self._tolerance: float = tolerance self._gear_ratio: float = gear_ratio self._motor_name: str = motor_config.motor_name - self.stall_limit: float = motor_config.stall_limit # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation # There is no communtation for the falcon, so use a talon FX controller instead @@ -154,8 +156,8 @@ def get_stall_percentage(self) -> float: Returns: - float: The percentage of stall current being drawn by the motor ''' - if abs(self._motor.get()) > self.stall_limit: - return (self._motor.get_supply_current().value / self._motor.get_motor_stall_current().value * abs(self._motor.get())) + if abs(self._motor.get()) > self.STALL_THRESHOLD: + return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) else: return 0 @@ -166,7 +168,7 @@ def get_stalled(self) -> bool: Returns: - bool: True if the motor is stalled, False otherwise ''' - return self.get_stall_percentage() > self.stall_limit + return self.get_stall_percentage() > self.STALL_LIMIT def print_diagnostics(self) -> None: ''' From c4baff7d6dd4cf42f2934f059928b3361b248b2c Mon Sep 17 00:00:00 2001 From: nfoert Date: Mon, 24 Nov 2025 19:36:34 -0500 Subject: [PATCH 22/25] feat: Implement LinearPositionMotor --- .../motor_templates/angular_pos_motor.py | 6 +- .../motor_templates/linear_pos_motor.py | 229 +++++++++++++++++- 2 files changed, 229 insertions(+), 6 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 3b4554a..250d2f0 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -11,7 +11,7 @@ from phoenix6.configs import CurrentLimitsConfigs, ExternalFeedbackConfigs, FeedbackConfigs, TalonFXConfiguration, TalonFXSConfiguration from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue, InvertedValue, MotorArrangementValue, NeutralModeValue -from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig +from src.FRC3484_Lib.SC_Datatypes import SC_AngularFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig class State(Enum): POWER = 0 @@ -23,7 +23,7 @@ class AngularPositionMotor(Subsystem): Parameters: motor_config: SC_TemplateMotorConfig - current_config: SC_TemplateMotorCurrentConfig + current_config: SC_AngularFeedForwardConfig pid_config: SC_PIDConfig feed_forward_config: SC_LinearFeedForwardConfig trapezoid_config: SC_TemplateMotorTrapezoidConfig @@ -39,7 +39,7 @@ def __init__( motor_config: SC_MotorConfig, current_config: SC_CurrentConfig, pid_config: SC_PIDConfig, - feed_forward_config: SC_LinearFeedForwardConfig, + feed_forward_config: SC_AngularFeedForwardConfig, trapezoid_config: SC_TrapezoidConfig, angle_tolerance: degrees, gear_ratio: float = 1.0, diff --git a/src/FRC3484_Lib/motor_templates/linear_pos_motor.py b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py index 5126572..216cb35 100644 --- a/src/FRC3484_Lib/motor_templates/linear_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py @@ -1,3 +1,226 @@ -class LinearPositionMotor: - def __init__(self) -> None: - pass \ No newline at end of file +from enum import Enum +from typing import override + +from commands2 import Subsystem +from wpilib import Timer, SmartDashboard +from wpimath.units import degrees, degrees_per_second, feet, feet_per_second +from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters +from wpimath.trajectory import TrapezoidProfile + +from phoenix6.hardware import TalonFX, TalonFXS, CANcoder +from phoenix6.configs import CurrentLimitsConfigs, ExternalFeedbackConfigs, FeedbackConfigs, TalonFXConfiguration, TalonFXSConfiguration +from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue, InvertedValue, MotorArrangementValue, NeutralModeValue + +from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig + +class State(Enum): + POWER = 0 + POSITION = 1 + +class LinearPositionMotor(Subsystem): + ''' + Defines a base motor class for angular position control + + Parameters: + motor_config: SC_TemplateMotorConfig + current_config: SC_TemplateMotorCurrentConfig + pid_config: SC_PIDConfig + feed_forward_config: SC_LinearFeedForwardConfig + trapezoid_config: SC_TemplateMotorTrapezoidConfig + position_tolerance: feet + gear_ratio: float = 1.0 + external_encoder: CANcoder | None = None + ''' + STALL_LIMIT: float = 0.75 + STALL_THRESHOLD: float = 0.1 + + def __init__( + self, + motor_config: SC_MotorConfig, + current_config: SC_CurrentConfig, + pid_config: SC_PIDConfig, + feed_forward_config: SC_LinearFeedForwardConfig, + trapezoid_config: SC_TrapezoidConfig, + position_tolerance: feet, + gear_ratio: float = 1.0, + external_encoder: CANcoder | None = None + ) -> None: + super().__init__() + + # Set up variables + self._state: State = State.POWER + + self._motor: TalonFX | TalonFXS + self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) + self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) + self._motor_name: str = motor_config.motor_name + + self._trapezoid: TrapezoidProfile = TrapezoidProfile(TrapezoidProfile.Constraints(trapezoid_config.max_velocity, trapezoid_config.max_acceleration)) + self._trapezoid_timer: Timer = Timer() + + self._encoder: CANcoder | None = external_encoder + + self._initial_state: TrapezoidProfile.State = TrapezoidProfile.State(0.0, 0.0) + self._target_state: TrapezoidProfile.State = TrapezoidProfile.State(0.0, 0.0) + + self._previous_velocity: feet_per_second = 0 + + self._gear_ratio: float = gear_ratio + self._position_tolerance: feet = position_tolerance + + # Set up motor + + # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation + # There is no communtation for the falcon, so use a talon FX controller instead + if motor_config.motor_type == "minion": + self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXSConfiguration() + + self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST + + if self._encoder is not None: + self._motor_config.external_feedback = ExternalFeedbackConfigs() \ + .with_feedback_remote_sensor_id(self._encoder.device_id) \ + .with_external_feedback_sensor_source(ExternalFeedbackSensorSourceValue.REMOTE_CANCODER) + + elif motor_config.motor_type == "falcon": + self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) + + self._motor_config = TalonFXConfiguration() + + if self._encoder is not None: + self._motor_config.feedback = FeedbackConfigs() \ + .with_feedback_remote_sensor_id(self._encoder.device_id) \ + .with_feedback_sensor_source(FeedbackSensorSourceValue.REMOTE_CANCODER) + else: + raise ValueError(f"Invalid motor type: {motor_config.motor_type}") + + self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) + + self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode + + self._motor_config.current_limits = CurrentLimitsConfigs() \ + .with_supply_current_limit_enable(current_config.current_limit_enabled) \ + .with_supply_current_limit(current_config.drive_current_limit) \ + .with_supply_current_lower_limit(current_config.drive_current_threshold) \ + .with_supply_current_lower_time(current_config.drive_current_time) + + _ = self._motor.configurator.apply(self._motor_config) + + self._trapezoid_timer.start() + _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) + + @override + def periodic(self) -> None: + ''' + Handles controlling the motors in position mode and printing diagnostics + ''' + if self._state == State.POSITION: + current_state = self._trapezoid.calculate(self._trapezoid_timer.get(), self._initial_state, self._target_state) + feed_forward = self._feed_forward_controller.calculate(self._previous_velocity, current_state.velocity) + pid = self._pid_controller.calculate(self.get_position(), current_state.position) + + self._motor.setVoltage(pid + feed_forward) + self._previous_velocity = current_state.velocity + + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", defaultValue=False): + self.print_diagnostics() + + def at_target_angle(self) -> bool: + ''' + Returns whether the motor is at the target angle or not + + Returns: + - bool: True if the motor is at the target angle, False otherwise + ''' + return abs(self._target_state.position - self.get_position()) < self._position_tolerance + + def get_position(self) -> degrees: + ''' + Returns the current position of the motor + + Returns: + - degrees: The current angle of the motor + ''' + return self._motor.get_position().value / self._gear_ratio + + def get_velocity(self) -> degrees_per_second: + ''' + Returns the current velocity of the motor + + Returns: + - degrees_per_second: The current velocity of the motor + ''' + return self._motor.get_velocity().value / self._gear_ratio + + def set_power(self, power: float) -> None: + ''' + Sets the power of the motor + + Parameters: + - power (float): The power to set the motor to + ''' + self._state = State.POWER + self._motor.set(power) + + def set_target_position(self, position: feet) -> None: + ''' + Sets the target position of the motor + + Parameters: + - position (feet): The angle to set the motor to + ''' + self._state = State.POSITION + + if position != self._target_state.position: + self._target_state = TrapezoidProfile.State(position, 0) + self._initial_state = TrapezoidProfile.State(self.get_position(), self.get_velocity()) + + self._trapezoid_timer.reset() + + def set_brake_mode(self) -> None: + ''' + Sets the motor to brake mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE + _ = self._motor.configurator.apply(self._motor_config) + + def set_coast_mode(self) -> None: + ''' + Sets the motor to coast mode + ''' + self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST + _ = self._motor.configurator.apply(self._motor_config) + + def get_stall_percentage(self) -> float: + ''' + Returns the percentage of stall current being drawn by the motor + + Returns: + - float: The percentage of stall current being drawn by the motor + ''' + if abs(self._motor.get()) > self.STALL_THRESHOLD: + return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) + else: + return 0 + + def get_stalled(self) -> bool: + ''' + Returns whether the motor is stalled or not + + Returns: + - bool: True if the motor is stalled, False otherwise + ''' + return self.get_stall_percentage() > self.STALL_LIMIT + + def print_diagnostics(self) -> None: + ''' + Prints diagnostic information to Smart Dashboard + ''' + _ = SmartDashboard.putNumber(f"{self._motor_name} Position (feet)", self.get_position()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) + _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) \ No newline at end of file From 18b0e2775052e80e4b7a0e3def329305b8a140b1 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 29 Nov 2025 10:45:25 -0500 Subject: [PATCH 23/25] feat: Make VelocityMotor inherit from PowerMotor --- .../motor_templates/velocity_motor.py | 87 ++----------------- 1 file changed, 7 insertions(+), 80 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 7ba1eb0..008bdd5 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -1,19 +1,14 @@ from typing import override from wpilib import SmartDashboard -from commands2.subsystem import Subsystem - -from phoenix6.configs import CurrentLimitsConfigs, TalonFXConfiguration, TalonFXSConfiguration -from phoenix6.hardware import TalonFX, TalonFXS -from phoenix6.signals import InvertedValue, MotorArrangementValue, NeutralModeValue -from phoenix6.controls import DutyCycleOut, VelocityVoltage from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters from wpimath.units import volts from FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_VelocityControl +from FRC3484_Lib.motor_templates.power_motor import PowerMotor -class VelocityMotor(Subsystem): +class VelocityMotor(PowerMotor): ''' Creates a motor template class that represents a motor that can be set to a target speed @@ -36,10 +31,8 @@ def __init__( gear_ratio: float, tolerance: float ) -> None: - super().__init__() + super().__init__(motor_config, current_config) - self._motor: TalonFX | TalonFXS - self._motor_config: TalonFXConfiguration | TalonFXSConfiguration self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) @@ -49,36 +42,6 @@ def __init__( self._gear_ratio: float = gear_ratio self._motor_name: str = motor_config.motor_name - # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation - # There is no communtation for the falcon, so use a talon FX controller instead - if motor_config.motor_type == "minion": - self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) - - self._motor_config = TalonFXSConfiguration() - - self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST - - elif motor_config.motor_type == "falcon": - self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) - - self._motor_config = TalonFXConfiguration() - else: - raise ValueError(f"Invalid motor type: {motor_config.motor_type}") - - self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) - - self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode - - self._motor_config.current_limits = CurrentLimitsConfigs() \ - .with_supply_current_limit_enable(current_config.current_limit_enabled) \ - .with_supply_current_limit(current_config.drive_current_limit) \ - .with_supply_current_lower_limit(current_config.drive_current_threshold) \ - .with_supply_current_lower_time(current_config.drive_current_time) - - _ = self._motor.configurator.apply(self._motor_config) - - _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) - @override def periodic(self) -> None: ''' @@ -125,20 +88,7 @@ def at_target_speed(self) -> bool: # Convert RPS to RPM, then subtract the target speed and compare to the tolerance return abs((self._motor.get_velocity().value * 60) - self._target_speed.speed) < self._tolerance - def set_brake_mode(self) -> None: - ''' - Sets the motor to brake mode - ''' - self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE - _ = self._motor.configurator.apply(self._motor_config) - - def set_coast_mode(self) -> None: - ''' - Sets the motor to coast mode - ''' - self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST - _ = self._motor.configurator.apply(self._motor_config) - + @override def set_power(self, power: float) -> None: ''' Sets the power of the motor for testing purposes @@ -147,37 +97,14 @@ def set_power(self, power: float) -> None: - power (float): The power to set the motor to ''' # TODO: Have a boolean for testing mode to disable PID and feed forward + # TODO: Should this really override the set_speed method from PowerMotor? self._target_speed = SC_VelocityControl(0.0, power) - - def get_stall_percentage(self) -> float: - ''' - Returns the percentage of stall current being drawn by the motor - - Returns: - - float: The percentage of stall current being drawn by the motor - ''' - if abs(self._motor.get()) > self.STALL_THRESHOLD: - return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) - else: - return 0 - - def get_stalled(self) -> bool: - ''' - Returns whether the motor is stalled or not - - Returns: - - bool: True if the motor is stalled, False otherwise - ''' - return self.get_stall_percentage() > self.STALL_LIMIT + @override def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' _ = SmartDashboard.putNumber(f"{self._motor_name} Speed (RPM)", self._motor.get_velocity().value * 60) _ = SmartDashboard.putNumber(f"{self._motor_name} At Target RPM", self.at_target_speed()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Power (%)", self._motor.get() * 100) - _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) - - \ No newline at end of file + super().print_diagnostics() \ No newline at end of file From 075e1de85f04b5f1191cfb6bfb3d8c304a9a5521 Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 29 Nov 2025 10:51:34 -0500 Subject: [PATCH 24/25] feat: AngularPositionMotor now inherits from PowerMotor --- .../motor_templates/angular_pos_motor.py | 77 +++---------------- .../motor_templates/velocity_motor.py | 4 +- 2 files changed, 12 insertions(+), 69 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 250d2f0..0f7762c 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -1,23 +1,23 @@ from enum import Enum from typing import override -from commands2 import Subsystem from wpilib import Timer, SmartDashboard from wpimath.units import degrees, degrees_per_second from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters from wpimath.trajectory import TrapezoidProfile -from phoenix6.hardware import TalonFX, TalonFXS, CANcoder -from phoenix6.configs import CurrentLimitsConfigs, ExternalFeedbackConfigs, FeedbackConfigs, TalonFXConfiguration, TalonFXSConfiguration -from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue, InvertedValue, MotorArrangementValue, NeutralModeValue +from phoenix6.hardware import CANcoder +from phoenix6.configs import ExternalFeedbackConfigs, FeedbackConfigs +from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue +from src.FRC3484_Lib.motor_templates.power_motor import PowerMotor from src.FRC3484_Lib.SC_Datatypes import SC_AngularFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig class State(Enum): POWER = 0 POSITION = 1 -class AngularPositionMotor(Subsystem): +class AngularPositionMotor(PowerMotor): ''' Defines a base motor class for angular position control @@ -45,13 +45,11 @@ def __init__( gear_ratio: float = 1.0, external_encoder: CANcoder | None = None ) -> None: - super().__init__() + super().__init__(motor_config, current_config) # Set up variables self._state: State = State.POWER - self._motor: TalonFX | TalonFXS - self._motor_config: TalonFXConfiguration | TalonFXSConfiguration self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) self._motor_name: str = motor_config.motor_name @@ -73,23 +71,14 @@ def __init__( # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation # There is no communtation for the falcon, so use a talon FX controller instead + # The portion for the external encoder is here, but the rest of the configuration is in the PowerMotor class if motor_config.motor_type == "minion": - self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) - - self._motor_config = TalonFXSConfiguration() - - self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST - if self._encoder is not None: self._motor_config.external_feedback = ExternalFeedbackConfigs() \ .with_feedback_remote_sensor_id(self._encoder.device_id) \ .with_external_feedback_sensor_source(ExternalFeedbackSensorSourceValue.REMOTE_CANCODER) elif motor_config.motor_type == "falcon": - self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) - - self._motor_config = TalonFXConfiguration() - if self._encoder is not None: self._motor_config.feedback = FeedbackConfigs() \ .with_feedback_remote_sensor_id(self._encoder.device_id) \ @@ -97,20 +86,9 @@ def __init__( else: raise ValueError(f"Invalid motor type: {motor_config.motor_type}") - self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) - - self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode - - self._motor_config.current_limits = CurrentLimitsConfigs() \ - .with_supply_current_limit_enable(current_config.current_limit_enabled) \ - .with_supply_current_limit(current_config.drive_current_limit) \ - .with_supply_current_lower_limit(current_config.drive_current_threshold) \ - .with_supply_current_lower_time(current_config.drive_current_time) - _ = self._motor.configurator.apply(self._motor_config) self._trapezoid_timer.start() - _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) @override def periodic(self) -> None: @@ -180,47 +158,12 @@ def set_target_angle(self, angle: degrees) -> None: self._trapezoid_timer.reset() - def set_brake_mode(self) -> None: - ''' - Sets the motor to brake mode - ''' - self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE - _ = self._motor.configurator.apply(self._motor_config) - - def set_coast_mode(self) -> None: - ''' - Sets the motor to coast mode - ''' - self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST - _ = self._motor.configurator.apply(self._motor_config) - - def get_stall_percentage(self) -> float: - ''' - Returns the percentage of stall current being drawn by the motor - - Returns: - - float: The percentage of stall current being drawn by the motor - ''' - if abs(self._motor.get()) > self.STALL_THRESHOLD: - return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) - else: - return 0 - - def get_stalled(self) -> bool: - ''' - Returns whether the motor is stalled or not - - Returns: - - bool: True if the motor is stalled, False otherwise - ''' - return self.get_stall_percentage() > self.STALL_LIMIT - + @override def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' _ = SmartDashboard.putNumber(f"{self._motor_name} Angle (degrees)", self.get_angle()) _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) \ No newline at end of file + _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) + super().print_diagnostics() \ No newline at end of file diff --git a/src/FRC3484_Lib/motor_templates/velocity_motor.py b/src/FRC3484_Lib/motor_templates/velocity_motor.py index 008bdd5..8d6393a 100644 --- a/src/FRC3484_Lib/motor_templates/velocity_motor.py +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -4,8 +4,8 @@ from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters from wpimath.units import volts -from FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_VelocityControl -from FRC3484_Lib.motor_templates.power_motor import PowerMotor +from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_VelocityControl +from src.FRC3484_Lib.motor_templates.power_motor import PowerMotor class VelocityMotor(PowerMotor): From 660a1830f6c12e28ef7cde202203e1e6cb2366cb Mon Sep 17 00:00:00 2001 From: nfoert Date: Sat, 29 Nov 2025 11:16:11 -0500 Subject: [PATCH 25/25] feat: LinearPositionMotor inherits from AngularPositionMotor --- .../motor_templates/angular_pos_motor.py | 8 +- .../motor_templates/linear_pos_motor.py | 191 ++++-------------- 2 files changed, 38 insertions(+), 161 deletions(-) diff --git a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py index 0f7762c..c231df8 100644 --- a/src/FRC3484_Lib/motor_templates/angular_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -11,7 +11,7 @@ from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue from src.FRC3484_Lib.motor_templates.power_motor import PowerMotor -from src.FRC3484_Lib.SC_Datatypes import SC_AngularFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig +from src.FRC3484_Lib.SC_Datatypes import SC_AngularFeedForwardConfig, SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig class State(Enum): POWER = 0 @@ -23,9 +23,9 @@ class AngularPositionMotor(PowerMotor): Parameters: motor_config: SC_TemplateMotorConfig - current_config: SC_AngularFeedForwardConfig + current_config: SC_CurrentConfig pid_config: SC_PIDConfig - feed_forward_config: SC_LinearFeedForwardConfig + feed_forward_config: SC_AngularFeedForwardConfig trapezoid_config: SC_TemplateMotorTrapezoidConfig angle_tolerance: degrees gear_ratio: float = 1.0 @@ -39,7 +39,7 @@ def __init__( motor_config: SC_MotorConfig, current_config: SC_CurrentConfig, pid_config: SC_PIDConfig, - feed_forward_config: SC_AngularFeedForwardConfig, + feed_forward_config: SC_AngularFeedForwardConfig | SC_LinearFeedForwardConfig, trapezoid_config: SC_TrapezoidConfig, angle_tolerance: degrees, gear_ratio: float = 1.0, diff --git a/src/FRC3484_Lib/motor_templates/linear_pos_motor.py b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py index 216cb35..e23c871 100644 --- a/src/FRC3484_Lib/motor_templates/linear_pos_motor.py +++ b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py @@ -1,23 +1,20 @@ from enum import Enum -from typing import override +from typing import final, override -from commands2 import Subsystem -from wpilib import Timer, SmartDashboard -from wpimath.units import degrees, degrees_per_second, feet, feet_per_second -from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters -from wpimath.trajectory import TrapezoidProfile +from wpilib import SmartDashboard +from wpimath.units import feet, feet_per_second -from phoenix6.hardware import TalonFX, TalonFXS, CANcoder -from phoenix6.configs import CurrentLimitsConfigs, ExternalFeedbackConfigs, FeedbackConfigs, TalonFXConfiguration, TalonFXSConfiguration -from phoenix6.signals import ExternalFeedbackSensorSourceValue, FeedbackSensorSourceValue, InvertedValue, MotorArrangementValue, NeutralModeValue +from phoenix6.hardware import CANcoder +from src.FRC3484_Lib.motor_templates.angular_pos_motor import AngularPositionMotor from src.FRC3484_Lib.SC_Datatypes import SC_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig class State(Enum): POWER = 0 POSITION = 1 -class LinearPositionMotor(Subsystem): +@final +class LinearPositionMotor(AngularPositionMotor): ''' Defines a base motor class for angular position control @@ -45,125 +42,46 @@ def __init__( gear_ratio: float = 1.0, external_encoder: CANcoder | None = None ) -> None: - super().__init__() + super().__init__( + motor_config=motor_config, + current_config=current_config, + pid_config=pid_config, + feed_forward_config=feed_forward_config, + trapezoid_config=trapezoid_config, + angle_tolerance=position_tolerance, + gear_ratio=gear_ratio, + external_encoder=external_encoder + ) - # Set up variables - self._state: State = State.POWER - - self._motor: TalonFX | TalonFXS - self._motor_config: TalonFXConfiguration | TalonFXSConfiguration - self._pid_controller: PIDController = PIDController(pid_config.Kp, pid_config.Ki, pid_config.Kd) - self._feed_forward_controller: SimpleMotorFeedforwardMeters = SimpleMotorFeedforwardMeters(feed_forward_config.S, feed_forward_config.V, feed_forward_config.A) - self._motor_name: str = motor_config.motor_name - - self._trapezoid: TrapezoidProfile = TrapezoidProfile(TrapezoidProfile.Constraints(trapezoid_config.max_velocity, trapezoid_config.max_acceleration)) - self._trapezoid_timer: Timer = Timer() - - self._encoder: CANcoder | None = external_encoder - - self._initial_state: TrapezoidProfile.State = TrapezoidProfile.State(0.0, 0.0) - self._target_state: TrapezoidProfile.State = TrapezoidProfile.State(0.0, 0.0) - - self._previous_velocity: feet_per_second = 0 - - self._gear_ratio: float = gear_ratio self._position_tolerance: feet = position_tolerance - # Set up motor - - # If the motor_type is minion, it needs a talon FXS controller to be able to set the correct commutation - # There is no communtation for the falcon, so use a talon FX controller instead - if motor_config.motor_type == "minion": - self._motor = TalonFXS(motor_config.can_id, motor_config.can_bus_name) - - self._motor_config = TalonFXSConfiguration() - - self._motor_config.commutation.motor_arrangement = MotorArrangementValue.MINION_JST - - if self._encoder is not None: - self._motor_config.external_feedback = ExternalFeedbackConfigs() \ - .with_feedback_remote_sensor_id(self._encoder.device_id) \ - .with_external_feedback_sensor_source(ExternalFeedbackSensorSourceValue.REMOTE_CANCODER) - - elif motor_config.motor_type == "falcon": - self._motor = TalonFX(motor_config.can_id, motor_config.can_bus_name) - - self._motor_config = TalonFXConfiguration() - - if self._encoder is not None: - self._motor_config.feedback = FeedbackConfigs() \ - .with_feedback_remote_sensor_id(self._encoder.device_id) \ - .with_feedback_sensor_source(FeedbackSensorSourceValue.REMOTE_CANCODER) - else: - raise ValueError(f"Invalid motor type: {motor_config.motor_type}") - - self._motor_config.motor_output.inverted = InvertedValue(motor_config.inverted) - - self._motor_config.motor_output.neutral_mode = motor_config.neutral_mode - - self._motor_config.current_limits = CurrentLimitsConfigs() \ - .with_supply_current_limit_enable(current_config.current_limit_enabled) \ - .with_supply_current_limit(current_config.drive_current_limit) \ - .with_supply_current_lower_limit(current_config.drive_current_threshold) \ - .with_supply_current_lower_time(current_config.drive_current_time) - - _ = self._motor.configurator.apply(self._motor_config) - - self._trapezoid_timer.start() - _ = SmartDashboard.putBoolean(f"{self._motor_name} Diagnostics", False) - - @override - def periodic(self) -> None: - ''' - Handles controlling the motors in position mode and printing diagnostics - ''' - if self._state == State.POSITION: - current_state = self._trapezoid.calculate(self._trapezoid_timer.get(), self._initial_state, self._target_state) - feed_forward = self._feed_forward_controller.calculate(self._previous_velocity, current_state.velocity) - pid = self._pid_controller.calculate(self.get_position(), current_state.position) - - self._motor.setVoltage(pid + feed_forward) - self._previous_velocity = current_state.velocity - - if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", defaultValue=False): - self.print_diagnostics() - - def at_target_angle(self) -> bool: + def at_target_position(self) -> bool: ''' - Returns whether the motor is at the target angle or not + Returns whether the motor is at the target position or not Returns: - - bool: True if the motor is at the target angle, False otherwise + - bool: True if the motor is at the target position, False otherwise ''' - return abs(self._target_state.position - self.get_position()) < self._position_tolerance + return abs(self._target_state.position - self.get_position() / self._gear_ratio) < self._position_tolerance - def get_position(self) -> degrees: + def get_position(self) -> feet: ''' Returns the current position of the motor Returns: - - degrees: The current angle of the motor + - feet: The current angle of the motor ''' - return self._motor.get_position().value / self._gear_ratio + return super().get_angle() / self._gear_ratio - def get_velocity(self) -> degrees_per_second: + @override + def get_velocity(self) -> feet_per_second: ''' Returns the current velocity of the motor - Returns: - - degrees_per_second: The current velocity of the motor - ''' - return self._motor.get_velocity().value / self._gear_ratio - - def set_power(self, power: float) -> None: - ''' - Sets the power of the motor - - Parameters: - - power (float): The power to set the motor to + Returns: + - feet_per_second: The current velocity of the motor ''' - self._state = State.POWER - self._motor.set(power) + return super().get_velocity() / self._gear_ratio def set_target_position(self, position: feet) -> None: ''' @@ -172,55 +90,14 @@ def set_target_position(self, position: feet) -> None: Parameters: - position (feet): The angle to set the motor to ''' - self._state = State.POSITION - - if position != self._target_state.position: - self._target_state = TrapezoidProfile.State(position, 0) - self._initial_state = TrapezoidProfile.State(self.get_position(), self.get_velocity()) - - self._trapezoid_timer.reset() - - def set_brake_mode(self) -> None: - ''' - Sets the motor to brake mode - ''' - self._motor_config.motor_output.neutral_mode = NeutralModeValue.BRAKE - _ = self._motor.configurator.apply(self._motor_config) - - def set_coast_mode(self) -> None: - ''' - Sets the motor to coast mode - ''' - self._motor_config.motor_output.neutral_mode = NeutralModeValue.COAST - _ = self._motor.configurator.apply(self._motor_config) - - def get_stall_percentage(self) -> float: - ''' - Returns the percentage of stall current being drawn by the motor - - Returns: - - float: The percentage of stall current being drawn by the motor - ''' - if abs(self._motor.get()) > self.STALL_THRESHOLD: - return (self._motor.get_supply_current().value / (self._motor.get_motor_stall_current().value * self._motor.get_supply_voltage().value / 12.0)) / abs(self._motor.get()) - else: - return 0 - - def get_stalled(self) -> bool: - ''' - Returns whether the motor is stalled or not - - Returns: - - bool: True if the motor is stalled, False otherwise - ''' - return self.get_stall_percentage() > self.STALL_LIMIT + super().set_target_angle(position * self._gear_ratio) + @override def print_diagnostics(self) -> None: ''' Prints diagnostic information to Smart Dashboard ''' _ = SmartDashboard.putNumber(f"{self._motor_name} Position (feet)", self.get_position()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity", self.get_velocity()) - _ = SmartDashboard.putNumber(f"{self._motor_name} Stall Percentage", self.get_stall_percentage()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} Stalled", self.get_stalled()) - _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Angle", self.at_target_angle()) \ No newline at end of file + _ = SmartDashboard.putNumber(f"{self._motor_name} Velocity (feet/s)", self.get_velocity()) + _ = SmartDashboard.putBoolean(f"{self._motor_name} At Target Position", self.at_target_position()) + return super().print_diagnostics() \ No newline at end of file