commit 0e37f59ce4d78fdcc64f7a46e5e453c18b919f2a
author Siddhant Kanwar <kanwarsiddhant@gmail.com> Mon Feb 21 19:26:50 2022 -0800
committer Austin Schuh <austin.linux@gmail.com> Mon Feb 21 21:42:36 2022 -0800
tree bc04607ad7397030a25144e7fff580e95c9a525d
parent 039d75607dee56e3dc35b39a860702d7f583d6b6

Add climber code without tests

This runs a loop to follow the request.

Signed-off-by: Siddhant Kanwar <kanwarsiddhant@gmail.com>
Change-Id: Iad516390561175f74eb33e0cda6a505376a372ed

y2022/control_loops/python/BUILD

diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index 54bce63..d3a5a62 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -107,6 +107,22 @@
     ],
 )
 
+py_binary(
+    name = "climber",
+    srcs = [
+        "climber.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:controls",
+        "//frc971/control_loops/python:linear_system",
+    ],
+)
+
 py_library(
     name = "python_init",
     srcs = ["__init__.py"],

y2022/control_loops/python/climber.py

diff --git a/y2022/control_loops/python/climber.py b/y2022/control_loops/python/climber.py
new file mode 100755
index 0000000..fd1707b
--- /dev/null
+++ b/y2022/control_loops/python/climber.py
@@ -0,0 +1,51 @@
+#!/usr/bin/python3
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import linear_system
+import numpy
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+    pass
+
+kClimber = linear_system.LinearSystemParams(
+    name='Climber',
+    motor=control_loop.Falcon(),
+    G=(1.0 / 10.0) * (1.0 / 3.0) * (1.0 / 3.0),
+    radius=22 * 0.25 / numpy.pi / 2.0 * 0.0254,
+    mass=2.0,
+    q_pos=0.10,
+    q_vel=1.35,
+    kalman_q_pos=0.12,
+    kalman_q_vel=2.00,
+    kalman_q_voltage=35.0,
+    kalman_r_position=0.05)
+
+def main(argv):
+    if FLAGS.plot:
+        R = numpy.matrix([[0.2], [0.0]])
+        linear_system.PlotKick(kClimber, R, plant_params=kClimber)
+        linear_system.PlotMotion(
+            kClimber, R, max_velocity=5.0, plant_params=kClimber)
+
+    # Write the generated constants out to a file.
+    if len(argv) != 5:
+        glog.fatal(
+            'Expected .h file name and .cc file name for the climber and integral climber.'
+        )
+    else:
+        namespaces = ['y2022', 'control_loops', 'superstructure', 'climber']
+        linear_system.WriteLinearSystem(kClimber,
+                                        argv[1:3], argv[3:5], namespaces)
+
+
+if __name__ == '__main__':
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))