Better tested the fridge, and handled disable.
Change-Id: Icedd5aea0495b2e81e46ed76845fbc95914b2056
diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
index c910f4c..974ae0f 100755
--- a/frc971/control_loops/python/arm.py
+++ b/frc971/control_loops/python/arm.py
@@ -5,6 +5,7 @@
import polytope
import polydrivetrain
import numpy
+import math
import sys
import matplotlib
from matplotlib import pylab
@@ -63,6 +64,11 @@
print 'Full speed is', C2 / C3 * 12.0
+ print 'Stall arm difference is', 12.0 * C2 / C1
+ print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
+
+ print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
+
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0, 0],