Update python models to have accurate constants
This should bring the python models all up to date. Thanks Sarah for
pulling some of these out of the CAD.
Change-Id: I948685629b8d6835dccd72e623bf110525cb4254
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 257ed23..e5b26fb 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -13,17 +13,27 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+# Inertia for a single 4" diameter, 1" wide neopreme wheel.
+J_wheel = 0.000319
+# Gear ratio between wheels (speed up!)
+G_per_wheel = 1.2
+# Gear ratio to the final wheel.
+G = (30.0 / 40.0) * numpy.power(G_per_wheel, 3.0)
+# Overall flywheel inertia.
+J = J_wheel * (
+ 1.0 + numpy.power(G, -2.0) + numpy.power(G, -4.0) + numpy.power(G, -6.0))
+
+# The position and velocity are measured for the final wheel.
kAccelerator = flywheel.FlywheelParams(
name='Accelerator',
motor=control_loop.Falcon(),
- G=1.0,
- J=0.006,
+ G=G,
+ J=J,
q_pos=0.08,
q_vel=4.00,
q_voltage=0.3,
r_pos=0.05,
- controller_poles=[.87],
- dt=0.00505)
+ controller_poles=[.87])
def main(argv):
diff --git a/y2020/control_loops/python/control_panel.py b/y2020/control_loops/python/control_panel.py
index 206a39e..e02aa13 100644
--- a/y2020/control_loops/python/control_panel.py
+++ b/y2020/control_loops/python/control_panel.py
@@ -17,12 +17,11 @@
except gflags.DuplicateFlagError:
pass
-#TODO(sabina): update moment
kControlPanel = angular_system.AngularSystemParams(
name='ControlPanel',
motor=control_loop.BAG(),
- G=(1.0),
- J=0.3,
+ G=1.0,
+ J=0.000009,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
diff --git a/y2020/control_loops/python/drivetrain.py b/y2020/control_loops/python/drivetrain.py
index 54745dd..696c8a2 100644
--- a/y2020/control_loops/python/drivetrain.py
+++ b/y2020/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
import sys
import gflags
@@ -11,11 +12,13 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
kDrivetrain = drivetrain.DrivetrainParams(
- J=1.5,
- mass=38.5,
- robot_radius=0.45 / 2.0,
- wheel_radius=4.0 * 0.0254 / 2.0,
- G=9.0 / 52.0,
+ J=6.0,
+ mass=58.0,
+ # TODO(austin): Measure radius a bit better.
+ robot_radius=0.7 / 2.0,
+ wheel_radius=6.0 * 0.0254 / 2.0,
+ motor_type=control_loop.Falcon(),
+ G=(8.0 / 70.0) * (17.0 / 24.0),
q_pos=0.14,
q_vel=1.30,
efficiency=0.80,
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 0b0fbb4..b789279 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -13,22 +13,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+# Inertia for a single 4" diameter, 2" wide neopreme wheel.
+J_wheel = 0.000319 * 2.0
+# Gear ratio to the final wheel.
+# 40 tooth on the flywheel
+# 48 for the falcon.
+# 60 tooth on the outer wheel.
+G = 48.0 / 40.0
+# Overall flywheel inertia.
+J = J_wheel * (1.0 + (40.0 / 60.0)**2.0)
+
+# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
motor=control_loop.Falcon(),
- G=1.0,
- J=0.006,
+ G=G,
+ J=J,
q_pos=0.08,
q_vel=4.00,
q_voltage=0.3,
r_pos=0.05,
- controller_poles=[.87],
- dt=0.00505)
+ controller_poles=[.87])
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[0.0], [100.0], [0.0]])
+ R = numpy.matrix([[0.0], [500.0], [0.0]])
flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
return 0
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index 8c99940..fa92446 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -17,18 +17,22 @@
except gflags.DuplicateFlagError:
pass
-'''
-Hood is an angular subsystem due to the mounting of the encoder on the hood joint.
-We are currently electing to ignore potential non-linearity.
-'''
-#TODO: update constants
+# Hood is an angular subsystem due to the mounting of the encoder on the hood
+# joint. We are currently electing to ignore potential non-linearity.
+
+range_of_travel_radians = (37.0 * numpy.pi / 180.0)
+# 0.083 inches/turn
+# 6.38 inches of travel
+turns_of_leadscrew_per_range_of_travel = 6.38 / 0.083
+
+radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
+
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- # meters / rad (used the displacement of the lead screw and the angle)
- G=(0.1601 / 0.6457),
- J=0.3,
+ G=radians_per_turn,
+ J=0.08389,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
@@ -39,7 +43,7 @@
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 4.0], [0.0]])
angular_system.PlotKick(kHood, R)
angular_system.PlotMotion(kHood, R)
diff --git a/y2020/control_loops/python/intake.py b/y2020/control_loops/python/intake.py
index f65679a..7e95758 100644
--- a/y2020/control_loops/python/intake.py
+++ b/y2020/control_loops/python/intake.py
@@ -17,12 +17,11 @@
except gflags.DuplicateFlagError:
pass
-#TODO: update constants
kIntake = angular_system.AngularSystemParams(
name='Intake',
motor=control_loop.BAG(),
- G=(1.0 / 1.0),
- J=0.3,
+ G=(12.0 / 24.0) * (1.0 / 7.0) * (1.0 / 7.0) * (16.0 / 32.0),
+ J=3.0 * 0.139 * 0.139,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index 58ea7d8..dd2244d 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -21,10 +21,8 @@
kTurret = angular_system.AngularSystemParams(
name='Turret',
motor=control_loop.Vex775Pro(),
- #TODO: Update Gear Ratios when they are ready
- G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
- #TODO: Get number from Bryan (moment of inertia)
- J=0.30,
+ G=(6.0 / 60.0) * (26.0 / 150.0),
+ J=0.11,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,