optimization: utilize math library for faster math (#111)

Author: martincarapia

pybot/autos.py

@@ -6,7 +6,7 @@
 DEFAULT_TRAJECTORY = 'outwayred'
 
 class FollowTrajectory(commands2.Command):
-    def __init__(self, drivetrain, intake, traj, is_red_alliance):
+    def __init__(self, drivetrain, intake, traj, is_red_alliance) -> None:
         """
         Initializes the AutonomousCommand.
 
@@ -20,12 +20,14 @@ def __init__(self, drivetrain, intake, traj, is_red_alliance):
         self.trajectory = choreo.load_swerve_trajectory(traj)
         self.timer = wpilib.Timer()
         self.is_red_alliance = is_red_alliance
-        self.addRequirements(self.drivetrain)  # Ensure the drivetrain is a requirement for this command
         self.laststamp = 0
         self.event_markers = []
         self.triggered_events = set()
 
-    def initialize(self):
+        self.addRequirements(self.drivetrain)  # Ensure the drivetrain is a requirement for this command
+        
+
+    def initialize(self) -> None:
         """
         This autonomous runs the autonomous command selected by your RobotContainer class.
         """
@@ -43,7 +45,7 @@ def initialize(self):
         # Reset and start the timer when the autonomous period begins
         self.timer.restart()
 
-    def execute(self):
+    def execute(self) -> None:
         """
         This function is called periodically during autonomous.
         """
@@ -67,7 +69,7 @@ def execute(self):
                 else:
                     self.drivetrain.stop()
 
-    def triggerEvent(self, event):
+    def triggerEvent(self, event) -> None:
         """
         Trigger the action associated with the event.
         """
@@ -81,7 +83,7 @@ def triggerEvent(self, event):
         elif event == "ResetHeading":
             self.drivetrain.seed_field_centric()
 
-    def isFinished(self):
+    def isFinished(self) -> bool:
         """
         Returns true when the command should end.
         """

pybot/lifter.py

@@ -1,51 +1,23 @@
-import logging
 from phoenix6.hardware import talon_fx
 from phoenix6.controls.follower import Follower
 from phoenix6.signals import NeutralModeValue
 
-# Configure logging
-logging.basicConfig(level=logging.INFO)
-logger = logging.getLogger(__name__)
-
 class Lifter:
-    def __init__(self, master_id, follower_id):
+    def __init__(self, master_id, follower_id) -> None:
         self.motor = talon_fx.TalonFX(master_id)
         self.motor.setNeutralMode(NeutralModeValue.BRAKE)
         self.follower = talon_fx.TalonFX(follower_id)
         self.follower.set_control(Follower(master_id, True))
         self.follower.setNeutralMode(NeutralModeValue.BRAKE)
 
-    def moveDown(self):
-        try:
-            if self.motor.get_reverse_limit() == 0:
-                logger.warning("Reverse limit reached; cannot move down")
-                self.stop()
-            else:
-                logger.info("Moving down")
-                self.motor.set(-0.2)  # Gentle downward movement
-        except Exception as e:
-            logger.error(f"Error moving down: {e}")
-
-    def moveUp(self):
-        try:
-            if self.motor.get_forward_limit() == 0:
-                logger.warning("Forward limit reached; cannot move up")
-                self.stop()
-            else:
-                logger.info("Moving up")
-                self.motor.set(0.3)  # Upward movement
-        except Exception as e:
-            logger.error(f"Error moving up: {e}")
+    def moveDown(self) -> None:
+                self.motor.set(-0.2)
 
+    def moveUp(self) -> None:
+                self.motor.set(0.3)
 
-    def stop(self):
-        try:
+    def stop(self) -> None:
             self.motor.stopMotor()
-        except Exception as e:
-            logger.error(f"Error stopping motor: {e}")
 
-    def setMotor(self, value):
-        try:
+    def setMotor(self, value) -> None:
             self.motor.set(value)
-        except Exception as e:
-            logger.error(f"Error setting motor: {e}")

pybot/robot.py

@@ -24,7 +24,6 @@ def robotInit(self) -> None:
         self.container = RobotContainer()
         self.scheduler = commands2.CommandScheduler.getInstance()
         self.registerTrajectories()
-        wpilib.CameraServer.launch ('vision.py:main')
 
     def registerTrajectories(self) -> None:
         self.chooser = autos.createChooser()

pybot/robotcontainer.py

@@ -7,21 +7,27 @@
 import commands2
 import commands2.button
 import commands2.cmd
-from commands2.sysid import SysIdRoutine
+#from commands2.sysid import SysIdRoutine
 from generated.tuner_constants import TunerConstants
 from telemetry import Telemetry
 
-from phoenix6 import swerve, SignalLogger
+from phoenix6 import swerve#, SignalLogger
 from wpimath.units import rotationsToRadians
 from lifter import Lifter  # Import the Lifter class
 import wpilib
 import logging
-
 import math
 
 # Configure logging
 logging.basicConfig(level=logging.DEBUG)
 
+MAX_SPEED_SCALING = 0.35
+DEAD_BAND = 0.1
+ELEVATOR_MOTOR_ID_1 = 20
+ELEVATOR_MOTOR_ID_2 = 16
+
+INTAKE_MOTOR_ID_TOP = 18
+INTAKE_MOTOR_ID_BOTTOM = 22
 
 class RobotContainer:
     """
@@ -33,14 +39,14 @@ class RobotContainer:
 
     def __init__(self) -> None:
         self._max_speed = (
-            TunerConstants.speed_at_12_volts * .35
+            TunerConstants.speed_at_12_volts * MAX_SPEED_SCALING
         )  # speed_at_12_volts desired top speed
         self._max_angular_rate = rotationsToRadians(
             TunerConstants.angular_at_12_volts * .1
         )
 
         # Setting up bindings for necessary control of the swerve drive platform
-        self._deadband = 0.1 # The input deadband
+        self._deadband = DEAD_BAND # The input deadband
         self._exponent = 3.0 # Exponential factor (try 2, 2.5, or 3)
 
         self._drive = (
@@ -61,13 +67,32 @@ def __init__(self) -> None:
         self.drivetrain = TunerConstants.create_drivetrain()
 
         # Initialize the elevator with motor IDs
-        self.elevator = Lifter(20, 16)
+        self.elevator = Lifter(ELEVATOR_MOTOR_ID_1, ELEVATOR_MOTOR_ID_2)
         # Initialize the intake with motor IDs
-        self.intake = Lifter(18, 22)
-        # making it closer to 1 will make the decay slower and smoother.
-
+        self.intake = Lifter(INTAKE_MOTOR_ID_TOP, INTAKE_MOTOR_ID_BOTTOM)
+        
         # Setup telemetry
-        self._registerTelemetery()
+        self._registerTelemetry()
+
+    def calculateJoystick(self) -> tuple[float, float]:
+            x0, y0 = self._joystick.getLeftX(), self._joystick.getLeftY()
+            magnitude = self.applyExponential(math.hypot(x0, y0), self._deadband, self._exponent) * self._max_speed
+            theta = math.atan2(y0, x0)
+
+            x1 = magnitude * math.cos(theta)
+            y1 = magnitude * math.sin(theta)
+
+            return x1, y1
+    
+    def applyRequest(self) -> swerve.requests.SwerveRequest:
+            (new_vx, new_vy) = self.calculateJoystick()
+
+            return (self._drive.with_velocity_x(self._driveMultiplier * new_vy # Drive left with negative X (left)
+            )  .with_velocity_y(self._driveMultiplier * new_vx) # Drive forward with negative Y (forward)
+            .with_rotational_rate(
+                self._driveMultiplier * self.applyExponential(self._joystick.getRightX(), self._deadband, self._exponent) * self._max_angular_rate
+            ))  # Drive counterclockwise with negative X (left)
+
 
     def configureButtonBindings(self) -> None:
         """
@@ -76,36 +101,14 @@ def configureButtonBindings(self) -> None:
         if not hasattr(self, '_joystick') or self._joystick is None:
             self._joystick = commands2.button.CommandXboxController(0)
 
+        
         # Cache the multiplier
         self._driveMultiplier = -1.0 if self.isRedAlliance() else 1.0
-
-        def calculateJoystick():
-            x0 = self._joystick.getLeftX()
-            y0 = self._joystick.getLeftY()
-
-            mag = self.apply_exponential(math.sqrt((x0 * x0) + (y0 * y0)), self._deadband, self._exponent) * self._max_speed
-            theta = math.atan2(y0, x0)
-
-            x1 = mag * math.cos(theta)
-            y1 = mag * math.sin(theta)
-
-            print('0: %f, %f    1: %f %f' % (x0, y0, x1, y1))
-
-            return (x1, y1)
-
-        def applyRequest():
-            (new_vx, new_vy) = calculateJoystick()
-
-            return (self._drive.with_velocity_x(self._driveMultiplier * new_vy)  # Drive forward with negative Y (forward)
-            .with_velocity_y(self._driveMultiplier * new_vx)  # Drive left with negative X (left)
-            .with_rotational_rate(
-                self._driveMultiplier * self.apply_exponential(self._joystick.getRightX(), self._deadband, self._exponent) * self._max_angular_rate
-            ))  # Drive counterclockwise with negative X (left)
-
+        
         # Note that X is defined as forward according to WPILib convention,
         # and Y is defined as to the left according to WPILib convention.
-        self.drivetrain.setDefaultCommand(self.drivetrain.apply_request(applyRequest))
-        # reset the field-centric heading on left bumper press
+        self.drivetrain.setDefaultCommand(self.drivetrain.apply_request(self.applyRequest))
+        # reset the field-centric heading on x button press
         self._joystick.x().onTrue(
             self.drivetrain.runOnce(lambda: self.resetHeading())
         )
@@ -133,7 +136,7 @@ def applyRequest():
             lambda: self.intake.stop()
         ))
 
-    def resetHeading(self):
+    def resetHeading(self) -> None:
         self.drivetrain.seed_field_centric()
 
     def getAutonomousCommand(self, selected: str) -> commands2.Command:
@@ -143,15 +146,15 @@ def getAutonomousCommand(self, selected: str) -> commands2.Command:
                                  selected,
                                  is_red_alliance = self.isRedAlliance())
 
-    def isRedAlliance(self):
+    def isRedAlliance(self) -> bool:
         return wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed
 
-    def _registerTelemetery (self):
+    def _registerTelemetry (self) -> None:
         self.drivetrain.register_telemetry(
             lambda state: self._logger.telemeterize(state)
         )
 
-    def apply_exponential(self, input: float, deadband: float, exponent: float) -> float:
+    def applyExponential(self, input: float, deadband: float, exponent: float) -> float:
         """
         Apply an exponential response curve with a deadband on the input
         such that the final output goes smoothly from 0 -> ±1 without
@@ -162,19 +165,9 @@ def apply_exponential(self, input: float, deadband: float, exponent: float) -> f
         :param exponent: The exponent for the curve (e.g. 2.0 for a squared curve).
         :return: A smoothly scaled & exponentiated value in [-1, 1].
         """
-        # 1. Apply input deadband (stick within ±deadband => output = 0)
         if abs(input) < deadband:
             return 0.0
 
-        # 2. Preserve sign and work with magnitude
-        sign = 1 if input > 0 else -1
-        magnitude = abs(input)
-
-        # 3. Scale from [deadband .. 1] to [0 .. 1]
-        scaled = (magnitude - deadband) / (1.0 - deadband)  # in [0..1]
-
-        # 4. Apply exponential. e.g. exponent=2 => x^2, exponent=3 => x^3
-        curved = scaled ** exponent
-
-        # 5. Reapply sign to restore forward/backward
-        return sign * curved
+        sign = math.copysign(1, input)  # Use math.copysign for clarity
+        scaled = (abs(input) - deadband) / (1.0 - deadband)
+        return sign * math.pow(scaled, exponent)