Wrist and angle adjust now use delta U controllers.
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])