Add the controller python used for designing the intake
When we were sizing the motor for the intake, we used this initial
design for the controller and for the mechanism (stolen from 2016) to
work out the numbers. Let's check it in as a starting point for
designing the controller.
Signed-off-by: Milo Lin <100027790@mvla.net>
Change-Id: I1343afe246264b58fd50efcf05bd48cc4dde4ee3
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index cad3221..df6c0f6 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -20,6 +20,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
+ radius = None,
dt=0.00505):
"""Constructs an AngularSystemParams object.
@@ -38,6 +39,7 @@
self.kalman_q_vel = kalman_q_vel
self.kalman_q_voltage = kalman_q_voltage
self.kalman_r_position = kalman_r_position
+ self.radius = radius
self.dt = dt
@@ -80,11 +82,17 @@
glog.debug('Controllability of %d',
numpy.linalg.matrix_rank(controllability))
glog.debug('J: %f', self.J)
- glog.debug('Stall torque: %f', self.motor.stall_torque / self.G)
- glog.debug('Stall acceleration: %f',
+ glog.debug('Stall torque: %f (N m)', self.motor.stall_torque / self.G)
+ if self.params.radius is not None:
+ glog.debug('Stall force: %f (N)',
+ self.motor.stall_torque / self.G / self.params.radius)
+ glog.debug('Stall force: %f (lbf)',
+ self.motor.stall_torque / self.G / self.params.radius * 0.224809)
+
+ glog.debug('Stall acceleration: %f (rad/s^2)',
self.motor.stall_torque / self.G / self.J)
- glog.debug('Free speed is %f',
+ glog.debug('Free speed is %f (rad/s)',
-self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
self.Q = numpy.matrix([[(1.0 / (self.params.q_pos**2.0)), 0.0],
diff --git a/y2022/BUILD b/y2022/BUILD
index 812cac1..9c75c26 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -232,3 +232,10 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2022/__init__.py b/y2022/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/__init__.py
diff --git a/y2022/control_loops/BUILD b/y2022/control_loops/BUILD
new file mode 100644
index 0000000..49bc419
--- /dev/null
+++ b/y2022/control_loops/BUILD
@@ -0,0 +1,7 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2022:python_init"],
+)
diff --git a/y2022/control_loops/__init__.py b/y2022/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/control_loops/__init__.py
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index e0b0daa..bc2b624 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -48,10 +48,26 @@
],
)
+py_binary(
+ name = "intake",
+ srcs = [
+ "intake.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
- deps = ["//y2020/control_loops:python_init"],
+ deps = ["//y2022/control_loops:python_init"],
)
diff --git a/y2022/control_loops/python/intake.py b/y2022/control_loops/python/intake.py
new file mode 100644
index 0000000..e5030e5
--- /dev/null
+++ b/y2022/control_loops/python/intake.py
@@ -0,0 +1,55 @@
+#!/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
+
+kIntake = angular_system.AngularSystemParams(
+ name='Intake',
+ motor=control_loop.Falcon(),
+ # TODO(Milo): Change gear ratios when we have all of them
+ G=0.02,
+ J=0.34,
+ 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=13 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kIntake, R)
+ angular_system.PlotMotion(kIntake, 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 intake and integral intake.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'intake']
+ angular_system.WriteAngularSystem(kIntake, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/superstructure/intake/BUILD b/y2022/control_loops/superstructure/intake/BUILD
new file mode 100644
index 0000000..f6e5be0
--- /dev/null
+++ b/y2022/control_loops/superstructure/intake/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "integral_intake_plant.h",
+ "integral_intake_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:intake) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:intake",
+ ],
+)
+
+cc_library(
+ name = "intake_plants",
+ srcs = [
+ "intake_plant.cc",
+ "integral_intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_plant.h",
+ "integral_intake_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)