commit 5d49af0ad7dae6f27a614ee866adc3c0f304b509
author Milo Lin <100027790@mvla.net> Sat Feb 05 12:50:32 2022 -0800
committer Austin Schuh <austin.linux@gmail.com> Tue Feb 08 21:26:28 2022 -0800
tree f7addeaa49fc3a0f0d053e71d638397a9bf53bfd
parent a9ea40d2dcc2960204afa483806ae712caa2c9a4

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>

frc971/control_loops/python/angular_system.py

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],