Jim changes

commands/drivetrain_handler.py

@@ -1,32 +1,63 @@
-import wpilib
-from commands2 import Command
-from subsystems.drivetrain import Drivetrain
-from commands.haltdrive import HaltDrive
-from controllers.commander import CommanderController
-from wpilib import Joystick
-from commands.note_lockon import NoteLockOnCommand
-
-class DrivetrainPriority(Command):
-    def __init__(self, controller: CommanderController):
-        super().__init__("Drivetrain Priority")
-        self.addRequirements(Drivetrain)
-        self.controller = controller 
+from enum import Enum
+from subsystems.speaker_tracker import SpeakerTracker
+from subsystems.note_tracker import NoteTracker
+from subsystems.drivetrain import DrivetrainDefaultCommand
+
+
+# We use an enumeration to keep track of what we are tracking
+class TrackerType(Enum):
+    SPEAKER = 1
+    NOTE = 2
+    AMP = 3
+    STAGE_POS_L = 4
+    STAGE_POS_R = 5
+    STAGE_POS_C = 6
+
+
+# Here we define the DrivetrainHandler class, which is a command that
+# handles the drivetrain when tracking an object. It extends the
+# DrivetrainDefaultCommand class, which is the command that handles the
+# driver input for the drivetrain.
+# What we do here is read in the stick data but let the tracking system
+# override that if it wants to.
+class DrivetrainHandler(DrivetrainDefaultCommand):
+
+    def __init__(self, speaker_tracker: SpeakerTracker,
+                 note_tracker: NoteTracker, tracker_type: TrackerType):
+        super().__init__(self.drivetrain, self.controller, self.gyro,
+                         self.intake)
+        self.speaker_tracker = speaker_tracker
+        self.note_tracker = note_tracker
+        self.tracking = tracker_type
 
     def execute(self):
-        if self.controller.getRawButton(4):
-            rotational_speed = NoteLockOnCommand.get_rotational_speed()  # Find the method
-            Drivetrain.drive(0, 0, rotational_speed)
-        else:
-            x_speed = driver_controller.getX(wpilib.GenericHID.Hand.kLeft)
-            y_speed = driver_controller.getY(wpilib.GenericHID.Hand.kLeft)
-            rotational_speed = driver_controller.getX(wpilib.GenericHID.Hand.kRight)
-            Drivetrain.drive(x_speed, y_speed, rotational_speed)
+        # Just like the default command we start by getting the driver input
+        xSpeed, ySpeed, rot = self.get_stick_data()
 
-    def isFinished(self):
-        return False
+        robot_centric = False
+
+        # We only update the 'rot' variable if we have a valid desired_rotation
+        # from the appropriate subsystem. Otherwise the stick value will be
+        # used.
+        speaker_rot = self.speaker_tracker.desired_rotation
+        note_rot = self.note_tracker.desired_rotation
+        amp_rot = self.amp_tracker.desired_rotation
+        if self.tracking == TrackerType.SPEAKER and speaker_rot is not None:
+            rot = speaker_rot
+        elif self.tracking == TrackerType.NOTE and note_rot is not None:
+            rot = note_rot
+            # Cut the xpeed down too
+            xSpeed /= 2
+            # and force to robot centric
+            robot_centric = True
+            # Should we also force the ySpeed to 0?
+            # ySpeed = 0
+        elif self.tracking == TrackerType.AMP and amp_rot is not None:
+            rot = amp_rot
+        # add more tracking ideas here.
 
-    def end(self):
-        HaltDrive # double check that this is right
+        self.drivetrain.drive(xSpeed, ySpeed, rot,
+                              robot_centric_force=robot_centric)
 
-    # def interrupted(self):
-    #    self.end()
+    def isFinished(self):
+        return False

commands/gyro_reset.py

@@ -0,0 +1,20 @@
+from commands2 import Command
+from subsystems.gyro import Gyro
+
+
+class GyroReset(Command):
+
+    def __init__(self, gyro: Gyro) -> None:
+        self.gyro = gyro
+
+    def initialize(self):
+        forward = 180 if self.drivetrain.shouldFlipPath() else 0
+        self.gyro.set_yaw(forward)
+
+    def execute(self):
+        # We do nothing here; the init can handle
+        # the gyro reset
+        pass
+
+    def isFinished(self):
+        return True

commands/speaker_lockon.py

@@ -0,0 +1,24 @@
+from commands2 import Command
+from subsystems.drivetrain import Drivetrain
+from subsystems.speaker_tracker import SpeakerTracker
+
+
+class SpeakerLockon(Command):
+
+    def __init__(self, drive: Drivetrain, speaker_tracker: SpeakerTracker()):
+        self.drive = drive
+        self.speaker_tracker = speaker_tracker
+        self.addRequirements(self.drive)
+
+    def initialize(self):
+        pass
+
+    def execute(self):
+        heading = self.speaker_tracker.speaker_heading
+        if abs(heading) < 3:
+            # Make no adjustment, we're already locked on
+            pass
+        else:
+
+    def isFinished(self) -> bool:
+        return False

constants.py

@@ -107,6 +107,7 @@ class RobotButtonMap():
 
     # Driver button assignments
     note_tracking = 5  # Left bumper
+    auto_tracking = 5  # Left bumper
     toggle_field_relative = 3
     speaker_tracking = 6  # Right bumper
     slow_drive_mode = 4  # Y Button