commit 2b1957aeb46d9cd96aa05477ed211bfabaccccbf
author Brian Silverman <brians> Sun Feb 14 20:29:57 2016 -0500
committer Brian Silverman <brians> Sun Feb 14 20:36:47 2016 -0500
tree 2f060426875e9235a30265d03923ced870047179
parent 10c2d11ec8c7839f0d8b7a7f75147b364fdf19f9

Create a nice general SimpleCappedStateFeedbackLoop

Change-Id: I79e686b1c6d689735b73634ccf9877e00bac4348

y2016/control_loops/superstructure/superstructure_controls.cc

diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7cd67e3..3c3e690 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -25,22 +25,10 @@
 }
 }  // namespace
 
-void SimpleCappedStateFeedbackLoop::CapU() {
-  mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
-  mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
-}
-
-void DoubleCappedStateFeedbackLoop::CapU() {
-  mutable_U(0, 0) = ::std::min(U(0, 0), shoulder_max_voltage_);
-  mutable_U(0, 0) = ::std::max(U(0, 0), -shoulder_max_voltage_);
-  mutable_U(1, 0) = ::std::min(U(1, 0), wrist_max_voltage_);
-  mutable_U(1, 0) = ::std::max(U(1, 0), -wrist_max_voltage_);
-}
-
 // Intake
 Intake::Intake()
-    : loop_(new SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1>(
-          ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop()))),
+    : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>(
+          ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop())),
       estimator_(constants::GetValues().intake.zeroing),
       profile_(::aos::controls::kLoopFrequency) {
   Y_.setZero();
@@ -147,7 +135,7 @@
 }
 
 void Intake::set_max_voltage(double voltage) {
-  loop_->set_max_voltage(voltage);
+  loop_->set_max_voltages(voltage);
 }
 
 void Intake::AdjustProfile(double max_angular_velocity,
@@ -171,7 +159,7 @@
 }
 
 Arm::Arm()
-    : loop_(new DoubleCappedStateFeedbackLoop(
+    : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>(
           ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
       shoulder_profile_(::aos::controls::kLoopFrequency),
       wrist_profile_(::aos::controls::kLoopFrequency),
@@ -368,7 +356,7 @@
 
 void Arm::set_max_voltage(double shoulder_max_voltage,
                           double wrist_max_voltage) {
-  loop_->set_max_voltage(shoulder_max_voltage, wrist_max_voltage);
+  loop_->set_max_voltages(shoulder_max_voltage, wrist_max_voltage);
 }
 
 void Arm::Reset() {