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_
diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
index 812a0c3..e17990a 100755
--- a/frc971/control_loops/python/arm.py
+++ b/frc971/control_loops/python/arm.py
@@ -10,6 +10,7 @@
import matplotlib
from matplotlib import pylab
+
class Arm(control_loop.ControlLoop):
def __init__(self, name="Arm", mass=None):
super(Arm, self).__init__(name)
@@ -52,19 +53,19 @@
# Position difference is 1 - 2
# Input is [Voltage 1, Voltage 2]
- C1 = self.spring / (self.J * 0.5)
- C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
- C3 = self.G * self.G * self.Kt / (self.R * self.J * 0.5 * self.Kv)
+ self.C1 = self.spring / (self.J * 0.5)
+ self.C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
+ self.C3 = self.G * self.G * self.Kt / (self.R * self.J * 0.5 * self.Kv)
self.A_continuous = numpy.matrix(
[[0, 1, 0, 0],
- [0, -C3, 0, 0],
+ [0, -self.C3, 0, 0],
[0, 0, 0, 1],
- [0, 0, -C1 * 2.0, -C3]])
+ [0, 0, -self.C1 * 2.0, -self.C3]])
- print 'Full speed is', C2 / C3 * 12.0
+ print 'Full speed is', self.C2 / self.C3 * 12.0
- print 'Stall arm difference is', 12.0 * C2 / C1
+ print 'Stall arm difference is', 12.0 * self.C2 / self.C1
print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
@@ -72,9 +73,9 @@
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0, 0],
- [C2 / 2.0, C2 / 2.0],
+ [self.C2 / 2.0, self.C2 / 2.0],
[0, 0],
- [C2 / 2.0, -C2 / 2.0]])
+ [self.C2 / 2.0, -self.C2 / 2.0]])
self.C = numpy.matrix([[1, 0, 1, 0],
[1, 0, -1, 0]])
@@ -118,6 +119,65 @@
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ print 'Observer (Converted to a KF)', numpy.linalg.inv(self.A) * self.L
+
+ self.InitializeState()
+
+
+class IntegralArm(Arm):
+ def __init__(self, name="IntegralArm", mass=None):
+ super(IntegralArm, self).__init__(name=name, mass=mass)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.A_continuous = numpy.matrix(numpy.zeros((5, 5)))
+ self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
+ self.A_continuous[1, 4] = self.C2
+
+ # Start with the unmodified input
+ self.B_continuous_unaugmented = self.B_continuous
+ self.B_continuous = numpy.matrix(numpy.zeros((5, 2)))
+ self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
+
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((2, 5)))
+ self.C[0:2, 0:4] = self.C_unaugmented
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+ print 'A cont', self.A_continuous
+ print 'B cont', self.B_continuous
+ print 'A discrete', self.A
+
+ q_pos = 0.08
+ q_vel = 0.40
+
+ q_pos_diff = 0.08
+ q_vel_diff = 0.40
+ q_voltage = 6.0
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos_diff ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel_diff ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+
+ r_volts = 0.05
+ self.R = numpy.matrix([[(r_volts ** 2.0), 0.0],
+ [0.0, (r_volts ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 5)));
+ self.K[0:2, 0:4] = self.K_unaugmented
+ self.K[0, 4] = 1;
+ self.K[1, 4] = 1;
+ print 'Kal', self.KalmanGain
+ self.L = self.A * self.KalmanGain
+
self.InitializeState()
@@ -225,31 +285,125 @@
pylab.show()
+def run_integral_test(arm, initial_X, goal, observer_arm, disturbance,
+ max_separation_error=0.01, show_graph=True,
+ iterations=400):
+ """Runs the integral control arm plant with an initial condition and goal.
+
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more than max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ arm: arm object to use.
+ initial_X: starting state.
+ goal: goal state.
+ observer_arm: arm object to use for the observer.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for.
+ disturbance: Voltage missmatch between controller and model.
+ """
+
+ arm.X = initial_X
+
+ # Various lists for graphing things.
+ t = []
+ x_avg = []
+ x_sep = []
+ x_hat_avg = []
+ x_hat_sep = []
+ v_avg = []
+ v_sep = []
+ u_left = []
+ u_right = []
+ u_error = []
+
+ sep_plot_gain = 100.0
+
+ unaugmented_goal = goal
+ goal = numpy.matrix(numpy.zeros((5, 1)))
+ goal[0:4, 0] = unaugmented_goal
+
+ for i in xrange(iterations):
+ X_hat = observer_arm.X_hat[0:4]
+
+ x_hat_avg.append(observer_arm.X_hat[0, 0])
+ x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
+
+ U = observer_arm.K * (goal - observer_arm.X_hat)
+ u_error.append(observer_arm.X_hat[4,0])
+ U = CapU(U)
+ x_avg.append(arm.X[0, 0])
+ v_avg.append(arm.X[1, 0])
+ x_sep.append(arm.X[2, 0] * sep_plot_gain)
+ v_sep.append(arm.X[3, 0])
+
+ observer_arm.PredictObserver(U)
+
+ arm.Update(U + disturbance)
+ observer_arm.Y = arm.Y
+ observer_arm.CorrectObserver(U)
+
+ t.append(i * arm.dt)
+ u_left.append(U[0, 0])
+ u_right.append(U[1, 0])
+
+ print 'End is', observer_arm.X_hat[4, 0]
+
+ if show_graph:
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x_avg, label='x avg')
+ pylab.plot(t, x_sep, label='x sep')
+ if observer_arm is not None:
+ pylab.plot(t, x_hat_avg, label='x_hat avg')
+ pylab.plot(t, x_hat_sep, label='x_hat sep')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u_left, label='u left')
+ pylab.plot(t, u_right, label='u right')
+ pylab.plot(t, u_error, label='u error')
+ pylab.legend()
+ pylab.show()
+
+
def main(argv):
loaded_mass = 25
#loaded_mass = 0
arm = Arm(mass=13 + loaded_mass)
- arm_controller = Arm(mass=13 + 15)
- observer_arm = Arm(mass=13 + 15)
+ #arm_controller = Arm(mass=13 + 15)
+ #observer_arm = Arm(mass=13 + 15)
#observer_arm = None
+ integral_arm = IntegralArm(mass=13 + loaded_mass)
+ integral_arm.X_hat[0, 0] += 0.02
+ integral_arm.X_hat[2, 0] += 0.02
+ integral_arm.X_hat[4] = 0
+
# Test moving the arm with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
- #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(arm, initial_X, R, controller_arm=arm_controller,
- observer_arm=observer_arm)
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
# Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name for the arm."
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name for the arm and augmented arm."
else:
- arm = Arm("Arm", 2)
+ arm = Arm("Arm", mass=13)
loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
loop_writer.Write(argv[1], argv[2])
+ integral_arm = IntegralArm("IntegralArm", mass=13)
+ loop_writer = control_loop.ControlLoopWriter("IntegralArm", [integral_arm])
+ if argv[3][-3:] == '.cc':
+ loop_writer.Write(argv[4], argv[3])
+ else:
+ loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/update_arm.sh b/frc971/control_loops/update_arm.sh
index 40ef68a..c4c1ab9 100755
--- a/frc971/control_loops/update_arm.sh
+++ b/frc971/control_loops/update_arm.sh
@@ -5,4 +5,6 @@
cd $(dirname $0)
./python/arm.py fridge/arm_motor_plant.cc \
- fridge/arm_motor_plant.h
+ fridge/arm_motor_plant.h \
+ fridge/integral_arm_plant.cc \
+ fridge/integral_arm_plant.h