Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

shooter: Fix divide by zero in leading shots #179

Merged
merged 3 commits into from
Apr 16, 2022
Merged

shooter: Fix divide by zero in leading shots #179

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d6d94d4
shooter: Fix divide by zero in leading shots
auscompgeek Apr 10, 2022
69689fc
shooter: Use Translation2d unary minus
auscompgeek Apr 10, 2022
614d653
[pre-commit.ci] auto fixes from pre-commit.com hooks
pre-commit-ci[bot] Apr 10, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 24 additions & 18 deletions controllers/shooter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,28 +85,34 @@ def setup(self) -> None:
def _track(self) -> None:
cur_pose = self.chassis.estimator.getEstimatedPosition()

# adjust shot to hit while moving
flight_time = 0.0 # Only compensate for time of flight if told to...
# clamp speed used for leading shots at our max allowed shooting speed
chassis_speed = self.chassis.translation_velocity.norm()
chassis_unit_velocity = self.chassis.translation_velocity / chassis_speed
chassis_velocity = chassis_unit_velocity * min(chassis_speed, self.MAX_SPEED)
for _ in range(3):
robot_movement = chassis_velocity * flight_time
effective_pose = Pose2d(
cur_pose.translation() + robot_movement, cur_pose.rotation()
)
self.distance = effective_pose.translation().distance(Translation2d())
flight_time = interpolate(
self.distance, self.ranges_lookup, self.times_lookup
if chassis_speed != 0:
# adjust shot to hit while moving
flight_time = 0.0 # Only compensate for time of flight if told to...

chassis_unit_velocity = self.chassis.translation_velocity / chassis_speed
chassis_velocity = chassis_unit_velocity * min(
chassis_speed, self.MAX_SPEED
)
if not self.lead_shots or self.distance < self.LEAD_MIN_DIST:
# Only run once if we aren't compensating. This will mean ToF is considered to be zero (ie no compensation)
break
for _ in range(3):
robot_movement = chassis_velocity * flight_time
effective_pose = Pose2d(
cur_pose.translation() + robot_movement, cur_pose.rotation()
)
self.distance = effective_pose.translation().distance(Translation2d())
if not self.lead_shots or self.distance < self.LEAD_MIN_DIST:
# Only run once if we aren't compensating.
# This will mean ToF is considered to be zero (ie no compensation)
break
flight_time = interpolate(
self.distance, self.ranges_lookup, self.times_lookup
)
else:
robot_movement = Translation2d()
effective_pose = cur_pose

self.field_effective_goal.setPose(
Pose2d(-robot_movement.X(), -robot_movement.Y(), Rotation2d(0))
)
self.field_effective_goal.setPose(Pose2d(-robot_movement, Rotation2d(0)))

turret_pose = self.chassis.robot_to_world(
self.shooter.turret_offset, effective_pose
Expand Down