Merge branch 'dev' into feat/dynamic-tolerance

command/controller.py

@@ -15,7 +15,8 @@
     WaitCommand,
     ParallelRaceGroup,
     WaitUntilCommand,
-    ConditionalCommand
+    ConditionalCommand,
+    ParallelDeadlineGroup
 )
 from commands2.command import Command
 from wpimath.geometry import Pose2d, Rotation2d  # noqa
@@ -442,23 +443,10 @@ def __init__(self, drivetrain: Drivetrain, wrist: Wrist, intake: Intake, limelig
                 ),
                 SequentialCommandGroup(
                     ParallelCommandGroup(
-                        SetWristIdle(wrist),
-                        DriveSwerveNoteLineup(drivetrain, limelight),
-                    ),
-                    ParallelCommandGroup(
-                        SequentialCommandGroup(
-                            InstantCommand(
-                                lambda: drivetrain.set_robot_centric(
-                                    (-config.object_detection_intaking_drivetrain_speed * drivetrain.max_vel, 0), 0)
-                                ),
+                        IntakeStageNote(wrist, intake),
+                        ParallelDeadlineGroup(
                             WaitUntilCommand(lambda: intake.detect_note()),
-                            InstantCommand(
-                                lambda: drivetrain.set_robot_centric((0, 0), 0)
-                            )
-                        ),
-                        SequentialCommandGroup(
-                            IntakeStageNote(wrist, intake),
-                            IntakeStageIdle(wrist, intake)
+                            DriveSwerveNoteLineup(drivetrain, limelight)
                         )
                     )
                 ),

command/drivetrain.py

@@ -292,24 +292,34 @@ def __init__(self, subsystem: Drivetrain, LimeLight: Limelight):
         '''
         Lines up the robot with the target
         :param drivetrain: Drivetrain subsystem
-        :param LimeLight: Limelight subsystem
-        :param target: (cube/cone) target to line up with'''
+        :param LimeLight: Limelight sensor
+        '''
         super().__init__(subsystem)
         self.drivetrain = subsystem
         self.limelight = LimeLight
         self.target_exists = False
         self.target_constrained = False
-        self.v_pid = PIDController(.09, 0, 0.01)
-        self.h_pid = PIDController(.07, 0, 0.01)
+        # self.v_pid = PIDController(.09, 0, 0.1)
+        self.h_pid = PIDController(.03, 0, 00)
+        self.o_pid = PIDController(
+            config.drivetrain_rotation_P,
+            config.drivetrain_rotation_I,
+            config.drivetrain_rotation_D
+        )
+        self.v_constant = 2.5
         self.is_pipeline: bool = False
         self.nt = ntcore.NetworkTableInstance.getDefault().getTable('drivetrain pid tune')
-
+        self.gyro_lock = 0
 
     def initialize(self):
         # self.limelight.set_pipeline_mode(config.LimelightPipeline.neural)
-        self.v_pid.reset()
+        # self.v_pid.reset()
+        self.o_pid.enableContinuousInput(radians(-180), radians(180))
+        self.o_pid.reset()
         self.h_pid.reset()
-        self.v_pid.setTolerance(config.object_detection_ty_threshold)
+        self.gyro_lock = 0
+        self.target_exists = False
+        # self.v_pid.setTolerance(config.object_detection_ty_threshold)
         self.h_pid.setTolerance(config.object_detection_tx_threshold)
 
     def execute(self):
@@ -322,9 +332,16 @@ def execute(self):
             # print(self.limelight.table.getNumber('tv', 3))
             self.drivetrain.set_robot_centric((0,0),0)
             return
+
+        if self.limelight.target_exists() and self.target_exists == False:
+            self.target_exists = True
+            self.gyro_lock = self.drivetrain.get_heading().radians()
+        else:
+            self.target_exists = False
+            return
         # self.nt.putBoolean('see target', True)
         # print("target")
-        tx, ty, ta = self.limelight.get_target(True)
+        tx, ty, ta = self.limelight.get_target()
 
         self.nt.putNumber('tx target', config.object_detection_tx)
         self.nt.putNumber('ty target', config.object_detection_ty)
@@ -335,23 +352,27 @@ def execute(self):
 
 
 
-        if self.target_exists == False and self.target_exists:
-            self.target_exists = True
+        # if self.target_exists == False and self.target_exists:
+        #     self.target_exists = True
 
         # print("Tracking...")
 
-        dy = self.v_pid.calculate(ty, config.object_detection_ty)
+        # dy = self.v_pid.calculate(ty, config.object_detection_ty)
         dx = self.h_pid.calculate(tx, config.object_detection_tx)
+        omega = self.o_pid.calculate(self.drivetrain.get_heading().radians(), self.gyro_lock)
+
+        dy = self.v_constant ** (-(abs(self.h_pid.getPositionError()) / 30)) if abs(self.h_pid.getPositionError()) > 0 else 1
+
 
 
         # dx *= states.drivetrain_controlled_vel * config.object_detection_drivetrain_speed_dx
-        # dy *= states.drivetrain_controlled_vel * config.object_detection_drivetrain_speed_dy
+        dy *= states.drivetrain_controlled_vel * config.object_detection_drivetrain_speed_dy
 
-        self.drivetrain.set_robot_centric((dy, -dx), 0)
+        self.drivetrain.set_robot_centric((-dy, -dx), -omega)
 
 
     def isFinished(self):
-        return self.h_pid.atSetpoint() and self.v_pid.atSetpoint()
+        # return self.h_pid.atSetpoint() #and self.v_pid.atSetpoint()
         return False
 
     def end(self, interrupted: bool = False):

config.py

@@ -229,7 +229,7 @@ class LimelightPosition:
 back_left_move_id = 11
 back_left_turn_id = 14
 back_left_encoder_port = AnalogEncoder(1 if comp_bot.get() else 0)
-back_left_encoder_zeroed_pos = 0.221 if comp_bot.get() else 0.458
+back_left_encoder_zeroed_pos = 0.221 if comp_bot.get() else 0.181
 
 back_right_move_id = 18
 back_right_turn_id = 16

oi/OI.py

@@ -74,26 +74,26 @@ def map_controls():
             command.IntakeIdle(Robot.intake)
         )
 
-        # Keymap.Intake.AUTO_INTAKE.onTrue(
-        #     command.AutoPickupNote(Robot.drivetrain, Robot.wrist, Robot.intake, Sensors.limelight_intake)
-        # ).onFalse(
-        #     commands2.ParallelCommandGroup(
-        #         # command.IntakeStageIdle(Robot.wrist, Robot.intake),
-        #         commands2.ConditionalCommand(
-        #             command.IntakeStageNote(Robot.wrist, Robot.intake).andThen(
-        #                 command.IntakeStageIdle(Robot.wrist, Robot.intake)),
-        #             command.IntakeStageIdle(Robot.wrist, Robot.intake),
-        #             lambda: Robot.intake.detect_note() or Robot.wrist.detect_note_first()
-        #         ),
-        #         command.DriveSwerveCustom(Robot.drivetrain)
-        #     )
-        # )
-
         Keymap.Intake.AUTO_INTAKE.onTrue(
-            command.DriveSwerveNoteLineup(Robot.drivetrain, Sensors.limelight_intake)
+            command.AutoPickupNote(Robot.drivetrain, Robot.wrist, Robot.intake, Sensors.limelight_intake)
         ).onFalse(
-            command.DriveSwerveCustom(Robot.drivetrain)
+            commands2.ParallelCommandGroup(
+                # command.IntakeStageIdle(Robot.wrist, Robot.intake),
+                commands2.ConditionalCommand(
+                    command.IntakeStageNote(Robot.wrist, Robot.intake).andThen(
+                        command.IntakeStageIdle(Robot.wrist, Robot.intake)),
+                    command.IntakeStageIdle(Robot.wrist, Robot.intake),
+                    lambda: Robot.intake.detect_note() or Robot.wrist.detect_note_first()
+                ),
+                command.DriveSwerveCustom(Robot.drivetrain)
+            )
         )
+
+        # Keymap.Intake.AUTO_INTAKE.onTrue(
+        #     command.DriveSwerveNoteLineup(Robot.drivetrain, Sensors.limelight_intake)
+        # ).onFalse(
+        #     command.DriveSwerveCustom(Robot.drivetrain)
+        # )
 
         Keymap.Elevator.ELEVATOR_HIGH.onTrue(
             command.SetElevator(Robot.elevator, config.Giraffe.kElevatorHigh.height)

robot.py

@@ -225,6 +225,9 @@ def teleopInit(self):
         self.scheduler.schedule(
             command.DeployIntake(Robot.intake).andThen(command.IntakeIdle(Robot.intake))
         )
+        # self.scheduler.schedule(
+        #     command.IntakeIdle(Robot.intake)
+        # )
 
         if Robot.wrist.note_in_feeder():
             states.flywheel_state = states.FlywheelState.shooting