Tuned intake controller in simulation.
Change-Id: If79519f6b146d7302e57d25591c03dff583c1d1d
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index b5e61c1..ae57730 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -19,22 +19,17 @@
pass
class Intake(control_loop.ControlLoop):
- def __init__(self, name="Intake", mass=None):
+ def __init__(self, name="Intake"):
super(Intake, 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 intake
- 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)
- # Intake length
- self.r = 18 * 0.0254
+ self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
- self.J = self.r * self.mass
+ self.J = 0.9
# Control loop time step
self.dt = 0.005
@@ -78,8 +71,8 @@
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.20
+ q_vel = 5.5
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
@@ -119,8 +112,8 @@
self.InitializeState()
class IntegralIntake(Intake):
- def __init__(self, name="IntegralIntake", mass=None):
- super(IntegralIntake, self).__init__(name=name, mass=mass)
+ def __init__(self, name="IntegralIntake"):
+ super(IntegralIntake, self).__init__(name=name)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
@@ -140,7 +133,7 @@
q_pos = 0.08
q_vel = 4.00
- q_voltage = 6.0
+ q_voltage = 3.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)]])
@@ -158,6 +151,7 @@
self.K[0, 2] = 1
self.InitializeState()
+
class ScenarioPlotter(object):
def __init__(self):
# Various lists for graphing things.
@@ -167,6 +161,7 @@
self.a = []
self.x_hat = []
self.u = []
+ self.offset = []
def run_test(self, intake, goal, iterations=200, controller_intake=None,
observer_intake=None):
@@ -220,8 +215,9 @@
if observer_intake is not None:
observer_intake.Y = intake.Y
observer_intake.CorrectObserver(U)
+ self.offset.append(observer_intake.X_hat[2, 0])
- intake.Update(U)
+ intake.Update(U + 2.0)
if observer_intake is not None:
observer_intake.PredictObserver(U)
@@ -239,29 +235,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()
- intake = Intake(mass=base_mass + load_mass)
- intake_controller = IntegralIntake(mass=base_mass + load_mass)
- observer_intake = IntegralIntake(mass=base_mass + load_mass)
+ intake = Intake()
+ intake_controller = IntegralIntake()
+ observer_intake = IntegralIntake()
# Test moving the intake 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(intake, goal=R, controller_intake=intake_controller,
observer_intake=observer_intake, iterations=200)
@@ -278,9 +273,9 @@
namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
- integral_intake = IntegralIntake("IntegralIntake", mass=base_mass + load_mass)
+ integral_intake = IntegralIntake("IntegralIntake")
integral_loop_writer = control_loop.ControlLoopWriter("IntegralIntake", [integral_intake],
- namespaces=['y2016', 'control_loops', 'superstructure'])
+ namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':