Improve intaking.
Add a stuck box detector (box intake velocity), and better handling for
boxes coming in sideways.
Change-Id: I5fa4a5b00f7dd1288f8fed33375953bdfe710559
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index d3997ca..d79a293 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -39,7 +39,7 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
void Arm::Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- const control_loops::ArmPosition *position,
+ bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
const bool intake_clear_of_box, double *proximal_output,
@@ -59,9 +59,20 @@
if (open_claw) {
claw_closed_ = false;
}
- if (outputs_disabled) {
+ if (close_claw) {
claw_closed_ = true;
}
+ if (outputs_disabled) {
+ if (claw_closed_count_ == 0) {
+ claw_closed_ = true;
+ } else {
+ --claw_closed_count_;
+ }
+ } else {
+ // Wait this many iterations before closing the claw. That prevents
+ // brownouts from closing the claw.
+ claw_closed_count_ = 50;
+ }
Y << position->proximal.encoder + proximal_offset_,
position->distal.encoder + distal_offset_;
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index d86b95b..6b2864a 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -35,7 +35,7 @@
static constexpr double kGotoPathVMax() { return 6.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- const control_loops::ArmPosition *position,
+ bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
const bool intake_clear_of_box, double *proximal_output,
@@ -114,6 +114,8 @@
// Start at the 0th index.
uint32_t current_node_ = 0;
+ uint32_t claw_closed_count_ = 0;
+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 2c36efa..e570c50 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -97,6 +97,8 @@
return controller_.output_position() < -0.1;
}
+ double output_position() const { return controller_.output_position(); }
+
private:
IntakeController controller_;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index e22219e..757b166 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -1,5 +1,7 @@
#include "y2018/control_loops/superstructure/superstructure.h"
+#include <chrono>
+
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/control_loops.q.h"
@@ -10,6 +12,10 @@
namespace control_loops {
namespace superstructure {
+using ::aos::monotonic_clock;
+
+namespace chrono = ::std::chrono;
+
namespace {
// The maximum voltage the intake roller will be allowed to use.
constexpr double kMaxIntakeRollerVoltage = 12.0;
@@ -34,12 +40,31 @@
arm_.Reset();
}
- const double left_intake_goal = ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.left_intake_angle));
- const double right_intake_goal = ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
+ const double clipped_box_distance =
+ ::std::min(1.0, ::std::max(0.0, position->box_distance));
+
+ const double box_velocity =
+ (clipped_box_distance - last_box_distance_) / 0.005;
+
+ constexpr double kFilteredBoxVelocityAlpha = 0.02;
+ filtered_box_velocity_ =
+ box_velocity * kFilteredBoxVelocityAlpha +
+ (1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_;
+ status->filtered_box_velocity = filtered_box_velocity_;
+
+ constexpr double kCenteringAngleGain = 0.0;
+ const double left_intake_goal =
+ ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0
+ : unsafe_goal->intake.left_intake_angle)) +
+ last_intake_center_error_ * kCenteringAngleGain;
+ const double right_intake_goal =
+ ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0
+ : unsafe_goal->intake.right_intake_angle)) -
+ last_intake_center_error_ * kCenteringAngleGain;
intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
@@ -51,13 +76,18 @@
output != nullptr ? &(output->right_intake) : nullptr,
&(status->right_intake));
+ const double intake_center_error =
+ intake_right_.output_position() - intake_left_.output_position();
+ last_intake_center_error_ = intake_center_error;
+
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
arm_.Iterate(
unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
unsafe_goal != nullptr ? unsafe_goal->grab_box : false,
- unsafe_goal != nullptr ? unsafe_goal->open_claw : false, &(position->arm),
- position->claw_beambreak_triggered,
+ unsafe_goal != nullptr ? unsafe_goal->open_claw : false,
+ unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
+ &(position->arm), position->claw_beambreak_triggered,
position->box_back_beambreak_triggered, intake_clear_of_box,
output != nullptr ? &(output->voltage_proximal) : nullptr,
output != nullptr ? &(output->voltage_distal) : nullptr,
@@ -87,16 +117,37 @@
double roller_voltage = ::std::max(
-kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
kMaxIntakeRollerVoltage));
- constexpr int kReverseTime = 15;
- if (unsafe_goal->intake.roller_voltage < 0.0) {
+ constexpr int kReverseTime = 14;
+ if (unsafe_goal->intake.roller_voltage < 0.0 ||
+ unsafe_goal->disable_box_correct) {
output->left_intake.voltage_rollers = roller_voltage;
output->right_intake.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
+ stuck_count_ = 0;
} else {
+ monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ const bool stuck = position->box_distance < 0.20 &&
+ filtered_box_velocity_ > -0.05 &&
+ !position->box_back_beambreak_triggered;
+ // Make sure we don't declare ourselves re-stuck too quickly. We want to
+ // wait 400 ms before triggering the stuck condition again.
+ if (!stuck) {
+ last_unstuck_time_ = monotonic_now;
+ }
+ if (monotonic_now < last_stuck_time_ + chrono::milliseconds(400)) {
+ last_unstuck_time_ = monotonic_now;
+ }
+
switch (rotation_state_) {
case RotationState::NOT_ROTATING:
- if (position->left_intake.beam_break) {
+ if (stuck &&
+ monotonic_now > last_stuck_time_ + chrono::milliseconds(400) &&
+ monotonic_now > last_unstuck_time_ + chrono::milliseconds(100)) {
+ rotation_state_ = RotationState::STUCK;
+ ++stuck_count_;
+ last_stuck_time_ = monotonic_now;
+ } else if (position->left_intake.beam_break) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
@@ -107,6 +158,13 @@
} else {
break;
}
+ case RotationState::STUCK: {
+ // Latch being stuck for 80 ms so we kick the box out far enough.
+ if (last_stuck_time_ + chrono::milliseconds(80) < monotonic_now) {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ last_unstuck_time_ = monotonic_now;
+ }
+ } break;
case RotationState::ROTATING_LEFT:
if (position->right_intake.beam_break) {
rotation_count_ = kReverseTime;
@@ -129,25 +187,57 @@
break;
}
- if (position->box_back_beambreak_triggered && roller_voltage > 0.0) {
- roller_voltage = 0;
+ constexpr double kHoldVoltage = 1.0;
+ constexpr double kStuckVoltage = 10.0;
+
+ if (position->box_back_beambreak_triggered &&
+ roller_voltage > kHoldVoltage) {
+ roller_voltage = kHoldVoltage;
}
switch (rotation_state_) {
- case RotationState::NOT_ROTATING:
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
- break;
+ case RotationState::NOT_ROTATING: {
+ double centering_gain = 13.0;
+ if (stuck_count_ > 1) {
+ if ((stuck_count_ - 1) % 2 == 0) {
+ centering_gain = 0.0;
+ }
+ }
+ output->left_intake.voltage_rollers =
+ roller_voltage - intake_center_error * centering_gain;
+ output->right_intake.voltage_rollers =
+ roller_voltage + intake_center_error * centering_gain;
+ } break;
+ case RotationState::STUCK: {
+ if (roller_voltage > kHoldVoltage) {
+ output->left_intake.voltage_rollers = -kStuckVoltage;
+ output->right_intake.voltage_rollers = -kStuckVoltage;
+ }
+ } break;
case RotationState::ROTATING_LEFT:
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = -roller_voltage;
+ if (position->left_intake.beam_break) {
+ output->left_intake.voltage_rollers = -roller_voltage * 0.9;
+ } else {
+ output->left_intake.voltage_rollers = -roller_voltage * 0.6;
+ }
+ output->right_intake.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->left_intake.voltage_rollers = -roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
+ output->left_intake.voltage_rollers = roller_voltage;
+ if (position->right_intake.beam_break) {
+ output->right_intake.voltage_rollers = -roller_voltage * 0.9;
+ } else {
+ output->right_intake.voltage_rollers = -roller_voltage * 0.6;
+ }
break;
}
}
+ } else {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ rotation_count_ = 0;
+ stuck_count_ = 0;
}
+ status->rotation_state = static_cast<uint32_t>(rotation_state_);
+ last_box_distance_ = clipped_box_distance;
}
} // namespace superstructure
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 2081e6c..ec0c9e1 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -36,14 +36,28 @@
intake::IntakeSide intake_right_;
arm::Arm arm_;
+ // The last centering error. This is the distance that the center of the two
+ // intakes is away from 0.
+ double last_intake_center_error_ = 0.0;
+ // The last distance that the box distance lidar measured.
+ double last_box_distance_ = 0.0;
+ // State variable for the box velocity low pass filter.
+ double filtered_box_velocity_ = 0.0;
+
enum class RotationState {
NOT_ROTATING = 0,
ROTATING_LEFT = 1,
- ROTATING_RIGHT = 2
+ ROTATING_RIGHT = 2,
+ STUCK = 3
};
RotationState rotation_state_ = RotationState::NOT_ROTATING;
int rotation_count_ = 0;
+ int stuck_count_ = 0;
+ ::aos::monotonic_clock::time_point last_stuck_time_ =
+ ::aos::monotonic_clock::min_time;
+ ::aos::monotonic_clock::time_point last_unstuck_time_ =
+ ::aos::monotonic_clock::min_time;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 9bdabed..e2cad73 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -132,12 +132,15 @@
bool grab_box;
bool open_claw;
+ bool close_claw;
bool deploy_fork;
bool hook_release;
double voltage_winch;
+
+ bool disable_box_correct;
};
message Status {
@@ -152,6 +155,9 @@
IntakeSideStatus right_intake;
ArmStatus arm;
+
+ double filtered_box_velocity;
+ uint32_t rotation_state;
};
message Position {