OTF shooting. Try to do auton with overlap.

commands/auton_commands/auto_shooter_launch_note.py

@@ -22,7 +22,7 @@ def __init__(self, shooter: Shooter, drivetrain: Drivetrain, tilt=sm.tilt_sub, r
         # self.addRequirements(drivetrain)
 
     def initialize(self) -> None:
-        print("Shooter launch note started")
+        print("AutoShooter launch note started")
         self.shooter.set_tilt(self.tilt)
         self.shooter.set_velocity(self.rpm)
         self.shooter.spin_up()

commands/auton_commands/otf_shooter_launch_note.py

@@ -0,0 +1,68 @@
+from wpilib import SmartDashboard, Timer
+from commands2 import Command
+from controllers.driver import DriverController
+from subsystems.amp import Amp
+from subsystems.drivetrain import Drivetrain
+from subsystems.intake import Intake
+
+from subsystems.shooter import Shooter
+from phoenix6.controls import DutyCycleOut
+
+class OTFShooterLaunchNote(Command):
+    def __init__(self, shooter: Shooter, drivetrain: Drivetrain, driver: DriverController,intake: Intake, amp: Amp,
+                  driverneeded = False, rpm=80, do_rotation = False) -> None:
+        super().__init__()
+        self.shooter = shooter
+        self.drivetrain = drivetrain
+        self.driver = driver
+        self.amp = amp
+        self.intake = intake
+        self.timer = Timer()
+        self.shot_timer = Timer()
+        self.rpm = rpm
+        self.do_rotation = do_rotation
+        self.addRequirements(shooter)
+        # self.addRequirements(drivetrain)
+
+    def setTilt(self, distance: float):
+        self.tilt = 0.7622085 + 0.1940302/(2 ** (distance / 0.9421715))
+
+    def initialize(self) -> None:
+        print("OTF Shooter launch note started")
+        self.shooter.go_otf_tilt()
+        self.shooter.set_velocity(self.rpm)
+        self.shooter.spin_up()
+        self.shot_fired = False
+        self.timer.restart()
+
+    def execute(self) -> None:
+        # TODO:
+        # Add in check for vision targets here?
+        up_to_speed = self.shooter.is_up_to_speed()
+        aimed = self.shooter.is_tilt_aimed()
+        driverRB = self.driver.get_speaker_lockon()
+        if not driverRB:
+            self.timer.restart()
+        if self.do_rotation:
+            drive_aimed = self.drivetrain.is_speaker_aimed()
+        else:
+            drive_aimed = True
+        if not self.shot_fired and up_to_speed and aimed and drive_aimed and self.timer.hasElapsed(0.5):
+            self.intake.feed()
+            self.amp.reverse()
+            self.shooter.feed_note()
+            self.shot_fired = True
+            self.shot_timer.restart()
+
+    def end(self, interrupted: bool) -> None:
+        self.shooter.feed_off()
+        self.shooter.prepare_to_load()
+        self.shooter.halt()
+        self.intake.halt()
+        self.amp.halt()
+        self.shooter.shooter_motor_left.set_control(DutyCycleOut(0.0))
+        self.shooter.shooter_motor_right.set_control(DutyCycleOut(0.0))
+        self.drivetrain.defcmd.speaker_tracking_off()
+
+    def isFinished(self) -> bool:
+        return self.shot_fired and self.shot_timer.hasElapsed(0.5)

commands/drivetopoint.py

@@ -30,7 +30,7 @@ def initialize(self):
         pass
 
     def execute(self):
-        # print(f"DTP target x = {self.x}, y = {self.y}, heading = {self.targetHeading}")
+        print(f"DTP target x = {self.x}, y = {self.y}, heading = {self.targetHeading}")
         pose = self.drivetrain.getPose()
         curr_yaw = self.gyro.get_yaw()
         currx = pose.X()

commands/rotate.py

@@ -15,7 +15,7 @@ def __init__(self, drive: Drivetrain, gyro: Gyro, targetHeading):
 
     def initialize(self):
         print(f"rotate started with heading = {self.targetHeading}")
-        self.drivetrain.set_lock_heading(self.targetHeading)
+        self.drivetrain.defcmd.set_lock_heading(self.targetHeading)
         self.timer.restart()
         pass
 

misc.py

@@ -110,3 +110,11 @@ def bor_rot(br, flip) -> float:
         offset = br - 90
         return (90 - offset)
     return br
+
+def angle_offset(angle, target):
+    offset = target - angle
+    while offset > 180:
+        offset -= 360
+    while offset < -180:
+        offset += 360
+    return offset

robot.py

@@ -16,6 +16,7 @@
 from commands.auto_rotate import AutoRotate
 from commands.delay import Delay
 from commands.auton_commands.auto_shooter_launch_note import AutoShooterLaunchNote
+from commands.auton_commands.otf_shooter_launch_note import OTFShooterLaunchNote
 from commands.auton_commands.pickup_note import AutoPickupNote
 from commands.eject_note import EjectNote
 
@@ -84,7 +85,7 @@ def robotInit(self) -> None:
         self.note_tracker = note_tracker.NoteTracker()
         self.intake = intake.Intake(self.commander, self.photoeyes)
         self.swerve = drivetrain.Drivetrain(self.gyro, self.driver,
-                                            self.note_tracker, self.intake)
+                                            self.note_tracker, self.intake, self.shooter)
         self.auto_selector = auto_selector.AutoSelector()
 
         self.climber = climber.Climber(self.driver, self.commander)
