semiauto: framework for goto and pointat

Author: martincarapia

pybot/robotcontainer.py

@@ -10,13 +10,14 @@
 #from commands2.sysid import SysIdRoutine
 from generated.tuner_constants import TunerConstants
 from telemetry import Telemetry
-
+from wpimath.geometry import Pose2d # , Rotation2d
 from phoenix6 import swerve#, SignalLogger
 from wpimath.units import rotationsToRadians
 from lifter import Lifter  # Import the Lifter class
 import wpilib
 import logging
 import math
+from wpimath.kinematics import ChassisSpeeds
 
 # Configure logging
 logging.basicConfig(level=logging.DEBUG)
@@ -28,6 +29,7 @@
 
 INTAKE_MOTOR_ID_TOP = 18
 INTAKE_MOTOR_ID_BOTTOM = 22
+DUMMY_POSE = Pose2d(2, 1, 1/2)
 
 class RobotContainer:
     """
@@ -74,6 +76,7 @@ def __init__(self) -> None:
         # Setup telemetry
         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
@@ -84,15 +87,22 @@ def calculateJoystick(self) -> tuple[float, float]:
 
             return x1, y1
 
-    def applyRequest(self) -> swerve.requests.SwerveRequest:
+    def defaultDriveRequest(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 create_go_to_coordinate_request(self):        
+        return self.drivetrain.go_to_coordinate(DUMMY_POSE)
 
+    def create_point_at_coordinate_request(self):
+        return self.drivetrain.point_at_coordinate(DUMMY_POSE, self.calculateJoystick())
+
+    
 
     def configureButtonBindings(self) -> None:
         """
@@ -107,7 +117,7 @@ def configureButtonBindings(self) -> None:
 
         # 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(self.applyRequest))
+        self.drivetrain.setDefaultCommand(self.drivetrain.apply_request(self.defaultDriveRequest))
         # reset the field-centric heading on x button press
         self._joystick.x().onTrue(
             self.drivetrain.runOnce(lambda: self.resetHeading())
@@ -136,6 +146,14 @@ def configureButtonBindings(self) -> None:
             lambda: self.intake.stop()
         ))
 
+        self._joystick.b().whileTrue(commands2.cmd.run(
+            lambda: self.create_point_at_coordinate_request(), self.drivetrain
+        ))
+
+        self._joystick.start().whileTrue(commands2.cmd.run(
+            lambda: self.create_go_to_coordinate_request(), self.drivetrain
+        ))
+
     def resetHeading(self) -> None:
         self.drivetrain.seed_field_centric()
 
@@ -146,15 +164,26 @@ def getAutonomousCommand(self, selected: str) -> commands2.Command:
                                  selected,
                                  is_red_alliance = self.isRedAlliance())
 
-    def isRedAlliance(self) -> bool:
-        return wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed
-    
     def _registerTelemetry (self) -> None:
         self.drivetrain.register_telemetry(
             lambda state: self._logger.telemeterize(state)
         )
 
-    def applyExponential(self, input: float, deadband: float, exponent: float) -> float:
+
+    @staticmethod
+    def compute_heading_to_target(current_pose: Pose2d, target_pose: Pose2d) -> float:
+        relative_pose = current_pose.relativeTo(target_pose)
+        return math.atan2(relative_pose.Y(), relative_pose.X())
+
+
+    @staticmethod
+    def isRedAlliance() -> bool:
+        return wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed
+    
+    
+
+    @staticmethod
+    def applyExponential(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

pybot/subsystems/command_swerve_drivetrain.py

@@ -327,6 +327,8 @@ def add_vision_measurement(self, vision_robot_pose: Pose2d, timestamp: units.sec
         :type vision_measurement_std_devs:  tuple[float, float, float] | None
         """
         swerve.SwerveDrivetrain.add_vision_measurement(self, vision_robot_pose, utils.fpga_to_current_time(timestamp), vision_measurement_std_devs)
+    
+
     def get_pose(self) -> Pose2d:
         """
         Gets the current pose of the robot.
@@ -335,6 +337,7 @@ def get_pose(self) -> Pose2d:
         :rtype: Pose2d
         """
         return super().get_state().pose
+    
     def follow_trajectory(self, sample):
         # Enable continuous input for heading controller
         self.heading_controller.enableContinuousInput(-math.pi, math.pi)
@@ -359,6 +362,47 @@ def follow_trajectory(self, sample):
 
         self.set_control(request)
 
+    def go_to_coordinate(self, target_pose: Pose2d):
+        # Enable continuous input for heading controller
+        self.heading_controller.enableContinuousInput(-math.pi, math.pi)
+
+        # Get current pose from swerve state
+        current_pose = self.get_pose()
+
+        # Calculate the necessary speeds using PID controllers
+        vx = self.x_controller.calculate(current_pose.X(), target_pose.X())
+        vy = self.y_controller.calculate(current_pose.Y(), target_pose.Y())
+        omega = self.heading_controller.calculate(current_pose.rotation().radians(), target_pose.rotation().radians())
+
+        speeds = ChassisSpeeds(vx, vy, omega)
+
+        # Create field-centric request with proper enum references
+        request = swerve.requests.ApplyFieldSpeeds().with_speeds(speeds) \
+            .with_drive_request_type(swerve.swerve_module.SwerveModule.DriveRequestType.VELOCITY) \
+            .with_steer_request_type(swerve.swerve_module.SwerveModule.SteerRequestType.POSITION) \
+            .with_desaturate_wheel_speeds(True)
+
+        self.set_control(request)
+    
+    def point_at_coordinate(self, target_pose: Pose2d, joyvalues: tuple[float, float]):
+        forward, strafe = joyvalues[1], joyvalues[0]
+        current_pose = self.get_pose()
+
+        heading_to_target = self.compute_heading_to_target(current_pose, target_pose)
+        current_heading = current_pose.rotation().radians()
+
+        self.heading_controller.setSetpoint(heading_to_target)
+        turn_command = self.heading_controller.calculate(current_heading)
+
+        request = (
+            swerve.requests.ApplyFieldSpeeds()
+            .with_speeds(ChassisSpeeds(forward, strafe, turn_command))
+            .with_drive_request_type(swerve.swerve_module.SwerveModule.DriveRequestType.VELOCITY)
+            .with_steer_request_type(swerve.swerve_module.SwerveModule.SteerRequestType.POSITION)
+            .with_desaturate_wheel_speeds(True)
+        )
+        self.set_control(request)
+
     def stop(self):
         """
         Stops the swerve drivetrain by setting all speeds to zero.
@@ -369,3 +413,8 @@ def stop(self):
             .with_steer_request_type(swerve.swerve_module.SwerveModule.SteerRequestType.POSITION) \
             .with_desaturate_wheel_speeds(True)
         self.set_control(request)
+
+    @staticmethod
+    def compute_heading_to_target(current_pose: Pose2d, target_pose: Pose2d) -> float:
+        relative_pose = current_pose.relativeTo(target_pose)
+        return math.atan2(relative_pose.Y(), relative_pose.X())