Housekeeping and gyro code optimizations.
- Optimized the gyro code for highest possible precision when
gauging shooter wheel velocity.
- Cleaned up some junk in various places.
NOTE: Stuff does not compile at the moment, I need to merge in
some of James' stuff for that.
diff --git a/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index fb78a10..f732b3d 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -13,7 +13,6 @@
: aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
loop_(new StateFeedbackLoop<1, 1, 1>(MakeShooterLoop())),
last_velocity_goal_(0) {
- loop_->Reset();
}
/*static*/ const double ShooterMotor::dt = 0.01;
@@ -26,10 +25,9 @@
double velocity_goal = goal->velocity;
// Our position here is actually a velocity.
average_velocity_ =
- (position == NULL ? loop_->X_hat(0, 0) : position->position);
+ (position == NULL ? loop_->X_hat(0, 0) : position->velocity);
double output_voltage = 0.0;
-// TODO (danielp): This must be modified for our index.
/* if (index_loop.status.FetchLatest() || index_loop.status.get()) {
if (index_loop.status->is_shooting) {
if (velocity_goal != last_velocity_goal_ &&
diff --git a/bot3/control_loops/shooter/shooter_data.csv b/bot3/control_loops/shooter/shooter_data.csv
deleted file mode 100644
index 3515070..0000000
--- a/bot3/control_loops/shooter/shooter_data.csv
+++ /dev/null
@@ -1,638 +0,0 @@
-0.009404, 0.000000, 1484.878965
-0.019423, 0.000000, 1484.878965
-0.029389, 0.000000, 1484.878965
-0.039354, 0.000000, 1484.878965
-0.049887, 0.000000, 1484.878965
-0.059522, 0.000000, 1484.878965
-0.069479, 0.000000, 1484.878965
-0.079381, 0.000000, 1484.878965
-0.089338, 0.000000, 1484.878965
-0.099357, 0.000000, 1484.878965
-0.109409, 0.000000, 1484.878965
-0.119355, 0.000000, 1484.878965
-0.129358, 0.000000, 1484.878965
-0.139354, 0.000000, 1484.878965
-0.149480, 0.000000, 1484.878965
-0.159363, 0.000000, 1484.878965
-0.169341, 0.000000, 1484.878965
-0.179367, 0.000000, 1484.878965
-0.189636, 0.000000, 1484.878965
-0.199875, 0.000000, 1484.878965
-0.210070, 0.000000, 1484.878965
-0.219349, 0.000000, 1484.878965
-0.229544, 0.000000, 1484.878965
-0.239404, 0.000000, 1484.878965
-0.249410, 0.000000, 1484.878965
-0.259839, 0.000000, 1484.878965
-0.269492, 0.000000, 1484.878965
-0.279847, 0.000000, 1484.878965
-0.290056, 0.000000, 1484.878965
-0.299362, 0.000000, 1484.878965
-0.309457, 0.000000, 1484.878965
-0.319829, 0.000000, 1484.878965
-0.329446, 0.000000, 1484.878965
-0.339818, 0.000000, 1484.878965
-0.349444, 0.000000, 1484.878965
-0.359899, 0.000000, 1484.878965
-0.370053, 0.000000, 1484.878965
-0.379510, 0.000000, 1484.878965
-0.390136, 0.000000, 1484.878965
-0.399366, 0.000000, 1484.878965
-0.409472, 0.000000, 1484.878965
-0.419898, 0.000000, 1484.878965
-0.430131, 0.000000, 1484.878965
-0.439363, 0.000000, 1484.878965
-0.449459, 0.000000, 1484.878965
-0.459840, 0.000000, 1484.878965
-0.469382, 0.000000, 1484.878965
-0.479846, 0.000000, 1484.878965
-0.489432, 0.000000, 1484.878965
-0.499342, 0.000000, 1484.878965
-0.509350, 0.000000, 1484.878965
-0.519406, 0.000000, 1484.878965
-0.530084, 0.000000, 1484.878965
-0.539341, 0.000000, 1484.878965
-0.549406, 0.000000, 1484.878965
-0.559401, 0.000000, 1484.878965
-0.569409, 0.000000, 1484.878965
-0.579831, 0.000000, 1484.878965
-0.589469, 0.000000, 1484.878965
-0.599356, 0.000000, 1484.878965
-0.610099, 0.000000, 1484.878965
-0.619333, 0.000000, 1484.878965
-0.629479, 0.000000, 1484.878965
-0.639805, 0.000000, 1484.878965
-0.650053, 0.000000, 1484.878965
-0.659423, 0.000000, 1484.878965
-0.669413, 0.000000, 1484.878965
-0.679822, 0.000000, 1484.878965
-0.690045, 0.000000, 1484.878965
-0.699411, 0.000000, 1484.878965
-0.709584, 0.000000, 1484.878965
-0.719866, 0.000000, 1484.878965
-0.729475, 0.000000, 1484.878965
-0.739328, 0.000000, 1484.878965
-0.749396, 0.000000, 1484.878965
-0.759836, 0.000000, 1484.878965
-0.769492, 0.000000, 1484.878965
-0.779340, 0.000000, 1484.878965
-0.789401, 0.000000, 1484.878965
-0.799405, 0.000000, 1484.878965
-0.809407, 0.000000, 1484.878965
-0.819830, 0.000000, 1484.878965
-0.829411, 0.000000, 1484.878965
-0.839413, 0.000000, 1484.878965
-0.849409, 0.000000, 1484.878965
-0.859349, 0.000000, 1484.878965
-0.869453, 0.000000, 1484.878965
-0.879831, 0.000000, 1484.878965
-0.889426, 0.000000, 1484.878965
-0.899332, 0.000000, 1484.878965
-0.909439, 0.000000, 1484.878965
-0.919830, 0.000000, 1484.878965
-0.929464, 0.000000, 1484.878965
-0.939328, 0.000000, 1484.878965
-0.949440, 0.000000, 1484.878965
-0.959871, 0.000000, 1484.878965
-0.969393, 0.000000, 1484.878965
-0.980080, 0.000000, 1484.878965
-0.989440, 0.000000, 1484.878965
-0.999370, 0.000000, 1484.878965
-1.009338, 0.000000, 1484.878965
-1.019368, 0.000000, 1484.878965
-1.029492, 0.000000, 1484.878965
-1.039816, 0.000000, 1484.878965
-1.049430, 0.000000, 1484.878965
-1.059324, 0.000000, 1484.878965
-1.069351, 0.000000, 1484.878965
-1.079867, 0.000000, 1484.878965
-1.089417, 0.000000, 1484.878965
-1.099324, 0.000000, 1484.878965
-1.109348, 0.000000, 1484.878965
-1.119389, 0.000000, 1484.878965
-1.129331, 0.000000, 1484.878965
-1.139306, 0.000000, 1484.878965
-1.149394, 0.000000, 1484.878965
-1.159374, 0.000000, 1484.878965
-1.169335, 0.000000, 1484.878965
-1.179817, 0.000000, 1484.878965
-1.189415, 0.000000, 1484.878965
-1.199338, 0.000000, 1484.878965
-1.209349, 0.000000, 1484.878965
-1.219333, 0.000000, 1484.878965
-1.229518, 0.000000, 1484.878965
-1.239329, 0.000000, 1484.878965
-1.249334, 0.000000, 1484.878965
-1.259316, 0.000000, 1484.878965
-1.269388, 0.000000, 1484.878965
-1.279357, 0.000000, 1484.878965
-1.289451, 0.000000, 1484.878965
-1.299350, 0.000000, 1484.878965
-1.309350, 0.000000, 1484.878965
-1.319848, 0.000000, 1484.878965
-1.329384, 0.000000, 1484.878965
-1.339375, 0.000000, 1484.878965
-1.349359, 0.000000, 1484.878965
-1.359384, 0.000000, 1484.878965
-1.369428, 0.000000, 1484.878965
-1.379443, 0.000000, 1484.878965
-1.389498, 0.000000, 1484.878965
-1.399332, 0.000000, 1484.878965
-1.409393, 0.000000, 1484.878965
-1.419325, 0.000000, 1484.878965
-1.430129, 0.000000, 1484.878965
-1.439419, 0.000000, 1484.878965
-1.449510, 0.000000, 1484.878965
-1.459828, 0.000000, 1484.878965
-1.469377, 0.000000, 1484.878965
-1.479834, 0.000000, 1484.878965
-1.489367, 0.000000, 1484.878965
-1.499316, 0.000000, 1484.878965
-1.509405, 0.000000, 1484.878965
-1.519341, 0.000000, 1484.878965
-1.529334, 0.000000, 1484.878965
-1.539305, 0.000000, 1484.878965
-1.550118, 0.000000, 1484.878965
-1.559386, 0.000000, 1484.878965
-1.569647, 0.000000, 1484.878965
-1.579395, 0.000000, 1484.878965
-1.589381, 0.000000, 1484.878965
-1.599819, 0.000000, 1484.878965
-1.609401, 0.000000, 1484.878965
-1.619404, 0.000000, 1484.878965
-1.629335, 0.000000, 1484.878965
-1.639327, 0.000000, 1484.878965
-1.649334, 0.000000, 1484.878965
-1.659341, 0.000000, 1484.878965
-1.669328, 0.000000, 1484.878965
-1.679850, 0.000000, 1484.878965
-1.689423, 0.000000, 1484.878965
-1.699320, 0.000000, 1484.878965
-1.710128, 0.000000, 1484.878965
-1.719388, 0.000000, 1484.878965
-1.730042, 0.000000, 1484.878965
-1.739338, 0.000000, 1484.878965
-1.749483, 0.000000, 1484.878965
-1.759420, 0.000000, 1484.878965
-1.769334, 0.000000, 1484.878965
-1.779289, 0.000000, 1484.878965
-1.789325, 0.000000, 1484.878965
-1.799395, 0.000000, 1484.878965
-1.809493, 0.000000, 1484.878965
-1.819312, 0.000000, 1484.878965
-1.829402, 0.000000, 1484.878965
-1.839317, 0.000000, 1484.878965
-1.849330, 0.000000, 1484.878965
-1.859354, 0.000000, 1484.878965
-1.869394, 0.000000, 1484.878965
-1.879816, 0.000000, 1484.878965
-1.889374, 0.000000, 1484.878965
-1.899381, 0.000000, 1484.878965
-1.909332, 0.000000, 1484.878965
-1.919359, 0.000000, 1484.878965
-1.929338, 0.000000, 1484.878965
-1.939359, 0.000000, 1484.878965
-1.949332, 0.000000, 1484.878965
-1.959325, 0.000000, 1484.878965
-1.969341, 0.000000, 1484.878965
-1.979362, 0.000000, 1484.878965
-1.989330, 0.000000, 1484.878965
-1.999479, 0.000000, 1484.878965
-2.009392, 0.000000, 1484.878965
-2.019318, 0.000000, 1484.878965
-2.029320, 0.000000, 1484.878965
-2.039323, 0.000000, 1484.878965
-2.049387, 0.000000, 1484.878965
-2.059818, 0.000000, 1484.878965
-2.069766, 0.000000, 1484.878965
-2.079835, 0.000000, 1484.878965
-2.089372, 0.000000, 1484.878965
-2.099322, 0.000000, 1484.878965
-2.109357, 0.000000, 1484.878965
-2.119387, 0.000000, 1484.878965
-2.129327, 0.000000, 1484.878965
-2.139458, 0.000000, 1484.878965
-2.149392, 0.000000, 1484.878965
-2.159826, 0.000000, 1484.878965
-2.169591, 0.000000, 1484.878965
-2.179656, 0.000000, 1484.878965
-2.189392, 0.000000, 1484.878965
-2.199491, 0.000000, 1484.878965
-2.209541, 0.000000, 1484.878965
-2.219287, 0.000000, 1484.878965
-2.229123, 0.000000, 1484.878965
-2.239347, 0.000000, 1484.878965
-2.249390, 0.000000, 1484.878965
-2.259407, 0.000000, 1484.878965
-2.269393, 0.000000, 1484.878965
-2.279375, 0.000000, 1484.878965
-2.289416, 0.000000, 1484.878965
-2.299368, 0.000000, 1484.878965
-2.309379, 0.000000, 1484.878965
-2.319382, 0.000000, 1484.878965
-2.329435, 0.000000, 1484.878965
-2.339329, 0.000000, 1484.878965
-2.349389, 0.000000, 1484.878965
-2.359454, 0.000000, 1484.878965
-2.369832, 0.000000, 1484.878965
-2.379390, 0.000000, 1484.878965
-2.389381, 0.000000, 1484.878965
-2.399429, 0.000000, 1484.878965
-2.409394, 0.000000, 1484.878965
-2.419367, 0.000000, 1484.878965
-2.429384, 0.000000, 1484.878965
-2.439408, 0.000000, 1484.878965
-2.449391, 0.000000, 1484.878965
-2.459343, 0.000000, 1484.878965
-2.469424, 0.000000, 1484.878965
-2.479357, 0.000000, 1484.878965
-2.489388, 0.000000, 1484.878965
-2.499413, 0.000000, 1484.878965
-2.510081, 0.000000, 1484.878965
-2.519397, 0.000000, 1484.878965
-2.529342, 0.000000, 1484.878965
-2.539372, 0.000000, 1484.878965
-2.549674, 0.000000, 1484.878965
-2.559586, 0.000000, 1484.878965
-2.569807, 0.000000, 1484.878965
-2.579362, 0.000000, 1484.878965
-2.589325, 0.000000, 1484.878965
-2.599300, 0.000000, 1484.878965
-2.609436, 0.000000, 1484.878965
-2.619476, 0.000000, 1484.878965
-2.629668, 0.000000, 1484.878965
-2.639301, 0.000000, 1484.878965
-2.649411, 0.000000, 1484.878965
-2.659301, 0.000000, 1484.878965
-2.669336, 0.000000, 1484.878965
-2.679460, 0.000000, 1484.878965
-2.689691, 0.000000, 1484.878965
-2.699310, 0.000000, 1484.878965
-2.710046, 0.000000, 1484.878965
-2.719584, 0.000000, 1484.878965
-2.729333, 0.000000, 1484.878965
-2.739288, 0.000000, 1484.878965
-2.749320, 0.000000, 1484.878965
-2.759517, 0.000000, 1484.878965
-2.769811, 0.000000, 1484.878965
-2.779463, 0.000000, 1484.878965
-2.789708, 0.000000, 1484.878965
-2.799310, 12.000000, 1484.878965
-2.809361, 12.000000, 1484.878965
-2.819345, 12.000000, 1484.943934
-2.829470, 12.000000, 1485.117183
-2.839666, 12.000000, 1485.355402
-2.849432, 12.000000, 1485.680245
-2.859547, 12.000000, 1486.091712
-2.869422, 12.000000, 1486.589805
-2.879352, 12.000000, 1487.152866
-2.889431, 12.000000, 1487.802552
-2.899538, 12.000000, 1488.538863
-2.909416, 12.000000, 1489.340142
-2.919676, 12.000000, 1490.163078
-2.929470, 11.856977, 1491.072638
-2.939341, 10.941776, 1492.090480
-2.949422, 9.709468, 1493.086665
-2.959343, 9.484298, 1494.212787
-2.969480, 9.024482, 1495.360566
-2.979482, 8.408468, 1496.616625
-2.989402, 7.584528, 1497.851029
-2.999839, 7.318006, 1499.193713
-3.009487, 6.726255, 1500.601366
-3.019345, 5.691274, 1502.030675
-3.029433, 5.445505, 1503.503297
-3.039661, 5.201068, 1505.019231
-3.049419, 4.805405, 1506.556821
-3.059323, 4.164102, 1508.116067
-3.069465, 3.901448, 1509.696970
-3.079658, 3.715078, 1511.321185
-3.089408, 3.656437, 1512.902087
-3.099346, 3.325052, 1514.504646
-3.109498, 3.310343, 1516.128861
-3.119381, 3.348580, 1517.753076
-3.129434, 3.031678, 1519.377291
-3.139461, 3.165136, 1520.979850
-3.149456, 3.276186, 1522.604064
-3.159650, 3.130954, 1524.228279
-3.169436, 3.017931, 1525.852494
-3.179542, 2.968543, 1527.455053
-3.189425, 3.107515, 1529.079268
-3.199654, 3.010200, 1530.681827
-3.209442, 3.078322, 1532.306042
-3.219518, 2.953745, 1533.908601
-3.229434, 3.021034, 1535.511159
-3.239303, 4.196084, 1537.113718
-3.249474, 2.523334, 1538.716277
-3.259481, 2.549791, 1540.318836
-3.269395, 2.856174, 1541.943051
-3.279668, 2.830169, 1543.545609
-3.289491, 2.903769, 1545.148168
-3.299343, 2.930722, 1546.729071
-3.309430, 2.915555, 1548.331629
-3.319847, 2.887528, 1549.934188
-3.329444, 3.022588, 1551.515091
-3.339468, 2.922583, 1553.095993
-3.349700, 3.155171, 1554.676896
-3.359307, 2.959473, 1556.257798
-3.369439, 2.963462, 1557.817045
-3.379865, 3.151337, 1559.397947
-3.389482, 3.237455, 1560.957194
-3.399670, 3.075969, 1562.538096
-3.409431, 3.127616, 1564.097342
-3.419355, 3.012946, 1565.656589
-3.429472, 3.094794, 1567.237491
-3.439645, 2.986884, 1568.796738
-3.449433, 3.228489, 1570.355984
-3.459540, 3.042079, 1571.915230
-3.469425, 3.052375, 1573.474477
-3.479338, 3.083112, 1575.055379
-3.489491, 2.926137, 1576.614626
-3.499576, 2.990285, 1578.152216
-3.509412, 3.204911, 1579.711462
-3.519368, 3.134930, 1581.270709
-3.529436, 3.050871, 1582.829955
-3.539512, 3.012085, 1584.389201
-3.549482, 3.160836, 1585.948448
-3.559848, 3.076263, 1587.486038
-3.569480, 2.996910, 1589.045284
-3.579466, 2.963210, 1590.604530
-3.589392, 3.113684, 1592.142121
-3.599645, 3.029053, 1593.679711
-3.609466, 3.111230, 1595.238957
-3.619478, 3.001191, 1596.776547
-3.629414, 3.083010, 1598.335794
-3.639639, 2.977469, 1599.873384
-3.649431, 3.061646, 1601.410974
-3.659343, 2.956478, 1602.970220
-3.669437, 3.040410, 1604.507810
-3.679660, 3.096927, 1606.067057
-3.689414, 3.104633, 1607.582991
-3.699466, 3.092523, 1609.120581
-3.709581, 3.079314, 1610.658171
-3.719844, 3.069195, 1612.195761
-3.729455, 3.060854, 1613.755008
-3.739454, 2.890971, 1615.270942
-3.749456, 3.120861, 1616.808532
-3.759652, 2.943141, 1618.367778
-3.769565, 2.963262, 1619.905368
-3.779406, 3.162518, 1621.442958
-3.789419, 3.096059, 1622.958892
-3.799647, 2.857986, 1624.496482
-3.809451, 3.063847, 1626.034073
-3.819535, 3.048330, 1627.550007
-3.829423, 3.158748, 1629.087597
-3.839290, 3.053598, 1630.625187
-3.849534, 2.974064, 1632.162777
-3.859585, 2.947301, 1633.678711
-3.869342, 3.104581, 1635.194645
-3.879651, 3.025376, 1636.753891
-3.889461, 3.112302, 1638.269825
-3.899471, 3.006920, 1639.785759
-3.909503, 3.093474, 1641.323349
-3.919650, 2.992726, 1642.860940
-3.929493, 3.081710, 1644.376873
-3.939457, 2.981346, 1645.914464
-3.949675, 2.908199, 1647.430398
-3.959566, 3.045687, 1648.946332
-3.969420, 3.126825, 1650.505578
-3.979850, 2.816686, 1652.021512
-3.989429, 3.120514, 1653.537446
-3.999650, 3.009544, 1655.053380
-4.009434, 3.053283, 1656.590970
-4.019490, 2.769875, 1658.106904
-4.029473, 2.932334, 1659.644494
-4.039639, 2.903060, 1661.182084
-4.049524, 3.176347, 1662.698018
-4.059463, 2.998063, 1664.213952
-4.069518, 3.013137, 1665.729886
-4.079855, 3.051287, 1667.267476
-4.089451, 3.065561, 1668.783410
-4.099514, 2.901543, 1670.299344
-4.109450, 2.971613, 1671.793622
-4.119835, 3.035151, 1673.331212
-4.129459, 3.052225, 1674.847146
-4.139393, 2.884954, 1676.363080
-4.149517, 3.114730, 1677.879014
-4.159643, 3.101587, 1679.373292
-4.169483, 3.212047, 1680.889226
-4.179478, 2.947840, 1682.405160
-4.189417, 3.111024, 1683.921094
-4.199352, 3.081820, 1685.415371
-4.209534, 3.196368, 1686.931305
-4.219472, 2.937203, 1688.447239
-4.229358, 3.102203, 1689.963173
-4.239581, 4.044265, 1691.479107
-4.249471, 2.568287, 1692.995041
-4.259564, 2.592233, 1694.510975
-4.269489, 2.874029, 1696.048565
-4.279649, 2.848913, 1697.564499
-4.289504, 3.102328, 1699.058777
-4.299467, 2.913154, 1700.574711
-4.309396, 2.926342, 1702.090645
-4.319635, 3.127416, 1703.584923
-4.329463, 3.066398, 1705.122513
-4.339462, 3.157757, 1706.616790
-4.349523, 3.054520, 1708.132724
-4.359685, 2.982710, 1709.627002
-4.369520, 3.124646, 1711.142936
-4.379481, 2.887285, 1712.658870
-4.389476, 3.058346, 1714.174804
-4.399662, 3.028207, 1715.647426
-4.409523, 3.140828, 1717.163360
-4.419532, 3.042732, 1718.679293
-4.429478, 3.131501, 1720.195227
-4.439668, 3.033777, 1721.667849
-4.449451, 3.127131, 1723.183783
-4.459519, 3.031674, 1724.699717
-4.469529, 3.125338, 1726.193995
-4.479652, 3.029621, 1727.709929
-4.489541, 3.123096, 1729.204206
-4.499285, 2.865452, 1730.720140
-4.509567, 3.035110, 1732.236074
-4.519358, 3.169872, 1733.752008
-4.529256, 3.046239, 1735.246286
-4.539423, 2.956041, 1736.762220
-4.549418, 3.092368, 1738.278154
-4.559287, 3.017417, 1739.772432
-4.569392, 3.113609, 1741.266709
-4.579389, 3.015401, 1742.804300
-4.589391, 3.107141, 1744.298577
-4.599271, 3.010950, 1745.792855
-4.609237, 3.104538, 1747.308789
-4.619397, 2.847040, 1748.803067
-4.629258, 3.016751, 1750.340657
-4.639391, 3.151513, 1751.834935
-4.649382, 3.027871, 1753.329212
-4.659249, 2.937667, 1754.845146
-4.669717, 3.073994, 1756.361080
-4.679709, 2.837164, 1757.877014
-4.689305, 3.009530, 1759.371292
-4.699414, 2.979957, 1760.865570
-4.709265, 3.254504, 1762.381504
-4.719267, 2.918277, 1763.875782
-4.729387, 3.014535, 1765.391715
-4.739263, 2.964507, 1766.885993
-4.749394, 3.242657, 1768.401927
-4.759241, 3.073913, 1769.917861
-4.769396, 2.934311, 1771.412139
-4.779436, 2.891905, 1772.928073
-4.789376, 3.055522, 1774.444007
-4.799259, 2.823787, 1775.938285
-4.809430, 2.993720, 1777.454219
-4.819715, 2.961969, 1778.948496
-4.829635, 3.235854, 1780.442774
-4.839637, 3.061570, 1781.958708
-4.849506, 3.081810, 1783.474642
-4.859505, 2.801335, 1784.968920
-4.869352, 3.134735, 1786.484854
-4.879650, 3.036602, 1787.979131
-4.889446, 3.084670, 1789.495065
-4.899459, 2.966217, 1790.989343
-4.909516, 3.056512, 1792.483621
-4.919642, 2.963315, 1793.999555
-4.929515, 3.058695, 1795.493833
-4.939540, 2.963436, 1796.988110
-4.949486, 3.056827, 1798.504044
-4.959635, 2.960935, 1800.019978
-4.969264, 3.054371, 1801.514256
-4.979559, 3.120496, 1803.030190
-4.989369, 2.975943, 1804.502812
-4.999639, 3.049612, 1806.018745
-5.009522, 3.114635, 1807.513023
-5.019539, 2.811284, 1809.028957
-5.029502, 3.124778, 1810.523235
-5.039639, 3.020720, 1812.017513
-5.049531, 3.069436, 1813.511790
-5.059571, 2.952407, 1815.006068
-5.069509, 3.205185, 1816.522002
-5.079646, 3.035861, 1818.016280
-5.089525, 3.224397, 1819.532214
-5.099572, 2.870103, 1821.026492
-5.109284, 3.134886, 1822.542426
-5.119546, 3.015399, 1824.036703
-5.129816, 3.065232, 1825.530981
-5.139566, 2.951614, 1827.025259
-5.149507, 3.044046, 1828.541193
-5.159648, 2.951048, 1830.035471
-5.169497, 3.208013, 1831.551405
-5.179545, 2.874516, 1833.045682
-5.189501, 3.137284, 1834.539960
-5.199633, 3.012938, 1836.034238
-5.209521, 3.222545, 1837.528516
-5.219560, 3.032578, 1839.044450
-5.229503, 3.056672, 1840.538727
-5.239594, 4.237957, 1842.033005
-5.249532, 2.592533, 1843.527283
-5.259542, 2.486074, 1845.043217
-5.269566, 2.894539, 1846.515838
-5.279650, 3.123100, 1848.010116
-5.289252, 3.165125, 1849.504394
-5.299546, 3.307151, 1850.998672
-5.309525, 3.213976, 1852.471293
-5.319632, 3.141194, 1853.943915
-5.329458, 3.284019, 1855.459849
-5.339575, 3.050528, 1856.932470
-5.349266, 3.226635, 1858.448404
-5.359682, 3.201591, 1859.921026
-5.369278, 3.319115, 1861.436960
-5.379549, 3.063938, 1862.931237
-5.389254, 3.071781, 1864.447171
-5.399651, 2.961775, 1865.919793
-5.409641, 3.063785, 1867.435727
-5.419564, 0.000000, 1868.930005
-5.429518, 0.000000, 1870.424282
-5.439596, 0.000000, 1871.918560
-5.449485, 0.000000, 1873.347869
-5.459469, 0.000000, 1874.733866
-5.469640, 0.000000, 1876.098207
-5.479575, 0.000000, 1877.419235
-5.489492, 0.000000, 1878.675294
-5.499454, 0.000000, 1879.909698
-5.509441, 0.000000, 1881.100789
-5.519669, 0.000000, 1882.270223
-5.529268, 0.000000, 1883.396346
-5.539531, 0.000000, 1884.479156
-5.549491, 0.000000, 1885.518653
-5.559591, 0.000000, 1886.536495
-5.569479, 0.000000, 1887.532680
-5.579553, 0.000000, 1888.485553
-5.589263, 0.000000, 1889.416769
-5.599628, 0.000000, 1890.283017
-5.609531, 0.000000, 1891.149265
-5.619452, 0.000000, 1891.993857
-5.629254, 0.000000, 1892.795136
-5.639595, 0.000000, 1893.574759
-5.649382, 0.000000, 1894.332726
-5.659518, 0.000000, 1895.047381
-5.669510, 0.000000, 1895.740379
-5.679630, 0.000000, 1896.433378
-5.689513, 0.000000, 1897.083064
-5.699559, 0.000000, 1897.689437
-5.709897, 0.000000, 1898.295811
-5.719663, 0.000000, 1898.880528
-5.729785, 0.000000, 1899.443590
-5.739550, 0.000000, 1899.984995
-5.749477, 0.000000, 1900.504743
-5.759631, 0.000000, 1901.002836
-5.769499, 0.000000, 1901.500928
-5.779545, 0.000000, 1901.955709
-5.789477, 0.000000, 1902.388833
-5.799588, 0.000000, 1902.821957
-5.809530, 0.000000, 1903.233424
-5.819550, 0.000000, 1903.623236
-5.829288, 0.000000, 1903.991391
-5.839583, 0.000000, 1904.359547
-5.849261, 0.000000, 1904.706046
-5.859488, 0.000000, 1905.030889
-5.869375, 0.000000, 1905.334076
-5.879665, 0.000000, 1905.658919
-5.889507, 0.000000, 1905.918793
-5.899530, 0.000000, 1906.200324
-5.909520, 0.000000, 1906.460198
-5.919633, 0.000000, 1906.720073
-5.929467, 0.000000, 1906.958291
-5.939535, 0.000000, 1907.174853
-5.949423, 0.000000, 1907.391415
-5.959583, 0.000000, 1907.586320
-5.969500, 0.000000, 1907.781226
-5.979548, 0.000000, 1907.954476
-5.989236, 0.000000, 1908.106069
-5.999631, 0.000000, 1908.279319
-6.009506, 0.000000, 1908.409256
-6.019548, 0.000000, 1908.560849
-6.029525, 0.000000, 1908.669130
-6.039582, 0.000000, 1908.799068
-6.049527, 0.000000, 1908.907349
-6.059538, 0.000000, 1909.015630
-6.069537, 0.000000, 1909.102254
-6.079650, 0.000000, 1909.188879
-6.089474, 0.000000, 1909.253848
-6.099533, 0.000000, 1909.318816
-6.109497, 0.000000, 1909.383785
-6.119587, 0.000000, 1909.448754
-6.129255, 0.000000, 1909.492066
-6.139524, 0.000000, 1909.513722
-6.149479, 0.000000, 1909.557035
-6.159584, 0.000000, 1909.578691
-6.169260, 0.000000, 1909.600347
-6.179536, 0.000000, 1909.600347
-6.189396, 0.000000, 1909.600347
-6.199574, 0.000000, 1909.600347
-6.209506, 0.000000, 1909.600347
-6.219521, 0.000000, 1909.600347
-6.229482, 0.000000, 1909.600347
-6.239565, 0.000000, 1909.600347
-6.249254, 0.000000, 1909.600347
-6.259521, 0.000000, 1909.600347
-6.269506, 0.000000, 1909.600347
-6.279589, 0.000000, 1909.600347
-6.289535, 0.000000, 1909.600347
-6.299542, 0.000000, 1909.600347
-6.309460, 0.000000, 1909.600347
-6.319858, 0.000000, 1909.600347
-6.329537, 0.000000, 1909.600347
-6.339535, 0.000000, 1909.600347
-6.349231, 0.000000, 1909.600347
-6.359649, 0.000000, 1909.600347
-6.369534, 0.000000, 1909.600347
-6.379535, 0.000000, 1909.600347
diff --git a/bot3/control_loops/shooter/shooter_motor.q b/bot3/control_loops/shooter/shooter_motor.q
index 714e77a..d581c5d 100644
--- a/bot3/control_loops/shooter/shooter_motor.q
+++ b/bot3/control_loops/shooter/shooter_motor.q
@@ -24,8 +24,8 @@
};
message Position {
- // The angle of the shooter wheel measured in rad/sec.
- double position;
+ // The speed of the shooter wheel measured in rad/sec.
+ double velocity;
};
message Output {
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index ab5dbc7..c71b96a 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -1,4 +1,4 @@
-#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
#include <vector>
@@ -7,40 +7,40 @@
namespace bot3 {
namespace control_loops {
-StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients() {
- Eigen::Matrix<double, 1, 1> A;
- A << 0.989057756738;
- Eigen::Matrix<double, 1, 1> B;
- B << 0.533205953514;
- Eigen::Matrix<double, 1, 1> C;
- C << 0;
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00994518845675, 0.0, 0.989057756738;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00267091861198, 0.533205953514;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
D << 0;
Eigen::Matrix<double, 1, 1> U_max;
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlantCoefficients<1, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<1, 1, 1> MakeShooterController() {
- Eigen::Matrix<double, 1, 1> L;
- L << 29.7111780621;
- Eigen::Matrix<double, 1, 1> K;
- K << 0.758151303088;
- return StateFeedbackController<1, 1, 1>(L, K, MakeShooterPlantCoefficients());
+StateFeedbackController<2, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.08905775674, 29.7111780621;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.450278144, 2.19032372689;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
-StateFeedbackPlant<1, 1, 1> MakeShooterPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeShooterPlantCoefficients());
- return StateFeedbackPlant<1, 1, 1>(plants);
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
}
-StateFeedbackLoop<1, 1, 1> MakeShooterLoop() {
- ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<1, 1, 1>(MakeShooterController());
- return StateFeedbackLoop<1, 1, 1>(controllers);
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
}
} // namespace control_loops
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index 1ab9702..fa2237d 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -6,13 +6,13 @@
namespace bot3 {
namespace control_loops {
-StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
-StateFeedbackController<1, 1, 1> MakeShooterController();
+StateFeedbackController<2, 1, 1> MakeShooterController();
-StateFeedbackPlant<1, 1, 1> MakeShooterPlant();
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
-StateFeedbackLoop<1, 1, 1> MakeShooterLoop();
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
} // namespace control_loops
} // namespace bot3