diff --git a/src/FRC3484_Lib/SC_Datatypes.py b/src/FRC3484_Lib/SC_Datatypes.py index e567358..14aca56 100644 --- a/src/FRC3484_Lib/SC_Datatypes.py +++ b/src/FRC3484_Lib/SC_Datatypes.py @@ -1,3 +1,5 @@ +from phoenix6.signals import NeutralModeValue +from phoenix6.units import degree from wpimath.units import \ seconds, \ inches, \ @@ -10,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 @@ -62,14 +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 +@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 ''' 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..c231df8 --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/angular_pos_motor.py @@ -0,0 +1,169 @@ +from enum import Enum +from typing import override + +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 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_LinearFeedForwardConfig, SC_PIDConfig, SC_MotorConfig, SC_CurrentConfig, SC_TrapezoidConfig + +class State(Enum): + POWER = 0 + POSITION = 1 + +class AngularPositionMotor(PowerMotor): + ''' + Defines a base motor class for angular position control + + Parameters: + motor_config: SC_TemplateMotorConfig + current_config: SC_CurrentConfig + pid_config: SC_PIDConfig + feed_forward_config: SC_AngularFeedForwardConfig + trapezoid_config: SC_TemplateMotorTrapezoidConfig + angle_tolerance: degrees + 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_AngularFeedForwardConfig | SC_LinearFeedForwardConfig, + trapezoid_config: SC_TrapezoidConfig, + angle_tolerance: degrees, + gear_ratio: float = 1.0, + external_encoder: CANcoder | None = None + ) -> None: + super().__init__(motor_config, current_config) + + # Set up variables + self._state: State = State.POWER + + 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: 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 + # 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": + 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": + 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.configurator.apply(self._motor_config) + + self._trapezoid_timer.start() + + @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_angle(), 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_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: + self._target_state = TrapezoidProfile.State(angle, 0) + self._initial_state = TrapezoidProfile.State(self.get_angle(), self.get_velocity()) + + self._trapezoid_timer.reset() + + @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.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/linear_pos_motor.py b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py new file mode 100644 index 0000000..e23c871 --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/linear_pos_motor.py @@ -0,0 +1,103 @@ +from enum import Enum +from typing import final, override + +from wpilib import SmartDashboard +from wpimath.units import feet, feet_per_second + +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 + +@final +class LinearPositionMotor(AngularPositionMotor): + ''' + 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__( + 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 + ) + + self._position_tolerance: feet = position_tolerance + + def at_target_position(self) -> bool: + ''' + Returns whether the motor is at the target position or not + + Returns: + - bool: True if the motor is at the target position, False otherwise + ''' + return abs(self._target_state.position - self.get_position() / self._gear_ratio) < self._position_tolerance + + def get_position(self) -> feet: + ''' + Returns the current position of the motor + + Returns: + - feet: The current angle of the motor + ''' + return super().get_angle() / self._gear_ratio + + @override + def get_velocity(self) -> feet_per_second: + ''' + Returns the current velocity of the motor + + Returns: + - feet_per_second: The current velocity of the motor + ''' + return super().get_velocity() / self._gear_ratio + + 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 + ''' + 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 (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 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..effd53b --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/power_motor.py @@ -0,0 +1,122 @@ +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_MotorConfig, SC_CurrentConfig + +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 + + Parameters: + - 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, + current_config: SC_CurrentConfig + ) -> None: + super().__init__() + + self._motor: TalonFX | TalonFXS + self._motor_config: TalonFXConfiguration | TalonFXSConfiguration + + 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) + + def periodic(self) -> None: + ''' + Handles printing diagnostic information to Smart Dashboard + ''' + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + self.print_diagnostics() + + 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) + + 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} 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 new file mode 100644 index 0000000..8d6393a --- /dev/null +++ b/src/FRC3484_Lib/motor_templates/velocity_motor.py @@ -0,0 +1,110 @@ +from typing import override + +from wpilib import SmartDashboard +from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters +from wpimath.units import volts + +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): + ''' + 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 + ''' + 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, + gear_ratio: float, + tolerance: float + ) -> None: + super().__init__(motor_config, current_config) + + 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_VelocityControl = SC_VelocityControl(0.0, 0.0) + + self._tolerance: float = tolerance + self._gear_ratio: float = gear_ratio + self._motor_name: str = motor_config.motor_name + + @override + def periodic(self) -> None: + ''' + Handles Smart Dashboard diagnostic information and actually controlling the motors + ''' + 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) + + elif self._target_speed.power != 0.0: + self._motor.set(self._target_speed.power) + + else: + 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) + + if SmartDashboard.getBoolean(f"{self._motor_name} Diagnostics", False): + self.print_diagnostics() + + 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_VelocityControl(speed.speed * self._gear_ratio, speed.power) + + 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.power == 0.0 and self._target_speed.speed == 0.0: + return True + + 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.speed) < self._tolerance + + @override + 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 + # TODO: Should this really override the set_speed method from PowerMotor? + self._target_speed = SC_VelocityControl(0.0, power) + + @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()) + super().print_diagnostics() \ No newline at end of file