Add the controller python used for designing the turret
When we were sizing the motor for the turret, we used this initial
design for the controller and for the mechanism to work out the
numbers. Let's check it in as a starting point for designing
the controller.
Change-Id: I1e7cd71eb48fa864bdbbc1671b3cd72d5d240eae
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/python/turret.py b/y2022/control_loops/python/turret.py
new file mode 100644
index 0000000..83ba4b3
--- /dev/null
+++ b/y2022/control_loops/python/turret.py
@@ -0,0 +1,54 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kTurret = angular_system.AngularSystemParams(
+ name='Turret',
+ motor=control_loop.Falcon(),
+ G=0.01,
+ J=2.0,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=24 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi], [0.0]])
+ angular_system.PlotKick(kTurret, R)
+ angular_system.PlotMotion(kTurret, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the turret and integral turret.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'turret']
+ angular_system.WriteAngularSystem(kTurret, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))