Override Hood and Shooter goals with vision distance
Change-Id: I9741880fe7f96cf4208ca2cf75d584d683a30cea
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 63ab691..2de0ab8 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -42,11 +42,33 @@
vision_status = vision::vision_status.get();
}
- hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
- &(position->hood),
- output != nullptr ? &(output->voltage_hood) : nullptr,
- &(status->hood));
- shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
+ // Create a copy of the goals so that we can modify them.
+ HoodGoal hood_goal;
+ ShooterGoal shooter_goal;
+ if (unsafe_goal != nullptr) {
+ hood_goal = unsafe_goal->hood;
+ shooter_goal = unsafe_goal->shooter;
+
+ distance_average_.Tick(::aos::monotonic_clock::now(), vision_status);
+ status->vision_distance = distance_average_.Get();
+ if (distance_average_.Valid()) {
+ LOG(DEBUG, "VisionDistance %f\n", status->vision_distance);
+ if (unsafe_goal->use_vision_for_shots) {
+ auto shot_params =
+ constants::GetValues().shot_interpolation_table.GetShooterData(
+ distance_average_.Get());
+ hood_goal.angle = shot_params.angle;
+ shooter_goal.angular_velocity = shot_params.power;
+ }
+ } else {
+ LOG(DEBUG, "VisionNotValid %f\n", status->vision_distance);
+ }
+ }
+
+ hood_.Iterate(
+ unsafe_goal != nullptr ? &hood_goal : nullptr, &(position->hood),
+ output != nullptr ? &(output->voltage_hood) : nullptr, &(status->hood));
+ shooter_.Iterate(unsafe_goal != nullptr ? &shooter_goal : nullptr,
&(position->theta_shooter), position->sent_time,
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));