Add wheel nonlinearity plotter
Change-Id: I67442e3729e3e36407a50ca7d89f35b32d5e4a28
diff --git a/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py b/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
new file mode 100755
index 0000000..57831c5
--- /dev/null
+++ b/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
@@ -0,0 +1,20 @@
+#!/usr/bin/python
+#
+# This is a quick script to show the effect of the wheel nonlinearity term on
+# turning rate
+
+from matplotlib import pylab
+import numpy
+
+if __name__ == '__main__':
+ x = numpy.arange(-1, 1, 0.01)
+
+ for nonlin in numpy.arange(0.2, 1.0, 0.1):
+ angular_range = numpy.pi * nonlin / 2.0
+ newx1 = numpy.sin(x * angular_range) / numpy.sin(angular_range)
+ newx2 = numpy.sin(newx1 * angular_range) / numpy.sin(angular_range)
+
+ pylab.plot(x, newx2, label="nonlin %f" % nonlin)
+
+ pylab.legend()
+ pylab.show()