@@ -132,6 +133,8 @@ def configure_driver_controls(self):
         note_track_button.onTrue(
             InstantCommand(self.swerve.defcmd.note_tracking_on)
         )
+        note_track_button.onTrue(IntakeNote(self.intake, self.shooter,
+                                           self.gyro, self.photoeyes))
         note_track_button.onFalse(
             InstantCommand(self.swerve.defcmd.note_tracking_off)
         )
@@ -170,9 +173,12 @@ def configure_commander_controls(self):
 
         amp_set_height_amp = JoystickButton(self.commander_joystick2,
                                             RBM.amp_lift_amp_c2)
-        # amp_set_height_amp.and_(self.photoeyes.get_intake_loaded() and 
+        amp_set_height_amp.and_(lambda: self.photoeyes.get_intake_loaded()).onTrue(AmpLoad(self.amp,
+                                                                                            self.intake, self.photoeyes))
+        amp_set_height_amp.and_(lambda: self.photoeyes.get_intake_unloaded()).onTrue(SetAmpHeight(self.amp, self.amp.Height.AMP))
+                                # ~self.photoeyes.get_amp_loaded()).onTrue(AmpLoad(self.amp, self.intake, self.photoeyes))      
         #                        ~self.photoeyes.get_amp_loaded()).onTrue(AmpLoad(self.amp, self.intake, self.photoeyes))
-        amp_set_height_amp.onTrue(SetAmpHeight(self.amp, self.amp.Height.AMP))
+        # amp_set_height_amp.onTrue(SetAmpHeight(self.amp, self.amp.Height.AMP))
 
         amp_set_height_trap = JoystickButton(self.commander_joystick2,
                                             RBM.amp_lift_trap_c2)
@@ -204,6 +210,11 @@ def configure_commander_controls(self):
                                           RBM.shooter_aim_safe_c1)
         safe_shot_button.onTrue(InstantCommand(self.shooter.safe_shot))
 
+        otf_shot_button = JoystickButton(self.commander_joystick1,
+                                          RBM.shooter_aim_otf_c1)
+        otf_shot_button.and_(lambda: self.driver.get_speaker_lockon()).whileTrue(OTFShooterLaunchNote(self.shooter,
+                    self.swerve, self.driver, self.intake, self.amp, driverneeded = True, rpm = 85, do_rotation = True))
+
         sub_shot_button = JoystickButton(self.commander_joystick1,
                                          RBM.shooter_aim_sub_c1)
         sub_shot_button.onTrue(InstantCommand(self.shooter.sub_shot))
@@ -218,10 +229,6 @@ def configure_commander_controls(self):
         shooter_spin.onTrue(InstantCommand(self.shooter.spin_up))
         shooter_spin.onFalse(InstantCommand(self.shooter.spin_down))
 
