First changes to start the AutoDrive branch

commands/drive_auto.py

@@ -0,0 +1,219 @@
+from commands2 import Command, PIDCommand
+from wpilib import PS4Controller, PS5Controller
+from controllers.driver import DriverController
+
+from subsystems.drivetrain import Drivetrain
+from subsystems.gyro import Gyro
+from subsystems.intake import Intake
+from subsystems.note_tracker import NoteTracker
+from wpilib import PIDController
+from constants import RobotPIDConstants as PIDC
+from wpimath.filter import SlewRateLimiter, LinearFilter
+
+class DriveAutoCommand(Command):
+    """
+    Command to mimic the drivetrain default command and also handle autonomous targetting for use in autonomous mode
+    """
+    def __init__(self, drivetrain: Drivetrain, controller: DriverController,
+                 photon: NoteTracker, gyro: Gyro, intake: Intake) -> None:
+        super().__init__()
+        self.drivetrain = drivetrain
+        self.controller = controller
+        self.photon = photon
+        self.gyro = gyro
+        self.intake = intake
+        self.note_pid = PS4Controller(*PIDCommand.note_tracking_pid)
+        self.note_translate_pid = PS5Controller(*PIDC.note_translate_pid)
+        self.speaker_pid = PIDController(*PIDC.speaker_tracking_pid)
+        self.straight_drive_pid = PIDController(*PIDC.straight_drive_pid)
+        self.straight_drive_pid.setTolerance(2.0)
+        # Slew rate limiters to make joystick inputs more gentle
+        self.xslew = SlewRateLimiter(5.0)
+        self.yslew = SlewRateLimiter(5.0)
+        self.rotslew = SlewRateLimiter(2)
+        self.note_yaw_filtered = LinearFilter.highPass(0.1, 0.02)
+        self.idle_counter = 0
+        self.desired_heading = None
+        self.addRequirements(drivetrain)
+
+        kMaxSpeed = self.drivetrain.get_max_speed()
+        kMaxAngularSpeed = self.drivetrain.get_max_angular_speed()
+
+    def initialize(self):
+        self.note_lockon = False
+        self.speaker_lockon = False
+        self.flipped = self.drivetrain.shouldFlipPath()
+        print(str(self.flipped) + " is the flipped value")
+
+    def note_tracking_on(self):
+        self.note_lockon = True
+
+    def note_tracking_off(self):
+        self.note_lockon = False
+
+    def speaker_tracking_on(self):
+        print("drivetrain.defcmd.speaker_tracking_on function called")
+        self.speaker_lockon = True
+
+    def speaker_tracking_off(self):
+        self.speaker_lockon = False
+
+    def _curr_heading(self) -> float:
+        return self.drivetrain.get_heading_rotation_2d().degrees()
+
+    def flipHeading(self):
+        self.desired_heading += 180
+        if self.desired_heading > 360:
+            self.desired_heading -= 360
+
+    def lock_heading(self):
+        self.desired_heading = self._curr_heading()
+
+    def is_note_tracking(self):
+        return self.drivetrain.note_tracking or fake
+
+    def is_note_visible(self):
+        return self.drivetrain.note_visible or fake
+
+    def is_speaker_tracking(self):
+        return self.drivetrain.speaker_tracking or fake
+
+    def is_speaker_visible(self):
+        return self.drivetrain.speaker_visible or fake
+
+    def is_speaker_aimed(self):
+        return self.drivetrain.speaker_aimed or fake
+
+    def is_amp_tracking(self):
+        return self.drivetrain.amp_tracking or fake
+
+    def is_amp_visible(self):
+        return self.drivetrain.amp_visible or fake
+
+    def execute(self) -> None:
+        # Heading is in degrees here
+        if self.desired_heading is None:
+            self.desired_heading = self._curr_heading()
+        curr = self.drivetrain.get_heading_rotation_2d().degrees()
+        xraw = self.controller.get_drive_x()
+        xSpeed = self.xslew.calculate(xraw)
+        if xraw == 0:
+            xSpeed /= 2
+        xSpeed *= self.kMaxSpeed
+
+        yraw = self.controller.get_drive_y()
+        ySpeed = self.yslew.calculate(yraw)
+        if yraw == 0:
+            ySpeed /= 2
+        ySpeed *= self.kMaxSpeed
+
+        rotraw = self.controller.get_drive_rot()
+        rot = self.rotslew.calculate(rotraw)
+        if rotraw == 0:
+            rot /= 2
+        rot *= self.kMaxAngularSpeed
+
+        master_throttle = self.controller.get_master_throttle()
+        xSpeed *= master_throttle
+        ySpeed *= master_throttle
+        rot *= master_throttle
+
+        if self.flipped:
+            xSpeed = -xSpeed
+            ySpeed = -ySpeed
+
+        # If the user is commanding rotation set the desired heading to the
+        # current heading so if they let off we can use PID to keep the robot
+        # driving straight
+        if rot != 0:
+            self.lock_heading()
+        else:
+            error = curr - self.desired_heading
+            if abs(error) > 1.0:
+                rot = self.straight_drive_pid.calculate(curr, self.desired_heading)
+            else:
+                rot = 0
+
+        if self.controller.get_yaw_reset():
+            forward = 180 if self.drivetrain.shouldFlipPath() else 0
+            self.gyro.set_yaw(forward)
+
+        # When in lockon mode, the robot will rotate to face the node
+        # that PhotonVision is detecting
+        robot_centric_force = False
+
+        self.drivetrain.note_tracking = False
+        self.drivetrain.note_visible = False
+        if self.note_lockon:
+            # TODO: Make sure the note intake command is running when this is happening
+            # The trick will be kicking the command on but not letting it go away immediately
+            # but also don't start a brand new one if it is already running
+
+            """
+            if intake_note.running is False:
+                sched = CommandScheduler.getInstance()
+                sched.schedule(
+                    intake_note.IntakeNote(self.intake, self.shooter, self.amp,
+                                           self.photoeyes)
+                )
+            """
+            self.note_tracking = True
+            robot_centric_force = True
+            pn = SmartDashboard.putNumber
+            yaw_raw = self.photon.getYawOffset()
+            if yaw_raw is not None:
+                self.note_visible = True
+                self.current_yaw = self.note_yaw_filtered.calculate(yaw_raw)
+                pn('drivetrain/note_tracker/yaw_raw', yaw_raw)
+                yaw = self.current_yaw
+                pn('drivetrain/note_tracker/yaw', yaw)
+            pitch = self.photon.getPitchOffset()
+            if yaw_raw is not None:
+                # Setpoint was 0 but moved to 2 to try and get the
+                # robot from going left of the note
+                rot = self.note_pid.calculate(yaw_raw, -2)
+                xSpeed = curr_note_intake_speed
+                # if abs(yaw_raw) < 1.7:
+                #     rot = 0
+                # else:
+                #     rot = self.note_pid.calculate(yaw_raw, 0)
+                #     xSpeed = 0.5
+                # else:
+                #     if pitch is not None and pitch > 0:
+                #         rot = self.note_pid.calculate(yaw, 0)
+                #     else:
+                #         rot = 0
+                #         # Force a translation to center on the note when close
+                #         ySpeed = self.note_translate_pid.calculate(yaw, 0)
+
+        self.slow_mode = False
+        if self.controller.get_slow_mode():
+            self.slow_mode = True
+            slow_mode_factor = 1/2
+            xSpeed *= slow_mode_factor
+            ySpeed *= slow_mode_factor
+            rot *= slow_mode_factor
+
+        self.drivetrain.speaker_tracking = False
+        self.drivetrain.speaker_visible = False
+        self.drivetrain.speaker_aimed = False
+        if self.speaker_lockon:
+            # TODO: Possible! Check with Nathan -- slow down the drivetrain by setting
+            # master_throttle to something like 0.8 or 0.6...
+            self.drivetrain.speaker_tracking = True
+            fid = 4 if self.drivetrain.is_red_alliance() else 7
+            speaker_heading = self.drivetrain.get_fid_heading(fid)
+            if speaker_heading is not None:
+                self.drivetrain.speaker_visible = True
+                if abs(speaker_heading) < 3.0:
+                    rot = 0
+                    self.drivetrain.speaker_aimed = True
+                else:
+                    rot = self.speaker_pid.calculate(speaker_heading, 0)
+
+        pb("Speaker tracking", self.drivetrain.speaker_tracking)
+        pb("Speaker visible", self.drivetrain.speaker_visible)
+        pb("Speaker aimed", self.drivetrain.speaker_aimed)
+
+        self.drivetrain.drive(xSpeed, ySpeed, rot,
+                              robot_centric_force=robot_centric_force)

commands/drive_target.py

@@ -0,0 +1,124 @@
+from cmath import sqrt
+from math import atan2, degrees
+from commands2 import Command
+from wpilib import SmartDashboard
+from subsystems.drivetrain import Drivetrain
+from enum import Enum
+
+class DriveTarget(Command):
+
+    def __init__(self, drivetrain: Drivetrain, target: Enum):
+        super().__init__()
+        self.drivetrain = drivetrain
+        self.target = target
+        self.red_alliance = self.drivetrain.isRedAlliance()
+        if self.red_alliance:
+            self.amp_x = 578.77 / 39.37 # inches to meters, ID 5
+            self.amp_y = 323 / 39.37 # inches to meters
+            self.amp_heading = 270  # degrees
+            self.speaker_x = 652.73 / 39.37 # inches to meters, ID 4
+            self.speaker_y = 218.42 / 39.37 # inches to meters
+            self.loading_x = 57.54 / 39.37 # inches to meters, ID 10
+            self.loading_y = 9.68 / 39.37 # inches to meters
+            self.loading_heading = 240  # degrees
+            self.stage_l_x = 468.69 / 39.37 # inches to meters, ID 11
+            self.stage_l_y = 146.19 / 39.37 # inches to meters
+            self.stage_l_heading = 120 # degrees
+            self.stage_r_x = 468.69 / 39.37 # inches to meters, ID 12
+            self.stage_r_y = 177.12 / 39.37 # inches to meters
+            self.stage_r_heading = 240 # degrees
+            self.stage_c_x = 441.74 / 39.37 # inches to meters, ID 13
+            self.stage_c_y = 161.65 / 39.37 # inches to meters
+            self.stage_c_heading = 0 # degrees
+        else:
+            self.amp_x = 72.5 / 39.37 # inches to meters, ID 6
+            self.amp_y = 323 / 39.37 # inches to meters
+            self.amp_heading = 270  # degrees
+            self.speaker_x = -1.5 / 39.37 # inches to meters, ID 7
+            self.speaker_y = 218.42 / 39.37 # inches to meters
+            self.loading_x = 593.68 / 39.37 # inches to meters, ID 1
+            self.loading_y = 9.68 / 39.37 # inches to meters
+            self.loading_heading = 300  # degrees
+            self.stage_l_x = 182.73 / 39.37 # inches to meters, ID 15
+            self.stage_l_y = 177.1 / 39.37 # inches to meters
+            self.stage_l_heading = 300 # degrees
+            self.stage_r_x = 182.73 / 39.37 # inches to meters, ID 16
+            self.stage_r_y = 146.19 / 39.37 # inches to meters
+            self.stage_r_heading = 60 # degrees
+            self.stage_c_x = 209.48 / 39.37 # inches to meters, ID 14
+            self.stage_c_y = 161.65 / 39.37 # inches to meters
+            self.stage_c_heading = 180 # degrees
+
+        # create an enum with options for amp, speaker, and loading
+    class Target(Enum):
+        AMP = 1
+        SPEAKER = 2
+        LOADING = 3
+        STAGE_L = 4
+        STAGE_R = 5
+        STAGE_C = 6
+
+    def distanceToAmp(self, currx, curry):
+        return sqrt((currx - self.amp_x) ** 2 + (curry - self.amp_y) ** 2)
+
+    def distanceToSpeaker(self, currx, curry):
+        return sqrt((currx - self.speaker_x) ** 2 + (curry - self.speaker_y) ** 2)
+
+    def distanceToLoading(self, currx, curry):
+        return sqrt((currx - self.loading_x) ** 2 + (curry - self.loading_y) ** 2)
+
+    def distanceToStageL(self, currx, curry):
+        return sqrt((currx - self.stage_l_x) ** 2 + (curry - self.stage_l_y) ** 2)
+
+    def distanceToStageR(self, currx, curry):
+        return sqrt((currx - self.stage_r_x) ** 2 + (curry - self.stage_r_y) ** 2)
+
+    def distanceToStageC(self, currx, curry):
+        return sqrt((currx - self.stage_c_x) ** 2 + (curry - self.stage_c_y) ** 2)
+
+
+    def execute(self):
+        pose = self.drivetrain.getPose()
+        currx = pose.X()
+        curry = pose.Y()
+        X = 0
+        Y = 0
+        heading = 0
+        distance = 0
+
+        match self.target:
+            case self.Target.AMP:
+                X = self.amp_x
+                Y = self.amp_y
+                heading = self.amp_heading
+                distance = self.distanceToAmp(currx, curry)
+            case self.Target.SPEAKER:
+                X = self.speaker_x
+                Y = self.speaker_y
+                deltaY = self.speaker_y - curry
+                deltaX = self.speaker_x - currx
+                heading = degrees(atan2(deltaY, deltaX))
+                distance = self.distanceToSpeaker(currx, curry)
+            case self.Target.LOADING:
+                X = self.loading_x
+                Y = self.loading_y
+                heading = self.loading_heading
+                distance = self.distanceToLoading(currx, curry)
+            case self.Target.STAGE_L:
+                X = self.stage_l_x
+                Y = self.stage_l_y
+                heading = self.stage_l_heading
+                distance = self.distanceToStageL(currx, curry)
+            case self.Target.STAGE_R:
+                X = self.stage_r_x
+                Y = self.stage_r_y
+                heading = self.stage_r_heading
+                distance = self.distanceToStageR(currx, curry)
+            case self.Target.STAGE_C:
+                X = self.stage_c_x
+                Y = self.stage_c_y
+                heading = self.stage_c_heading
+                distance = self.distanceToStageC(currx, curry)
+
+        return X, Y, heading, distance
+

