Wrote the 5 autonomous modes.
Change-Id: I46d3607811c78be14a3ca5f445899b15b0627fb5
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 97fe0dd..e5f0fff 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -46,7 +46,7 @@
// incorporating a small safety margin makes writing test cases much easier
// since you can directly compare statuses against the constants in the
// CollisionAvoidance class.
- constexpr double kSafetyMargin = 0.01; // radians
+ constexpr double kSafetyMargin = 0.02; // radians
// Avoid colliding the shooter with the frame.
// If the shoulder is below a certain angle or we want to move it below