Claw, shooter and drivetrain are now generated.
Change-Id: I0aaa42a77cfef8362dcd39ea660fdf9e484279ea
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();
}