Merge remote-tracking branch 'danielp/bot3-changes' into bot3-changes
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 69c3121..5d22300 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -53,16 +53,16 @@
if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
LOG(ERROR, "Stale position. %d ms > %d ms. Outputs disabled.\n",
msec_age, kPositionTimeoutMs);
- ZeroOutputs();
- return;
+ //ZeroOutputs();
+ //eturn;
} else {
LOG(ERROR, "Stale position. %d ms\n", msec_age);
}
} else {
LOG(ERROR, "Never had a position.\n");
if (fail_no_position) {
- ZeroOutputs();
- return;
+ // ZeroOutputs();
+ // return;
}
}
}
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
index 245e212..d507b40 100644
--- a/bot3/atom_code/atom_code.gyp
+++ b/bot3/atom_code/atom_code.gyp
@@ -11,10 +11,7 @@
'<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto',
'<(DEPTH)/bot3/input/input.gyp:joystick_reader',
- #'../input/input.gyp:AutoMode',
'<(DEPTH)/bot3/output/output.gyp:MotorWriter',
- '<(DEPTH)/bot3/output/output.gyp:CameraServer',
- #'camera/camera.gyp:frc971',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:get',
'<(DEPTH)/bot3/input/input.gyp:gyro_sensor_receiver',
],
diff --git a/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index 165d99f..25bfc74 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -1,4 +1,5 @@
#include "bot3/control_loops/shooter/shooter.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -21,15 +22,14 @@
void ShooterMotor::RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Output *output,
control_loops::ShooterLoop::Status *status) {
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_ &&
@@ -73,6 +73,9 @@
if (output) {
output->voltage = output_voltage;
+ output->intake = goal->intake;
+ output->push = goal->push;
+ LOG(DEBUG, "goal: %lf, volt: %lf, push:%d\n", goal->intake, output_voltage, goal->push);
}
}
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
index 28e6b14..2be0439 100644
--- a/bot3/control_loops/shooter/shooter.gyp
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -29,7 +29,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- #'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
],
'export_dependent_settings': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
index ac7aadd..b9514c3 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -28,7 +28,7 @@
virtual void RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Output *output,
control_loops::ShooterLoop::Status *status);
private:
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_lib_test.cc b/bot3/control_loops/shooter/shooter_lib_test.cc
index fddc415..445ef57 100644
--- a/bot3/control_loops/shooter/shooter_lib_test.cc
+++ b/bot3/control_loops/shooter/shooter_lib_test.cc
@@ -34,7 +34,7 @@
void SendPositionMessage() {
::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
my_shooter_loop_.position.MakeMessage();
- position->position = shooter_plant_->Y(0, 0);
+ position->velocity = shooter_plant_->Y(0, 0);
position.Send();
}
diff --git a/bot3/control_loops/shooter/shooter_motor.q b/bot3/control_loops/shooter/shooter_motor.q
index 87eaa16..d581c5d 100644
--- a/bot3/control_loops/shooter/shooter_motor.q
+++ b/bot3/control_loops/shooter/shooter_motor.q
@@ -8,6 +8,10 @@
message Goal {
// Goal velocity in rad/sec
double velocity;
+ // PWM output to intake.
+ double intake;
+ // Whether to activate pusher piston.
+ bool push;
};
message Status {
@@ -15,16 +19,27 @@
bool ready;
// The average velocity over the last 0.1 seconds.
double average_velocity;
+ // True if Frisbees are being fired into the shooter.
+ bool firing;
};
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 {
+ // Output to shooter, Volts.
+ double voltage;
+ // PWM output to intake.
+ double intake;
+ // Whether to activate pusher piston.
+ bool push;
};
queue Goal goal;
queue Position position;
- queue aos.control_loops.Output output;
+ queue Output output;
queue Status status;
};
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index cb707b2..79fd6be 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>
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index bb6bac7..0331d24 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -29,27 +29,14 @@
}
inline double shooter_translate(int32_t in) {
- return static_cast<double>(in) / 500 /*counter rate*/ * (2 * M_PI);
-}
-
-// Translates values from the ADC into voltage.
-inline double adc_translate(uint16_t in) {
- static const double kVRefN = 0;
- static const double kVRefP = 3.3;
- static const int kMaximumValue = 0x3FF;
- return kVRefN +
- (static_cast<double>(in) / static_cast<double>(kMaximumValue) *
- (kVRefP - kVRefN));
+ return 1.0 / (static_cast<double>(in) / (10 ^ 8)/*ticks per sec*/)
+ * (2 * M_PI);
}
inline double gyro_translate(int64_t in) {
return in / 16.0 / 1000.0 / (180.0 / M_PI);
}
-inline double battery_translate(uint16_t in) {
- return adc_translate(in) * 80.8 / 17.8;
-}
-
} // namespace
class GyroSensorReceiver : public USBReceiver {
@@ -68,17 +55,13 @@
.right_encoder(drivetrain_translate(data()->main.wrist))
.left_encoder(drivetrain_translate(data()->main.shooter))
.Send();
+ /*drivetrain.position.MakeWithBuilder()
+ .right_encoder(0)
+ .left_encoder(0)
+ .Send();*/
shooter.position.MakeWithBuilder()
- .position(shooter_translate(data()->bot3.shooter_cycle_ticks));
-
- LOG(DEBUG, "battery %f %f %" PRIu16 "\n",
- battery_translate(data()->main.battery_voltage),
- adc_translate(data()->main.battery_voltage),
- data()->main.battery_voltage);
- LOG(DEBUG, "halls l=%f r=%f\n",
- adc_translate(data()->main.left_drive_hall),
- adc_translate(data()->main.right_drive_hall));
+ .velocity(shooter_translate(data()->bot3.shooter_cycle_ticks));
}
};
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index 3e1cfa7..b43fc48 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -12,6 +12,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
],
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index 5efe8b4..199d1e6 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -8,12 +8,14 @@
#include "aos/common/logging/logging.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "bot3/autonomous/auto.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
#include "frc971/queues/CameraTarget.q.h"
using ::bot3::control_loops::drivetrain;
+using ::bot3::control_loops::shooter;
using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
// using ::frc971::vision::target_angle;
@@ -32,15 +34,10 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
-const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
+const ButtonLocation kPush(3, 9);
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kIntake(3, 10);
-const ButtonLocation kForceFire(3, 12);
-const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+const ButtonLocation kFire(3, 3);
+const ButtonLocation kIntake(3, 4);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -126,6 +123,41 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
+
+ // 3, 4, 9, 9 fires, 3 pickups
+
+ shooter.status.FetchLatest();
+ bool push = false;
+ double velocity = 0.0;
+ double intake = 0.0;
+ if (data.IsPressed(kPush) && shooter.status->ready) {
+ push = true;
+ }
+ if (data.IsPressed(kFire)) {
+ velocity = 500;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 1))) {
+ velocity = 50;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 2))) {
+ velocity = 250;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 5))) {
+ velocity = 300;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 7))) {
+ velocity = 350;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 8))) {
+ velocity = 400;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 10))) {
+ velocity = 450;
+ }
+ if (data.IsPressed(kIntake)) {
+ intake = 0.8;
+ }
+ shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
#if 0
::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
index 43a45a3..a16dfc4 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -47,13 +47,83 @@
shooter.output.FetchLatest();
if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(2, shooter.output->voltage / 12.0, kVictorBounds);
+ SetPWMOutput(1, LinearizeVictor(shooter.output->voltage / 12.0), kVictorBounds);
+ SetPWMOutput(7, shooter.output->intake, kVictorBounds);
+ SetSolenoid(4, shooter.output->push);
+ LOG(DEBUG, "shooter: %lf, intake: %lf, push: %d", shooter.output->voltage, shooter.output->intake, shooter.output->push);
} else {
DisablePWMOutput(2);
+ DisablePWMOutput(1);
LOG(WARNING, "shooter not new enough\n");
}
// TODO(danielp): Add stuff for intake and shooter.
}
+
+ // This linearizes the value from the victor.
+ // Copied form the 2012 code.
+ double LinearizeVictor(double goal_speed) {
+ // These values were derived by putting the robot up on blocks, and driving it
+ // at various speeds. The power required to drive at these speeds was then
+ // recorded and fit with gnuplot.
+ const double deadband_value = 0.082;
+ // If we are outside the range such that the motor is actually moving,
+ // subtract off the constant offset from the deadband. This makes the
+ // function odd and intersect the origin, making the fitting easier to do.
+ if (goal_speed > deadband_value) {
+ goal_speed -= deadband_value;
+ } else if (goal_speed < -deadband_value) {
+ goal_speed += deadband_value;
+ } else {
+ goal_speed = 0.0;
+ }
+ goal_speed = goal_speed / (1.0 - deadband_value);
+
+ double goal_speed2 = goal_speed * goal_speed;
+ double goal_speed3 = goal_speed2 * goal_speed;
+ double goal_speed4 = goal_speed3 * goal_speed;
+ double goal_speed5 = goal_speed4 * goal_speed;
+ double goal_speed6 = goal_speed5 * goal_speed;
+ double goal_speed7 = goal_speed6 * goal_speed;
+
+ // Constants for the 5th order polynomial
+ double victor_fit_e1 = 0.437239;
+ double victor_fit_c1 = -1.56847;
+ double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0
+ * victor_fit_c1 - 116.0) / 125.0);
+ double answer_5th_order = (victor_fit_a1 * goal_speed5
+ + victor_fit_c1 * goal_speed3
+ + victor_fit_e1 * goal_speed);
+
+ // Constants for the 7th order polynomial
+ double victor_fit_c2 = -5.46889;
+ double victor_fit_e2 = 2.24214;
+ double victor_fit_g2 = -0.042375;
+ double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2
+ + victor_fit_g2) - 116.0) / 125.0);
+ double answer_7th_order = (victor_fit_a2 * goal_speed7
+ + victor_fit_c2 * goal_speed5
+ + victor_fit_e2 * goal_speed3
+ + victor_fit_g2 * goal_speed);
+
+
+ // Average the 5th and 7th order polynomials, and add a bit of linear power in
+ // as well. The average turns out to nicely fit the data in gnuplot with nice
+ // smooth curves, and the linear power gives it a bit more punch at low speeds
+ // again. Stupid victors.
+ double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ + .15 * goal_speed * (1.0 - deadband_value);
+
+ // Add the deadband power back in to make it so that the motor starts moving
+ // when any power is applied. This is what the fitting assumes.
+ if (answer > 0.001) {
+ answer += deadband_value;
+ } else if (answer < -0.001) {
+ answer -= deadband_value;
+ }
+
+ return answer;
+ }
+
};
} // namespace output
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
index 54f36e3..12a128c 100644
--- a/bot3/output/output.gyp
+++ b/bot3/output/output.gyp
@@ -1,29 +1,6 @@
{
'targets': [
{
- 'target_name': 'CameraServer',
- 'type': 'executable',
- 'sources': [
- '<(DEPTH)/frc971/output/CameraServer.cc',
- ],
- 'dependencies': [
- '<(AOS)/atom_code/output/output.gyp:http_server',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/atom_code/atom_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
- '<(AOS)/atom_code/atom_code.gyp:configuration',
- ],
- 'copies': [
- {
- 'destination': '<(rsync_dir)',
- 'files': [
- 'robot.html.tpl',
- ],
- },
- ],
- },
- {
'target_name': 'MotorWriter',
'type': '<(aos_target)',
'sources': [
diff --git a/bot3/output/robot.html.tpl b/bot3/output/robot.html.tpl
deleted file mode 100644
index 7ae51a6..0000000
--- a/bot3/output/robot.html.tpl
+++ /dev/null
@@ -1,56 +0,0 @@
-<!DOCTYPE HTML>
-<html>
- <head>
- <title>971 Camera Code: Robot Stream</title>
- <style type="text/css">
- #body {
- display: block;
- margin: 0px;
- margin-top: 0px;
- margin-right: 0px;
- margin-bottom: 0px;
- margin-left: 0px;
- }
- #img {
- position: absolute;
- left: 50%;
- top: 0%;
- margin: 0 0 0 -320px;
- }
- #center {
- left: 50%;
- position: absolute;
- width: 2px;
- height: 100%;
- background-color: red;
- }
- #middle {
- top: 240px;
- margin-top: -1px;
- width: 100%;
- position: absolute;
- height: 2px;
- background-color: red;
- }
- #footer {
- top: 482px;
- left: 10px;
- position: absolute;
- }
- #center {
- margin-left: {{CENTER}}px;
- }
- </style>
- </head>
- <body id="body">
- <img id="img" src="http://{{HOST}}:9714" />
- <div id="center"></div>
- <div id="middle"></div>
- <div id="footer">
- <!--<form>
- <input type="button" value="Camera Controls"
- onclick="window.open('control.htm', 'Camera_Controls')">
- </form>-->
- </div>
- </body>
-</html>
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index 82f1b52..c625026 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -13,9 +13,10 @@
// How long (in ms) to wait after a falling edge on the bottom indexer sensor
// before reading the indexer encoder.
static const int kBottomFallDelayTime = 32;
+
// How long to wait for a revolution of the shooter wheel (on the third robot)
// before said wheel is deemed "stopped". (In secs)
-static const uint8_t kWheelStopThreshold = 1;
+static const float kWheelStopThreshold = 2.0;
// The timer to use for timestamping sensor readings.
// This is a constant to avoid hard-coding it in a lot of places, but there ARE
@@ -91,9 +92,6 @@
static inline void reset_TC(void) {
TIM2->TCR |= (1 << 1); // Put it into reset.
- while (TIM2->TC != 0) { // Wait for reset.
- continue;
- }
TIM2->TCR = 1; // Take it out of reset + make sure it's enabled.
}
@@ -505,17 +503,14 @@
// Set up timer for bot3 photosensor.
// Make sure timer two is powered.
SC->PCONP |= (1 << 22);
- // We don't need all the precision the CCLK can provide.
- // We'll use CCLK/8. (12.5 mhz).
- SC->PCLKSEL1 |= (0x3 << 12);
- // Use timer prescale to get that freq down to 500 hz.
- TIM2->PR = 25000;
+ // Rate of clock signal is just CCLK.
+ SC->PCLKSEL1 |= (1 << 12);
// Select capture 2.0 function on pin 0.4.
PINCON->PINSEL0 |= (0x3 << 8);
// Set timer to capture and interrupt on rising edge.
TIM2->CCR = 0x5;
// Set up match interrupt.
- TIM2->MR0 = kWheelStopThreshold * 500;
+ TIM2->MR0 = kWheelStopThreshold * (10 ^ 8);
TIM2->MCR = 1;
// Enable timer IRQ, and make it lower priority than the encoders.
NVIC_SetPriority(TIMER3_IRQn, 1);