Added debug prints for detecting collisions.
Change-Id: If7e0f61550d8cb07ecd7851cc30766770ee449b7
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index a6071aa..1c5dbec 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -124,6 +124,11 @@
shoulder_angle <=
CollisionAvoidance::kMinShoulderAngleForIntakeInterference &&
intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
+ LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n", intake_angle,
+ CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
+ CollisionAvoidance::kMinShoulderAngleForIntakeInterference,
+ shoulder_angle,
+ CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing);
return true;
}
@@ -132,6 +137,10 @@
if (shoulder_angle <
CollisionAvoidance::kMinShoulderAngleForHorizontalShooter &&
::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
+ LOG(DEBUG, "Collided: Shoulder %f < %f and wrist |%f| < %f.\n",
+ shoulder_angle,
+ CollisionAvoidance::kMinShoulderAngleForHorizontalShooter, wrist_angle,
+ kMaxWristAngleForSafeArmStowing);
return true;
}