Split the accelerating and decelerating gains for the superstructure.
Change-Id: Iae2c6d994a35443d0bc26fc5b811247ff2c7bf8b
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 9e12b6b..70740d5 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -24,10 +24,11 @@
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm"):
+ def __init__(self, name="Arm", J=None):
super(Arm, self).__init__(name=name)
- self._shoulder = Shoulder(name=name)
+ self._shoulder = Shoulder(name=name, J=J)
self._shooter = Wrist(name=name)
+ self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
# Do a coordinate transformation.
# X_shooter_grounded = X_shooter + X_shoulder
@@ -94,7 +95,10 @@
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
glog.debug('Shoulder K')
- glog.debug(self._shoulder.K)
+ glog.debug(repr(self._shoulder.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self._shoulder.A -
+ self._shoulder.B * self._shoulder.K)[0]))
# Compute controller gains.
# self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
@@ -135,8 +139,8 @@
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm"):
- super(IntegralArm, self).__init__(name=name)
+ def __init__(self, name="IntegralArm", J=None):
+ super(IntegralArm, self).__init__(name=name, J=J)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
@@ -154,11 +158,11 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
- q_pos_shoulder = 0.08
- q_vel_shoulder = 4.00
- q_voltage_shoulder = 3.0
+ q_pos_shoulder = 0.10
+ q_vel_shoulder = 0.005
+ q_voltage_shoulder = 3.5
q_pos_shooter = 0.08
- q_vel_shooter = 4.00
+ q_vel_shooter = 2.00
q_voltage_shooter = 1.0
self.Q = numpy.matrix(numpy.zeros((6, 6)))
self.Q[0, 0] = q_pos_shoulder ** 2.0
@@ -236,7 +240,7 @@
goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
shoulder_profile = TrapezoidProfile(arm.dt)
- shoulder_profile.set_maximum_acceleration(50.0)
+ shoulder_profile.set_maximum_acceleration(12.0)
shoulder_profile.set_maximum_velocity(10.0)
shoulder_profile.SetGoal(goal[0, 0])
shooter_profile = TrapezoidProfile(arm.dt)
@@ -318,8 +322,6 @@
self.u_shoulder.append(U[0, 0])
self.u_shooter.append(U[1, 0])
- glog.debug('Time: %f', self.t[-1])
-
ff_U -= U_uncapped - U
goal = controller.A * goal + controller.B * ff_U
@@ -378,7 +380,7 @@
scenario_plotter = ScenarioPlotter()
arm = Arm()
- arm_controller = IntegralArm()
+ arm_controller = IntegralArm(name='AcceleratingIntegralArm', J=12)
arm_observer = IntegralArm()
# Test moving the shoulder with constant separation.
@@ -406,8 +408,12 @@
namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
+ decelerating_arm_controller = IntegralArm(name='DeceleratingIntegralArm', J=5)
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralArm', [arm_controller], namespaces=namespaces)
+ 'IntegralArm', [arm_controller, decelerating_arm_controller],
+ namespaces=namespaces)
+ integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
+ arm_controller.shoulder_Kv))
integral_loop_writer.Write(argv[3], argv[4])
if FLAGS.plot:
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index 63cc0f4..da45bd1 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -42,7 +42,7 @@
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
- self.J = 0.34 + 0.1
+ self.J = 0.34 + 0.6
# Control loop time step
self.dt = 0.005
@@ -138,11 +138,12 @@
self.C = numpy.matrix(numpy.zeros((1, 3)))
self.C[0:1, 0:2] = self.C_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
q_pos = 0.12
q_vel = 2.00
- q_voltage = 3.0
+ q_voltage = 4.0
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
[0.0, 0.0, (q_voltage ** 2.0)]])
@@ -235,7 +236,10 @@
self.v.append(intake.X[1, 0])
self.a.append((self.v[-1] - last_v) / intake.dt)
- intake.Update(U + 0.0)
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ intake.Update(U + offset)
observer_intake.PredictObserver(U)
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 07a67f6..6beac61 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -17,7 +17,7 @@
pass
class Shoulder(control_loop.ControlLoop):
- def __init__(self, name="Shoulder", mass=None):
+ def __init__(self, name="Shoulder", J=None):
super(Shoulder, self).__init__(name)
# TODO(constants): Update all of these & retune poles.
# Stall Torque in N m
@@ -39,7 +39,10 @@
# Gear ratio
self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (42.0 / 12.0)
- self.J = 3.0
+ if J is None:
+ self.J = 10.0
+ else:
+ self.J = J
# Control loop time step
self.dt = 0.005
@@ -67,8 +70,8 @@
controllability = controls.ctrb(self.A, self.B)
- q_pos = 0.14
- q_vel = 4.5
+ q_pos = 0.16
+ q_vel = 1.5
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 4d4f5f1..660e577 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -39,7 +39,7 @@
# Gear ratio
self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
- self.J = 0.15
+ self.J = 0.20
# Control loop time step
self.dt = 0.005