Tuned shoulder in simulation
Change-Id: I3c7044a214ba639a593b706241a852713491241c
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 05ab1b6..554228e 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -23,18 +23,13 @@
super(Shoulder, self).__init__(name)
# TODO(constants): Update all of these & retune poles.
# Stall Torque in N m
- self.stall_torque = 0.476
+ self.stall_torque = 0.71
# Stall Current in Amps
- self.stall_current = 80.730
+ self.stall_current = 134
# Free Speed in RPM
- self.free_speed = 13906.0
+ self.free_speed = 18730
# Free Current in Amps
- self.free_current = 5.820
- # Mass of the shoulder
- if mass is None:
- self.mass = 5.0
- else:
- self.mass = mass
+ self.free_current = 0.7
# Resistance of the motor
self.R = 12.0 / self.stall_current
@@ -44,11 +39,9 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
- # Shoulder length
- self.r = 18 * 0.0254
+ self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (58.0 / 16.0)
- self.J = self.r * self.mass
+ self.J = 3.0
# Control loop time step
self.dt = 0.005
@@ -76,10 +69,8 @@
controllability = controls.ctrb(self.A, self.B)
- print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
-
- q_pos = 0.15
- q_vel = 2.5
+ q_pos = 0.14
+ q_vel = 4.5
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
@@ -89,13 +80,6 @@
print 'K', self.K
print 'Poles are', 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
-
q_pos = 0.05
q_vel = 2.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
@@ -119,8 +103,8 @@
self.InitializeState()
class IntegralShoulder(Shoulder):
- def __init__(self, name="IntegralShoulder", mass=None):
- super(IntegralShoulder, self).__init__(name=name, mass=mass)
+ def __init__(self, name="IntegralShoulder"):
+ super(IntegralShoulder, self).__init__(name=name)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
@@ -158,6 +142,7 @@
self.K[0, 2] = 1
self.InitializeState()
+
class ScenarioPlotter(object):
def __init__(self):
# Various lists for graphing things.
@@ -167,6 +152,7 @@
self.a = []
self.x_hat = []
self.u = []
+ self.offset = []
def run_test(self, shoulder, goal, iterations=200, controller_shoulder=None,
observer_shoulder=None):
@@ -220,8 +206,9 @@
if observer_shoulder is not None:
observer_shoulder.Y = shoulder.Y
observer_shoulder.CorrectObserver(U)
+ self.offset.append(observer_shoulder.X_hat[2, 0])
- shoulder.Update(U)
+ shoulder.Update(U + 2.0)
if observer_shoulder is not None:
observer_shoulder.PredictObserver(U)
@@ -239,29 +226,28 @@
pylab.subplot(3, 1, 2)
pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
pylab.subplot(3, 1, 3)
pylab.plot(self.t, self.a, label='a')
-
pylab.legend()
+
pylab.show()
def main(argv):
argv = FLAGS(argv)
- base_mass = 4
- load_mass = 0
-
scenario_plotter = ScenarioPlotter()
- shoulder = Shoulder(mass=base_mass + load_mass)
- shoulder_controller = IntegralShoulder(mass=base_mass + load_mass)
- observer_shoulder = IntegralShoulder(mass=base_mass + load_mass)
+ shoulder = Shoulder()
+ shoulder_controller = IntegralShoulder()
+ observer_shoulder = IntegralShoulder()
# Test moving the shoulder with constant separation.
initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0], [0.0]])
scenario_plotter.run_test(shoulder, goal=R, controller_shoulder=shoulder_controller,
observer_shoulder=observer_shoulder, iterations=200)
@@ -278,9 +264,9 @@
namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
- integral_shoulder = IntegralShoulder("IntegralShoulder", mass=base_mass + load_mass)
+ integral_shoulder = IntegralShoulder("IntegralShoulder")
integral_loop_writer = control_loop.ControlLoopWriter("IntegralShoulder", [integral_shoulder],
- namespaces=['y2016', 'control_loops', 'superstructure'])
+ namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':