Add the catapult simulation and design code
This is mostly shamelessly stolen from rotory system. It is different
enough because we have different control goals to not warrent being
lumped with the rotory system.
This lets us compute final speeds given the gear ratio and masses,
letting us optimize the gear ratio.
This gives us a model to start using when writing the optimization
problem for the fixed horizon MPC for shooting.
Change-Id: I03ac3448aa6ef717965fef7f41ef0a1197ea77a5
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
new file mode 100755
index 0000000..bb8d74d
--- /dev/null
+++ b/y2022/control_loops/python/catapult.py
@@ -0,0 +1,84 @@
+#!/usr/bin/python3
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import math
+import sys
+import math
+from y2022.control_loops.python import catapult_lib
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', True, 'If true, plot the loop response.')
+
+ball_mass = 0.25
+ball_diameter = 9.5 * 0.0254
+lever = 17.5 * 0.0254
+
+G = (14.0 / 72.0) * (12.0 / 33.0)
+
+
+def AddResistance(motor, resistance):
+ motor.resistance += resistance
+ return motor
+
+J_ball = 1.5 * ball_mass * lever * lever
+# Assuming carbon fiber, calculate the mass of the bar.
+M_bar = (1750 * lever * 0.0254 * 0.0254 * (1.0 - (1 - 0.07)**2.0))
+# And the moment of inertia.
+J_bar = 1.0 / 3.0 * M_bar * lever**2.0
+
+# Do the same for a theoretical cup. Assume a 40 thou thick carbon cup.
+M_cup = (1750 * 0.0254 * 0.04 * 2 * math.pi * (ball_diameter / 2.)**2.0)
+J_cup = M_cup * lever**2.0 + M_cup * (ball_diameter / 2.)**2.0
+
+print("J ball", ball_mass * lever * lever)
+print("J bar", J_bar)
+print("bar mass", M_bar)
+print("J cup", J_cup)
+print("cup mass", M_cup)
+
+J = (J_ball + J_bar + J_cup * 1.5)
+print("J", J)
+
+kFinisher = catapult_lib.CatapultParams(
+ name='Finisher',
+ motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
+ G=G,
+ J=J,
+ lever=lever,
+ q_pos=0.01,
+ q_vel=10.0,
+ q_voltage=4.0,
+ r_pos=0.01,
+ controller_poles=[.93],
+ dt=0.0005)
+
+
+def main(argv):
+ # Do all our math with a lower voltage so we have headroom.
+ U = numpy.matrix([[9.0]])
+ print("For G:", G, " max speed ", catapult_lib.MaxSpeed(params=kFinisher, U=U, final_position = math.pi / 2.0))
+
+ if FLAGS.plot:
+ catapult_lib.PlotShot(kFinisher, U, final_position = math.pi / 4.0)
+
+ gs = []
+ speed = []
+ for i in numpy.linspace(0.01, 0.15, 150):
+ kFinisher.G = i
+ gs.append(kFinisher.G)
+ speed.append(catapult_lib.MaxSpeed(params=kFinisher, U=U, final_position = math.pi / 2.0))
+ pylab.plot(gs, speed, label = "max_speed")
+ pylab.show()
+ return 0
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))