-        reset_odo = JoystickButton(self.commander_joystick2,
-                                   RBM.reset_odometry_c2)
-        reset_odo.onTrue(InstantCommand(self.resetOdometryToCurrentPose))
-
     def resetOdometryToCurrentPose(self):
         curr_pose = self.swerve.getPose()
         self.swerve.resetOdometry(curr_pose)
@@ -236,7 +243,7 @@ def get_auton(self, value) -> None:
         elif value == 4:
             self.auton_method = self.auto_station_2_4note_pole_last_fast
         else:
-            self.auton_method = self.auto_station_2_4note_pole_last_fast
+            self.auton_method = self.auto_station_2_4note_pole_last
 
     def robotPeriodic(self) -> None:
         # Rough idea of how to incorporate vision into odometry
@@ -460,8 +467,10 @@ def auto_station_2_4note_pole_last(self):
         ).asProxy()
         rotate_shot2 = Rotate(self.swerve, self.gyro, bor_rot(180, flip)).asProxy()
         # load_rotate2 = ParallelCommandGroup([load_shooter2, rotate_shot2]).asProxy()
-        shoot2 = AutoShooterLaunchNote(self.shooter, self.swerve,
-                                       shooter.tilt_safe, 80, do_rotation=True).asProxy()
+        # shoot2 = AutoShooterLaunchNote(self.shooter, self.swerve,
+        #                               shooter.tilt_safe, 80, do_rotation=True).asProxy()
+
+        shoot2 = OTFShooterLaunchNote(self.shooter, self.swerve, self.driver, self.intake, self.amp, do_rotation=True) 
         rotate_note3 = Rotate(self.swerve, self.gyro, bor_rot(90, flip)).asProxy()
         note_3_target = (
             borx(2.9, flip),
@@ -555,10 +564,10 @@ def auto_station_2_4note_pole_last_fast(self):
 
         # shoot_sub = AutoShooterLaunchNote(self.shooter, self.swerve, shooter.tilt_sub, 80)
 
-        back_target = (borx(1.8, flip), bory(5.55, flip), bor_rot(180, flip))
+        back_target = (borx(1.8, flip), bory(5.55, flip), bor_rot(1, flip))
         slide_back = DriveToPoint(self.swerve, self.gyro, *back_target)
 
-        rotate1 = DriveToPoint(self.swerve, self.gyro, x = None, y = None, targetHeading = bor_rot(2, flip))
+        # rotate1 = DriveToPoint(self.swerve, self.gyro, x = None, y = None, targetHeading = bor_rot(2, flip))
 
         lock_note2 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, note_lockon=True, timeout=5)
         pickup_note2 = IntakeNote(self.intake, self.shooter, self.amp, self.photoeyes)
@@ -569,7 +578,7 @@ def auto_station_2_4note_pole_last_fast(self):
         load_rotate2 = ParallelCommandGroup([load_shooter2, rotate_shot2])
 
         align_shot2 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, speaker_lockon=True, timeout=5)
-        Launch2 = AutoShooterLaunchNote(self.shooter, self.swerve, shooter.tilt_safe, 80, do_rotation=True)
+        Launch2 = OTFShooterLaunchNote(self.shooter, self.swerve, self.driver, self.intake, self.amp, 80, do_rotation=True)
         shoot_note2 = ParallelRaceGroup([align_shot2, Launch2])
 
         rotate3 = AutoRotate(self.swerve, self.gyro, bor_rot(90, flip))
@@ -615,7 +624,7 @@ def auto_station_2_4note_pole_last_fast(self):
             reset_swerve,
             delaycmd,
             slide_back, 
-            rotate1,
+            # rotate1,
             get_note2
             ]
         """ 

subsystems/amp.py

@@ -13,7 +13,7 @@
 class Amp(Subsystem):
 
     class Height():
-        HOME = 0.14
+        HOME = 0.6
         AMP = 17.327
         TRAP = 20.327
         LIMIT = 30.35

subsystems/auto_selector.py

@@ -11,7 +11,7 @@ class AutoPath():
 
     def __init__(self):
         super().__init__()
-        self.auton_method = 4
+        self.auton_method = 3
         SmartDashboard.putNumber('auton/route', self.auton_method)
 
     def get_auton(self) -> int: