Merge pull request #106 from Choate-Robotics/feat/dynamic-tolerance

Feat/dynamic tolerance

command/drivetrain.py

@@ -105,7 +105,7 @@ def __init__(self, drivetrain, target_calc: TrajectoryCalculator):
             config.
             period
             )
-        self.theta_controller.setTolerance(radians(3 if RobotState.isAutonomous() else 3), radians(4 if RobotState.isAutonomous() else 4))
+        # self.theta_controller.setTolerance(radians(3 if RobotState.isAutonomous() else 3), radians(4 if RobotState.isAutonomous() else 4))
         self.table = ntcore.NetworkTableInstance.getDefault().getTable('Drivetrain Aim')
 
     def initialize(self) -> None:
@@ -136,6 +136,10 @@ def execute(self) -> None:
             config.drivetrain_aiming_offset = self.table.getNumber('drivetrain offset', config.drivetrain_aiming_offset)
             # put graphs
 
+        self.theta_controller.setTolerance(
+            self.target_calc.get_shot_pos_tolerance()
+        )
+
         dx, dy = (
             self.subsystem.axis_dx.value * (-1 if config.drivetrain_reversed else 1),
             self.subsystem.axis_dy.value * (-1 if config.drivetrain_reversed else 1),

config.py

@@ -257,6 +257,12 @@ class LimelightPosition:
 wrist_shot_tolerance: degrees = 1.75 if comp_bot.get() else 2 
 wrist_velocity_shot_tolerance: degrees = 1
 shot_height_offset_scalar: float = 0.014
+speaker_length: meters = 41.83 * inches_to_meters
+note_length: meters = 14 * inches_to_meters
+min_drivetrain_tolerance: degrees = 1
+max_drivetrain_tolerance: degrees = 13
+drivetrain_static_tolerance_offset: degrees = 1
+
 
 
 # Flywheel

oi/keymap.py

@@ -55,6 +55,10 @@ class Intake:
             lambda: Controllers.OPERATOR_CONTROLLER.getRawAxis(-controllerOPERATOR.RT) > 0.4
         )
 
+        # INTAKE_IN = commands2.button.Trigger(
+        #     lambda: Controllers.DRIVER_CONTROLLER.getRawAxis(-controllerDRIVER.LT) > 0.4
+        # )
+
 
         INTAKE_OUT = commands2.button.Trigger(
             lambda: Controllers.OPERATOR_CONTROLLER.getRawAxis(-controllerOPERATOR.LT) > 0.4

sensors/trajectory_calc.py

@@ -39,6 +39,8 @@ def __init__(self, odometry: FieldOdometry, elevator: Elevator, flywheel: Flywhe
         self.delta_z = 0
         self.shoot_angle:radians = 0
         self.base_rotation2d = Rotation2d(0)
+        self.bot_shot_tolerance:radians = radians(1)
+        self.t_total = 0
         self.elevator = elevator
         self.flywheel = flywheel
         self.table = ntcore.NetworkTableInstance.getDefault().getTable('shot calculations')
@@ -144,6 +146,7 @@ def update_shooter(self):
         theta_1 = self.calculate_angle_no_air(self.distance_to_target, self.delta_z)
         if not self.use_air_resistance:
             self.shoot_angle = theta_1
+            self.t_total = self.distance_to_target / (self.flywheel.get_velocity_linear() * np.cos(theta_1))
             return theta_1
         else:
             theta_2 = theta_1 + np.radians(1)
@@ -174,6 +177,37 @@ def get_rotation_to_speaker(self):
         robot_pose_2d = self.odometry.getPose()
         robot_to_speaker = speaker_translation - robot_pose_2d.translation()
         return robot_to_speaker.angle()
+
+    def get_shot_pos_tolerance(self):
+        """
+        returns tolerance of base to face target
+        :return: base tolerance angle
+        """
+        speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation()
+
+        bot_speaker_offset = config.speaker_length/2 - config.note_length/2
+
+        bot_speaker_translation:Translation2d = speaker_translation - Translation2d(0, bot_speaker_offset)
+
+        robot_pose_2d = self.odometry.getPose()
+        bot_speaker_origin = bot_speaker_translation - robot_pose_2d.translation()
+        small_angle = bot_speaker_origin.angle().radians()
+        self.table.putNumber('bot speaker angle', degrees(small_angle))
+        big_angle = self.get_rotation_to_speaker().radians()
+        tolerance = abs(abs(small_angle) - abs(big_angle)) - radians(config.drivetrain_static_tolerance_offset)
+        # return tolerance
+        return min(
+            max(tolerance, radians(config.min_drivetrain_tolerance)),
+            radians(config.max_drivetrain_tolerance)
+        )
+
+    def get_shot_vel_tolerance(self):
+        """
+        returns tolerance of base to face target
+        :return: base tolerance angle
+        """
+        tolerance = self.get_shot_pos_tolerance()
+        pass
 
     @staticmethod
     def get_rotation_auto(robot_pose: Pose2d) -> Rotation2d:
@@ -193,6 +227,7 @@ def update_base(self):
         :return: base target angle
         """
         self.base_rotation2d = self.get_rotation_to_speaker()
+        self.bot_shot_tolerance = self.get_shot_pos_tolerance()
         return self.base_rotation2d
 
     def update(self):
@@ -209,6 +244,7 @@ def update_tables(self):
         self.table.putNumber('wrist angle', degrees(self.get_theta()))
         self.table.putNumber('distance to target', self.distance_to_target)
         self.table.putNumber('bot angle', self.get_bot_theta().degrees())
+        self.table.putNumber('bot tolerance', degrees(self.get_shot_pos_tolerance()))
         self.table.putNumber('delta z', self.delta_z)
         self.table.putNumber('flywheel speed', self.get_flywheel_speed(self.distance_to_target))