Wrist and angle adjust now use delta U controllers.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.gyp b/frc971/control_loops/angle_adjust/angle_adjust.gyp
index cb3ba1b..bc5c26d 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.gyp
+++ b/frc971/control_loops/angle_adjust/angle_adjust.gyp
@@ -21,6 +21,7 @@
'sources': [
'angle_adjust.cc',
'angle_adjust_motor_plant.cc',
+ 'unaugmented_angle_adjust_motor_plant.cc',
],
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index 7dc86c7..620d784 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -8,6 +8,7 @@
#include "aos/common/queue_testutils.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+#include "frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h"
#include "frc971/constants.h"
@@ -26,7 +27,7 @@
// angle_adjust, which will be treated as 0 by the encoder.
explicit AngleAdjustMotorSimulation(double initial_position)
: angle_adjust_plant_(
- new StateFeedbackPlant<2, 1, 1>(MakeAngleAdjustPlant())),
+ new StateFeedbackPlant<2, 1, 1>(MakeRawAngleAdjustPlant())),
my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
".frc971.control_loops.angle_adjust.position",
@@ -42,6 +43,7 @@
angle_adjust_plant_->X(1, 0) = 0.0;
angle_adjust_plant_->Y = angle_adjust_plant_->C() * angle_adjust_plant_->X;
last_position_ = angle_adjust_plant_->Y(0, 0);
+ last_voltage_ = 0.0;
calibration_value_[0] = 0.0;
calibration_value_[1] = 0.0;
}
@@ -110,7 +112,7 @@
void Simulate() {
last_position_ = angle_adjust_plant_->Y(0, 0);
EXPECT_TRUE(my_angle_adjust_loop_.output.FetchLatest());
- angle_adjust_plant_->U << my_angle_adjust_loop_.output->voltage;
+ angle_adjust_plant_->U << last_voltage_;
angle_adjust_plant_->Update();
// Assert that we are in the right physical range.
@@ -123,6 +125,7 @@
EXPECT_GE(upper_physical_limit, angle_adjust_plant_->Y(0, 0));
EXPECT_LE(lower_physical_limit, angle_adjust_plant_->Y(0, 0));
+ last_voltage_ = my_angle_adjust_loop_.output->voltage;
}
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
@@ -131,6 +134,7 @@
AngleAdjustLoop my_angle_adjust_loop_;
double initial_position_;
double last_position_;
+ double last_voltage_;
double calibration_value_[2];
};
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
index c23a64b..185030f 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
@@ -7,40 +7,40 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00844804908295, 0.0, 0.706562970689;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000186726546509, 0.0353055515475;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
+StateFeedbackPlantCoefficients<3, 1, 1> MakeAngleAdjustPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00844804908295, 0.000186726546509, 0.0, 0.706562970689, 0.0353055515475, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
Eigen::Matrix<double, 1, 1> D;
- D << 0;
+ D << 0.0;
Eigen::Matrix<double, 1, 1> U_max;
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 1, 1> MakeAngleAdjustController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.60656297069, 51.0341417582;
- Eigen::Matrix<double, 1, 2> K;
- K << 311.565731672, 11.2839301509;
- return StateFeedbackController<2, 1, 1>(L, K, MakeAngleAdjustPlantCoefficients());
+StateFeedbackController<3, 1, 1> MakeAngleAdjustController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 2.45656297069, 164.64938515, 2172.97100986;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 147.285618609, 4.58304321916, 0.956562970689;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeAngleAdjustPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeAngleAdjustPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+StateFeedbackPlant<3, 1, 1> MakeAngleAdjustPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeAngleAdjustPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
}
-StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeAngleAdjustController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+StateFeedbackLoop<3, 1, 1> MakeAngleAdjustLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeAngleAdjustController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
}
} // namespace control_loops
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
index 8db821f..7141fa9 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
@@ -6,13 +6,13 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustPlantCoefficients();
+StateFeedbackPlantCoefficients<3, 1, 1> MakeAngleAdjustPlantCoefficients();
-StateFeedbackController<2, 1, 1> MakeAngleAdjustController();
+StateFeedbackController<3, 1, 1> MakeAngleAdjustController();
-StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant();
+StateFeedbackPlant<3, 1, 1> MakeAngleAdjustPlant();
-StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop();
+StateFeedbackLoop<3, 1, 1> MakeAngleAdjustLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
new file mode 100644
index 0000000..ea84dce
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustRawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00844804908295, 0.0, 0.706562970689;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000186726546509, 0.0353055515475;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeAngleAdjustRawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.60656297069, 51.0341417582;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 311.565731672, 11.2839301509;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeAngleAdjustRawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawAngleAdjustPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeAngleAdjustRawPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawAngleAdjustLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeAngleAdjustRawController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h
new file mode 100644
index 0000000..c066e0a
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustRawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeAngleAdjustRawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawAngleAdjustPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawAngleAdjustLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
index bee58bc..3c87e86 100755
--- a/frc971/control_loops/python/angle_adjust.py
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -6,8 +6,8 @@
from matplotlib import pylab
class AngleAdjust(control_loop.ControlLoop):
- def __init__(self):
- super(AngleAdjust, self).__init__("AngleAdjust")
+ def __init__(self, name="AngleAdjustRaw"):
+ super(AngleAdjust, self).__init__(name)
# Stall Torque in N m
self.stall_torque = .428
# Stall Current in Amps
@@ -41,11 +41,14 @@
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
- self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
- self.dt, self.C)
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
self.PlaceControllerPoles([.45, .8])
+ print "Unaugmented controller poles at"
+ print self.K
+
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
@@ -54,6 +57,47 @@
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
+ self.InitializeState()
+
+class AngleAdjustDeltaU(AngleAdjust):
+ def __init__(self, name="AngleAdjust"):
+ super(AngleAdjustDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+
+ self.A = numpy.matrix([[0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
+
+ self.B = numpy.matrix([[0.0],
+ [0.0],
+ [1.0]])
+
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
+
+ self.PlaceControllerPoles([0.60, 0.35, 0.80])
+
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, 0.15])
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
def main(argv):
# Simulate the response of the system to a step input.
angle_adjust_data = numpy.genfromtxt(
@@ -76,9 +120,9 @@
pylab.show()
# Simulate the closed loop response of the system to a step input.
- angle_adjust = AngleAdjust()
+ angle_adjust = AngleAdjustDeltaU()
close_loop_x = []
- R = numpy.matrix([[1.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
for _ in xrange(100):
U = numpy.clip(angle_adjust.K * (R - angle_adjust.X_hat), angle_adjust.U_min, angle_adjust.U_max)
angle_adjust.UpdateObserver(U)
@@ -89,9 +133,16 @@
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 3:
+ if len(argv) != 5:
print "Expected .cc file name and .h file name"
else:
+ loop_writer = control_loop.ControlLoopWriter("RawAngleAdjust",
+ [AngleAdjust()])
+ if argv[3][-3:] == '.cc':
+ loop_writer.Write(argv[4], argv[3])
+ else:
+ loop_writer.Write(argv[3], argv[4])
+
loop_writer = control_loop.ControlLoopWriter("AngleAdjust", [angle_adjust])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 4ff2623..754ba62 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -140,20 +140,23 @@
"""
self._name = name
- def ContinuousToDiscrete(self, A_continuous, B_continuous, dt, C):
- """Calculates the discrete time values for A and B as well as initializing
- X and Y to the correct sizes.
+ def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+ """Calculates the discrete time values for A and B.
Args:
A_continuous: numpy.matrix, The continuous time A matrix
B_continuous: numpy.matrix, The continuous time B matrix
dt: float, The time step of the control loop
- C: C
+
+ Returns:
+ (A, B), numpy.matrix, the control matricies.
"""
- self.A, self.B = controls.c2d(
- A_continuous, B_continuous, dt)
+ return controls.c2d(A_continuous, B_continuous, dt)
+
+ def InitializeState(self):
+ """Sets X, Y, and X_hat to zero defaults."""
self.X = numpy.zeros((self.A.shape[0], 1))
- self.Y = C * self.X
+ self.Y = self.C * self.X
self.X_hat = numpy.zeros((self.A.shape[0], 1))
def PlaceControllerPoles(self, poles):
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
index f26b99a..f21ed03 100755
--- a/frc971/control_loops/python/wrist.py
+++ b/frc971/control_loops/python/wrist.py
@@ -6,8 +6,8 @@
from matplotlib import pylab
class Wrist(control_loop.ControlLoop):
- def __init__(self):
- super(Wrist, self).__init__("Wrist")
+ def __init__(self, name="RawWrist"):
+ super(Wrist, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 1.4
# Stall Current in Amps
@@ -42,12 +42,10 @@
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
- self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
- self.dt, self.C)
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.86, .46])
-
- print self.K
+ self.PlaceControllerPoles([0.85, 0.45])
self.rpl = .05
self.ipl = 0.008
@@ -57,9 +55,56 @@
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
+ self.InitializeState()
+
+
+class WristDeltaU(Wrist):
+ def __init__(self, name="Wrist"):
+ super(WristDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+
+ self.A = numpy.matrix([[0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
+
+ self.B = numpy.matrix([[0.0],
+ [0.0],
+ [1.0]])
+
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
+
+ self.PlaceControllerPoles([0.60, 0.37, 0.80])
+
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, 0.15])
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+def ClipDeltaU(wrist, delta_u):
+ old_u = numpy.matrix([[wrist.X[2, 0]]])
+ new_u = numpy.clip(old_u + delta_u, wrist.U_min, wrist.U_max)
+ return new_u - old_u
+
def main(argv):
# Simulate the response of the system to a step input.
- wrist = Wrist()
+ wrist = WristDeltaU()
simulated_x = []
for _ in xrange(100):
wrist.Update(numpy.matrix([[12.0]]))
@@ -69,22 +114,36 @@
pylab.show()
# Simulate the closed loop response of the system to a step input.
- wrist = Wrist()
+ wrist = WristDeltaU()
close_loop_x = []
- R = numpy.matrix([[1.0], [0.0]])
+ close_loop_u = []
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ wrist.X[2, 0] = -5
for _ in xrange(100):
U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
+ U = ClipDeltaU(wrist, U)
wrist.UpdateObserver(U)
wrist.Update(U)
- close_loop_x.append(wrist.X[0, 0])
+ close_loop_x.append(wrist.X[0, 0] * 10)
+ close_loop_u.append(wrist.X[2, 0])
pylab.plot(range(100), close_loop_x)
+ pylab.plot(range(100), close_loop_u)
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name"
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name for"
+ print "both the plant and unaugmented plant"
else:
+ unaug_wrist = Wrist("RawWrist")
+ unaug_loop_writer = control_loop.ControlLoopWriter("RawWrist",
+ [unaug_wrist])
+ if argv[3][-3:] == '.cc':
+ unaug_loop_writer.Write(argv[4], argv[3])
+ else:
+ unaug_loop_writer.Write(argv[3], argv[4])
+
loop_writer = control_loop.ControlLoopWriter("Wrist", [wrist])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/update_angle_adjust.sh b/frc971/control_loops/update_angle_adjust.sh
index 575dd32..096f95e 100755
--- a/frc971/control_loops/update_angle_adjust.sh
+++ b/frc971/control_loops/update_angle_adjust.sh
@@ -2,4 +2,7 @@
#
# Updates the angle adjust controller.
-./python/angle_adjust.py angle_adjust/angle_adjust_motor_plant.h angle_adjust/angle_adjust_motor_plant.cc
+./python/angle_adjust.py angle_adjust/angle_adjust_motor_plant.h \
+ angle_adjust/angle_adjust_motor_plant.cc \
+ angle_adjust/unaugmented_angle_adjust_motor_plant.h \
+ angle_adjust/unaugmented_angle_adjust_motor_plant.cc \
diff --git a/frc971/control_loops/update_wrist.sh b/frc971/control_loops/update_wrist.sh
index ea4c353..1a3b54c 100755
--- a/frc971/control_loops/update_wrist.sh
+++ b/frc971/control_loops/update_wrist.sh
@@ -2,4 +2,7 @@
#
# Updates the wrist controller.
-./python/wrist.py wrist/wrist_motor_plant.h wrist/wrist_motor_plant.cc
+./python/wrist.py wrist/wrist_motor_plant.h \
+ wrist/wrist_motor_plant.cc \
+ wrist/unaugmented_wrist_motor_plant.h \
+ wrist/unaugmented_wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc
new file mode 100644
index 0000000..a9871c4
--- /dev/null
+++ b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawWristController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h
new file mode 100644
index 0000000..c049420
--- /dev/null
+++ b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawWristController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawWristPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawWristLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
index 022dcfd..6b50fad 100644
--- a/frc971/control_loops/wrist/wrist.gyp
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -25,6 +25,7 @@
'sources': [
'wrist.cc',
'wrist_motor_plant.cc',
+ 'unaugmented_wrist_motor_plant.cc',
],
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
index a53f603..7f0bd2e 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -7,6 +7,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+#include "frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h"
#include "frc971/control_loops/zeroed_joint.h"
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index e3e6039..4903a28 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -24,7 +24,7 @@
// Constructs a motor simulation. initial_position is the inital angle of the
// wrist, which will be treated as 0 by the encoder.
WristMotorSimulation(double initial_position)
- : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeWristPlant())),
+ : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawWristPlant())),
my_wrist_loop_(".frc971.control_loops.wrist",
0x1a7b7094, ".frc971.control_loops.wrist.goal",
".frc971.control_loops.wrist.position",
@@ -41,6 +41,7 @@
wrist_plant_->Y = wrist_plant_->C() * wrist_plant_->X;
last_position_ = wrist_plant_->Y(0, 0);
calibration_value_ = 0.0;
+ last_voltage_ = 0.0;
}
// Returns the absolute angle of the wrist.
@@ -95,7 +96,7 @@
void Simulate() {
last_position_ = wrist_plant_->Y(0, 0);
EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- wrist_plant_->U << my_wrist_loop_.output->voltage;
+ wrist_plant_->U << last_voltage_;
wrist_plant_->Update();
// Assert that we are in the right physical range.
@@ -110,6 +111,7 @@
wrist_plant_->Y(0, 0));
EXPECT_LE(wrist_lower_physical_limit,
wrist_plant_->Y(0, 0));
+ last_voltage_ = my_wrist_loop_.output->voltage;
}
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
@@ -118,6 +120,7 @@
double initial_position_;
double last_position_;
double calibration_value_;
+ double last_voltage_;
};
class WristTest : public ::testing::Test {
@@ -222,7 +225,7 @@
}
my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
}
- if (i == 430) {
+ if (i == 410) {
EXPECT_TRUE(wrist_motor_.is_zeroing());
}
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.cc b/frc971/control_loops/wrist/wrist_motor_plant.cc
index 408fd85..49a7d6d 100644
--- a/frc971/control_loops/wrist/wrist_motor_plant.cc
+++ b/frc971/control_loops/wrist/wrist_motor_plant.cc
@@ -7,40 +7,40 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeWristPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000326582411818, 0.0631746179893;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
+StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
Eigen::Matrix<double, 1, 1> D;
- D << 0;
+ D << 0.0;
Eigen::Matrix<double, 1, 1> U_max;
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 1, 1> MakeWristController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.71581823335, 64.8264890043;
- Eigen::Matrix<double, 1, 2> K;
- K << 119.668313646, 7.2297495639;
- return StateFeedbackController<2, 1, 1>(L, K, MakeWristPlantCoefficients());
+StateFeedbackController<3, 1, 1> MakeWristController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 2.56581823335, 182.185686601, 1214.37916748;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 79.7788757639, 3.78830897822, 1.04581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeWristPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeWristPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
}
-StateFeedbackLoop<2, 1, 1> MakeWristLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeWristController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
}
} // namespace control_loops
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.h b/frc971/control_loops/wrist/wrist_motor_plant.h
index 169e923..a40ffe5 100644
--- a/frc971/control_loops/wrist/wrist_motor_plant.h
+++ b/frc971/control_loops/wrist/wrist_motor_plant.h
@@ -6,13 +6,13 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeWristPlantCoefficients();
+StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients();
-StateFeedbackController<2, 1, 1> MakeWristController();
+StateFeedbackController<3, 1, 1> MakeWristController();
-StateFeedbackPlant<2, 1, 1> MakeWristPlant();
+StateFeedbackPlant<3, 1, 1> MakeWristPlant();
-StateFeedbackLoop<2, 1, 1> MakeWristLoop();
+StateFeedbackLoop<3, 1, 1> MakeWristLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 3230622..01e95e3 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -13,34 +13,62 @@
class WristTest_NoWindupNegative_Test;
};
+// Note: Everything in this file assumes that there is a 1 cycle delay between
+// power being requested and it showing up at the motor. It assumes that
+// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
+// that isn't true.
+
template<int kNumZeroSensors>
class ZeroedJoint;
// This class implements the CapU function correctly given all the extra
// information that we know about from the wrist motor.
template<int kNumZeroSensors>
-class ZeroedStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
public:
- ZeroedStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop,
ZeroedJoint<kNumZeroSensors> *zeroed_joint)
- : StateFeedbackLoop<2, 1, 1>(loop),
- zeroed_joint_(zeroed_joint) {
+ : StateFeedbackLoop<3, 1, 1>(loop),
+ zeroed_joint_(zeroed_joint),
+ voltage_(0.0),
+ last_voltage_(0.0) {
}
// Caps U, but this time respects the state of the wrist as well.
virtual void CapU();
+
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
+
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // Zeros the accumulator.
+ void ZeroPower() { voltage_ = 0.0; }
private:
ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
+
+ // The accumulated voltage to apply to the motor.
+ double voltage_;
+ double last_voltage_;
+ double uncapped_voltage_;
};
template<int kNumZeroSensors>
void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ // Do all our computations with the voltage, and then compute what the delta
+ // is to make that happen.
if (zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY) {
if (Y(0, 0) >= zeroed_joint_->config_data_.upper_limit) {
- U(0, 0) = std::min(0.0, U(0, 0));
+ voltage_ = std::min(0.0, voltage_);
}
if (Y(0, 0) <= zeroed_joint_->config_data_.lower_limit) {
- U(0, 0) = std::max(0.0, U(0, 0));
+ voltage_ = std::max(0.0, voltage_);
}
}
@@ -49,8 +77,20 @@
double limit = is_ready ?
12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
- U(0, 0) = std::min(limit, U(0, 0));
- U(0, 0) = std::max(-limit, U(0, 0));
+ voltage_ = std::min(limit, voltage_);
+ voltage_ = std::max(-limit, voltage_);
+ U(0, 0) = voltage_ - old_voltage;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 2.0) {
+ X_hat(2, 0) = last_voltage_ + 2.0;
+ } else if (X_hat(2, 0) < last_voltage_ -2.0) {
+ X_hat(2, 0) = last_voltage_ - 2.0;
+ }
+
+ last_voltage_ = voltage_;
}
@@ -85,7 +125,7 @@
double hall_effect_positions[kNumZeroSensors];
};
- ZeroedJoint(StateFeedbackLoop<2, 1, 1> loop)
+ ZeroedJoint(StateFeedbackLoop<3, 1, 1> loop)
: loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
state_(UNINITIALIZED),
error_count_(0),
@@ -250,7 +290,8 @@
// Reset the zeroing goal.
zeroing_position_ = absolute_position;
// Clear the observer state.
- loop_->X_hat << absolute_position, 0.0;
+ loop_->X_hat << absolute_position, 0.0, 0.0;
+ loop_->ZeroPower();
// Set the goal to here to make it so it doesn't move when disabled.
loop_->R = loop_->X_hat;
// Only progress if we are enabled.
@@ -276,7 +317,7 @@
} else {
// Slowly creep off the sensor.
zeroing_position_ -= config_data_.zeroing_off_speed * dt;
- loop_->R << zeroing_position_, -config_data_.zeroing_off_speed;
+ loop_->R << zeroing_position_, -config_data_.zeroing_off_speed, 0.0;
break;
}
}
@@ -315,7 +356,7 @@
} else {
// Slowly creep towards the sensor.
zeroing_position_ += config_data_.zeroing_speed * dt;
- loop_->R << zeroing_position_, config_data_.zeroing_speed;
+ loop_->R << zeroing_position_, config_data_.zeroing_speed, 0.0;
}
break;
}
@@ -324,7 +365,7 @@
LOG(DEBUG, "READY\n");
{
const double limited_goal = ClipGoal(goal_angle);
- loop_->R << limited_goal, goal_velocity;
+ loop_->R << limited_goal, goal_velocity, 0.0;
break;
}
@@ -348,20 +389,20 @@
case MOVING_OFF:
case ZEROING:
// Check if we have cliped and adjust the goal.
- if (loop_->U_uncapped(0, 0) > config_data_.max_zeroing_voltage) {
- double dx = (loop_->U_uncapped(0, 0) -
+ if (loop_->uncapped_voltage() > config_data_.max_zeroing_voltage) {
+ double dx = (loop_->uncapped_voltage() -
config_data_.max_zeroing_voltage) / loop_->K(0, 0);
zeroing_position_ -= dx;
capped_goal_ = true;
- } else if(loop_->U_uncapped(0, 0) < -config_data_.max_zeroing_voltage) {
- double dx = (loop_->U_uncapped(0, 0) +
+ } else if(loop_->uncapped_voltage() < -config_data_.max_zeroing_voltage) {
+ double dx = (loop_->uncapped_voltage() +
config_data_.max_zeroing_voltage) / loop_->K(0, 0);
zeroing_position_ -= dx;
capped_goal_ = true;
}
break;
}
- return loop_->U(0, 0);
+ return loop_->voltage();
}
} // namespace control_loops