Break collision detection into CollisionAvoidance
This lets us re-use the code for when we want to write the "slow
running" state in the superstructure.
Change-Id: Ie61437293ff2f27453623b4d63f3d63067fb1d24
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 346a33b..25b7939 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -48,6 +48,17 @@
void UpdateGoal(double shoulder_angle_goal, double wrist_angle_goal,
double intake_angle_goal);
+ // Returns true if any of the limbs and frame are somehow currently
+ // interfering with one another. This is based purely on the angles that the
+ // limbs are reporting.
+ bool collided() const;
+
+ // Detects collision with the specified angles. This is especially useful for
+ // unit testing where we have proper ground truth for all the angles.
+ static bool collided_with_given_angles(double shoulder_angle,
+ double wrist_angle,
+ double intake_angle);
+
// TODO(phil): Verify that these numbers actually make sense. Someone needs
// to verify these either on the CAD or the real robot.
@@ -162,7 +173,10 @@
double move_distance);
// Returns true if collision avoidance is turned on. False if not.
- bool collision_avoidance_enabled() { return collision_avoidance_enabled_; }
+ bool collision_avoidance_enabled() const { return collision_avoidance_enabled_; }
+
+ // Returns true if anything is currently considered "collided".
+ bool collided() const { return collision_avoidance_.collided(); }
protected:
virtual void RunIteration(