Added A_continuous and B_continuous to StateFeedbackPlantCoefficients
Change-Id: I0c2649e0ef4986c6b9122bf59adc8ad1d45572f4
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index d6bd85f..bb34b85 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -276,15 +276,17 @@
num_states, num_inputs, num_outputs, self._name)]
ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
ans.append(self._DumpMatrix('C', self.C))
ans.append(self._DumpMatrix('D', self.D))
ans.append(self._DumpMatrix('U_max', self.U_max))
ans.append(self._DumpMatrix('U_min', self.U_min))
ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
- num_outputs))
+ '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
+ num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index e535ae7..a81f2cc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -26,7 +26,9 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A_(other.A()),
+ A_continuous_(other.A_continuous()),
B_(other.B()),
+ B_continuous_(other.B_continuous()),
C_(other.C()),
D_(other.D()),
U_min_(other.U_min()),
@@ -34,21 +36,41 @@
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &A_continuous,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ &B_continuous,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
- : A_(A), B_(B), C_(C), D_(D), U_min_(U_min), U_max_(U_max) {}
+ : A_(A),
+ A_continuous_(A_continuous),
+ B_(B),
+ B_continuous_(B_continuous),
+ C_(C),
+ D_(D),
+ U_min_(U_min),
+ U_max_(U_max) {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
return A_;
}
double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_continuous()
+ const {
+ return A_continuous_;
+ }
+ double A_continuous(int i, int j) const { return A_continuous()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
return B_;
}
double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B_continuous() const {
+ return B_continuous_;
+ }
+ double B_continuous(int i, int j) const { return B_continuous()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
return C_;
}
@@ -68,7 +90,9 @@
private:
const Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous_;
const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous_;
const Eigen::Matrix<double, number_of_outputs, number_of_states> C_;
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D_;
const Eigen::Matrix<double, number_of_inputs, 1> U_min_;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 6f97bc7..a08841e 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -14,6 +14,8 @@
// compile or have assertion failures at runtime.
const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index a4767d1..6a6bb3e 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -108,6 +108,16 @@
A_unaugmented = self.A
B_unaugmented = self.B
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
+
self.A = numpy.matrix([[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0]])
@@ -147,6 +157,16 @@
A_unaugmented = self.A
B_unaugmented = self.B
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
+
self.A = numpy.matrix([[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0]])
diff --git a/y2015/control_loops/fridge/BUILD b/y2015/control_loops/fridge/BUILD
index a9bb737..dbecb75 100644
--- a/y2015/control_loops/fridge/BUILD
+++ b/y2015/control_loops/fridge/BUILD
@@ -75,6 +75,21 @@
],
)
+genrule(
+ name = 'genrule_arm_motor',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015/control_loops/python:arm) $(OUTS)',
+ tools = [
+ '//y2015/control_loops/python:arm',
+ ],
+ outs = [
+ 'arm_motor_plant.h',
+ 'arm_motor_plant.cc',
+ 'integral_arm_plant.h',
+ 'integral_arm_plant.cc',
+ ],
+)
+
cc_test(
name = 'fridge_lib_test',
srcs = [
diff --git a/y2015/control_loops/fridge/arm_motor_plant.cc b/y2015/control_loops/fridge/arm_motor_plant.cc
deleted file mode 100644
index 6e3205a..0000000
--- a/y2015/control_loops/fridge/arm_motor_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015/control_loops/fridge/arm_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- 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 << 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;
- 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<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeArmController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.759390396955, -0.759390396955, 54.1048167043, -54.1048167043;
- Eigen::Matrix<double, 2, 4> K;
- 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.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());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeArmPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeArmPlantCoefficients()));
- return StateFeedbackPlant<4, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeArmLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeArmController()));
- return StateFeedbackLoop<4, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2015/control_loops/fridge/arm_motor_plant.h b/y2015/control_loops/fridge/arm_motor_plant.h
deleted file mode 100644
index 3bf8d09..0000000
--- a/y2015/control_loops/fridge/arm_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
-#define Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeArmController();
-
-StateFeedbackPlant<4, 2, 2> MakeArmPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeArmLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index 4c24a1f..518d41f 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -54,7 +54,7 @@
Fridge::Fridge(FridgeQueue *fridge)
: aos::controls::ControlLoop<FridgeQueue>(fridge),
arm_loop_(new CappedStateFeedbackLoop<5>(StateFeedbackLoop<5, 2, 2>(
- ::frc971::control_loops::MakeIntegralArmLoop()))),
+ MakeIntegralArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop<4>(
StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
index 6acc983..16a1f15 100644
--- a/y2015/control_loops/fridge/fridge_lib_test.cc
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -34,8 +34,7 @@
static constexpr double kNoiseScalar = 0.1;
// Constructs a simulation.
FridgeSimulation()
- : arm_plant_(new StateFeedbackPlant<4, 2, 2>(
- ::frc971::control_loops::MakeArmPlant())),
+ : arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
elevator_plant_(new StateFeedbackPlant<4, 2, 2>(MakeElevatorPlant())),
left_arm_pot_encoder_(
constants::GetValues().fridge.left_arm_zeroing.index_difference),
diff --git a/y2015/control_loops/fridge/integral_arm_plant.cc b/y2015/control_loops/fridge/integral_arm_plant.cc
deleted file mode 100644
index da269d7..0000000
--- a/y2015/control_loops/fridge/integral_arm_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015/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/y2015/control_loops/fridge/integral_arm_plant.h b/y2015/control_loops/fridge/integral_arm_plant.h
deleted file mode 100644
index 80a4876..0000000
--- a/y2015/control_loops/fridge/integral_arm_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
-#define Y2015_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 // Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
diff --git a/y2015/control_loops/python/arm.py b/y2015/control_loops/python/arm.py
index 9fa270e..d3d77c5 100755
--- a/y2015/control_loops/python/arm.py
+++ b/y2015/control_loops/python/arm.py
@@ -1,18 +1,22 @@
#!/usr/bin/python
-import control_loop
-import controls
-import polytope
-import polydrivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
import numpy
import math
import sys
-import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm", mass=None):
+ def __init__(self, name='Arm', mass=None):
super(Arm, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
@@ -63,12 +67,12 @@
[0, 0, 0, 1],
[0, 0, -self.C1 * 2.0, -self.C3]])
- print 'Full speed is', self.C2 / self.C3 * 12.0
+ glog.debug('Full speed is %f', self.C2 / self.C3 * 12.0)
- 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
+ glog.debug('Stall arm difference is %f', 12.0 * self.C2 / self.C1)
+ glog.debug('Stall arm difference first principles is %f', self.stall_torque * self.G / self.spring)
- print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
+ glog.debug('5 degrees of arm error is %f', self.spring / self.r * (math.pi * 5.0 / 180.0))
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -86,8 +90,8 @@
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
- print 'Rank of augmented controllability matrix.', numpy.linalg.matrix_rank(
- controllability)
+ glog.debug('Rank of augmented controllability matrix. %d', numpy.linalg.matrix_rank(
+ controllability))
q_pos = 0.02
q_vel = 0.300
@@ -101,11 +105,10 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'Controller'
- print self.K
+ glog.debug('Controller\n %s', repr(self.K))
- print 'Controller Poles'
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('Controller Poles\n %s',
+ numpy.linalg.eig(self.A - self.B * self.K)[0])
self.rpl = 0.20
self.ipl = 0.05
@@ -119,13 +122,14 @@
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
+ glog.debug('Observer (Converted to a KF):\n%s',
+ repr(numpy.linalg.inv(self.A) * self.L))
self.InitializeState()
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm", mass=None):
+ def __init__(self, name='IntegralArm', mass=None):
super(IntegralArm, self).__init__(name=name, mass=mass)
self.A_continuous_unaugmented = self.A_continuous
@@ -144,9 +148,9 @@
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
+ glog.debug('A cont: %s', repr(self.A_continuous))
+ glog.debug('B cont %s', repr(self.B_continuous))
+ glog.debug('A discrete %s', repr(self.A))
q_pos = 0.08
q_vel = 0.40
@@ -175,7 +179,7 @@
self.K[0:2, 0:4] = self.K_unaugmented
self.K[0, 4] = 1
self.K[1, 4] = 1
- print 'Kal', self.KalmanGain
+ glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
self.InitializeState()
@@ -264,10 +268,10 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print numpy.linalg.inv(arm.A)
- print "delta time is ", arm.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+ glog.debug(repr(numpy.linalg.inv(arm.A)))
+ glog.debug('delta time is %f', arm.dt)
+ glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
+ glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -350,7 +354,7 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print 'End is', observer_arm.X_hat[4, 0]
+ glog.debug('End is %f', observer_arm.X_hat[4, 0])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -370,40 +374,40 @@
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)
- #observer_arm = None
+ if FLAGS.plot:
+ loaded_mass = 25
+ #loaded_mass = 0
+ arm = Arm(mass=13 + loaded_mass)
+ #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
+ 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.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)
+ # Test moving the arm with constant separation.
+ 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) != 5:
- print "Expected .h file name and .cc file name for the arm and augmented arm."
+ glog.fatal('Expected .h file name and .cc file name for the arm and augmented arm.')
else:
- 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])
+ namespaces = ['y2015', 'control_loops', 'fridge']
+ arm = Arm('Arm', mass=13)
+ loop_writer = control_loop.ControlLoopWriter('Arm', [arm],
+ namespaces=namespaces)
+ 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])
+ integral_arm = IntegralArm('IntegralArm', mass=13)
+ loop_writer = control_loop.ControlLoopWriter('IntegralArm', [integral_arm],
+ namespaces=namespaces)
+ loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/claw.py b/y2015/control_loops/python/claw.py
index 86a261d..9987089 100755
--- a/y2015/control_loops/python/claw.py
+++ b/y2015/control_loops/python/claw.py
@@ -18,7 +18,7 @@
pass
class Claw(control_loop.ControlLoop):
- def __init__(self, name="Claw", mass=None):
+ def __init__(self, name='Claw', mass=None):
super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
@@ -74,7 +74,7 @@
controllability = controls.ctrb(self.A, self.B)
- print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+ glog.debug('Free speed is %f', self.free_speed * numpy.pi * 2.0 / 60.0 / self.G)
q_pos = 0.15
q_vel = 2.5
@@ -84,15 +84,15 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'K', self.K
- print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('K: %s', repr(self.K))
+ glog.debug('Poles are: %s', repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
- print 'L is', self.L
+ glog.debug('L is: %s', repr(self.L))
q_pos = 0.05
q_vel = 2.65
@@ -105,9 +105,9 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- print 'Kal', self.KalmanGain
+ glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
- print 'KalL is', self.L
+ glog.debug('KalL is: %s', repr(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -118,7 +118,7 @@
def run_test(claw, initial_X, goal, max_separation_error=0.01,
- show_graph=False, iterations=200, controller_claw=None,
+ iterations=200, controller_claw=None,
observer_claw=None):
"""Runs the claw plant with an initial condition and goal.
@@ -131,8 +131,6 @@
claw: claw object to use.
initial_X: starting state.
goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
iterations: Number of timesteps to run the model for.
controller_claw: claw object to get K from, or None if we should
use claw.
@@ -177,32 +175,32 @@
t.append(i * claw.dt)
u.append(U[0, 0])
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x, label='x')
- if observer_claw is not None:
- pylab.plot(t, x_hat, label='x_hat')
- pylab.legend()
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x, label='x')
+ if observer_claw is not None:
+ pylab.plot(t, x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u, label='u')
- pylab.legend()
- pylab.show()
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u, label='u')
+ pylab.legend()
+ pylab.show()
def main(argv):
- loaded_mass = 0
- #loaded_mass = 0
- claw = Claw(mass=4 + loaded_mass)
- claw_controller = Claw(mass=5 + 0)
- observer_claw = Claw(mass=5 + 0)
- #observer_claw = None
+ if FLAGS.plot:
+ loaded_mass = 0
+ #loaded_mass = 0
+ claw = Claw(mass=4 + loaded_mass)
+ claw_controller = Claw(mass=5 + 0)
+ observer_claw = Claw(mass=5 + 0)
+ #observer_claw = None
- # Test moving the claw with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0]])
- run_test(claw, initial_X, R, controller_claw=claw_controller,
- observer_claw=observer_claw)
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0]])
+ run_test(claw, initial_X, R, controller_claw=claw_controller,
+ observer_claw=observer_claw)
# Write the generated constants out to a file.
if len(argv) != 3:
@@ -215,4 +213,6 @@
loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/elevator.py b/y2015/control_loops/python/elevator.py
index dc9caa1..d522034 100755
--- a/y2015/control_loops/python/elevator.py
+++ b/y2015/control_loops/python/elevator.py
@@ -8,6 +8,14 @@
import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
class Elevator(control_loop.ControlLoop):
def __init__(self, name="Elevator", mass=None):
super(Elevator, self).__init__(name)
@@ -57,7 +65,7 @@
[0, 0, 0, 1],
[0, 0, -C1 * 2.0, -C3]])
- print "Full speed is", C2 / C3 * 12.0
+ glog.debug('Full speed is', C2 / C3 * 12.0)
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -74,11 +82,11 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print self.A
+ glog.debug(repr(self.A))
controllability = controls.ctrb(self.A, self.B)
- print "Rank of augmented controllability matrix.", numpy.linalg.matrix_rank(
- controllability)
+ glog.debug('Rank of augmented controllability matrix: %d',
+ numpy.linalg.matrix_rank(controllability))
q_pos = 0.02
q_vel = 0.400
@@ -92,9 +100,9 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
+ glog.debug(repr(self.K))
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug(repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.20
self.ipl = 0.05
@@ -194,10 +202,10 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print numpy.linalg.inv(elevator.A)
- print "delta time is ", elevator.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+ glog.debug(repr(numpy.linalg.inv(elevator.A)))
+ glog.debug('delta time is %f', elevator.dt)
+ glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
+ glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -232,7 +240,7 @@
# Write the generated constants out to a file.
if len(argv) != 3:
- print "Expected .h file name and .cc file name for the elevator."
+ glog.fatal('Expected .h file name and .cc file name for the elevator.')
else:
namespaces = ['y2015', 'control_loops', 'fridge']
elevator = Elevator("Elevator")
@@ -244,4 +252,6 @@
loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015_bot3/control_loops/python/elevator3.py b/y2015_bot3/control_loops/python/elevator3.py
index 4b15bd0..e67ed47 100755
--- a/y2015_bot3/control_loops/python/elevator3.py
+++ b/y2015_bot3/control_loops/python/elevator3.py
@@ -80,8 +80,8 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- glog.info('K %s', str(self.K))
- glog.info('Poles are %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', str(self.K))
+ glog.debug('Poles are %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
@@ -100,7 +100,7 @@
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
- glog.info('KalL is %s', str(self.L))
+ glog.debug('KalL is %s', str(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -237,8 +237,6 @@
def main(argv):
- argv = FLAGS(argv)
-
loaded_mass = 7+4.0
#loaded_mass = 0
#observer_elevator = None
@@ -255,7 +253,7 @@
for i in xrange(0, 7):
elevator = Elevator(mass=i*totemass + loaded_mass)
- glog.info('Actual poles are %s', str(numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]))
+ glog.debug('Actual poles are %s', str(numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]))
elevator.X = initial_X
scenario_plotter.run_test(elevator, goal=up_R, controller_elevator=elevator_controller,
@@ -282,4 +280,6 @@
integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 64732b5..6c0f2f3 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import scipy
import sys
from matplotlib import pylab
@@ -132,6 +133,11 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
+ glog.debug('A: \n%s', repr(self.A_continuous))
+ glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
+ glog.debug('schur(A): \n%s', repr(scipy.linalg.schur(self.A_continuous)))
+ glog.debug('A_dt(A): \n%s', repr(self.A))
+
q_pos = 0.01
q_vel = 2.0
q_voltage = 0.2