Add hanger code.
This adds the goal nodes for the hanger, and the ability to hang.
Change-Id: Ib33c19298cd08e98abf27d9789982d78f8df191e
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index c283f19..f69bab3 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -44,7 +44,8 @@
const bool box_back_beambreak_triggered,
const bool intake_clear_of_box, double *proximal_output,
double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status) {
+ bool *claw_closed, control_loops::ArmStatus *status,
+ bool suicide) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -163,6 +164,18 @@
state_ = State::ESTOP;
} else if (outputs_disabled) {
state_ = State::DISABLED;
+ } else if (suicide) {
+ state_ = State::PREP_CLIMB;
+ climb_count_ = 50;
+ }
+ break;
+
+ case State::PREP_CLIMB:
+ --climb_count_;
+ if (climb_count_ <= 0) {
+ state_ = State::ESTOP;
+ } else if (!suicide) {
+ state_ = State::RUNNING;
}
break;
@@ -171,8 +184,9 @@
break;
}
- const bool disable = outputs_disabled ||
- (state_ != State::RUNNING && state_ != State::GOTO_PATH);
+ const bool disable = outputs_disabled || (state_ != State::RUNNING &&
+ state_ != State::GOTO_PATH &&
+ state_ != State::PREP_CLIMB);
if (disable) {
close_enough_for_full_power_ = false;
}
@@ -344,7 +358,11 @@
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
*distal_output = ::std::max(
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
- *release_arm_brake = true;
+ if (state_ != State::PREP_CLIMB) {
+ *release_arm_brake = true;
+ } else {
+ *release_arm_brake = false;
+ }
*claw_closed = claw_closed_;
}
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 8e1860f..a39b550 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -38,7 +38,8 @@
const bool box_back_beambreak_triggered,
const bool intake_clear_of_box, double *proximal_output,
double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status);
+ bool *claw_closed, control_loops::ArmStatus *status,
+ bool suicide);
void Reset();
@@ -48,6 +49,7 @@
DISABLED,
GOTO_PATH,
RUNNING,
+ PREP_CLIMB,
ESTOP,
};
@@ -98,6 +100,8 @@
bool close_enough_for_full_power_ = false;
+ int32_t climb_count_ = 0;
+
EKF arm_ekf_;
TrajectoryFollower follower_;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 3b6224c..37fb4f9 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -62,7 +62,20 @@
output != nullptr ? &(output->voltage_proximal) : nullptr,
output != nullptr ? &(output->voltage_distal) : nullptr,
output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
+ output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm),
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false);
+
+ if (output) {
+ if (unsafe_goal) {
+ output->hook_release = unsafe_goal->hook_release;
+ output->voltage_winch = unsafe_goal->voltage_winch;
+ output->forks_release = unsafe_goal->deploy_fork;
+ } else {
+ output->voltage_winch = 0.0;
+ output->hook_release = false;
+ output->forks_release = false;
+ }
+ }
status->estopped = status->left_intake.estopped ||
status->right_intake.estopped || status->arm.estopped;
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 4020094..0da9d17 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -134,6 +134,10 @@
bool open_claw;
bool deploy_fork;
+
+ bool hook_release;
+
+ double voltage_winch;
};
message Status {
@@ -182,6 +186,9 @@
// Voltage sent to the motors on the distal joint of the arm.
double voltage_distal;
+ // Voltage sent to the hanger. Positive pulls the robot up.
+ double voltage_winch;
+
// Clamped (when true) or unclamped (when false) status sent to the
// pneumatic claw on the arm.
bool claw_grabbed;