Claw, shooter and drivetrain are now generated.
Change-Id: I0aaa42a77cfef8362dcd39ea660fdf9e484279ea
diff --git a/y2014/constants.cc b/y2014/constants.cc
index 6e5e293..a2928a1 100644
--- a/y2014/constants.cc
+++ b/y2014/constants.cc
@@ -64,7 +64,7 @@
kCompRightDriveShifter,
false,
0.5,
- control_loops::MakeVelocityDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
0.02, // drivetrain done delta
5.0, // drivetrain max speed
@@ -102,7 +102,7 @@
kCompRightDriveShifter,
false,
kRobotWidth,
- control_loops::MakeVelocityDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
0.02, // drivetrain done delta
5.0, // drivetrain max speed
@@ -149,7 +149,7 @@
kPracticeRightDriveShifter,
false,
kRobotWidth,
- control_loops::MakeVelocityDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
0.02, // drivetrain done delta
5.0, // drivetrain max speed
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 1c93cf5..5c1b843 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -47,6 +47,8 @@
namespace frc971 {
namespace control_loops {
+using ::y2014::control_loops::claw::kDt;
+
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
const double kRezeroThreshold = 0.07;
@@ -613,8 +615,6 @@
const control_loops::ClawQueue::Position *position,
control_loops::ClawQueue::Output *output,
control_loops::ClawQueue::Status *status) {
- constexpr double dt = 0.01;
-
// Disable the motors now so that all early returns will return with the
// motors disabled.
if (output) {
@@ -716,7 +716,7 @@
values.claw.start_fine_tune_pos) <
values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
- bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_speed;
LOG(DEBUG, "Ready to fine tune the bottom\n");
@@ -729,7 +729,7 @@
}
} else {
mode_ = FINE_TUNE_BOTTOM;
- bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_speed;
if (top_claw_.front_or_back_triggered() ||
@@ -773,7 +773,7 @@
values.claw.start_fine_tune_pos) <
values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
- top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_goal_ += values.claw.claw_zeroing_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_speed;
LOG(DEBUG, "Ready to fine tune the top\n");
@@ -786,7 +786,7 @@
}
} else {
mode_ = FINE_TUNE_TOP;
- top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_goal_ += values.claw.claw_zeroing_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_speed;
if (top_claw_.front_or_back_triggered() ||
@@ -841,8 +841,8 @@
if (enabled) {
// Time to slowly move back up to find any position to narrow down the
// zero.
- top_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
- bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
+ top_claw_goal_ += values.claw.claw_zeroing_off_speed * kDt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_off_speed;
LOG(DEBUG, "Bottom is known.\n");
@@ -851,8 +851,8 @@
// We don't know where either claw is. Slowly start moving down to find
// any hall effect.
if (enabled) {
- top_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
- bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
+ top_claw_goal_ -= values.claw.claw_zeroing_off_speed * kDt;
+ bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
-values.claw.claw_zeroing_off_speed;
LOG(DEBUG, "Both are unknown.\n");
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 0bf287f..acec791 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -285,7 +285,7 @@
.bottom_angle(::std::nan(""))
.separation_angle(::std::nan(""))
.Send();
- for (int i = 0; i < 500; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(5)) {
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
@@ -299,7 +299,7 @@
.bottom_angle(0.1)
.separation_angle(0.2)
.Send();
- for (int i = 0; i < 500; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(5)) {
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
@@ -397,7 +397,7 @@
.bottom_angle(0.1)
.separation_angle(0.2)
.Send();
- for (int i = 0; i < 700; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(7)) {
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
@@ -508,14 +508,15 @@
class WindupClawTest : public ClawTest {
protected:
- void TestWindup(ClawMotor::CalibrationMode mode, int start_time, double offset) {
+ void TestWindup(ClawMotor::CalibrationMode mode, ::aos::time::Time start_time, double offset) {
int capped_count = 0;
const frc971::constants::Values& values = constants::GetValues();
bool kicked = false;
bool measured = false;
- for (int i = 0; i < 700; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(7)) {
claw_motor_plant_.SendPositionMessage();
- if (i >= start_time && mode == claw_motor_.mode() && !kicked) {
+ if (::aos::time::Time::Now() >= start_time &&
+ mode == claw_motor_.mode() && !kicked) {
EXPECT_EQ(mode, claw_motor_.mode());
// Move the zeroing position far away and verify that it gets moved
// back.
@@ -574,7 +575,8 @@
.separation_angle(0.2)
.Send();
- TestWindup(ClawMotor::UNKNOWN_LOCATION, 100, 971.0);
+ TestWindup(ClawMotor::UNKNOWN_LOCATION, ::aos::time::Time::InSeconds(1.0),
+ 971.0);
VerifyNearGoal();
}
@@ -587,7 +589,8 @@
.separation_angle(0.2)
.Send();
- TestWindup(ClawMotor::UNKNOWN_LOCATION, 100, -971.0);
+ TestWindup(ClawMotor::UNKNOWN_LOCATION, ::aos::time::Time::InSeconds(1.0),
+ -971.0);
VerifyNearGoal();
}
@@ -600,7 +603,8 @@
.separation_angle(0.2)
.Send();
- TestWindup(ClawMotor::FINE_TUNE_BOTTOM, 200, -971.0);
+ TestWindup(ClawMotor::FINE_TUNE_BOTTOM, ::aos::time::Time::InSeconds(2.0),
+ -971.0);
VerifyNearGoal();
}
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 85aad66..21d3051 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -42,6 +42,24 @@
],
)
+genrule(
+ name = 'genrule_polydrivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2014/control_loops/python:polydrivetrain) $(OUTS)',
+ tools = [
+ '//y2014/control_loops/python:polydrivetrain',
+ ],
+ tags = [
+ 'local',
+ ],
+ outs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.h',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+)
+
cc_library(
name = 'polydrivetrain_plants',
srcs = [
@@ -63,11 +81,9 @@
name = 'drivetrain_lib',
srcs = [
'drivetrain.cc',
- 'polydrivetrain_cim_plant.cc',
],
hdrs = [
'drivetrain.h',
- 'polydrivetrain_cim_plant.h',
],
deps = [
':drivetrain_queue',
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index a326125..b6dffa0 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -15,8 +15,8 @@
#include "y2014/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
@@ -29,6 +29,8 @@
namespace frc971 {
namespace control_loops {
+using ::y2014::control_loops::drivetrain::kDt;
+
class DrivetrainMotorsSS {
public:
class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
@@ -286,7 +288,7 @@
throttle_(0.0),
quickturn_(false),
stale_count_(0),
- position_time_delta_(0.01),
+ position_time_delta_(kDt),
left_gear_(LOW),
right_gear_(LOW),
counter_(0) {
@@ -410,7 +412,7 @@
} else {
last_position_ = position_;
position_ = *position;
- position_time_delta_ = (stale_count_ + 1) * 0.01;
+ position_time_delta_ = (stale_count_ + 1) * kDt;
stale_count_ = 0;
}
diff --git a/y2014/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/y2014/control_loops/drivetrain/polydrivetrain_cim_plant.cc
deleted file mode 100644
index 0ee9a7a..0000000
--- a/y2014/control_loops/drivetrain/polydrivetrain_cim_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2014/control_loops/drivetrain/polydrivetrain_cim_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients() {
- Eigen::Matrix<double, 1, 1> A;
- A << 0.614537580221;
- Eigen::Matrix<double, 1, 1> B;
- B << 15.9657598852;
- Eigen::Matrix<double, 1, 1> C;
- C << 1;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<1, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<1, 1, 1> MakeCIMController() {
- Eigen::Matrix<double, 1, 1> L;
- L << 0.604537580221;
- Eigen::Matrix<double, 1, 1> K;
- K << 0.0378646293422;
- Eigen::Matrix<double, 1, 1> A_inv;
- A_inv << 1.62723978514;
- return StateFeedbackController<1, 1, 1>(L, K, A_inv, MakeCIMPlantCoefficients());
-}
-
-StateFeedbackPlant<1, 1, 1> MakeCIMPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>(new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients()));
- return StateFeedbackPlant<1, 1, 1>(&plants);
-}
-
-StateFeedbackLoop<1, 1, 1> MakeCIMLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<1, 1, 1>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<1, 1, 1>>(new StateFeedbackController<1, 1, 1>(MakeCIMController()));
- return StateFeedbackLoop<1, 1, 1>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/polydrivetrain_cim_plant.h b/y2014/control_loops/drivetrain/polydrivetrain_cim_plant.h
deleted file mode 100644
index 62af188..0000000
--- a/y2014/control_loops/drivetrain/polydrivetrain_cim_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients();
-
-StateFeedbackController<1, 1, 1> MakeCIMController();
-
-StateFeedbackPlant<1, 1, 1> MakeCIMPlant();
-
-StateFeedbackLoop<1, 1, 1> MakeCIMLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
diff --git a/y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
deleted file mode 100644
index a7d80ce..0000000
--- a/y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ /dev/null
@@ -1,133 +0,0 @@
-#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.735841675858, 0.0410810558113, 0.0410810558113, 0.735841675858;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0517213538779, -0.00804353916233, -0.00804353916233, 0.0517213538779;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.735048848179, 0.0131811893199, 0.045929121897, 0.915703853642;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0518765869984, -0.00481755802263, -0.00899277497558, 0.0308091755839;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.915703853642, 0.045929121897, 0.0131811893199, 0.735048848179;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0308091755839, -0.00899277497558, -0.00481755802263, 0.0518765869984;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.915439806567, 0.0146814193986, 0.0146814193986, 0.915439806567;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0309056814511, -0.00536587314624, -0.00536587314624, 0.0309056814511;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
- Eigen::Matrix<double, 2, 2> K;
- K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.36323698074, -0.0761076958907, -0.0761076958907, 1.36323698074;
- return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
- Eigen::Matrix<double, 2, 2> K;
- K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.36167854796, -0.0196008159867, -0.0682979543713, 1.09303924439;
- return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
- Eigen::Matrix<double, 2, 2> K;
- K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.09303924439, -0.0682979543713, -0.0196008159867, 1.36167854796;
- return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
- Eigen::Matrix<double, 2, 2> K;
- K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0926521463, -0.0175234726538, -0.0175234726538, 1.0926521463;
- return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>> plants(4);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients()));
- plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients()));
- plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients()));
- plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients()));
- return StateFeedbackPlant<2, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 2, 2>>> controllers(4);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController()));
- controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController()));
- controllers[2] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController()));
- controllers[3] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController()));
- return StateFeedbackLoop<2, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
deleted file mode 100644
index dfac7be..0000000
--- a/y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
-
-StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant();
-
-StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
index 6536f93..21406e3 100644
--- a/y2014/control_loops/python/BUILD
+++ b/y2014/control_loops/python/BUILD
@@ -10,7 +10,7 @@
],
)
-py_library(
+py_binary(
name = 'polydrivetrain',
srcs = [
'polydrivetrain.py',
@@ -21,13 +21,24 @@
],
)
+py_library(
+ name = 'polydrivetrain_lib',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
py_binary(
name = 'claw',
srcs = [
'claw.py',
],
deps = [
- ':polydrivetrain',
+ ':polydrivetrain_lib',
'//frc971/control_loops/python:controls',
]
)
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index f1fcae3..6808ce6 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -484,6 +484,8 @@
namespaces=namespaces)
loop_writer.AddConstant(control_loop.Constant("kClawMomentOfInertiaRatio",
"%f", claw.J_top / claw.J_bottom))
+ loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ claw.dt))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index 1372c25..b94a501 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -295,6 +295,8 @@
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
drivetrain_high_low, drivetrain_high_high],
namespaces = namespaces)
+ dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ drivetrain_low_low.dt))
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index b066c75..3dafd21 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -397,14 +397,16 @@
def main(argv):
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 7:
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
+ namespaces = ['y2014', 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
@@ -414,10 +416,10 @@
cim_writer = control_loop.ControlLoopWriter(
"CIM", [drivetrain.CIM()])
- if argv[5][-3:] == '.cc':
- cim_writer.Write(argv[6], argv[5])
+ if argv[3][-3:] == '.cc':
+ cim_writer.Write(argv[4], argv[3])
else:
- cim_writer.Write(argv[5], argv[6])
+ cim_writer.Write(argv[3], argv[4])
return
vl_plot = []
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index cfb7e35..7324b18 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -7,7 +7,7 @@
from matplotlib import pylab
class SprungShooter(control_loop.ControlLoop):
- def __init__(self, name="RawSprungShooter"):
+ def __init__(self, name="RawSprungShooter", verbose=False):
super(SprungShooter, self).__init__(name)
# Stall Torque in N m
self.stall_torque = .4982
@@ -68,7 +68,7 @@
class Shooter(SprungShooter):
- def __init__(self, name="RawShooter"):
+ def __init__(self, name="RawShooter", verbose=False):
super(Shooter, self).__init__(name)
# State feedback matrices
@@ -96,7 +96,7 @@
class SprungShooterDeltaU(SprungShooter):
- def __init__(self, name="SprungShooter"):
+ def __init__(self, name="SprungShooter", verbose=False):
super(SprungShooterDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
@@ -116,17 +116,19 @@
self.PlaceControllerPoles([0.50, 0.35, 0.80])
- print "K"
- print self.K
- print "Placed controller poles are"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ if verbose:
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl, 0.90])
- print "Placed observer poles are"
- print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ if verbose:
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
@@ -135,7 +137,7 @@
class ShooterDeltaU(Shooter):
- def __init__(self, name="Shooter"):
+ def __init__(self, name="Shooter", verbose=False):
super(ShooterDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
@@ -155,17 +157,19 @@
self.PlaceControllerPoles([0.55, 0.45, 0.80])
- print "K"
- print self.K
- print "Placed controller poles are"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ if verbose:
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl, 0.90])
- print "Placed observer poles are"
- print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ if verbose:
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
@@ -189,8 +193,8 @@
args = parser.parse_args(argv[1:])
# Simulate the response of the system to a goal.
- sprung_shooter = SprungShooterDeltaU()
- raw_sprung_shooter = SprungShooter()
+ sprung_shooter = SprungShooterDeltaU(verbose=args.plot)
+ raw_sprung_shooter = SprungShooter(verbose=args.plot)
close_loop_x = []
close_loop_u = []
goal_position = -0.3
@@ -214,8 +218,8 @@
pylab.plot(range(500), close_loop_u)
pylab.show()
- shooter = ShooterDeltaU()
- raw_shooter = Shooter()
+ shooter = ShooterDeltaU(verbose=args.plot)
+ raw_shooter = Shooter(verbose=args.plot)
close_loop_x = []
close_loop_u = []
goal_position = -0.3
@@ -237,8 +241,8 @@
pylab.show()
# Write the generated constants out to a file.
- unaug_sprung_shooter = SprungShooter("RawSprungShooter")
- unaug_shooter = Shooter("RawShooter")
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter", verbose=args.plot)
+ unaug_shooter = Shooter("RawShooter", verbose=args.plot)
namespaces = ['y2014', 'control_loops', 'shooter']
unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
[unaug_sprung_shooter,
@@ -247,8 +251,8 @@
unaug_loop_writer.Write(args.unaugmented_shooterh,
args.unaugmented_shootercc)
- sprung_shooter = SprungShooterDeltaU()
- shooter = ShooterDeltaU()
+ sprung_shooter = SprungShooterDeltaU(verbose=args.plot)
+ shooter = ShooterDeltaU(verbose=args.plot)
loop_writer = control_loop.ControlLoopWriter("Shooter",
[sprung_shooter, shooter],
namespaces=namespaces)
@@ -257,6 +261,8 @@
sprung_shooter.max_extension))
loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
sprung_shooter.Ks))
+ loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ sprung_shooter.dt))
loop_writer.Write(args.shooterh, args.shootercc)
if __name__ == '__main__':
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 7506a0c..29682f2 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -17,6 +17,7 @@
using ::y2014::control_loops::shooter::kSpringConstant;
using ::y2014::control_loops::shooter::kMaxExtension;
+using ::y2014::control_loops::shooter::kDt;
using ::y2014::control_loops::shooter::MakeShooterLoop;
using ::aos::time::Time;
@@ -209,8 +210,6 @@
const control_loops::ShooterQueue::Position *position,
control_loops::ShooterQueue::Output *output,
control_loops::ShooterQueue::Status *status) {
- constexpr double dt = 0.01;
-
if (goal && ::std::isnan(goal->shot_power)) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because got a shot power of NAN.\n");
@@ -345,7 +344,7 @@
if (!disabled) {
shooter_.SetGoalPosition(
- shooter_.goal_position() + values.shooter.zeroing_speed * dt,
+ shooter_.goal_position() + values.shooter.zeroing_speed * kDt,
values.shooter.zeroing_speed);
}
cap_goal = true;
@@ -610,7 +609,7 @@
shooter_.SetGoalPosition(
::std::min(
values.shooter.upper_limit,
- shooter_.goal_position() + values.shooter.unload_speed * dt),
+ shooter_.goal_position() + values.shooter.unload_speed * kDt),
values.shooter.unload_speed);
}
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index d8c81e7..648bbd7 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -359,7 +359,7 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 200; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(2)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -376,7 +376,7 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 120; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.2)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -389,7 +389,7 @@
.Send();
bool hit_fire = false;
- for (int i = 0; i < 400; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(5.2)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -416,7 +416,7 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 150; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -426,7 +426,7 @@
shooter_queue_.goal.MakeWithBuilder().shot_requested(true).Send();
bool hit_fire = false;
- for (int i = 0; i < 400; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(5.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -451,7 +451,7 @@
// power.
TEST_F(ShooterTest, LoadTooFar) {
shooter_queue_.goal.MakeWithBuilder().shot_power(500.0).Send();
- for (int i = 0; i < 160; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.6)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -466,7 +466,7 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 150; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -475,7 +475,7 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_.goal.MakeWithBuilder().shot_power(14.0).Send();
- for (int i = 0; i < 100; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.0)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -492,7 +492,7 @@
TEST_F(ShooterTest, Unload) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 150; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -501,9 +501,8 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
- for (int i = 0;
- i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
- ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(8.0) &&
+ shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -518,7 +517,7 @@
// Tests that it rezeros while unloading.
TEST_F(ShooterTest, RezeroWhileUnloading) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 150; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -527,7 +526,7 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_motor_.shooter_.offset_ += 0.01;
- for (int i = 0; i < 50; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(2.0)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -536,9 +535,8 @@
shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
- for (int i = 0;
- i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
- ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(10.0) &&
+ shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -553,7 +551,7 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 150; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -564,9 +562,8 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0;
- i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
- ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(9.5) &&
+ shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -593,7 +590,7 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 150; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(1.5)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -604,9 +601,8 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0;
- i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
- ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(9.5) &&
+ shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -638,7 +634,7 @@
TEST_F(ShooterTest, StartsOnDistal) {
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 200; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(2.0)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -658,7 +654,7 @@
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- for (int i = 0; i < 300; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(3.0)) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -687,7 +683,7 @@
bool initialized = false;
Reinitialize(start_pos);
shooter_queue_.goal.MakeWithBuilder().shot_power(120.0).Send();
- for (int i = 0; i < 200; ++i) {
+ while (::aos::time::Time::Now() < ::aos::time::Time::InSeconds(2.0)) {
shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
initialized = true;
shooter_motor_.Iterate();
diff --git a/y2014/control_loops/update_polydrivetrain.sh b/y2014/control_loops/update_polydrivetrain.sh
index 2b559c6..6cc3e56 100755
--- a/y2014/control_loops/update_polydrivetrain.sh
+++ b/y2014/control_loops/update_polydrivetrain.sh
@@ -8,7 +8,5 @@
./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
drivetrain/polydrivetrain_dog_motor_plant.cc \
- drivetrain/polydrivetrain_clutch_motor_plant.h \
- drivetrain/polydrivetrain_clutch_motor_plant.cc \
drivetrain/polydrivetrain_cim_plant.h \
drivetrain/polydrivetrain_cim_plant.cc