Added simulation data for the angle adjust and tuned the loop.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_data.csv b/frc971/control_loops/angle_adjust/angle_adjust_data.csv
new file mode 100644
index 0000000..3e3a9fb
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_data.csv
@@ -0,0 +1,273 @@
+0.007086, 0.696383, -0.000596
+0.016561, 0.696383, -0.000596
+0.026648, 0.696383, -0.000596
+0.036501, 0.696383, -0.000596
+0.046475, 0.696383, -0.000596
+0.056429, 0.696383, -0.000596
+0.066661, 0.696383, -0.000596
+0.076538, 0.696383, -0.000596
+0.086562, 0.696383, -0.000596
+0.096496, 0.696383, -0.000596
+0.106453, 0.696383, -0.000596
+0.116673, 0.696383, -0.000596
+0.126529, 0.696383, -0.000596
+0.136500, 0.696383, -0.000596
+0.146462, 0.696383, -0.000596
+0.156442, 0.696383, -0.000596
+0.166658, 0.696383, -0.000596
+0.176508, 0.696383, -0.000596
+0.186466, 0.696383, -0.000596
+0.196514, 0.696383, -0.000596
+0.206469, 0.696383, -0.000596
+0.216679, 0.696383, -0.000596
+0.226464, 0.696383, -0.000596
+0.236401, 0.696383, -0.000596
+0.246476, 0.696383, -0.000596
+0.256391, 0.696383, -0.000596
+0.266650, 0.696383, -0.000596
+0.276517, 0.696383, -0.000596
+0.286565, 0.696383, -0.000596
+0.296526, 0.696383, -0.000596
+0.306469, 0.696383, -0.000596
+0.316703, 0.696383, -0.000596
+0.326546, 0.696383, -0.000596
+0.336500, 0.696383, -0.000596
+0.346466, 0.696383, -0.000596
+0.356397, 0.696383, -0.000596
+0.366658, 0.696383, -0.000596
+0.376517, 0.696383, -0.000596
+0.386461, 0.696383, -0.000596
+0.396497, 0.696383, -0.000596
+0.406516, 0.696383, -0.000596
+0.416662, 0.696383, -0.000596
+0.426498, 0.696383, -0.000596
+0.436506, 0.696383, -0.000596
+0.446560, 0.696383, -0.000596
+0.456494, 0.696383, -0.000596
+0.466635, 0.696383, -0.000596
+0.476505, 0.696383, -0.000596
+0.486529, 0.696383, -0.000596
+0.496494, 0.696383, -0.000596
+0.506479, 0.696383, -0.000596
+0.516679, 0.696383, -0.000596
+0.526550, 0.696383, -0.000596
+0.536505, 0.696383, -0.000596
+0.546452, 0.696383, -0.000596
+0.556481, 0.696383, -0.000596
+0.566531, 0.696383, -0.000596
+0.576513, 0.696383, -0.000596
+0.586463, 0.696383, -0.000596
+0.596771, 0.696383, -0.000596
+0.606503, 0.696383, -0.000596
+0.616679, 0.696383, -0.000596
+0.626483, 0.696383, -0.000596
+0.636496, 0.696383, -0.000596
+0.646654, 0.696383, -0.000596
+0.656423, 0.696383, -0.000596
+0.666661, 0.696383, -0.000596
+0.676531, 0.696383, -0.000596
+0.686526, 0.696383, -0.000596
+0.696670, 0.696383, -0.000596
+0.706628, 0.696383, -0.000596
+0.716675, 0.696383, -0.000596
+0.726719, 0.696383, -0.000596
+0.736498, 0.696383, -0.000596
+0.746484, 0.696383, -0.000596
+0.756468, 0.696383, -0.000596
+0.766526, 0.696383, -0.000596
+0.776498, 0.696383, -0.000596
+0.786469, 0.696383, -0.000596
+0.796251, 0.696383, -0.000596
+0.806312, 0.696383, -0.000596
+0.816579, 0.696383, -0.000596
+0.826352, 0.696383, -0.000596
+0.836395, 0.696383, -0.000596
+0.846460, 0.696383, -0.000596
+0.856333, 0.696383, -0.000596
+0.866312, 0.696383, -0.000596
+0.876541, 0.696383, -0.000596
+0.886231, 0.696383, -0.000596
+0.896378, 0.696383, -0.000596
+0.906549, 0.696383, -0.000596
+0.916219, 0.696383, -0.000596
+0.926300, 0.696383, -0.000596
+0.936212, 0.696383, -0.000596
+0.946392, 12.000000, -0.000596
+0.956205, 12.000000, -0.000596
+0.966382, 12.000000, 0.001192
+0.976384, 12.000000, 0.005962
+0.988371, 12.000000, 0.014906
+0.996207, 12.000000, 0.022657
+1.006429, 12.000000, 0.032794
+1.016788, 12.000000, 0.043526
+1.026528, 12.000000, 0.055451
+1.036616, 12.000000, 0.067376
+1.049418, 12.000000, 0.084071
+1.056874, 12.000000, 0.093015
+1.066581, 12.000000, 0.105536
+1.076708, 12.000000, 0.118653
+1.086697, 12.000000, 0.131771
+1.096754, 12.000000, 0.144888
+1.106694, 12.000000, 0.158006
+1.116795, 12.000000, 0.171123
+1.126945, 12.000000, 0.184241
+1.136491, 12.000000, 0.197358
+1.146490, 12.000000, 0.211072
+1.156720, 8.838210, 0.224189
+1.166497, 4.659384, 0.237307
+1.176550, 1.566925, 0.250424
+1.186490, -1.463977, 0.262945
+1.196706, -4.497083, 0.273081
+1.206413, -6.664959, 0.280833
+1.216504, -8.191987, 0.285603
+1.226718, -8.899323, 0.285603
+1.238720, -8.265740, 0.282025
+1.246502, -7.344013, 0.277851
+1.256515, -5.283271, 0.270696
+1.266504, -3.292020, 0.262945
+1.276639, -1.703019, 0.254598
+1.286435, 0.722641, 0.247443
+1.296401, 2.376863, 0.240884
+1.306274, 3.669941, 0.235518
+1.316431, 4.798566, 0.231940
+1.326280, 5.585790, 0.230748
+1.336257, 5.131469, 0.231344
+1.346265, 5.052962, 0.233133
+1.356246, 4.237080, 0.236710
+1.366233, 3.588899, 0.240288
+1.376252, 2.132660, 0.244462
+1.386339, 1.550053, 0.248039
+1.396224, 0.328756, 0.252213
+1.406339, -0.379540, 0.255194
+1.416340, -1.273041, 0.256983
+1.426200, -1.551323, 0.258175
+1.436332, -1.593706, 0.258772
+1.446348, -1.695210, 0.258175
+1.456324, -1.569522, 0.257579
+1.466365, -0.979101, 0.256387
+1.476513, -0.794073, 0.255194
+1.486723, -0.508791, 0.254002
+1.496495, -0.189683, 0.252809
+1.506430, 0.093002, 0.251617
+1.516565, 0.386294, 0.250424
+1.526518, 0.681860, 0.249828
+1.536477, 0.659148, 0.249828
+1.546440, 0.541822, 0.249828
+1.556459, 0.602564, 0.249828
+1.566511, 0.597167, 0.249828
+1.576515, 0.587492, 0.249828
+1.586463, 0.593212, 0.249828
+1.596495, 0.592427, 0.249828
+1.606519, 0.591648, 0.249828
+1.616647, 0.592179, 0.249828
+1.626482, 0.592082, 0.249828
+1.636498, 0.592021, 0.249828
+1.646521, 0.592070, 0.249828
+1.656480, 0.592059, 0.249828
+1.666552, 0.592055, 0.249828
+1.676484, 0.592059, 0.249828
+1.686525, 0.592058, 0.249828
+1.696670, 0.592057, 0.249828
+1.706459, 0.592058, 0.249828
+1.716735, 0.592058, 0.249828
+1.726493, 0.592058, 0.249828
+1.736508, 0.592058, 0.249828
+1.746452, 0.592058, 0.249828
+1.756477, 0.592058, 0.249828
+1.766815, 0.592058, 0.249828
+1.776500, 0.592058, 0.249828
+1.786441, 0.592058, 0.249828
+1.796462, 0.592058, 0.249828
+1.806512, 0.592058, 0.249828
+1.816550, 0.592058, 0.249828
+1.826504, 0.592058, 0.249828
+1.836496, 0.592058, 0.249828
+1.846519, 0.592058, 0.249828
+1.856477, 0.592058, 0.249828
+1.866533, 0.592058, 0.249828
+1.876513, 0.592058, 0.249828
+1.886541, 0.592058, 0.249828
+1.896490, 0.592058, 0.249828
+1.906479, -12.000000, 0.249828
+1.916446, -12.000000, 0.249828
+1.926530, -12.000000, 0.248635
+1.936520, -12.000000, 0.244462
+1.946463, -12.000000, 0.236710
+1.956469, -12.000000, 0.226574
+1.966515, -12.000000, 0.215842
+1.976560, -12.000000, 0.203917
+1.986476, -12.000000, 0.191396
+1.996480, -12.000000, 0.177682
+2.006460, -12.000000, 0.163968
+2.016545, -12.000000, 0.150254
+2.026495, -12.000000, 0.135944
+2.036471, -12.000000, 0.121635
+2.046473, -12.000000, 0.107325
+2.056469, -12.000000, 0.093015
+2.066524, -12.000000, 0.078705
+2.076516, -12.000000, 0.064395
+2.086512, -12.000000, 0.049489
+2.096561, -11.491993, 0.034582
+2.106457, -4.953303, 0.020272
+2.116661, -2.354450, 0.005366
+2.126490, 1.313071, -0.008347
+2.136461, 4.880571, -0.020272
+2.146526, 7.088633, -0.029216
+2.156469, 9.296628, -0.034582
+2.166524, 9.779309, -0.036371
+2.176504, 9.677380, -0.033986
+2.186457, 8.409415, -0.029216
+2.196463, 7.037205, -0.022657
+2.206499, 4.964211, -0.014906
+2.216552, 3.206835, -0.007155
+2.226456, 0.878265, 0.000596
+2.236471, -0.788450, 0.007751
+2.246517, -2.095142, 0.012521
+2.256472, -3.529778, 0.016099
+2.266416, -3.784184, 0.017887
+2.276485, -3.906740, 0.017291
+2.286520, -3.591089, 0.015502
+2.296497, -2.772296, 0.012521
+2.306461, -2.173367, 0.008944
+2.316561, -1.324239, 0.005962
+2.326547, -0.303999, 0.002385
+2.336490, 0.515297, -0.000596
+2.346453, 1.081194, -0.002981
+2.356491, 1.877482, -0.005366
+2.366526, 1.984534, -0.005962
+2.376468, 2.197044, -0.005962
+2.386411, 1.936623, -0.005962
+2.396484, 2.018387, -0.005366
+2.406517, 1.716757, -0.004770
+2.416569, 1.598133, -0.003577
+2.426463, 1.469285, -0.002981
+2.436457, 0.993349, -0.001789
+2.446504, 0.754160, -0.001192
+2.456498, 0.786747, -0.000596
+2.466418, 0.572920, -0.000596
+2.476482, 0.737242, -0.000596
+2.486521, 0.701741, -0.000596
+2.496489, 0.685572, -0.000596
+2.506441, 0.700476, -0.000596
+2.516718, 0.696605, -0.000596
+2.526498, 0.695450, -0.000596
+2.536473, 0.696784, -0.000596
+2.546448, 0.696379, -0.000596
+2.556479, 0.696304, -0.000596
+2.566517, 0.696421, -0.000596
+2.576491, 0.696380, -0.000596
+2.586447, 0.696376, -0.000596
+2.596495, 0.696386, -0.000596
+2.606515, 0.696382, -0.000596
+2.616555, 0.696382, -0.000596
+2.626465, 0.696383, -0.000596
+2.636463, 0.696383, -0.000596
+2.646513, 0.696383, -0.000596
+2.656499, 0.696383, -0.000596
+2.666488, 0.696383, -0.000596
+2.676556, 0.696383, -0.000596
+2.686505, 0.696383, -0.000596
+2.696457, 0.696383, -0.000596
+2.706448, 0.696383, -0.000596
+2.716562, 0.696383, -0.000596
+2.726498, 0.696383, -0.000596
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
index a3c89a7..2c97ca4 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
@@ -8,9 +8,9 @@
StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00135041324202, 0.0, 0.000610875713246;
+ A << 1.0, 0.00844804908295, 0.0, 0.706562970689;
Eigen::Matrix<double, 2, 1> B;
- B << 0.00111752476609, 0.129120861909;
+ B << 0.000186726546509, 0.0353055515475;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -24,9 +24,9 @@
StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.900610875713, 1.85371819524;
+ L << 1.60656297069, 51.0341417582;
Eigen::Matrix<double, 1, 2> K;
- K << 12.7787251077, -5.83693180893;
+ K << 708.103935619, 16.2677296642;
return StateFeedbackLoop<2, 1, 1>(L, K, MakeAngleAdjustPlant());
}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
index 0bcd582..8085154 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
@@ -1,5 +1,5 @@
-#ifndef FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
+#ifndef FRC971_CONTROL_LOOPS_ANGLEADJUST_ANGLEADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLEADJUST_ANGLEADJUST_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
@@ -13,4 +13,4 @@
} // namespace frc971
} // namespace control_loops
-#endif // FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
+#endif // FRC971_CONTROL_LOOPS_ANGLEADJUST_ANGLEADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
index ed04198..f49c305 100755
--- a/frc971/control_loops/python/angle_adjust.py
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -13,11 +13,11 @@
# Stall Current in Amps
self.stall_current = 63.8
# Free Speed in RPM
- self.free_speed = 16000.0
+ self.free_speed = 14900.0
# Free Current in Amps
self.free_current = 1.2
# Moment of inertia of the angle adjust about the shooter's pivot in kg m^2
- self.J = 0.41085133
+ self.J = 9.4
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -44,7 +44,7 @@
self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
self.dt, self.C)
- self.PlaceControllerPoles([.89, .85])
+ self.PlaceControllerPoles([.5, .5])
self.rpl = .05
self.ipl = 0.008
@@ -56,13 +56,23 @@
def main(argv):
# Simulate the response of the system to a step input.
+ angle_adjust_data = numpy.genfromtxt(
+ 'angle_adjust/angle_adjust_data.csv', delimiter=',')
angle_adjust = AngleAdjust()
simulated_x = []
- for _ in xrange(100):
- angle_adjust.Update(numpy.matrix([[12.0]]))
+ real_x = []
+ initial_x = angle_adjust_data[0, 2]
+ for i in xrange(angle_adjust_data.shape[0]):
+ angle_adjust.Update(numpy.matrix([[angle_adjust_data[i, 1] - 0.7]]))
simulated_x.append(angle_adjust.X[0, 0])
+ x_offset = angle_adjust_data[i, 2] - initial_x
+ real_x.append(x_offset)
- pylab.plot(range(100), simulated_x)
+ sim_delay = 2
+ pylab.plot(range(sim_delay, angle_adjust_data.shape[0] + sim_delay),
+ simulated_x, label='Simulation')
+ pylab.plot(range(angle_adjust_data.shape[0]), real_x, label='Reality')
+ pylab.legend()
pylab.show()
# Simulate the closed loop response of the system to a step input.
@@ -82,8 +92,11 @@
if len(argv) != 3:
print "Expected .cc file name and .h file name"
else:
- angle_adjust.DumpHeaderFile(argv[1])
- angle_adjust.DumpCppFile(argv[2], argv[1])
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ angle_adjust.DumpHeaderFile(argv[1])
+ angle_adjust.DumpCppFile(argv[2], argv[1])
if __name__ == '__main__':
sys.exit(main(sys.argv))