Recalibrated all our loops.
Shooter works much much better now. There's still a bit of
oscillation, but it's a lot better.
Change-Id: Ie3b8e09125d62e5b2011c6874fab97641c0b6526
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
index e94c227..d6043b3 100755
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -57,7 +57,7 @@
self.A_continuous, self.B_continuous, self.dt)
q_indexer_vel = 13.0
- q_pos = 0.04
+ q_pos = 0.05
q_vel = 0.8
self.Q = numpy.matrix([[(1.0 / (q_indexer_vel ** 2.0)), 0.0, 0.0],
[0.0, (1.0 / (q_pos ** 2.0)), 0.0],
@@ -176,9 +176,9 @@
self.Kff = numpy.matrix(numpy.zeros((2, 6)))
self.Kff[:, 0:4] = orig_Kff
- q_pos = 0.12
+ q_pos = 0.40
q_vel = 2.00
- q_voltage = 4.0
+ q_voltage = 8.0
if voltage_error_noise is not None:
q_voltage = voltage_error_noise
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index d42620a..6c7c2d0 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -57,7 +57,7 @@
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
- self.J = 0.08 + 1.1 + \
+ self.J = 0.08 + 2.3 + \
self.big_gear_inertia * ((self.G1 / self.G) ** 2) + \
self.big_gear_inertia * ((self.G2 / self.G) ** 2) + \
self.motor_inertia * ((1.0 / self.G) ** 2.0)
@@ -94,8 +94,8 @@
glog.debug(repr(self.A_continuous))
# Calculate the LQR controller gain
- q_pos = 0.006
- q_vel = 0.30
+ q_pos = 0.015
+ q_vel = 0.40
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 1a4f910..5aef754 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -20,7 +20,6 @@
pylab.plot(time, numpy.subtract(list1, list2), label='diff')
pylab.legend()
-
class VelocityShooter(control_loop.HybridControlLoop):
def __init__(self, name='VelocityShooter'):
super(VelocityShooter, self).__init__(name)
@@ -39,7 +38,7 @@
# Moment of inertia of the shooter wheel in kg m^2
# 1400.6 grams/cm^2
# 1.407 *1e-4 kg m^2
- self.J = 0.00120
+ self.J = 0.00100
# Resistance of the motor, divided by 2 to account for the 2 motors
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -93,8 +92,8 @@
self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
self.A_continuous[0:1, 0:1] = self.A_continuous_unaugmented
- self.A_continuous[1, 0] = 105.0
- self.A_continuous[1, 1] = -105.0
+ self.A_continuous[1, 0] = 225.0
+ self.A_continuous[1, 1] = -self.A_continuous[1, 0]
self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
@@ -102,10 +101,19 @@
self.C = numpy.matrix([[0, 1]])
self.D = numpy.matrix([[0]])
+ # The states are [unfiltered_velocity, velocity]
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.75, 0.75])
+ self.PlaceControllerPoles([.70, 0.60])
+
+ q_vel = 40.0
+ q_filteredvel = 30.0
+ self.Q = numpy.matrix([[(1.0 / (q_vel ** 2.0)), 0.0],
+ [0.0, (1.0 / (q_filteredvel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (3.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('K %s', repr(self.K))
glog.debug('System poles are %s',
@@ -195,11 +203,12 @@
glog.debug('A_dt(A): \n%s', repr(self.A))
q_pos = 0.01
- q_vel = 2.0
- q_voltage = 0.3
+ q_vel = 4.0
+ q_velfilt = 1.5
+ q_voltage = 2.0
self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0, 0.0],
- [0.0, 0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_velfilt ** 2.0), 0.0],
[0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
r_pos = 0.0003
@@ -261,6 +270,7 @@
else:
initial_t = 0
+ last_U = numpy.matrix([[0.0]])
for i in xrange(iterations):
X_hat = shooter.X
@@ -290,16 +300,17 @@
observer_shooter.CorrectObserver(U)
self.offset.append(observer_shooter.X_hat[3, 0])
- applied_U = U.copy()
- if i > 30:
+ applied_U = last_U.copy()
+ if i > 60:
applied_U += 2
shooter.Update(applied_U)
if observer_shooter is not None:
if hybrid_obs:
- observer_shooter.PredictHybridObserver(U, shooter.dt)
+ observer_shooter.PredictHybridObserver(last_U, shooter.dt)
else:
- observer_shooter.PredictObserver(U)
+ observer_shooter.PredictObserver(last_U)
+ last_U = U.copy()
self.t.append(initial_t + i * shooter.dt)
@@ -328,17 +339,10 @@
scenario_plotter = ScenarioPlotter()
if FLAGS.plot:
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter = IntegralShooter()
iterations = 200
initial_X = numpy.matrix([[0.0], [0.0], [0.0]])
R = numpy.matrix([[0.0], [100.0], [100.0], [0.0]])
- scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter, iterations=iterations)
-
- scenario_plotter.Plot()
scenario_plotter_int = ScenarioPlotter()
@@ -351,8 +355,6 @@
hybrid_obs = True)
scenario_plotter_int.Plot()
- PlotDiff(scenario_plotter.x_hat, scenario_plotter_int.x_hat,
- scenario_plotter.t)
pylab.show()
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
index edd5b53..d64fceb 100755
--- a/y2017/control_loops/python/turret.py
+++ b/y2017/control_loops/python/turret.py
@@ -46,7 +46,7 @@
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
self.J = 0.06 + self.motor_inertia * ((1.0 / self.G) ** 2.0)
- glog.info('Turret J is: %f', self.J)
+ glog.debug('Turret J is: %f', self.J)
# Control loop time step
self.dt = 0.005