Motor Template Classes

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
 '''

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()

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()