Tuned arm some more, still needs lots of work.
Change-Id: I8d9b2abb1a80783029a9811a0b953b8115d36a2e
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index ae14b04..9e12b6b 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -156,10 +156,10 @@
q_pos_shoulder = 0.08
q_vel_shoulder = 4.00
- q_voltage_shoulder = 6.0
+ q_voltage_shoulder = 3.0
q_pos_shooter = 0.08
q_vel_shooter = 4.00
- q_voltage_shooter = 6.0
+ q_voltage_shooter = 1.0
self.Q = numpy.matrix(numpy.zeros((6, 6)))
self.Q[0, 0] = q_pos_shoulder ** 2.0
self.Q[1, 1] = q_vel_shoulder ** 2.0
@@ -302,7 +302,7 @@
#observer.X_hat[0, 0] += 0.20
#arm.X[0, 0] += 0.20
pass
- U_error = numpy.matrix([[0.0], [0.0]])
+ U_error = numpy.matrix([[2.0], [2.0]])
# Kick it and see what happens.
#if (initial_t + i * arm.dt) % 0.4 > 0.2:
#U_error = numpy.matrix([[4.0], [0.0]])
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index a1be86a..1fcf393 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -40,7 +40,10 @@
# Gear ratio
self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
- self.J = 0.9
+ # Measured in CAD
+ # self.J = 0.9
+ # With extra mass to compensate for friction.
+ self.J = 1.2
# Control loop time step
self.dt = 0.005
@@ -96,8 +99,8 @@
glog.debug('L is %s', repr(self.L))
- q_pos = 0.05
- q_vel = 2.65
+ q_pos = 0.10
+ q_vel = 1.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
[0.0, (q_vel ** 2.0)]])
@@ -138,8 +141,8 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
- q_pos = 0.08
- q_vel = 4.00
+ q_pos = 0.12
+ q_vel = 2.00
q_voltage = 3.0
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],