From c70749ead8af1d29a9128302449b3e39dec10e3f Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Sun, 10 Mar 2024 17:39:29 -0500 Subject: [PATCH 01/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 50 +++++++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 12 deletions(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index 3b3d5a52..5462c4d4 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -11,7 +11,7 @@ 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 wpimath.geometry import Rotation2d, Translation3d, Translation2d, Transform2d @@ -44,26 +44,39 @@ def __init__(self, odometry: FieldOdometry, elevator: Elevator, flywheel: Flywhe self.table = ntcore.NetworkTableInstance.getDefault().getTable('shot calculations') self.numerical_integration = NumericalIntegration() self.use_air_resistance = False + self.v0_effective = 0 def init(self): self.speaker = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation() self.speaker_z = POI.Coordinates.Structures.Scoring.kSpeaker.getZ() + + + def get_drivetrain_speeds_speaker_origin(self): + + vels = self.odometry.drivetrain.chassis_speeds + + drivetrain_angle = self.get_rotation_to_speaker() + + + vels = self.odometry.drivetrain.chassis_speeds.fromRobotRelativeSpeeds(vels, drivetrain_angle) + + return vels + + def get_drivetrain_speeds_field_origin(self): + vels = self.odometry.drivetrain.chassis_speeds + + vels = self.odometry.drivetrain.chassis_speeds.fromRobotRelativeSpeeds(vels, self.odometry.getPose().rotation()) + + return vels def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: """ 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 - ) + rvx = self.get_drivetrain_speeds_speaker_origin().vx # Calculate the horizontal angle without considering velocities @@ -76,8 +89,9 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: # 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 = self.flywheel.get_velocity_linear()# + rvx + rvy - v_effective = config.v0_flywheel - + # v_effective = config.v0_flywheel + v_effective = self.flywheel.get_velocity_linear() - rvx * np.cos(phi0) + self.v0_effective = v_effective # Calculate the angle with floor velocities result_angle = ( 0.5 * np.arcsin( @@ -144,13 +158,25 @@ 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_rotation_to_speaker_moving(self): + speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation() + t_total = self.distance_to_target / self.v0_effective if self.v0_effective != 0 else 0 + + rvels = self.get_drivetrain_speeds_field_origin() + + robot_pose_2d = self.odometry.getPose() + robot_pose_2d_w_speeds = robot_pose_2d + Transform2d(rvels.vx * t_total, rvels.vy * t_total, rvels.omega * t_total) + robot_to_speaker = speaker_translation - robot_pose_2d_w_speeds.translation() + return robot_to_speaker.angle() def update_base(self): """ updates rotation of base to score shot :return: base target angle """ - self.base_rotation2d = self.get_rotation_to_speaker() + # self.base_rotation2d = self.get_rotation_to_speaker() + self.base_rotation2d = self.get_rotation_to_speaker_moving() return self.base_rotation2d def update(self): From d03b0d61af94fa6d3dc14824fd74bc5500b2a68c Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Sun, 10 Mar 2024 18:01:23 -0500 Subject: [PATCH 02/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index 5462c4d4..eef0391b 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -90,7 +90,7 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: # v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(drivetrain_angle.radians()) + rvy * np.cos(drivetrain_angle.radians()) # v_effective = self.flywheel.get_velocity_linear()# + rvx + rvy # v_effective = config.v0_flywheel - v_effective = self.flywheel.get_velocity_linear() - rvx * np.cos(phi0) + v_effective = config.v0_flywheel + rvx * np.cos(phi0) self.v0_effective = v_effective # Calculate the angle with floor velocities result_angle = ( From 59b6436a2b10c43edd6c7a899260fb34a8dc8591 Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Sun, 10 Mar 2024 18:19:13 -0500 Subject: [PATCH 03/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index eef0391b..b8a20bcd 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -194,6 +194,17 @@ def update_tables(self): 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('v effective', self.v0_effective) + self.table.putNumberArray('drivetrain speeds speaker', [ + self.get_drivetrain_speeds_speaker_origin().vx, + self.get_drivetrain_speeds_speaker_origin().vy, + self.get_drivetrain_speeds_speaker_origin().omega + ]) + self.table.putNumberArray('drivetrain speeds field', [ + self.get_drivetrain_speeds_field_origin().vx, + self.get_drivetrain_speeds_field_origin().vy, + self.get_drivetrain_speeds_field_origin().omega + ]) def run_sim(self, shooter_theta): def hit_target(t, u): From 65eee2dc0cc80212eec59d2969481a5e87c9f1e8 Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Wed, 13 Mar 2024 10:12:18 -0500 Subject: [PATCH 04/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index aafdaf47..020d4624 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -166,7 +166,7 @@ def get_rotation_to_speaker(self): def get_rotation_to_speaker_moving(self): speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation() - t_total = self.distance_to_target / self.v0_effective if self.v0_effective != 0 else 0 + t_total = self.get_distance_to_target() / (self.v0_effective * np.cos(self.get_theta())) if self.v0_effective != 0 else 0 rvels = self.get_drivetrain_speeds_field_origin() From d2399d91e09fbb84d0fb28565e19ebd2655619fd Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Wed, 13 Mar 2024 14:18:57 -0500 Subject: [PATCH 05/11] kk --- command/flywheel.py | 3 ++- sensors/trajectory_calc.py | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/command/flywheel.py b/command/flywheel.py index 2d9a7fd0..5c5bc8d0 100644 --- a/command/flywheel.py +++ b/command/flywheel.py @@ -22,7 +22,8 @@ def initialize(self): self.subsystem.set_velocity_linear(self.velocity) def execute(self): - print(self.subsystem.get_velocity_linear(), 'Current velocity') + ... + # print(self.subsystem.get_velocity_linear(), 'Current velocity') def isFinished(self): return self.subsystem.within_velocity_linear(self.velocity, 2) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index 020d4624..c1b7eaad 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -168,6 +168,7 @@ def get_rotation_to_speaker_moving(self): speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation() t_total = self.get_distance_to_target() / (self.v0_effective * np.cos(self.get_theta())) if self.v0_effective != 0 else 0 + rvels = self.get_drivetrain_speeds_field_origin() robot_pose_2d = self.odometry.getPose() From 0e54ec1151a51427a0b09169e6dd6fedb8c7c280 Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Wed, 13 Mar 2024 14:57:43 -0500 Subject: [PATCH 06/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index c1b7eaad..f7879207 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -90,7 +90,7 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: # v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(drivetrain_angle.radians()) + rvy * np.cos(drivetrain_angle.radians()) # v_effective = self.flywheel.get_velocity_linear()# + rvx + rvy # v_effective = config.v0_flywheel - v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(phi0) + v_effective = self.flywheel.get_velocity_linear() - rvx * np.cos(phi0) self.v0_effective = v_effective From 5a2a71b9f601573bc0494a71fe0adf16e148765c Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Wed, 13 Mar 2024 15:07:23 -0500 Subject: [PATCH 07/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index f7879207..ea2a1c62 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -76,7 +76,9 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: - rvx = self.get_drivetrain_speeds_speaker_origin().vx + rvx = self.get_drivetrain_speeds_field_origin().vx + + rvs = rvx * np.cos(self.get_rotation_to_speaker().radians()) # Calculate the horizontal angle without considering velocities @@ -90,7 +92,7 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: # v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(drivetrain_angle.radians()) + rvy * np.cos(drivetrain_angle.radians()) # v_effective = self.flywheel.get_velocity_linear()# + rvx + rvy # v_effective = config.v0_flywheel - v_effective = self.flywheel.get_velocity_linear() - rvx * np.cos(phi0) + v_effective = self.flywheel.get_velocity_linear() - rvs * np.cos(phi0) self.v0_effective = v_effective From 88d73cacb51f54c4c70203ccd16f76bbf2e9445a Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Thu, 14 Mar 2024 19:22:10 -0500 Subject: [PATCH 08/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index ea2a1c62..57529c8c 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -92,7 +92,7 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: # v_effective = self.flywheel.get_velocity_linear() + rvx * np.cos(drivetrain_angle.radians()) + rvy * np.cos(drivetrain_angle.radians()) # v_effective = self.flywheel.get_velocity_linear()# + rvx + rvy # v_effective = config.v0_flywheel - v_effective = self.flywheel.get_velocity_linear() - rvs * np.cos(phi0) + v_effective = self.flywheel.get_velocity_linear() + (rvs * np.cos(phi0)) self.v0_effective = v_effective From ddf3c40d3166c76491edfb52834ca19a69c46ed3 Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Thu, 14 Mar 2024 19:33:50 -0500 Subject: [PATCH 09/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index 57529c8c..5c1b6ddd 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -168,13 +168,13 @@ def get_rotation_to_speaker(self): def get_rotation_to_speaker_moving(self): speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kSpeaker.getTranslation() - t_total = self.get_distance_to_target() / (self.v0_effective * np.cos(self.get_theta())) if self.v0_effective != 0 else 0 + t_total = self.get_distance_to_target() / (self.v0_effective * np.cos(self.get_theta())) if self.v0_effective > 0 else 0 rvels = self.get_drivetrain_speeds_field_origin() robot_pose_2d = self.odometry.getPose() - robot_pose_2d_w_speeds = robot_pose_2d + Transform2d(rvels.vx * t_total, rvels.vy * t_total, rvels.omega * t_total) + robot_pose_2d_w_speeds = robot_pose_2d + Transform2d(rvels.vx * t_total, rvels.vy * t_total, 0) robot_to_speaker = speaker_translation - robot_pose_2d_w_speeds.translation() return robot_to_speaker.angle() From fbb4471db99ae510e9f2c488bdd03694fcac1e79 Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Thu, 14 Mar 2024 20:04:08 -0500 Subject: [PATCH 10/11] small updates --- constants.py | 2 +- sensors/trajectory_calc.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/constants.py b/constants.py index 29e1c4b6..182e881f 100644 --- a/constants.py +++ b/constants.py @@ -90,7 +90,7 @@ class Scoring: speaker_z = speaker_z_top #(speaker_z_top + speaker_z_bottom) / 2 - speaker_x = speaker_depth / 2.5 + speaker_x = speaker_depth / 1.5 amp_y = field_width diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index 5c1b6ddd..daa54777 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -103,9 +103,9 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: result_angle = ( 0.5 * np.arcsin( np.sin(phi0) - + constants.g + + (constants.g * distance_to_target - * np.cos(phi0) + * np.cos(phi0)) / (v_effective ** 2) ) + 0.5 * phi0 From 95e2752b035d434f8ce8485e829b730e231f013b Mon Sep 17 00:00:00 2001 From: Inspirol <91295122+Inspirol@users.noreply.github.com> Date: Sat, 16 Mar 2024 23:50:45 -0500 Subject: [PATCH 11/11] Update trajectory_calc.py --- sensors/trajectory_calc.py | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/sensors/trajectory_calc.py b/sensors/trajectory_calc.py index daa54777..f0cc2473 100644 --- a/sensors/trajectory_calc.py +++ b/sensors/trajectory_calc.py @@ -51,16 +51,14 @@ def init(self): self.speaker_z = POI.Coordinates.Structures.Scoring.kSpeaker.getZ() - def get_drivetrain_speeds_speaker_origin(self): + def get_drivetrain_speeds_speaker_distance(self): - vels = self.odometry.drivetrain.chassis_speeds - - drivetrain_angle = self.get_rotation_to_speaker() + rvx = self.get_drivetrain_speeds_field_origin().vx + rvs = rvx * np.cos(self.get_rotation_to_speaker().radians()) - vels = self.odometry.drivetrain.chassis_speeds.fromRobotRelativeSpeeds(vels, drivetrain_angle) - return vels + return rvs def get_drivetrain_speeds_field_origin(self): vels = self.odometry.drivetrain.chassis_speeds @@ -76,9 +74,8 @@ def calculate_angle_no_air(self, distance_to_target: float, delta_z) -> float: - rvx = self.get_drivetrain_speeds_field_origin().vx - rvs = rvx * np.cos(self.get_rotation_to_speaker().radians()) + rvs = self.get_drivetrain_speeds_speaker_distance() # Calculate the horizontal angle without considering velocities @@ -203,11 +200,9 @@ def update_tables(self): self.table.putNumber('bot angle', self.get_bot_theta().degrees()) self.table.putNumber('delta z', self.delta_z) self.table.putNumber('v effective', self.v0_effective) - self.table.putNumberArray('drivetrain speeds speaker', [ - self.get_drivetrain_speeds_speaker_origin().vx, - self.get_drivetrain_speeds_speaker_origin().vy, - self.get_drivetrain_speeds_speaker_origin().omega - ]) + self.table.putNumber('drivetrain speeds speaker distance', + self.get_drivetrain_speeds_speaker_distance(), + ) self.table.putNumberArray('drivetrain speeds field', [ self.get_drivetrain_speeds_field_origin().vx, self.get_drivetrain_speeds_field_origin().vy,