constants.py

@@ -35,25 +35,6 @@ class RobotMotorMap():
     # Can Ranges: Swerve Motors = 11-24, Swerve Cancoders = 31-34, Pigeon = 35,
     # Shooter = 41-45, Intake = 51-52, Amp = 54-55, Climber = 57-58
 
-    # ## CHASSIS BOT ## #
-    # Swerve
-    # front_right_drive = 11
-    # front_right_turn = 21
-    # front_right_turn_encoder = 31
-
-    # front_left_drive = 12
-    # front_left_turn = 22
-    # front_left_turn_encoder = 32
-
-    # back_right_drive = 13
-    # back_right_turn = 23
-    # back_right_turn_encoder = 33
-
-    # back_left_drive = 14
-    # back_left_turn = 24
-    # back_left_turn_encoder = 34
-    # ## CHASSIS BOT ## #
-
     if is_sim():
         intake_motor_feed = 5
     else:
@@ -107,14 +88,14 @@ class RobotButtonMap():
 
     # Driver button assignments
     note_tracking = 5  # Left bumper
-    toggle_field_relative = 3
+    toggle_field_relative = 3 ### X Button per driver
     speaker_tracking = 6  # Right bumper
     slow_drive_mode = 4  # Y Button
-    flip_heading = 1
-    swap_direction = 2
+    flip_heading = 1 ### A Button per copilot
+    swap_direction = 2 ### B Button per copilot
 
 
-    RESET_YAW = 8
+    RESET_YAW = 8 ### Start Button per copilot
 
     # Button Panel List
     # INTAKE: Intake Ready, Intake Eject