Fix/wrist aim

sensors/trajectory_calc.py

@@ -1,19 +1,21 @@
 # import math
 # import time
 
+from math import degrees, isnan, radians
+
+import ntcore
+
 # import matplotlib.pyplot as plt
 # import ntcore
 import numpy as np
-from math import degrees, radians, isnan
-import config, ntcore
+from wpimath.geometry import Rotation2d, Translation2d, Translation3d
+
+import config
 import constants
 from sensors.field_odometry import FieldOdometry
 from subsystem import Elevator, Flywheel
 from toolkit.utils.toolkit_math import NumericalIntegration, extrapolate
 from utils import POI
-from wpimath.geometry import Rotation2d, Translation3d, Translation2d
-
-
 
 # from scipy.integrate import solve_ivp
 # from wpimath.geometry import Pose2d, Pose3d, Rotation2d, Translation2d
@@ -37,11 +39,13 @@ def __init__(self, odometry: FieldOdometry, elevator: Elevator, flywheel: Flywhe
         self.k = 0.5 * constants.c * constants.rho_air * constants.a
         self.distance_to_target = 0
         self.delta_z = 0
-        self.shoot_angle:radians = 0
+        self.shoot_angle: radians = 0
         self.base_rotation2d = Rotation2d(0)
         self.elevator = elevator
         self.flywheel = flywheel
-        self.table = ntcore.NetworkTableInstance.getDefault().getTable('shot calculations')
+        self.table = ntcore.NetworkTableInstance.getDefault().getTable(
+            "shot calculations"
+        )
         self.numerical_integration = NumericalIntegration()
         self.use_air_resistance = False
 
@@ -54,45 +58,46 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> radians:
         Calculates the angle of the trajectory without air resistance.
         """
 
-        vels = self.odometry.drivetrain.chassis_speeds
-
-        drivetrain_angle = self.get_rotation_to_speaker()
-
-
-        vels = self.odometry.drivetrain.chassis_speeds.fromRobotRelativeSpeeds(vels, drivetrain_angle)
-
-        rvx, rvy, rvo = (
-            vels.vx, vels.vy, vels.omega
-        )
-
+        # vels = self.odometry.drivetrain.chassis_speeds
 
-        # Calculate the horizontal angle without considering velocities
-        phi0 = np.arctan(delta_z / distance_to_target) if distance_to_target != 0 else np.radians(90)
+        # drivetrain_angle = self.get_rotation_to_speaker()
+
+        # vels = self.odometry.drivetrain.chassis_speeds.fromRobotRelativeSpeeds(
+        #     vels, drivetrain_angle
+        # )
 
+        # rvx, rvy, rvo = (vels.vx, vels.vy, vels.omega)
+
+        # Calculate the horizontal angle without considering velocities
+        phi0 = (
+            np.arctan(delta_z / distance_to_target)
+            if distance_to_target != 0
+            else np.radians(90)
+        )
 
         # Calculate the impact of floor velocities on the trajectory
 
-
         # Calculate the effective velocity
-        # v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(drivetrain_angle.radians()) + rvy * np.cos(drivetrain_angle.radians())
-        v_effective = config.v0_flywheel_minimum# + rvx + rvy
+        # v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(drivetrain_angle.radians()) +
+        # rvy * np.sin(drivetrain_angle.radians())
+        v_effective = config.v0_flywheel_minimum + distance_to_target  # + rvx + rvy
         # v_effective = config.v0_flywheel
 
         if v_effective == 0:
             return config.Giraffe.kIdle.wrist_angle
 
         # Calculate the angle with floor velocities
         result_angle = (
-            0.5 * np.arcsin(
+            0.5
+            * np.arcsin(
                 np.sin(phi0)
-                + constants.g
-                * distance_to_target
-                * np.cos(phi0)
-                / (v_effective ** 2)
+                + constants.g * distance_to_target * np.cos(phi0) / (v_effective**2)
             )
             + 0.5 * phi0
-        )
-
+        ) + radians(
+            -0.03 * distance_to_target**2 - 1.1 * distance_to_target + 1.71
+        )  # This is the offset for the shooter angle found by experimentation
+
         if isnan(result_angle):
             result_angle = config.Giraffe.kIdle.wrist_angle
 
@@ -103,16 +108,17 @@ def update_shooter(self):
         function runs sim to calculate a final angle with air resistance considered
         :return: target angle
         """
-        if type(self.speaker) == Translation3d:
+        if type(self.speaker) is Translation3d:
             self.speaker = self.speaker.toTranslation2d()
 
         self.distance_to_target = (
-            self.odometry.getPose().translation().distance(self.speaker) - constants.shooter_offset_y
+            self.odometry.getPose().translation().distance(self.speaker)
+            - constants.shooter_offset_y
         )
         # print("distance_to_target", self.distance_to_target)
 
         self.delta_z = (
-                self.speaker_z - self.elevator.get_length() - constants.shooter_height
+            self.speaker_z - self.elevator.get_length() - constants.shooter_height
         )
         theta_1 = self.calculate_angle_no_air(self.distance_to_target, self.delta_z)
         if not self.use_air_resistance:
@@ -137,13 +143,15 @@ def update_shooter(self):
                     self.shoot_angle = theta_2
                     return theta_2
                 correction_angle = z_goal_error * z_to_angle_conversion
-                
+
     def get_rotation_to_speaker(self):
         """
         returns rotation of base to face target
         :return: base target angle
         """
-        speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation()
+        speaker_translation: Translation2d = (
+            POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation()
+        )
         robot_pose_2d = self.odometry.getPose()
         robot_to_speaker = speaker_translation - robot_pose_2d.translation()
         return robot_to_speaker.angle()
@@ -167,11 +175,11 @@ def update(self):
         self.update_tables()
 
     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('delta z', self.delta_z)
-        
+        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("delta z", self.delta_z)
+
     def run_sim(self, shooter_theta):
         def hit_target(t, u):
             # We've hit the target if the distance to target is 0.
@@ -220,9 +228,8 @@ def get_bot_theta(self) -> Rotation2d:
         Returns the angle of the Robot
         """
         return self.base_rotation2d
-    
+
     def get_distance_to_target(self) -> float:
-
         return self.distance_to_target
 
     def deriv(self, t, u):

subsystem/drivetrain.py

@@ -26,7 +26,6 @@
     SwerveDrivetrain,
     SwerveNode,
 )
-
 foc_active = False
 
 
@@ -43,6 +42,7 @@ def init(self):
         print(f"Initializing {self.name}", self.counter)
         self.m_move.init()
         self.m_turn.init()
+        self.m_turn.optimize_normal_sparkmax()
         self.counter += 1
 
     def zero(self):

subsystem/elevator.py

@@ -29,7 +29,10 @@ def __init__(self) -> None:
 
     def init(self) -> None:
         self.motor_extend.init()
+        self.motor_extend.optimize_normal_sparkmax()
+
         self.motor_extend_follower.init()
+        self.motor_extend_follower.optimize_sparkmax_absolute_encoder()
 
         # Set the motor_extend encoder to the motor's absolute encoder
         self.motor_extend_encoder = self.motor_extend_follower.get_absolute_encoder()

subsystem/flywheel.py

@@ -13,7 +13,6 @@
 
 from units.SI import radians_per_second, meters_per_second, radians
 
-
 class Flywheel(Subsystem):
 
     def __init__(self):
@@ -117,6 +116,10 @@ def angular_velocity_to_linear_velocity(angular_velocity):
     def init(self) -> None:
         self.motor_1.init()
         self.motor_2.init()
+
+        self.motor_1.optimize_sparkmax_no_position()
+        self.motor_2.optimize_sparkmax_no_position()
+
         self.motor_1.motor.setSmartCurrentLimit(200)
         self.motor_2.motor.setSmartCurrentLimit(200)
 

subsystem/intake.py

@@ -5,7 +5,6 @@
 from rev import AnalogInput, CANSparkMax
 import ntcore
 
-
 class Intake(Subsystem):
 
     def __init__(self):
@@ -37,8 +36,11 @@ def __init__(self):
 
     def init(self):
         self.inner_motor.init()
+        self.inner_motor.optimize_sparkmax_no_position()
         self.outer_motor.init()
+        self.outer_motor.optimize_sparkmax_analog_sensor()
         self.deploy_motor.init()
+        self.deploy_motor.optimize_sparkmax_no_position()
         self.distance_sensor = self.outer_motor.get_analog()
 
     def set_inner_velocity(self, vel: float):

subsystem/wrist.py

@@ -10,7 +10,6 @@
 from units.SI import radians
 from wpilib import DigitalInput
 
-
 class Wrist(Subsystem):
     def __init__(self):
         super().__init__()
@@ -35,17 +34,10 @@ def __init__(self):
 
     def init(self):
         self.wrist_motor.init()
-        # self.wrist_motor.motor.restoreFactoryDefaults(True)
-        # self.wrist_motor.motor.setClosedLoopRampRate(config.wrist_time_to_max_vel)
-        # self.wrist_motor.pid_controller.setFeedbackDevice(self.wrist_motor.abs_encoder())
-        # self.wrist_motor.pid_controller.setFeedbackDevice(self.wrist_motor.encoder)
-
-        # self.wrist_motor.pid_controller.setPositionPIDWrappingEnabled(False)
-        # self.wrist_motor.pid_controller.setPositionPIDWrappingMinInput(self.radians_to_abs(constants.wrist_max_rotation))
-        # self.wrist_motor.pid_controller.setPositionPIDWrappingMaxInput(self.radians_to_abs(constants.wrist_min_rotation))
-        # self.wrist_motor.motor.burnFlash()
+        self.wrist_motor.optimize_sparkmax_absolute_encoder()
         self.wrist_abs_encoder = self.wrist_motor.abs_encoder()
         self.feed_motor.init()
+        self.feed_motor.optimize_sparkmax_no_position()
         self.beam_break_first = DigitalInput(config.feeder_beam_break_first_channel)
         self.beam_break_second = DigitalInput(config.feeder_beam_break_second_channel)
 

toolkit/motors/ctre_motors.py

@@ -92,12 +92,15 @@ class TalonFX(PIDMotor):
     _foc: bool
 
     _inverted: bool
+
+    _optimized: bool
 
-    def __init__(self, can_id: int, foc: bool = True, inverted: bool = False, config: TalonConfig = None):
+    def __init__(self, can_id: int, foc: bool = True, inverted: bool = False, config: TalonConfig = None, optimize: bool = True):
         self._inverted = inverted
         self._foc = foc
         self._can_id = can_id
         self._talon_config = config
+        self._optimized = optimize
 
     def init(self):
         print('Initializing TalonFX', self._can_id)
@@ -107,9 +110,13 @@ def init(self):
         self._motor_vel = self._motor.get_velocity()
         self._motor_current = self._motor.get_torque_current()
         if self._talon_config is not None:
-            ...
             self._talon_config._apply_settings(self._motor, self._inverted)
+
         self.__setup_controls()
+
+        if self._optimized:
+            if self.optimize_normal_operation() == StatusCode.OK:
+                print('talon optimized')
 
     def __setup_controls(self):
         self._mm_v_v = controls.MotionMagicVelocityVoltage(0)
@@ -145,3 +152,17 @@ def get_sensor_velocity(self) -> rotations_per_second:
     def get_motor_current(self) -> float:
         self._motor_current.refresh()
         return self._motor_current.value
+
+    def optimize_normal_operation(self, ms:int=25) -> StatusCode.OK:
+        """removes every status signal except for motor position, current, and velocty to optimize bus utilization
+
+        Args: 
+            ms: (int, optional) the update frequency of the status signals (default is 25ms)
+
+        Returns:
+            StatusCode.OK: if the talon was optimized
+        """
+        self._motor_pos.set_update_frequency(ms)
+        self._motor_vel.set_update_frequency(ms)
+        self._motor_current.set_update_frequency(ms)
+        return self._motor.optimize_bus_utilization()