Merge "Add a BUILD for gflags."
diff --git a/bot3/control_loops/control_loops.gyp b/bot3/control_loops/control_loops.gyp
new file mode 100644
index 0000000..b63816e
--- /dev/null
+++ b/bot3/control_loops/control_loops.gyp
@@ -0,0 +1,15 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'position_sensor_sim',
+ 'type': 'static_library',
+ 'sources': ['position_sensor_sim.cc'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops',
+ },
+ 'dependencies': [
+ '<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_queue',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain.h b/bot3/control_loops/drivetrain/drivetrain.h
index 551349d..586962f 100644
--- a/bot3/control_loops/drivetrain/drivetrain.h
+++ b/bot3/control_loops/drivetrain/drivetrain.h
@@ -16,11 +16,11 @@
// Constants
// TODO(comran): Get actual constants.
-constexpr double kDrivetrainTurnWidth = 0.5;
+constexpr double kDrivetrainTurnWidth = 0.63;
constexpr double kDrivetrainDoneDistance = 0.02;
-constexpr double kDrivetrainEncoderRatio = 20.0 / 50.0;
+constexpr double kDrivetrainEncoderRatio = 18.0 / 44.0;
constexpr double kDrivetrainHighGearRatio =
- kDrivetrainEncoderRatio * 20.0 / 50.0;
+ kDrivetrainEncoderRatio * 18.0 / 60.0;
constexpr double kDrivetrainLowGearRatio = kDrivetrainHighGearRatio;
const bool kDrivetrainClutchTransmission = false;
const ::frc971::constants::ShifterHallEffect kDrivetrainRightShifter{
diff --git a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index e24fa71..85cb721 100644
--- a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
}
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 95913ff..ed8e990 100644
--- a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/bot3/control_loops/elevator/elevator.cc b/bot3/control_loops/elevator/elevator.cc
new file mode 100644
index 0000000..a28dd7d
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.cc
@@ -0,0 +1,284 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include <cmath>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/elevator/integral_elevator_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+void SimpleCappedStateFeedbackLoop::CapU() {
+ mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
+ mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
+}
+
+double SimpleCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
+ // Compute K matrix to compensate for position errors.
+ double Kp = K(0, 0);
+
+ // Compute how much we need to change R in order to achieve the change in U
+ // that was observed.
+ return -(1.0 / Kp) * (U_uncapped() - U())(0, 0);
+}
+
+Elevator::Elevator(control_loops::ElevatorQueue *elevator)
+ : aos::controls::ControlLoop<control_loops::ElevatorQueue>(elevator),
+ loop_(new SimpleCappedStateFeedbackLoop(MakeIntegralElevatorLoop())),
+ profile_(::aos::controls::kLoopFrequency) {}
+
+bool Elevator::CheckZeroed() {
+ return state_ == RUNNING;
+}
+
+void Elevator::Correct() {
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << current_position();
+ loop_->Correct(Y);
+}
+
+double Elevator::current_position() {
+ return current_position_.encoder + offset_;
+}
+
+double Elevator::GetZeroingVelocity() {
+ return zeroing_velocity_;
+}
+
+// If the hall_effect is true that means we need to move up until is false.
+// Then we should move down.
+double Elevator::FindZeroingVelocity() {
+ if (glitch_filter_.filtered_value()) {
+ zeroing_velocity_ = kZeroingSlowVelocity;
+ } else {
+ zeroing_velocity_ = -kZeroingVelocity;
+ }
+
+ return zeroing_velocity_;
+}
+
+double Elevator::UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+
+void Elevator::SetOffset(double offset) {
+ LOG(INFO, "Changing Elevator offset from %f to %f\n", offset_, offset);
+ double doffset = offset - offset_;
+
+ loop_->mutable_X_hat(0, 0) += doffset;
+
+ // Modify the zeroing goal.
+ goal_ += doffset;
+
+ // Update the cached offset values to the actual values.
+ offset_ = offset;
+}
+
+void Elevator::RunIteration(
+ const control_loops::ElevatorQueue::Goal *unsafe_goal,
+ const control_loops::ElevatorQueue::Position *position,
+ control_loops::ElevatorQueue::Output *output,
+ control_loops::ElevatorQueue::Status *status) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ state_ = UNINITIALIZED;
+ }
+ glitch_filter_.Update(position->bottom_hall_effect, position->encoder);
+
+ // Bool to track if we should turn the motors on or not.
+ bool disable = output == nullptr;
+
+ // Save the current position so it can be used easily in the class.
+ current_position_ = *position;
+
+ if (state_ != UNINITIALIZED) {
+ Correct();
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ LOG(DEBUG, "Uninitialized\n");
+ // Startup. Assume that we are at the origin everywhere.
+ offset_ = -position->encoder;
+ loop_->mutable_X_hat().setZero();
+ LOG(INFO, "Initializing elevator offset to %f\n", offset_);
+ Correct();
+ state_ = ZEROING;
+ disable = true;
+ glitch_filter_.Reset(position->bottom_hall_effect);
+ break;
+
+ case ZEROING:
+ LOG(DEBUG, "Zeroing elevator\n");
+
+ if (glitch_filter_.negedge()) {
+ state_ = RUNNING;
+ SetOffset(-glitch_filter_.negedge_value() + kHallEffectPosition);
+ }
+
+ // We need to check FindZeroingVelocity() every time
+ // in order for zeroing to work.
+ {
+ double zeroing_velocity_temp = FindZeroingVelocity();
+ if (state_ != RUNNING && !disable) {
+ // Move the elevator either up or down based on where the zeroing hall
+ // effect is located.
+
+ goal_velocity_ = zeroing_velocity_temp;
+ goal_ += goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ }
+ }
+
+ // Bypass motion profiles while we are zeroing.
+ // This is also an important step right after the elevator is zeroed and
+ // we reach into the elevator's state matrix and change it based on the
+ // newly-obtained offset.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << goal_, goal_velocity_;
+ profile_.MoveCurrentState(current);
+ }
+ break;
+
+ case RUNNING:
+ if (unsafe_goal) {
+ profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_velocity, 0.50));
+ profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_acceleration, 2.0));
+
+ // Use the profiles to limit the elevator's movements.
+ const double unfiltered_goal = ::std::max(
+ ::std::min(unsafe_goal->height, kElevUpperLimit), kElevLowerLimit);
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unfiltered_goal, unsafe_goal->velocity);
+ goal_ = goal_state(0, 0);
+ goal_velocity_ = goal_state(1, 0);
+ }
+
+ if (state_ != RUNNING && state_ != ESTOP) {
+ state_ = UNINITIALIZED;
+ }
+ break;
+
+ case ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Limit the goals so we can't exceed the hardware limits if we are RUNNING.
+ if (state_ == RUNNING) {
+ // Limit the elevator goal to min/max allowable heights.
+ if (goal_ >= kElevUpperLimit) {
+ LOG(WARNING, "Elevator goal above limit, %f > %f\n", goal_,
+ kElevUpperLimit);
+ goal_ = kElevUpperLimit;
+ }
+
+ if (goal_ <= kElevLowerLimit) {
+ LOG(WARNING, "Elevator goal below limit, %f < %f\n", goal_,
+ kElevLowerLimit);
+ goal_ = kElevLowerLimit;
+ }
+ }
+
+ // Check the hard limits.
+ if (state_ == RUNNING) {
+ if (current_position() >= kElevUpperHardLimit) {
+ LOG(ERROR, "Elevator at %f out of bounds [%f, %f], ESTOPing\n",
+ current_position(), kElevLowerHardLimit, kElevUpperHardLimit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+
+ if (current_position() <= kElevLowerHardLimit) {
+ LOG(ERROR, "Elevator at %f out of bounds [%f, %f], ESTOPing\n",
+ current_position(), kElevLowerHardLimit, kElevUpperHardLimit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+ }
+
+ // Set the goals.
+ loop_->mutable_R() << goal_, goal_velocity_, 0.0;
+
+ const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
+ loop_->set_max_voltage(max_voltage);
+
+ if (state_ == ESTOP) {
+ disable = true;
+ }
+ loop_->Update(disable);
+
+ if (state_ == ZEROING || state_ == RUNNING) {
+ if (loop_->U() != loop_->U_uncapped()) {
+ double deltaR = loop_->UnsaturateOutputGoalChange();
+
+ // Move the elevator goal by the amount observed.
+ LOG(WARNING, "Moving elevator goal by %f to handle saturation\n", deltaR);
+ goal_ += deltaR;
+
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << goal_, goal_velocity_;
+ profile_.MoveCurrentState(current);
+ }
+ }
+
+ if (output) {
+ output->elevator = loop_->U(0, 0);
+ if(unsafe_goal) {
+ output->passive_support = unsafe_goal->passive_support;
+ output->can_support = unsafe_goal->can_support;
+ }
+ }
+
+ status->zeroed = state_ == RUNNING;
+
+ status->height = loop_->X_hat(0, 0);
+ status->velocity = loop_->X_hat(1, 0);
+
+ status->goal_height = goal_;
+ status->goal_velocity = goal_velocity_;
+
+ status->estopped = (state_ == ESTOP);
+ status->state = state_;
+}
+
+void GlitchFilter::Update(bool hall_effect, double encoder) {
+ posedge_ = false;
+ negedge_ = false;
+ if (hall_effect != accepted_value_) {
+ if (count_ == 0) {
+ first_encoder_ = encoder;
+ }
+ ++count_;
+ } else {
+ last_encoder_ = encoder;
+ count_ = 0;
+ }
+ if (count_ >= 2) {
+ if (hall_effect) {
+ posedge_ = true;
+ posedge_value_ = (first_encoder_ + last_encoder_) / 2.0;
+ } else {
+ negedge_ = true;
+ negedge_value_ = (first_encoder_ + last_encoder_) / 2.0;
+ }
+ accepted_value_ = hall_effect;
+ count_ = 0;
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator.gyp b/bot3/control_loops/elevator/elevator.gyp
new file mode 100644
index 0000000..c184429
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.gyp
@@ -0,0 +1,71 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'elevator_queue',
+ 'type': 'static_library',
+ 'sources': ['elevator.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/elevator',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'elevator_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'elevator.cc',
+ 'elevator_motor_plant.cc',
+ 'integral_elevator_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'elevator_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/voltage_cap/voltage_cap.gyp:voltage_cap',
+ ],
+ 'export_dependent_settings': [
+ 'elevator_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'elevator_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'elevator_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'elevator_lib',
+ '<(DEPTH)/bot3/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ {
+ 'target_name': 'elevator',
+ 'type': 'executable',
+ 'sources': [
+ 'elevator_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'elevator_lib',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/elevator/elevator.h b/bot3/control_loops/elevator/elevator.h
new file mode 100644
index 0000000..3185187
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.h
@@ -0,0 +1,194 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+class ElevatorTest_DisabledGoalTest_Test;
+class ElevatorTest_ElevatorGoalPositiveWindupTest_Test;
+class ElevatorTest_ElevatorGoalNegativeWindupTest_Test;
+class ElevatorTest_PositiveRunawayProfileTest_Test;
+class ElevatorTest_NegativeRunawayProfileTest_Test;
+} // namespace testing
+
+// Constants
+constexpr double kZeroingVoltage = 4.0;
+// TODO(austin): Slow down the zeroing velocity.
+constexpr double kZeroingVelocity = 0.05;
+constexpr double kZeroingSlowVelocity = 0.01;
+
+// Game pieces
+constexpr double kToteHeight = 0.3;
+
+// TODO(comran): Get actual constants for the ones below.
+// Gearing
+
+constexpr double kElevEncoderRatio = 18.0 / 48.0;
+const int kElevGearboxOutputPulleyTeeth = 22;
+// 25 pitch chain.
+constexpr double kElevGearboxOutputPitch = 0.25 * 0.0254;
+
+constexpr double kElevGearboxOutputRadianDistance =
+ kElevGearboxOutputPulleyTeeth * kElevGearboxOutputPitch / (2.0 * M_PI);
+
+// Limits
+constexpr double kElevLowerHardLimit = -0.005;
+constexpr double kElevUpperHardLimit = 0.720000;
+constexpr double kElevLowerLimit = 0.010000;
+constexpr double kElevUpperLimit = 0.680000;
+
+// Zeroing
+namespace {
+// TODO(Adam): Find the actual value of the hall effect position.
+constexpr double kHallEffectPosition = 0.01;
+} // namespace
+// End constants
+
+class SimpleCappedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+ SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> &&loop)
+ : StateFeedbackLoop<3, 1, 1>(::std::move(loop)), max_voltage_(12.0) {}
+
+ void set_max_voltage(double max_voltage) {
+ max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
+ }
+
+ void CapU() override;
+
+ // Returns the amount to change the position goal in order to no longer
+ // saturate the controller.
+ double UnsaturateOutputGoalChange();
+
+ private:
+ double max_voltage_;
+};
+
+// The GlitchFilter filters out single cycle changes in order to filter out
+// sensor noise. It also captures the sensor value at the time that the
+// original transition happens (not the de-bounced transition) to remove delay
+// caused by this filter.
+class GlitchFilter {
+ public:
+ void Reset(bool hall_effect) {
+ accepted_value_ = hall_effect;
+ posedge_ = false;
+ negedge_ = false;
+ count_ = 0;
+ }
+
+ // Updates the filter state with new observations.
+ void Update(bool hall_effect, double encoder);
+
+ // Returns a debounced hall effect value.
+ bool filtered_value() const { return accepted_value_; }
+
+ // Returns true if last cycle had a posedge.
+ bool posedge() const { return posedge_; }
+ // Returns the encoder value across the last posedge.
+ double posedge_value() const { return posedge_value_; }
+
+ // Returns true if last cycle had a negedge.
+ bool negedge() const { return negedge_; }
+ // Returns the encoder value across the last negedge.
+ double negedge_value() const { return negedge_value_; }
+
+ private:
+ int count_ = 0;
+ bool accepted_value_ = false;
+ bool posedge_ = false;
+ bool negedge_ = false;
+ double posedge_value_ = 0;
+ double negedge_value_ = 0;
+
+ double first_encoder_ = 0;
+ double last_encoder_ = 0;
+};
+
+class Elevator
+ : public aos::controls::ControlLoop<control_loops::ElevatorQueue> {
+ public:
+ explicit Elevator(control_loops::ElevatorQueue *elevator_queue =
+ &control_loops::elevator_queue);
+
+ bool CheckZeroed();
+
+ // Getter for the current elevator positions with zeroed offset.
+ double current_position();
+
+ double GetZeroingVelocity();
+
+ enum State {
+ // Waiting to receive data before doing anything.
+ UNINITIALIZED = 0,
+ // Moving the elevator to find an index pulse.
+ ZEROING = 2,
+ // All good!
+ RUNNING = 3,
+ // Internal error caused the elevator to abort.
+ ESTOP = 4,
+ };
+
+ State state() const { return state_; }
+
+ protected:
+ void RunIteration(const control_loops::ElevatorQueue::Goal *goal,
+ const control_loops::ElevatorQueue::Position *position,
+ control_loops::ElevatorQueue::Output *output,
+ control_loops::ElevatorQueue::Status *status) override;
+
+ private:
+ friend class testing::ElevatorTest_DisabledGoalTest_Test;
+ friend class testing::ElevatorTest_ElevatorGoalPositiveWindupTest_Test;
+ friend class testing::ElevatorTest_ElevatorGoalNegativeWindupTest_Test;
+ friend class testing::ElevatorTest_PositiveRunawayProfileTest_Test;
+ friend class testing::ElevatorTest_NegativeRunawayProfileTest_Test;
+
+ void SetOffset(double offset);
+
+ // Returns the current elevator zeroing velocity.
+ double FindZeroingVelocity();
+
+ // Corrects the Observer with the current position.
+ void Correct();
+
+ double UseUnlessZero(double target_value, double default_value);
+
+ // The state feedback control loop or loops to talk to.
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop> loop_;
+
+ // Offsets from the encoder position to the absolute position. Add these to
+ // the encoder position to get the absolute position.
+ double offset_ = 0.0;
+
+ bool zeroed_ = false;
+ State state_ = State::UNINITIALIZED;
+
+ // Variables for detecting the zeroing hall effect edge.
+ bool zeroing_hall_effect_edge_detector_initialized_ = false;
+ bool zeroing_hall_effect_previous_reading_ = false;
+
+ // Current velocity to move at while zeroing.
+ double zeroing_velocity_ = 0.0;
+
+ // The goals for the elevator.
+ double goal_ = 0.0;
+ double goal_velocity_ = 0.0;
+
+ control_loops::ElevatorQueue::Position current_position_;
+
+ aos::util::TrapezoidProfile profile_;
+ GlitchFilter glitch_filter_;
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ELEVATOR_H_
diff --git a/bot3/control_loops/elevator/elevator.q b/bot3/control_loops/elevator/elevator.q
new file mode 100644
index 0000000..6b2ba26
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.q
@@ -0,0 +1,79 @@
+package bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ElevatorQueue {
+ implements aos.control_loops.ControlLoop;
+
+ // Elevator heights are the vertical distance (in meters) from a defined zero.
+
+ message Goal {
+ // Height of the elevator.
+ double height;
+
+ // Linear velocity of the elevator.
+ float velocity;
+
+ // Maximum elevator profile velocity or 0 for the default.
+ float max_velocity;
+
+ // Maximum elevator profile acceleration or 0 for the default.
+ float max_acceleration;
+
+ // Whether the passive elevator is supporting the stack/can; true means it
+ // is supporting; false means it is not.
+ bool passive_support;
+ // Whether the can support is restraining the can; true means it
+ // is supporting; false means it is not.
+ bool can_support;
+ };
+
+ message Position {
+ double encoder;
+
+ // bottom hall effect sensor for zeroing purposes.
+ bool bottom_hall_effect;
+ };
+
+ message Status {
+ // Is the elevator zeroed?
+ bool zeroed;
+
+ // Estimated height of the elevator.
+ double height;
+
+ // Estimated velocity of the elevator.
+ float velocity;
+
+ // Goal height and velocity of the elevator.
+ double goal_height;
+ float goal_velocity;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // The internal state of the state machine.
+ int32_t state;
+ };
+
+ message Output {
+ // Voltage for the active elevator.
+ double elevator;
+
+ // Toggle for the passive elevator that supports the stack in the robot.
+ // True means support the stack, false means release the support from the
+ // stack.
+ bool passive_support;
+ // Toggle for the that supports the can in the robot.
+ // True means support the can, false means release the can.
+ bool can_support;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ElevatorQueue elevator_queue;
diff --git a/bot3/control_loops/elevator/elevator_lib_test.cc b/bot3/control_loops/elevator/elevator_lib_test.cc
new file mode 100644
index 0000000..d174567
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_lib_test.cc
@@ -0,0 +1,545 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include <math.h>
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/time.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "bot3/control_loops/position_sensor_sim.h"
+#include "bot3/control_loops/elevator/elevator_motor_plant.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+using ::aos::time::Time;
+using ::bot3::control_loops::PositionSensorSimulator;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// TODO(comran+adam): Check these over with Austin, Ben, Brian, and others to
+// make sure we didn't forget anything -- especially the zeroing tests!!!
+
+class ElevatorSimulator {
+ public:
+ ElevatorSimulator()
+ : plant_(new StateFeedbackPlant<2, 1, 1>(MakeElevatorPlant())),
+ position_sim_(),
+ queue_(".bot3.control_loops.elevator_queue", 0xca8daa3b,
+ ".bot3.control_loops.elevator_queue.goal",
+ ".bot3.control_loops.elevator_queue.position",
+ ".bot3.control_loops.elevator_queue.output",
+ ".bot3.control_loops.elevator_queue.status") {
+ // Initialize the elevator.
+ InitializePosition(kElevLowerLimit);
+ }
+
+ void InitializePosition(double start_pos) {
+ InitializePosition(start_pos, kHallEffectPosition);
+ }
+
+ void InitializePosition(double start_pos, double hall_effect_position) {
+ plant_->mutable_X(0, 0) = start_pos;
+ plant_->mutable_X(1, 0) = 0.0;
+ position_sim_.Initialize(start_pos, hall_effect_position);
+ }
+
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ElevatorQueue::Position> position =
+ queue_.position.MakeMessage();
+ position_sim_.GetSensorValues(position.get());
+ position.Send();
+ }
+
+ void set_acceleration_offset(double acceleration_offset) {
+ acceleration_offset_ = acceleration_offset;
+ }
+
+ double height() const { return plant_->Y(0, 0); }
+
+ void Simulate() {
+ EXPECT_TRUE(queue_.output.FetchLatest());
+
+ plant_->mutable_U() << queue_.output->elevator;
+
+ plant_->Update();
+ plant_->mutable_X()(1, 0) += acceleration_offset_ * 0.005;
+
+ const double height = plant_->Y(0, 0);
+
+ position_sim_.MoveTo(height);
+
+ EXPECT_GE(height, kElevLowerHardLimit);
+ EXPECT_LE(height, kElevUpperHardLimit);
+ }
+
+ void MoveTo(double position) { position_sim_.MoveTo(position); }
+
+ private:
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> plant_;
+
+ PositionSensorSimulator position_sim_;
+
+ ElevatorQueue queue_;
+ double acceleration_offset_ = 0.0;
+};
+
+class ElevatorTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ ElevatorTest()
+ : queue_(".bot3.control_loops.elevator_queue", 0xca8daa3b,
+ ".bot3.control_loops.elevator_queue.goal",
+ ".bot3.control_loops.elevator_queue.position",
+ ".bot3.control_loops.elevator_queue.output",
+ ".bot3.control_loops.elevator_queue.status"),
+ elevator_(&queue_),
+ plant_() {
+ }
+
+ // Checks to see if fetching position/status from queues does not return null
+ // pointers.
+ void VerifyNearGoal() {
+ queue_.goal.FetchLatest();
+ queue_.status.FetchLatest();
+ EXPECT_TRUE(queue_.goal.get() != nullptr);
+ EXPECT_TRUE(queue_.status.get() != nullptr);
+ EXPECT_NEAR(queue_.goal->height, queue_.status->height, 0.001);
+ EXPECT_NEAR(queue_.goal->height, plant_.height(), 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ plant_.SendPositionMessage();
+ elevator_.Iterate();
+ plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to shared memory that
+ // is no longer valid.
+ ElevatorQueue queue_;
+
+ // Create a control loop and simulation.
+ Elevator elevator_;
+ ElevatorSimulator plant_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(ElevatorTest, DoesNothing) {
+ // Set the goals to the starting values. This should theoretically guarantee
+ // that the controller does nothing.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(kElevLowerLimit)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ // Run a few iterations.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
+TEST_F(ElevatorTest, ReachesGoal) {
+ // Set a reasonable goal.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(kElevLowerLimit + 0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ // Give it a lot of time to get there.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop doesn't try and go beyond the physical range of the
+// mechanisms.
+TEST_F(ElevatorTest, RespectsRange) {
+ // We're going to send the elevator to -5, which should be significantly too
+ // low.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(-5.0)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ queue_.status.FetchLatest();
+ EXPECT_NEAR(kElevLowerLimit, queue_.status->height, 0.001);
+
+ // We're going to give the elevator some ridiculously high goal.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(50.0)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ queue_.status.FetchLatest();
+ EXPECT_NEAR(kElevUpperLimit, queue_.status->height, 0.001);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(ElevatorTest, ZeroTest) {
+ queue_.goal.MakeWithBuilder()
+ .height(0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send();
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the lower hardstops doesn't cause an abort.
+TEST_F(ElevatorTest, LowerHardstopStartup) {
+ plant_.InitializePosition(kElevLowerHardLimit);
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InMS(5000));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(ElevatorTest, UpperHardstopStartup) {
+ plant_.InitializePosition(kElevUpperHardLimit);
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InMS(20000));
+
+ VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(ElevatorTest, ResetTest) {
+ plant_.InitializePosition(kElevLowerLimit);
+ queue_.goal.MakeWithBuilder().height(0.05).Send();
+ RunForTime(Time::InMS(5000));
+
+ EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+ VerifyNearGoal();
+ SimulateSensorReset();
+ RunForTime(Time::InMS(100));
+ EXPECT_NE(Elevator::RUNNING, elevator_.state());
+ RunForTime(Time::InMS(10000));
+ EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+ VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(ElevatorTest, DisabledGoalTest) {
+ queue_.goal.MakeWithBuilder().height(0.45).Send();
+
+ RunForTime(Time::InMS(100), false);
+ EXPECT_EQ(0.0, elevator_.goal_);
+
+ // Now make sure they move correctly
+ RunForTime(Time::InMS(4000), true);
+ EXPECT_NE(0.0, elevator_.goal_);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(ElevatorTest, ElevatorGoalPositiveWindupTest) {
+ plant_.InitializePosition(0.0, kHallEffectPosition);
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::ZEROING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_goal = elevator_.goal_;
+ elevator_.goal_ += 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::ZEROING);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(ElevatorTest, ElevatorGoalNegativeWindupTest) {
+ plant_.InitializePosition(0.0, kHallEffectPosition);
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::ZEROING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_goal = elevator_.goal_;
+ elevator_.goal_ -= 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::ZEROING);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(ElevatorTest, ZeroNoGoal) {
+ RunForTime(Time::InSeconds(5));
+
+ EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+}
+
+// Zeroing tests
+// TODO(comran+adam): The following tests don't look like they are complete, so
+// we will need to get these tests checked & passed before running the elevator
+// control loop.
+
+// Tests that starting in the middle zeros correctly.
+TEST_F(ElevatorTest, CasualStart) {
+ const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.5).Send();
+
+ // Find a reasonable value for the time.
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that a start position above the upper soft limit zeros correctly.
+TEST_F(ElevatorTest, AboveSoftLimitStart) {
+ const double start_position = (kElevUpperHardLimit + kElevUpperLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.5).Send();
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that a start position below the lower soft limit zeros correctly.
+TEST_F(ElevatorTest, BelowSoftLimitStart) {
+ const double start_position = (kElevLowerHardLimit + kElevLowerLimit) / 2;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests it zeroes when we start above the hall effect sensor.
+TEST_F(ElevatorTest, OverHallEffect) {
+ const double start_position = kHallEffectPosition;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that we go down fast, up slow, and then get zeroed.
+TEST_F(ElevatorTest, DownAndUp) {
+ const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+
+ // Make sure we ended off the hall effect.
+ queue_.position.FetchLatest();
+ EXPECT_TRUE(queue_.position.get() != nullptr);
+ EXPECT_FALSE(queue_.position->bottom_hall_effect);
+}
+
+// Verify that we can zero while disabled.
+TEST_F(ElevatorTest, ZeroWhileDisabled) {
+ const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+
+ // Running iteration while disabled
+ RunIteration(false);
+ RunIteration(false);
+
+ // Move elevator below hall effect.
+ plant_.MoveTo(kHallEffectPosition - 0.1);
+ RunIteration(false);
+ plant_.MoveTo(kHallEffectPosition - 0.1);
+ RunIteration(false);
+ // Make sure it only zeroes while going up.
+ EXPECT_TRUE(!elevator_.CheckZeroed());
+ // Move above the hall effect.
+ plant_.MoveTo(kHallEffectPosition + 0.1);
+ RunIteration(false);
+ RunIteration(false);
+ // Make sure we are zeroed.
+ EXPECT_TRUE(elevator_.CheckZeroed());
+}
+
+// Tests that the loop can reach a goal with a constant force.
+TEST_F(ElevatorTest, ReachesGoalWithIntegral) {
+ // Set a reasonable goal.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(kElevLowerLimit + 0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ plant_.set_acceleration_offset(0.1);
+ // Give it a lot of time to get there.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the goal (with motion profile) doesn't run away from the elevator
+TEST_F(ElevatorTest, PositiveRunawayProfileTest) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+
+
+ double orig_goal = elevator_.goal_;
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << elevator_.goal_ + 100.0, elevator_.goal_velocity_;
+ elevator_.profile_.MoveCurrentState(current);
+ }
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::RUNNING);
+}
+
+// Tests that the goal (with motion profile) doesn't run away from the elevator
+TEST_F(ElevatorTest, NegativeRunawayProfileTest) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+
+
+ double orig_goal = elevator_.goal_;
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << elevator_.goal_ - 100.0, elevator_.goal_velocity_;
+ elevator_.profile_.MoveCurrentState(current);
+ }
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::RUNNING);
+}
+
+// Tests that the glitch filter handles positive edges.
+TEST(GlitchFilterTest, PosedgeDetect) {
+ GlitchFilter filter;
+ filter.Reset(false);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+
+ filter.Update(false, 0.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+
+ filter.Update(true, 1.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+
+ filter.Update(true, 2.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_TRUE(filter.posedge());
+
+ filter.Update(true, 3.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+ EXPECT_EQ(0.5, filter.posedge_value());
+}
+
+// Tests that the glitch filter handles negative edges.
+TEST(GlitchFilterTest, NegedgeDetect) {
+ GlitchFilter filter;
+ filter.Reset(true);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+
+ filter.Update(true, 0.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+
+ filter.Update(false, 1.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+
+ filter.Update(false, 2.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_TRUE(filter.negedge());
+
+ filter.Update(false, 3.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+ EXPECT_EQ(0.5, filter.negedge_value());
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator_main.cc b/bot3/control_loops/elevator/elevator_main.cc
new file mode 100644
index 0000000..f7a5eef
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::bot3::control_loops::Elevator elevator;
+ elevator.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.cc b/bot3/control_loops/elevator/elevator_motor_plant.cc
new file mode 100644
index 0000000..d3985b6
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "bot3/control_loops/elevator/elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00329835431624, 0.0, 0.407009515002;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000238685884904, 0.0831773970353;
+ 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<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeElevatorController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 0.843942422954, 1.30830010079;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 327.58799433, 4.77067036522;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.0, -0.00810387520358, 0.0, 2.4569450176;
+ return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeElevatorPlantCoefficients()));
+ return StateFeedbackPlant<2, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeElevatorController()));
+ return StateFeedbackLoop<2, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.h b/bot3/control_loops/elevator/elevator_motor_plant.h
new file mode 100644
index 0000000..d87c79e
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeElevatorController();
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.cc b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
new file mode 100644
index 0000000..fba91c1
--- /dev/null
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "bot3/control_loops/elevator/integral_elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00329835431624, 0.000238685884904, 0.0, 0.407009515002, 0.0831773970353, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.000238685884904, 0.0831773970353, 0.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.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<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeIntegralElevatorController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 0.819604251485, 7.84671425417, 56.221891957;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 327.58799433, 4.77067036522, 1.0;
+ Eigen::Matrix<double, 3, 3> A_inv;
+ A_inv << 1.0, -0.00810387520358, 0.000435373360429, 0.0, 2.4569450176, -0.204362291223, 0.0, 0.0, 1.0;
+ return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeIntegralElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeIntegralElevatorPlantCoefficients()));
+ return StateFeedbackPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<3, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeIntegralElevatorController()));
+ return StateFeedbackLoop<3, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.h b/bot3/control_loops/elevator/integral_elevator_motor_plant.h
new file mode 100644
index 0000000..64a4539
--- /dev/null
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeIntegralElevatorController();
+
+StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/intake/intake.cc b/bot3/control_loops/intake/intake.cc
index 935730b..126e491 100644
--- a/bot3/control_loops/intake/intake.cc
+++ b/bot3/control_loops/intake/intake.cc
@@ -17,18 +17,8 @@
if (output != nullptr) {
output->Zero();
- const int16_t intake_movement = goal->movement;
-
- if (intake_movement > 0) {
- // Suck.
- output->intake = kIntakeVoltageFullPower;
- } else if (intake_movement < 0) {
- // Spit.
- output->intake = -kIntakeVoltageFullPower;
- } else {
- // Stationary.
- output->intake = 0.0;
- }
+ output->intake = goal->movement;
+ output->claw_closed = goal->claw_closed;
}
}
diff --git a/bot3/control_loops/intake/intake.q b/bot3/control_loops/intake/intake.q
index 3047f6b..70934b2 100644
--- a/bot3/control_loops/intake/intake.q
+++ b/bot3/control_loops/intake/intake.q
@@ -6,14 +6,19 @@
implements aos.control_loops.ControlLoop;
message Goal {
- // Positive = suck, negative = spit, zero = stationary.
- int16_t movement;
+ // Units: volts
+ double movement;
+
+ bool claw_closed;
};
message Position {};
message Output {
+ // Positive or negative, depending on whether we're sucking or spitting.
double intake;
+
+ bool claw_closed;
};
message Status {};
diff --git a/bot3/control_loops/position_sensor_sim.cc b/bot3/control_loops/position_sensor_sim.cc
new file mode 100644
index 0000000..480cd98
--- /dev/null
+++ b/bot3/control_loops/position_sensor_sim.cc
@@ -0,0 +1,28 @@
+#include "bot3/control_loops/position_sensor_sim.h"
+
+#include <cmath>
+
+namespace bot3 {
+namespace control_loops {
+
+void PositionSensorSimulator::Initialize(double start_position,
+ double hall_effect_position) {
+ hall_effect_position_ = hall_effect_position;
+ start_position_ = start_position;
+ cur_pos_ = start_position;
+}
+
+void PositionSensorSimulator::MoveTo(double new_pos) { cur_pos_ = new_pos; }
+
+void PositionSensorSimulator::GetSensorValues(
+ control_loops::ElevatorQueue::Position* position) {
+ position->encoder = cur_pos_ - start_position_;
+ if (cur_pos_ <= hall_effect_position_) {
+ position->bottom_hall_effect = true;
+ } else {
+ position->bottom_hall_effect = false;
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/position_sensor_sim.h b/bot3/control_loops/position_sensor_sim.h
new file mode 100644
index 0000000..f025010
--- /dev/null
+++ b/bot3/control_loops/position_sensor_sim.h
@@ -0,0 +1,47 @@
+#ifndef BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#define BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+// NOTE: All encoder values in this class are assumed to be in
+// translated SI units.
+
+class PositionSensorSimulator {
+ public:
+
+ // Set new parameters for the sensors. This is useful for unit tests to change
+ // the simulated sensors' behavior on the fly.
+ // start_position: The position relative to absolute zero where the simulated
+ // structure starts. For example, to simulate the elevator
+ // starting at 40cm above absolute zero, set this to 0.4.
+ void Initialize(double start_position, double hall_effect_position);
+
+ // Simulate the structure moving to a new position. The new value is measured
+ // relative to absolute zero. This will update the simulated sensors with new
+ // readings.
+ // new_position: The new position relative to absolute zero.
+ void MoveTo(double new_position);
+
+ // Get the current values of the simulated sensors.
+ // values: The target structure will be populated with simulated sensor
+ // readings. The readings will be in SI units. For example the units
+ // can be given in radians, meters, etc.
+ void GetSensorValues(control_loops::ElevatorQueue::Position* position);
+
+ private:
+ // the position of the bottom hall effect sensor.
+ double hall_effect_position_;
+ // Current position of the mechanism relative to absolute zero.
+ double cur_pos_;
+ // Starting position of the mechanism relative to absolute zero. See the
+ // `starting_position` parameter in the constructor for more info.
+ double start_position_;
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif /* BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_ */
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
deleted file mode 100644
index 881880f..0000000
--- a/bot3/control_loops/python/control_loop.py
+++ /dev/null
@@ -1,338 +0,0 @@
-import controls
-import numpy
-
-class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double";
- self.formatToType['%d'] = "int";
- def __str__ (self):
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (self.formatToType[self.formatt], self.name, self.value)
-
-
-class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
- """Constructs a control loop writer.
-
- Args:
- gain_schedule_name: string, Name of the overall controller.
- loops: array[ControlLoop], a list of control loops to gain schedule
- in order.
- namespaces: array[string], a list of names of namespaces to nest in
- order. If None, the default will be used.
- """
- self._gain_schedule_name = gain_schedule_name
- self._loops = loops
- if namespaces:
- self._namespaces = namespaces
- else:
- self._namespaces = ['frc971', 'control_loops']
-
- self._namespace_start = '\n'.join(
- ['namespace %s {' % name for name in self._namespaces])
-
- self._namespace_end = '\n'.join(
- ['} // namespace %s' % name for name in reversed(self._namespaces)])
-
- self._constant_list = []
-
- def AddConstant(self, constant):
- """Adds a constant to write.
-
- Args:
- constant: Constant, the constant to add to the header.
- """
- self._constant_list.append(constant)
-
- def _TopDirectory(self):
- return self._namespaces[0]
-
- def _HeaderGuard(self, header_file):
- return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
- header_file.upper().replace('.', '_').replace('/', '_') +
- '_')
-
- def Write(self, header_file, cc_file):
- """Writes the loops to the specified files."""
- self.WriteHeader(header_file)
- self.WriteCC(header_file, cc_file)
-
- def _GenericType(self, typename):
- """Returns a loop template using typename for the type."""
- num_states = self._loops[0].A.shape[0]
- num_inputs = self._loops[0].B.shape[1]
- num_outputs = self._loops[0].C.shape[0]
- return '%s<%d, %d, %d>' % (
- typename, num_states, num_inputs, num_outputs)
-
- def _ControllerType(self):
- """Returns a template name for StateFeedbackController."""
- return self._GenericType('StateFeedbackController')
-
- def _LoopType(self):
- """Returns a template name for StateFeedbackLoop."""
- return self._GenericType('StateFeedbackLoop')
-
- def _PlantType(self):
- """Returns a template name for StateFeedbackPlant."""
- return self._GenericType('StateFeedbackPlant')
-
- def _CoeffType(self):
- """Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType('StateFeedbackPlantCoefficients')
-
- def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
- """Writes the header file to the file named header_file.
- Set double_appendage to true in order to include a ratio of
- moments of inertia constant. Currently, only used for 2014 claw."""
- with open(header_file, 'w') as fd:
- header_guard = self._HeaderGuard(header_file)
- fd.write('#ifndef %s\n'
- '#define %s\n\n' % (header_guard, header_guard))
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- fd.write('\n')
-
- fd.write(self._namespace_start)
-
- for const in self._constant_list:
- fd.write(str(const))
-
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlantHeader())
- fd.write('\n')
- fd.write(loop.DumpControllerHeader())
- fd.write('\n')
-
- fd.write('%s Make%sPlant();\n\n' %
- (self._PlantType(), self._gain_schedule_name))
-
- fd.write('%s Make%sLoop();\n\n' %
- (self._LoopType(), self._gain_schedule_name))
-
- fd.write(self._namespace_end)
- fd.write('\n\n')
- fd.write("#endif // %s\n" % header_guard)
-
- def WriteCC(self, header_file_name, cc_file):
- """Writes the cc file to the file named cc_file."""
- with open(cc_file, 'w') as fd:
- fd.write('#include \"%s/control_loops/%s\"\n' %
- (self._TopDirectory(), header_file_name))
- fd.write('\n')
- fd.write('#include <vector>\n')
- fd.write('\n')
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- fd.write('\n')
- fd.write(self._namespace_start)
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlant())
- fd.write('\n')
-
- for loop in self._loops:
- fd.write(loop.DumpController())
- fd.write('\n')
-
- fd.write('%s Make%sPlant() {\n' %
- (self._PlantType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._CoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._CoeffType(), self._CoeffType(),
- loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
- fd.write('}\n\n')
-
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerType(), self._ControllerType(),
- loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._LoopType())
- fd.write('}\n\n')
-
- fd.write(self._namespace_end)
- fd.write('\n')
-
-
-class ControlLoop(object):
- def __init__(self, name):
- """Constructs a control loop object.
-
- Args:
- name: string, The name of the loop to use when writing the C++ files.
- """
- self._name = name
-
- def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
- """Calculates the discrete time values for A and B.
-
- Args:
- A_continuous: numpy.matrix, The continuous time A matrix
- B_continuous: numpy.matrix, The continuous time B matrix
- dt: float, The time step of the control loop
-
- Returns:
- (A, B), numpy.matrix, the control matricies.
- """
- return controls.c2d(A_continuous, B_continuous, dt)
-
- def InitializeState(self):
- """Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
- self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
-
- def PlaceControllerPoles(self, poles):
- """Places the controller poles.
-
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.K = controls.dplace(self.A, self.B, poles)
-
- def PlaceObserverPoles(self, poles):
- """Places the observer poles.
-
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.L = controls.dplace(self.A.T, self.C.T, poles).T
-
- def Update(self, U):
- """Simulates one time step with the provided U."""
- #U = numpy.clip(U, self.U_min, self.U_max)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
-
- def PredictObserver(self, U):
- """Runs the predict step of the observer update."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
-
- def CorrectObserver(self, U):
- """Runs the correct step of the observer update."""
- self.X_hat += numpy.linalg.inv(self.A) * self.L * (
- self.Y - self.C * self.X_hat - self.D * U)
-
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- self.X_hat = (self.A * self.X_hat + self.B * U +
- self.L * (self.Y - self.C * self.X_hat - self.D * U))
-
- def _DumpMatrix(self, matrix_name, matrix):
- """Dumps the provided matrix into a variable called matrix_name.
-
- Args:
- matrix_name: string, The variable name to save the matrix to.
- matrix: The matrix to dump.
-
- Returns:
- string, The C++ commands required to populate a variable named matrix_name
- with the contents of matrix.
- """
- ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
- matrix.shape[0], matrix.shape[1], matrix_name)]
- first = True
- for x in xrange(matrix.shape[0]):
- for y in xrange(matrix.shape[1]):
- element = matrix[x, y]
- if first:
- ans.append(' %s << ' % matrix_name)
- first = False
- else:
- ans.append(', ')
- ans.append(str(element))
-
- ans.append(';\n')
- return ''.join(ans)
-
- def DumpPlantHeader(self):
- """Writes out a c++ header declaration which will create a Plant object.
-
- Returns:
- string, The header declaration for the function.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
- num_states, num_inputs, num_outputs, self._name)
-
- def DumpPlant(self):
- """Writes out a c++ function which will create a PlantCoefficients object.
-
- Returns:
- string, The function which will create the object.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
- ' Make%sPlantCoefficients() {\n' % (
- num_states, num_inputs, num_outputs, self._name)]
-
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('B', self.B))
- ans.append(self._DumpMatrix('C', self.C))
- ans.append(self._DumpMatrix('D', self.D))
- ans.append(self._DumpMatrix('U_max', self.U_max))
- ans.append(self._DumpMatrix('U_min', self.U_min))
-
- ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
- num_outputs))
- ans.append('}\n')
- return ''.join(ans)
-
- def PlantFunction(self):
- """Returns the name of the plant coefficient function."""
- return 'Make%sPlantCoefficients()' % self._name
-
- def ControllerFunction(self):
- """Returns the name of the controller function."""
- return 'Make%sController()' % self._name
-
- def DumpControllerHeader(self):
- """Writes out a c++ header declaration which will create a Controller object.
-
- Returns:
- string, The header declaration for the function.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())
-
- def DumpController(self):
- """Returns a c++ function which will create a Controller object.
-
- Returns:
- string, The function which will create the object.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())]
-
- ans.append(self._DumpMatrix('L', self.L))
- ans.append(self._DumpMatrix('K', self.K))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
-
- ans.append(' return StateFeedbackController<%d, %d, %d>'
- '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
- num_states, num_inputs, num_outputs, self._name))
- ans.append('}\n')
- return ''.join(ans)
diff --git a/bot3/control_loops/python/controls.py b/bot3/control_loops/python/controls.py
old mode 100644
new mode 100755
diff --git a/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
old mode 100644
new mode 100755
index 8ed62d6..c3144be
--- a/bot3/control_loops/python/drivetrain.py
+++ b/bot3/control_loops/python/drivetrain.py
@@ -63,13 +63,13 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 10
+ self.J = 8
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
self.rb = 0.9603 / 2.0
# Radius of the wheels, in meters.
- self.r = .0515938
+ self.r = 0.0508
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
@@ -78,7 +78,7 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_const = 28.0 / 50.0 * 20.0 / 64.0
+ self.G_const = 18.0 / 44.0 * 18.0 / 60.0
self.G_low = self.G_const
self.G_high = self.G_const
@@ -232,7 +232,8 @@
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
- drivetrain_high_low, drivetrain_high_high])
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/bot3/control_loops/python/elevator.py b/bot3/control_loops/python/elevator.py
deleted file mode 100644
index 2d72ff2..0000000
--- a/bot3/control_loops/python/elevator.py
+++ /dev/null
@@ -1,247 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import controls
-import polytope
-import polydrivetrain
-import numpy
-import sys
-import matplotlib
-from matplotlib import pylab
-
-class Elevator(control_loop.ControlLoop):
- def __init__(self, name="Elevator", mass=None):
- super(Elevator, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Mass of the elevator
- # TODO(comran): Get actual value.
- self.mass = 13.0
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- # TODO(comran): Get actual value.
- self.G = (56.0 / 12.0) * (84.0 / 14.0)
- # Pulley diameter
- # TODO(comran): Get actual value.
- self.r = 32 * 0.005 / numpy.pi / 2.0
- # Control loop time step
- self.dt = 0.005
-
- # Elevator spring constant (N/m)
- # TODO(comran): Get actual value.
- self.spring = 800.0
-
- # State is [average position, average velocity]
- # Input is [V]
-
- # TODO(comran): Change everything below.
-
- C1 = self.spring / (self.mass * 0.5)
- C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
- C3 = self.G * self.G * self.Kt / (
- self.R * self.r * self.r * self.mass * 0.5 * self.Kv)
-
- self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -C3, 0, 0],
- [0, 0, 0, 1],
- [0, 0, -C1 * 2.0, -C3]])
-
- print "Full speed is", C2 / C3 * 12.0
-
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [C2 / 2.0, C2 / 2.0],
- [0, 0],
- [C2 / 2.0, -C2 / 2.0]])
-
- self.C = numpy.matrix([[1, 0, 1, 0],
- [1, 0, -1, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
-
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
-
- print self.A
-
- controlability = controls.ctrb(self.A, self.B);
- print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(
- controlability)
-
- q_pos = 0.02
- q_vel = 0.400
- q_pos_diff = 0.01
- q_vel_diff = 0.45
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
- [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
-
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, 1.0 / (12.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
-
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
-
- self.rpl = 0.20
- self.ipl = 0.05
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- self.InitializeState()
-
-
-def CapU(U):
- if U[0, 0] - U[1, 0] > 24:
- return numpy.matrix([[12], [-12]])
- elif U[0, 0] - U[1, 0] < -24:
- return numpy.matrix([[-12], [12]])
- else:
- max_u = max(U[0, 0], U[1, 0])
- min_u = min(U[0, 0], U[1, 0])
- if max_u > 12:
- return U - (max_u - 12)
- if min_u < -12:
- return U - (min_u + 12)
- return U
-
-
-def run_test(elevator, initial_X, goal, max_separation_error=0.01,
- show_graph=True, iterations=200, controller_elevator=None,
- observer_elevator=None):
- """Runs the elevator plant with an initial condition and goal.
-
- The tests themselves are not terribly sophisticated; I just test for
- whether the goal has been reached and whether the separation goes
- outside of the initial and goal values by more than max_separation_error.
- Prints out something for a failure of either condition and returns
- False if tests fail.
- Args:
- elevator: elevator object to use.
- initial_X: starting state.
- goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for.
- controller_elevator: elevator object to get K from, or None if we should
- use elevator.
- observer_elevator: elevator object to use for the observer, or None if we
- should use the actual state.
- """
-
- elevator.X = initial_X
-
- if controller_elevator is None:
- controller_elevator = elevator
-
- if observer_elevator is not None:
- observer_elevator.X_hat = initial_X + 0.01
- observer_elevator.X_hat = initial_X
-
- # Various lists for graphing things.
- t = []
- x_avg = []
- x_sep = []
- x_hat_avg = []
- x_hat_sep = []
- v_avg = []
- v_sep = []
- u_left = []
- u_right = []
-
- sep_plot_gain = 100.0
-
- for i in xrange(iterations):
- X_hat = elevator.X
- if observer_elevator is not None:
- X_hat = observer_elevator.X_hat
- x_hat_avg.append(observer_elevator.X_hat[0, 0])
- x_hat_sep.append(observer_elevator.X_hat[2, 0] * sep_plot_gain)
- U = controller_elevator.K * (goal - X_hat)
- U = CapU(U)
- x_avg.append(elevator.X[0, 0])
- v_avg.append(elevator.X[1, 0])
- x_sep.append(elevator.X[2, 0] * sep_plot_gain)
- v_sep.append(elevator.X[3, 0])
- if observer_elevator is not None:
- observer_elevator.PredictObserver(U)
- elevator.Update(U)
- if observer_elevator is not None:
- observer_elevator.Y = elevator.Y
- observer_elevator.CorrectObserver(U)
-
- t.append(i * elevator.dt)
- u_left.append(U[0, 0])
- u_right.append(U[1, 0])
-
- print numpy.linalg.inv(elevator.A)
- print "delta time is ", elevator.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
-
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x_avg, label='x avg')
- pylab.plot(t, x_sep, label='x sep')
- if observer_elevator is not None:
- pylab.plot(t, x_hat_avg, label='x_hat avg')
- pylab.plot(t, x_hat_sep, label='x_hat sep')
- pylab.legend()
-
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u_left, label='u left')
- pylab.plot(t, u_right, label='u right')
- pylab.legend()
- pylab.show()
-
-
-def main(argv):
- loaded_mass = 25
- #loaded_mass = 0
- elevator = Elevator(mass=13 + loaded_mass)
- elevator_controller = Elevator(mass=13 + 15)
- observer_elevator = Elevator(mass=13 + 15)
- #observer_elevator = None
-
- # Test moving the elevator with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
- #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(elevator, initial_X, R, controller_elevator=elevator_controller,
- observer_elevator=observer_elevator)
-
- # Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name for the elevator."
- else:
- elevator = Elevator("Elevator")
- loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/elevator3.py b/bot3/control_loops/python/elevator3.py
index 120e8cc..04edd53 100755
--- a/bot3/control_loops/python/elevator3.py
+++ b/bot3/control_loops/python/elevator3.py
@@ -259,15 +259,24 @@
scenario_plotter.Plot()
# Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name for the Elevator."
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name for the Elevator and integral elevator."
else:
elevator = Elevator("Elevator")
- loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
+ loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator],
+ namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
loop_writer.Write(argv[1], argv[2])
+ integral_elevator = IntegralElevator("IntegralElevator")
+ integral_loop_writer = control_loop.ControlLoopWriter("IntegralElevator", [integral_elevator],
+ namespaces=['bot3', 'control_loops'])
+ if argv[3][-3:] == '.cc':
+ integral_loop_writer.Write(argv[4], argv[3])
+ else:
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/polydrivetrain.py b/bot3/control_loops/python/polydrivetrain.py
old mode 100644
new mode 100755
index a62c8c8..1090c8f
--- a/bot3/control_loops/python/polydrivetrain.py
+++ b/bot3/control_loops/python/polydrivetrain.py
@@ -405,7 +405,8 @@
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ vdrivetrain.drivetrain_high_high],
+ namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
@@ -413,7 +414,8 @@
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ "CIM", [drivetrain.CIM()],
+ namespaces=['bot3', 'control_loops'])
if argv[5][-3:] == '.cc':
cim_writer.Write(argv[6], argv[5])
diff --git a/bot3/control_loops/python/polydrivetrain_test.py b/bot3/control_loops/python/polydrivetrain_test.py
old mode 100644
new mode 100755
diff --git a/bot3/control_loops/update_drivetrain.sh b/bot3/control_loops/update_drivetrain.sh
old mode 100644
new mode 100755
index 369cbc5..b5757d3
--- a/bot3/control_loops/update_drivetrain.sh
+++ b/bot3/control_loops/update_drivetrain.sh
@@ -4,6 +4,8 @@
cd $(dirname $0)
+export PYTHONPATH=../../frc971/control_loops/python
+
./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
drivetrain/drivetrain_dog_motor_plant.cc \
drivetrain/drivetrain_clutch_motor_plant.h \
diff --git a/bot3/control_loops/update_elevator.sh b/bot3/control_loops/update_elevator.sh
new file mode 100755
index 0000000..ee4d6ed
--- /dev/null
+++ b/bot3/control_loops/update_elevator.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+#
+# Updates the elevator controllers.
+
+cd $(dirname $0)
+
+export PYTHONPATH=../../frc971/control_loops/python
+
+./python/elevator3.py elevator/elevator_motor_plant.h \
+ elevator/elevator_motor_plant.cc
diff --git a/bot3/control_loops/update_polydrivetrain.sh b/bot3/control_loops/update_polydrivetrain.sh
old mode 100644
new mode 100755
index 37987d5..b1bb2b4
--- a/bot3/control_loops/update_polydrivetrain.sh
+++ b/bot3/control_loops/update_polydrivetrain.sh
@@ -4,6 +4,8 @@
cd $(dirname $0)
+export PYTHONPATH=../../frc971/control_loops/python
+
./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
drivetrain/polydrivetrain_dog_motor_plant.cc \
drivetrain/polydrivetrain_clutch_motor_plant.h \
diff --git a/bot3/prime/prime.gyp b/bot3/prime/prime.gyp
index f0a0e27..5772f92 100644
--- a/bot3/prime/prime.gyp
+++ b/bot3/prime/prime.gyp
@@ -4,7 +4,7 @@
'target_name': 'All',
'type': 'none',
'dependencies': [
- '<(AOS)/build/aos_all.gyp:Prime',
+ '../../frc971/frc971.gyp:All',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop_test',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:position_sensor_sim_test',
@@ -13,11 +13,11 @@
'../control_loops/drivetrain/drivetrain.gyp:replay_drivetrain_bot3',
'../control_loops/intake/intake.gyp:intake',
'../control_loops/intake/intake.gyp:intake_lib_test',
- #'../autonomous/autonomous.gyp:auto_bot3',
'../bot3.gyp:joystick_reader_bot3',
- '<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing_test',
- #'../http_status/http_status.gyp:http_status',
- '<(DEPTH)/frc971/control_loops/voltage_cap/voltage_cap.gyp:voltage_cap_test',
+ '../control_loops/elevator/elevator.gyp:elevator',
+ '../control_loops/elevator/elevator.gyp:elevator_lib_test',
+ #'../control_loops/elevator/elevator.gyp:replay_elevator',
+ #'../autonomous/autonomous.gyp:auto_bot3',
'../actors/actors.gyp:binaries',
],
'copies': [
diff --git a/bot3/wpilib/wpilib.gyp b/bot3/wpilib/wpilib.gyp
index 311f6cd..2f0e6cd 100644
--- a/bot3/wpilib/wpilib.gyp
+++ b/bot3/wpilib/wpilib.gyp
@@ -24,12 +24,11 @@
'<(DEPTH)/frc971/wpilib/wpilib.gyp:loop_output_handler',
'<(DEPTH)/frc971/wpilib/wpilib.gyp:buffered_pcm',
'<(DEPTH)/frc971/wpilib/wpilib.gyp:gyro_sender',
- '<(DEPTH)/frc971/wpilib/wpilib.gyp:dma_edge_counting',
- '<(DEPTH)/frc971/wpilib/wpilib.gyp:interrupt_edge_counting',
- '<(DEPTH)/frc971/wpilib/wpilib.gyp:encoder_and_potentiometer',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
'<(DEPTH)/frc971/wpilib/wpilib.gyp:logging_queue',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib',
+ '<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_lib',
+ '<(DEPTH)/bot3/control_loops/intake/intake.gyp:intake_lib',
],
},
],
diff --git a/bot3/wpilib/wpilib_interface.cc b/bot3/wpilib/wpilib_interface.cc
index 49be884..dfffddc 100644
--- a/bot3/wpilib/wpilib_interface.cc
+++ b/bot3/wpilib/wpilib_interface.cc
@@ -19,6 +19,8 @@
#include "frc971/control_loops/control_loops.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+#include "bot3/control_loops/intake/intake.q.h"
#include "frc971/wpilib/hall_effect.h"
#include "frc971/wpilib/joystick_sender.h"
@@ -26,11 +28,10 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/gyro_sender.h"
-#include "frc971/wpilib/dma_edge_counting.h"
-#include "frc971/wpilib/interrupt_edge_counting.h"
-#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/logging.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "bot3/control_loops/elevator/elevator.h"
+#include "bot3/control_loops/intake/intake.h"
#include "Encoder.h"
#include "Talon.h"
@@ -48,31 +49,36 @@
using ::aos::util::SimpleLogInterval;
using ::bot3::control_loops::drivetrain_queue;
-using ::frc971::wpilib::DMAEncoderAndPotentiometer;
-using ::frc971::PotAndIndexPosition;
-using ::frc971::wpilib::InterruptEncoderAndPotentiometer;
-using ::frc971::wpilib::DMASynchronizer;
+using ::bot3::control_loops::elevator_queue;
+using ::bot3::control_loops::intake_queue;
using ::frc971::wpilib::BufferedPcm;
+using ::frc971::wpilib::BufferedSolenoid;
using ::frc971::wpilib::LoopOutputHandler;
using ::frc971::wpilib::JoystickSender;
using ::frc971::wpilib::GyroSender;
+using ::frc971::wpilib::HallEffect;
namespace bot3 {
namespace wpilib {
double drivetrain_translate(int32_t in) {
- return static_cast<double>(in) /
- (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
::bot3::control_loops::kDrivetrainEncoderRatio *
(4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-// TODO(comran): Check/update the values below for the bot3.
+double elevator_translate(int32_t in) {
+ return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ ::bot3::control_loops::kElevEncoderRatio * (2 * M_PI /*radians*/) *
+ ::bot3::control_loops::kElevGearboxOutputRadianDistance;
+}
+
static const double kMaximumEncoderPulsesPerSecond =
- 19500.0 /* free speed RPM */ * 12.0 / 56.0 /* belt reduction */ /
- 60.0 /* seconds / minute */ * 256.0 /* CPR */ *
+ 4650.0 /* free speed RPM */ * 18.0 / 48.0 /* belt reduction */ /
+ 60.0 /* seconds / minute */ * 512.0 /* CPR */ *
4.0 /* index pulse = 1/4 cycle */;
+// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader {
public:
SensorReader() {
@@ -82,6 +88,7 @@
static_cast<int>(1 / 4.0 / kMaximumEncoderPulsesPerSecond * 1e9 + 0.5));
}
+ // Drivetrain setters.
void set_left_encoder(::std::unique_ptr<Encoder> left_encoder) {
left_encoder_ = ::std::move(left_encoder);
}
@@ -90,11 +97,14 @@
right_encoder_ = ::std::move(right_encoder);
}
- // All of the DMA-related set_* calls must be made before this, and it doesn't
- // hurt to do all of them.
- // TODO(comran): Do we still need dma?
- void set_dma(::std::unique_ptr<DMA> dma) {
- dma_synchronizer_.reset(new DMASynchronizer(::std::move(dma)));
+ // Elevator setters.
+ void set_elevator_encoder(::std::unique_ptr<Encoder> encoder) {
+ filter_.Add(encoder.get());
+ elevator_encoder_ = ::std::move(encoder);
+ }
+
+ void set_elevator_zeroing_hall_effect(::std::unique_ptr<HallEffect> hall) {
+ zeroing_hall_effect_ = ::std::move(hall);
}
void operator()() {
@@ -104,7 +114,6 @@
my_pid_ = getpid();
ds_ = DriverStation::GetInstance();
- dma_synchronizer_->Start();
LOG(INFO, "Things are now started\n");
::aos::SetCurrentThreadRealtimePriority(kPriority);
@@ -115,6 +124,7 @@
}
void RunIteration() {
+ // General
{
auto new_state = ::aos::robot_state.MakeMessage();
@@ -135,6 +145,7 @@
new_state.Send();
}
+ // Drivetrain
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
@@ -145,7 +156,17 @@
drivetrain_message.Send();
}
- dma_synchronizer_->RunIteration();
+ // Elevator
+ {
+ // Update control loop positions.
+ auto elevator_message = elevator_queue.position.MakeMessage();
+ elevator_message->encoder =
+ elevator_translate(elevator_encoder_->GetRaw());
+ elevator_message->bottom_hall_effect =
+ zeroing_hall_effect_->Get();
+
+ elevator_message.Send();
+ }
}
void Quit() { run_ = false; }
@@ -157,60 +178,20 @@
int32_t my_pid_;
DriverStation *ds_;
- void CopyPotAndIndexPosition(
- const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
- ::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> potentiometer_translate, bool reverse,
- double potentiometer_offset) {
- const double multiplier = reverse ? -1.0 : 1.0;
- position->encoder =
- multiplier * encoder_translate(encoder.polled_encoder_value());
- position->pot = multiplier * potentiometer_translate(
- encoder.polled_potentiometer_voltage()) +
- potentiometer_offset;
- position->latched_encoder =
- multiplier * encoder_translate(encoder.last_encoder_value());
- position->latched_pot =
- multiplier *
- potentiometer_translate(encoder.last_potentiometer_voltage()) +
- potentiometer_offset;
- position->index_pulses = encoder.index_posedge_count();
- }
-
- void CopyPotAndIndexPosition(
- const InterruptEncoderAndPotentiometer &encoder,
- PotAndIndexPosition *position,
- ::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> potentiometer_translate, bool reverse,
- double potentiometer_offset) {
- const double multiplier = reverse ? -1.0 : 1.0;
- position->encoder =
- multiplier * encoder_translate(encoder.encoder()->GetRaw());
- position->pot = multiplier * potentiometer_translate(
- encoder.potentiometer()->GetVoltage()) +
- potentiometer_offset;
- position->latched_encoder =
- multiplier * encoder_translate(encoder.last_encoder_value());
- position->latched_pot =
- multiplier *
- potentiometer_translate(encoder.last_potentiometer_voltage()) +
- potentiometer_offset;
- position->index_pulses = encoder.index_posedge_count();
- }
-
- ::std::unique_ptr<DMASynchronizer> dma_synchronizer_;
-
- ::std::unique_ptr<Encoder> left_encoder_;
- ::std::unique_ptr<Encoder> right_encoder_;
+ ::std::unique_ptr<Encoder> left_encoder_, right_encoder_, elevator_encoder_;
+ ::std::unique_ptr<HallEffect> zeroing_hall_effect_;
::std::atomic<bool> run_{true};
DigitalGlitchFilter filter_;
};
+// Writes out our pneumatic outputs.
class SolenoidWriter {
public:
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
- : pcm_(pcm) {}
+ : pcm_(pcm),
+ elevator_(".bot3.control_loops.elevator_queue.output"),
+ intake_(".bot3.control_loops.intake_queue.output") {}
void set_pressure_switch(::std::unique_ptr<DigitalSource> pressure_switch) {
pressure_switch_ = ::std::move(pressure_switch);
@@ -220,6 +201,20 @@
compressor_relay_ = ::std::move(compressor_relay);
}
+ void set_elevator_passive_support(
+ ::std::unique_ptr<BufferedSolenoid> elevator_passive_support) {
+ elevator_passive_support_ = ::std::move(elevator_passive_support);
+ }
+
+ void set_elevator_can_support(
+ ::std::unique_ptr<BufferedSolenoid> elevator_can_support) {
+ elevator_can_support_ = ::std::move(elevator_can_support);
+ }
+
+ void set_intake_claw(::std::unique_ptr<BufferedSolenoid> intake_claw) {
+ intake_claw_ = ::std::move(intake_claw);
+ }
+
void operator()() {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(30);
@@ -227,11 +222,31 @@
while (run_) {
::aos::time::PhasedLoopXMS(20, 1000);
- ::aos::joystick_state.FetchLatest();
+ // Elevator
+ {
+ elevator_.FetchLatest();
+ if (elevator_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *elevator_);
+ elevator_passive_support_->Set(elevator_->passive_support);
+ elevator_can_support_->Set(elevator_->passive_support);
+ }
+ }
+ // Intake
+ {
+ intake_.FetchLatest();
+ if (intake_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *intake_);
+ intake_claw_->Set(intake_->claw_closed);
+ }
+ }
+
+ // Compressor
+ ::aos::joystick_state.FetchLatest();
{
::frc971::wpilib::PneumaticsToLog to_log;
{
+ // Refill if pneumatic pressure goes too low.
const bool compressor_on = !pressure_switch_->Get();
to_log.compressor_on = compressor_on;
if (compressor_on) {
@@ -252,12 +267,21 @@
private:
const ::std::unique_ptr<BufferedPcm> &pcm_;
+
+ ::std::unique_ptr<BufferedSolenoid> elevator_passive_support_;
+ ::std::unique_ptr<BufferedSolenoid> elevator_can_support_;
+ ::std::unique_ptr<BufferedSolenoid> intake_claw_;
+
::std::unique_ptr<DigitalSource> pressure_switch_;
::std::unique_ptr<Relay> compressor_relay_;
+ ::aos::Queue<::bot3::control_loops::ElevatorQueue::Output> elevator_;
+ ::aos::Queue<::bot3::control_loops::IntakeQueue::Output> intake_;
+
::std::atomic<bool> run_{true};
};
+// Writes out drivetrain voltages.
class DrivetrainWriter : public LoopOutputHandler {
public:
void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
@@ -290,6 +314,71 @@
::std::unique_ptr<Talon> right_drivetrain_talon_;
};
+// Writes out elevator voltages.
+class ElevatorWriter : public LoopOutputHandler {
+ public:
+ void set_elevator_talon1(::std::unique_ptr<Talon> t) {
+ elevator_talon1_ = ::std::move(t);
+ }
+ void set_elevator_talon2(::std::unique_ptr<Talon> t) {
+ elevator_talon2_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::bot3::control_loops::elevator_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::bot3::control_loops::elevator_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ elevator_talon1_->Set(queue->elevator / 12.0);
+ elevator_talon2_->Set(queue->elevator / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Elevator output too old.\n");
+ elevator_talon1_->Disable();
+ elevator_talon2_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> elevator_talon1_;
+ ::std::unique_ptr<Talon> elevator_talon2_;
+};
+
+// Writes out intake voltages.
+class IntakeWriter : public LoopOutputHandler {
+ public:
+ void set_intake_talon1(::std::unique_ptr<Talon> t) {
+ intake_talon1_ = ::std::move(t);
+ }
+ void set_intake_talon2(::std::unique_ptr<Talon> t) {
+ intake_talon2_ = ::std::move(t);
+ }
+
+
+ private:
+ virtual void Read() override {
+ ::bot3::control_loops::intake_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::bot3::control_loops::intake_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ intake_talon1_->Set(queue->intake / 12.0);
+ intake_talon2_->Set(queue->intake / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Intake output too old.\n");
+ intake_talon1_->Disable();
+ intake_talon2_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> intake_talon1_;
+ ::std::unique_ptr<Talon> intake_talon2_;
+};
+
// TODO(brian): Replace this with ::std::make_unique once all our toolchains
// have support.
template <class T, class... U>
@@ -313,27 +402,43 @@
SensorReader reader;
LOG(INFO, "Creating the reader\n");
- // TODO(comran): Find talon/encoder numbers.
- reader.set_left_encoder(encoder(2));
- reader.set_right_encoder(encoder(3));
- reader.set_dma(make_unique<DMA>());
+ reader.set_elevator_encoder(encoder(6));
+ reader.set_elevator_zeroing_hall_effect(
+ make_unique<HallEffect>(6));
+
+ reader.set_left_encoder(encoder(0));
+ reader.set_right_encoder(encoder(1));
+
::std::thread reader_thread(::std::ref(reader));
GyroSender gyro_sender;
::std::thread gyro_thread(::std::ref(gyro_sender));
DrivetrainWriter drivetrain_writer;
drivetrain_writer.set_left_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(8)));
- drivetrain_writer.set_right_drivetrain_talon(
::std::unique_ptr<Talon>(new Talon(0)));
+ drivetrain_writer.set_right_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(7)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ ElevatorWriter elevator_writer;
+ elevator_writer.set_elevator_talon1(::std::unique_ptr<Talon>(new Talon(1)));
+ elevator_writer.set_elevator_talon2(::std::unique_ptr<Talon>(new Talon(6)));
+
+ IntakeWriter intake_writer;
+ intake_writer.set_intake_talon1(::std::unique_ptr<Talon>(new Talon(2)));
+ intake_writer.set_intake_talon2(::std::unique_ptr<Talon>(new Talon(5)));
+
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(9));
solenoid_writer.set_compressor_relay(make_unique<Relay>(0));
+ // TODO (jasmine): Find solenoid numbers
+ solenoid_writer.set_elevator_passive_support(pcm->MakeSolenoid(0));
+ solenoid_writer.set_elevator_can_support(pcm->MakeSolenoid(1));
+ solenoid_writer.set_intake_claw(pcm->MakeSolenoid(2));
::std::thread solenoid_thread(::std::ref(solenoid_writer));
+ // TODO(comran): Find talon/encoder numbers ^^^
// Wait forever. Not much else to do...
PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));
@@ -359,5 +464,4 @@
} // namespace wpilib
} // namespace bot3
-
START_ROBOT_CLASS(::bot3::wpilib::WPILibRobot);