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