Added integral control to the arm.
Change-Id: I866d2749837841f3f8d34e6ffe0e1a0b11c4120e
diff --git a/frc971/control_loops/fridge/arm_motor_plant.cc b/frc971/control_loops/fridge/arm_motor_plant.cc
index c7d09ec..556b812 100644
--- a/frc971/control_loops/fridge/arm_motor_plant.cc
+++ b/frc971/control_loops/fridge/arm_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00385609864291, 0.0, 0.0, 0.0, 0.580316842139, 0.0, 0.0, 0.0, 0.0, 0.997415079306, 0.00385216571322, 0.0, 0.0, -0.94787542156, 0.578159966602;
+ A << 1.0, 0.00479642025454, 0.0, 0.0, 0.0, 0.919688585028, 0.0, 0.0, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, -0.18154390621, 0.919241022297;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000138504938452, 0.000138504938452, 0.050815736504, 0.050815736504, 0.000138436627927, -0.000138436627927, 0.0507639082867, -0.0507639082867;
+ B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 1, 0, 1, 0, -1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,11 +25,11 @@
StateFeedbackController<4, 2, 2> MakeArmController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.59015842107, 0.59015842107, 19.0789855292, 19.0789855292, 0.587787522954, -0.587787522954, 18.4121865078, -18.4121865078;
+ L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.759390396955, -0.759390396955, 54.1048167043, -54.1048167043;
Eigen::Matrix<double, 2, 4> K;
- K << 132.284914985, 5.45575813704, 227.613831364, 6.12234622766, 132.284914985, 5.45575813704, -227.613831364, -6.12234622766;
+ K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00664481600894, 0.0, 0.0, 0.0, 1.72319658398, 0.0, 0.0, 0.0, 0.0, 0.996283279443, -0.00663803879795, 0.0, 0.0, 1.63337568847, 1.71874225747;
+ A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 0.0, 1.08732457517, 0.0, 0.0, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.197397150694, 1.08682415753;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeArmPlantCoefficients());
}
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 9452637..46d4f28 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -6,7 +6,7 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/fridge/elevator_motor_plant.h"
-#include "frc971/control_loops/fridge/arm_motor_plant.h"
+#include "frc971/control_loops/fridge/integral_arm_plant.h"
#include "frc971/control_loops/voltage_cap/voltage_cap.h"
#include "frc971/zeroing/zeroing.h"
@@ -24,32 +24,35 @@
} // namespace
-void CappedStateFeedbackLoop::CapU() {
- VoltageCap(max_voltage_, U(0, 0), U(1, 0), &mutable_U(0, 0),
- &mutable_U(1, 0));
+template <int S>
+void CappedStateFeedbackLoop<S>::CapU() {
+ VoltageCap(max_voltage_, this->U(0, 0), this->U(1, 0), &this->mutable_U(0, 0),
+ &this->mutable_U(1, 0));
}
+template <int S>
Eigen::Matrix<double, 2, 1>
-CappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
+CappedStateFeedbackLoop<S>::UnsaturateOutputGoalChange() {
// Compute the K matrix used to compensate for position errors.
Eigen::Matrix<double, 2, 2> Kp;
Kp.setZero();
- Kp.col(0) = K().col(0);
- Kp.col(1) = K().col(2);
+ Kp.col(0) = this->K().col(0);
+ Kp.col(1) = this->K().col(2);
Eigen::Matrix<double, 2, 2> Kp_inv = Kp.inverse();
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
- Eigen::Matrix<double, 2, 1> deltaR = -Kp_inv * (U_uncapped() - U());
+ Eigen::Matrix<double, 2, 1> deltaR =
+ -Kp_inv * (this->U_uncapped() - this->U());
return deltaR;
}
Fridge::Fridge(control_loops::FridgeQueue *fridge)
: aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
- arm_loop_(new CappedStateFeedbackLoop(
- StateFeedbackLoop<4, 2, 2>(MakeArmLoop()))),
- elevator_loop_(new CappedStateFeedbackLoop(
+ arm_loop_(new CappedStateFeedbackLoop<5>(
+ StateFeedbackLoop<5, 2, 2>(MakeIntegralArmLoop()))),
+ elevator_loop_(new CappedStateFeedbackLoop<4>(
StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
right_arm_estimator_(constants::GetValues().fridge.right_arm_zeroing),
@@ -461,7 +464,7 @@
}
// Set the goals.
- arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity, 0.0, 0.0;
+ arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity, 0.0, 0.0, 0.0;
elevator_loop_->mutable_R() << elevator_goal_, elevator_goal_velocity, 0.0,
0.0;
diff --git a/frc971/control_loops/fridge/fridge.gyp b/frc971/control_loops/fridge/fridge.gyp
index 1226692..dc25b63 100644
--- a/frc971/control_loops/fridge/fridge.gyp
+++ b/frc971/control_loops/fridge/fridge.gyp
@@ -24,7 +24,7 @@
'type': 'static_library',
'sources': [
'fridge.cc',
- 'arm_motor_plant.cc',
+ 'integral_arm_plant.cc',
'elevator_motor_plant.cc',
],
'dependencies': [
@@ -45,6 +45,7 @@
'type': 'executable',
'sources': [
'fridge_lib_test.cc',
+ 'arm_motor_plant.cc',
],
'dependencies': [
'<(EXTERNALS):gtest',
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index fc10594..e9cbaa5 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -6,8 +6,6 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/fridge/fridge.q.h"
-#include "frc971/control_loops/fridge/arm_motor_plant.h"
-#include "frc971/control_loops/fridge/elevator_motor_plant.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
@@ -19,15 +17,16 @@
class FridgeTest_ArmGoalNegativeWindupTest_Test;
class FridgeTest_ElevatorGoalNegativeWindupTest_Test;
class FridgeTest_SafeArmZeroing_Test;
-}
+} // namespace testing
-class CappedStateFeedbackLoop : public StateFeedbackLoop<4, 2, 2> {
+template<int S>
+class CappedStateFeedbackLoop : public StateFeedbackLoop<S, 2, 2> {
public:
- CappedStateFeedbackLoop(StateFeedbackLoop<4, 2, 2> &&loop)
- : StateFeedbackLoop<4, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
+ CappedStateFeedbackLoop(StateFeedbackLoop<S, 2, 2> &&loop)
+ : StateFeedbackLoop<S, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
void set_max_voltage(double max_voltage) {
- max_voltage_ = ::std::max(-12.0, ::std::min(12.0, max_voltage));
+ max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
}
void CapU() override;
@@ -114,8 +113,8 @@
void Correct();
// The state feedback control loop or loops to talk to.
- ::std::unique_ptr<CappedStateFeedbackLoop> arm_loop_;
- ::std::unique_ptr<CappedStateFeedbackLoop> elevator_loop_;
+ ::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
+ ::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
zeroing::ZeroingEstimator left_arm_estimator_;
zeroing::ZeroingEstimator right_arm_estimator_;
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 1dff2ef..db25b08 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -8,8 +8,11 @@
#include "gtest/gtest.h"
#include "aos/common/queue.h"
#include "aos/common/time.h"
+#include "aos/common/commonmath.h"
#include "aos/common/controls/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/fridge/arm_motor_plant.h"
+#include "frc971/control_loops/fridge/elevator_motor_plant.h"
#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -116,13 +119,25 @@
position.Send();
}
+ // Sets the difference between the commanded and applied power for the arm.
+ // This lets us test that the integrator for the arm works.
+ void set_arm_power_error(double arm_power_error) {
+ arm_power_error_ = arm_power_error;
+ }
// Simulates for a single timestep.
void Simulate() {
EXPECT_TRUE(fridge_queue_.output.FetchLatest());
// Feed voltages into physics simulation.
- arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
- fridge_queue_.output->right_arm;
+ if (arm_power_error_ != 0.0) {
+ arm_plant_->mutable_U() << ::aos::Clip(
+ fridge_queue_.output->left_arm + arm_power_error_, -12, 12),
+ ::aos::Clip(fridge_queue_.output->right_arm + arm_power_error_, -12,
+ 12);
+ } else {
+ arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
+ fridge_queue_.output->right_arm;
+ }
elevator_plant_->mutable_U() << fridge_queue_.output->left_elevator,
fridge_queue_.output->right_elevator;
@@ -189,6 +204,7 @@
double elevator_initial_difference_ = 0.0;
double arm_initial_difference_ = 0.0;
+ double arm_power_error_ = 0.0;
};
class FridgeTest : public ::aos::testing::ControlLoopTest {
@@ -517,8 +533,11 @@
TEST_F(FridgeTest, ArmGoalPositiveWindupTest) {
fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+ int i = 0;
while (fridge_.state() != Fridge::ZEROING_ARM) {
RunIteration();
+ ++i;
+ ASSERT_LE(i, 10000);
}
// Kick it.
@@ -611,6 +630,20 @@
}
}
+// Tests that the arm integrator works.
+TEST_F(FridgeTest, ArmIntegratorTest) {
+ fridge_plant_.InitializeArmPosition(
+ (constants::GetValues().fridge.arm.lower_hard_limit +
+ constants::GetValues().fridge.arm.lower_hard_limit) /
+ 2.0);
+ fridge_plant_.set_arm_power_error(1.0);
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+
+ RunForTime(Time::InMS(8000));
+
+ VerifyNearGoal();
+}
+
// Phil:
// TODO(austin): Check that we e-stop if encoder index pulse is not n revolutions away from last one. (got extra counts from noise, etc).
// TODO(austin): Check that we e-stop if pot disagrees too much with encoder after we are zeroed.
diff --git a/frc971/control_loops/fridge/integral_arm_plant.cc b/frc971/control_loops/fridge/integral_arm_plant.cc
new file mode 100644
index 0000000..a101044
--- /dev/null
+++ b/frc971/control_loops/fridge/integral_arm_plant.cc
@@ -0,0 +1,49 @@
+#include "frc971/control_loops/fridge/integral_arm_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients() {
+ Eigen::Matrix<double, 5, 5> A;
+ A << 1.0, 0.00479642025454, 0.0, 0.0, 4.92993559969e-05, 0.0, 0.919688585028, 0.0, 0.0, 0.0194484035162, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, 0.0, -0.18154390621, 0.919241022297, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 5, 2> B;
+ B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532, 0.0, 0.0;
+ Eigen::Matrix<double, 2, 5> C;
+ C << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<5, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<5, 2, 2> MakeIntegralArmController() {
+ Eigen::Matrix<double, 5, 2> L;
+ L << 0.461805946837, 0.461805946837, 5.83483983392, 5.83483983392, 0.429725467802, -0.429725467802, 0.18044816586, -0.18044816586, 31.0623964848, 31.0623964848;
+ Eigen::Matrix<double, 2, 5> K;
+ K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 1.0, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119, 1.0;
+ Eigen::Matrix<double, 5, 5> A_inv;
+ A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 5.21292341391e-05, 0.0, 1.08732457517, 0.0, 0.0, -0.0211467270909, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.0, 0.197397150694, 1.08682415753, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+ return StateFeedbackController<5, 2, 2>(L, K, A_inv, MakeIntegralArmPlantCoefficients());
+}
+
+StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>(new StateFeedbackPlantCoefficients<5, 2, 2>(MakeIntegralArmPlantCoefficients()));
+ return StateFeedbackPlant<5, 2, 2>(&plants);
+}
+
+StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<5, 2, 2>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<5, 2, 2>>(new StateFeedbackController<5, 2, 2>(MakeIntegralArmController()));
+ return StateFeedbackLoop<5, 2, 2>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/fridge/integral_arm_plant.h b/frc971/control_loops/fridge/integral_arm_plant.h
new file mode 100644
index 0000000..f43f915
--- /dev/null
+++ b/frc971/control_loops/fridge/integral_arm_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients();
+
+StateFeedbackController<5, 2, 2> MakeIntegralArmController();
+
+StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant();
+
+StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_