commit 6ca0f797a4760be0d4ac834773f2ca773f05ea08
author Austin Schuh <austin.linux@gmail.com> Sat Mar 12 14:06:14 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 12 23:02:15 2016 -0800
tree ed69b289c5891a7a27e105d75e0dbdcbca5b9f17
parent 3f0b1197fd7948af5525a86a742cef62be41e92c

Fixed collision detection.

Tests all pass, added gain scheduling to arm, etc.

Change-Id: I4de5427b816961ead5b23c7d9287ff40dcc10f46

frc971/control_loops/state_feedback_loop.h

diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index e78a786..e535ae7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -148,13 +148,9 @@
 
   int plant_index() const { return plant_index_; }
   void set_plant_index(int plant_index) {
-    if (plant_index < 0) {
-      plant_index_ = 0;
-    } else if (plant_index >= static_cast<int>(coefficients_.size())) {
-      plant_index_ = static_cast<int>(coefficients_.size()) - 1;
-    } else {
-      plant_index_ = plant_index;
-    }
+    assert(plant_index >= 0);
+    assert(plant_index < static_cast<int>(coefficients_.size()));
+    plant_index_ = plant_index;
   }
 
   void Reset() {