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/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index 183f7bf..fbf5f8b 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -93,7 +93,6 @@
pt = points[pt_i]
delta = last_pt[1] - pt[1]
if abs(delta) > numpy.pi:
- print(delta)
return points[pt_i:] + points[:pt_i]
last_pt = pt
return points
@@ -123,9 +122,6 @@
# Fully computed theta constrints.
lines_theta = list(p1.intersection(p2).exterior.coords)
-print("Theta constraint.")
-print(", ".join("{%s, %s}" % (a, b) for a, b in lines_theta))
-
lines1_theta_back = back_to_xy_loop(lines1_theta)
lines2_theta_back = back_to_xy_loop(lines2_theta)
@@ -419,8 +415,7 @@
def do_button_press(self, event):
self.last_pos = (event.x, event.y)
self.now_segment_pt = self.cur_pt_in_theta()
- print('Clicked at theta: (%f, %f)' % (self.now_segment_pt[0],
- self.now_segment_pt[1]))
+ print('Clicked at theta: %s' % (repr(self.now_segment_pt,)))
if not self.theta_version:
print('Clicked at xy, circular index: (%f, %f, %f)' %
(self.last_pos[0], self.last_pos[1],
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index a976145..6d7b628 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -420,6 +420,16 @@
up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+self_hang = numpy.array(
+ [numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array(
+ [numpy.pi / 2.0 - (-0.25), numpy.pi / 2.0])
+
+above_hang = numpy.array(
+ [numpy.pi / 2.0 - 0.008739, numpy.pi / 2.0 - (-0.101927)])
+below_hang = numpy.array(
+ [numpy.pi / 2.0 - 0.329954, numpy.pi / 2.0 - (-0.534816)])
+
up_c1 = to_theta((0.63, 1.17), circular_index=-1)
up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -452,7 +462,12 @@
(front_switch, "FrontSwitch"),
(back_switch, "BackSwitch"),
(neutral, "Neutral"),
- (up, "Up")] # yapf: disable
+ (up, "Up"),
+ (above_hang, "AboveHang"),
+ (below_hang, "BelowHang"),
+ (self_hang, "SelfHang"),
+ (partner_hang, "PartnerHang"),
+] # yapf: disable
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
@@ -477,7 +492,6 @@
unnamed_segments = [
AngleSegment(neutral, back_switch),
- #XYSegment(neutral, front_switch),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
XYSegment(neutral, front_low_box),
@@ -503,6 +517,12 @@
XYSegment(back_middle2_box, back_middle1_box),
XYSegment(back_middle2_box, back_low_box),
XYSegment(back_middle1_box, back_low_box),
+
+ AngleSegment(up, above_hang),
+ AngleSegment(above_hang, below_hang),
+ AngleSegment(up, below_hang),
+ AngleSegment(up, self_hang),
+ AngleSegment(up, partner_hang),
]
segments = named_segments + unnamed_segments
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;
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index bb6c1b1..a5eaf14 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -52,6 +52,11 @@
const ButtonLocation kArmBackLowBox(4, 12);
const ButtonLocation kArmBackSwitch(3, 12);
+const ButtonLocation kArmAboveHang(3, 7);
+const ButtonLocation kArmBelowHang(3, 2);
+
+const ButtonLocation kWinch(3, 5);
+
const ButtonLocation kArmNeutral(3, 13);
const ButtonLocation kArmUp(3, 9);
@@ -96,7 +101,6 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
drivetrain_input_reader_->HandleDrivetrain(data);
- robot_velocity_ = drivetrain_input_reader_->robot_velocity();
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
@@ -177,8 +181,33 @@
arm_goal_position_ = arm::BackLowBoxIndex();
} else if (data.IsPressed(kArmBackSwitch)) {
arm_goal_position_ = arm::BackSwitchIndex();
+ } else if (data.IsPressed(kArmAboveHang)) {
+ if (data.IsPressed(kIntakeIn)) {
+ arm_goal_position_ = arm::SelfHangIndex();
+ } else if (data.IsPressed(kIntakeOut)) {
+ arm_goal_position_ = arm::PartnerHangIndex();
+ } else {
+ arm_goal_position_ = arm::AboveHangIndex();
+ }
+ } else if (data.IsPressed(kArmBelowHang)) {
+ arm_goal_position_ = arm::BelowHangIndex();
}
+ new_superstructure_goal->deploy_fork =
+ data.IsPressed(kArmAboveHang) && data.IsPressed(kClawOpen);
+
+ if (new_superstructure_goal->deploy_fork) {
+ intake_goal_ = -2.0;
+ }
+
+ if (data.IsPressed(kWinch)) {
+ new_superstructure_goal->voltage_winch = 12.0;
+ } else {
+ new_superstructure_goal->voltage_winch = 0.0;
+ }
+
+ new_superstructure_goal->hook_release = data.IsPressed(kArmBelowHang);
+
new_superstructure_goal->arm_goal_position = arm_goal_position_;
if (data.IsPressed(kClawOpen) || data.PosEdge(kArmPickupBoxFromIntake)) {
@@ -222,13 +251,11 @@
}
// Current goals to send to the robot.
- double intake_goal_ = -M_PI * 2.0 / 3.0;
+ double intake_goal_ = 0.0;
bool was_running_ = false;
bool auto_running_ = false;
- double robot_velocity_ = 0.0;
-
int arm_goal_position_ = 0;
::aos::common::actions::ActionQueue action_queue_;
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 0426444..cf19b26 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -722,6 +722,10 @@
distal_victor_ = ::std::move(t);
}
+ void set_hanger_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ hanger_victor_ = ::std::move(t);
+ }
+
void set_left_intake_elastic_victor(::std::unique_ptr<::frc::VictorSP> t) {
left_intake_elastic_victor_ = ::std::move(t);
}
@@ -774,6 +778,9 @@
distal_victor_->SetSpeed(::aos::Clip(queue->voltage_distal,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
+ hanger_victor_->SetSpeed(
+ ::aos::Clip(-queue->voltage_winch, -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
}
virtual void Stop() override {
@@ -786,11 +793,13 @@
proximal_victor_->SetDisabled();
distal_victor_->SetDisabled();
+ hanger_victor_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> left_intake_rollers_victor_,
right_intake_rollers_victor_, left_intake_elastic_victor_,
- right_intake_elastic_victor_, proximal_victor_, distal_victor_;
+ right_intake_elastic_victor_, proximal_victor_, distal_victor_,
+ hanger_victor_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -879,7 +888,8 @@
superstructure_writer.set_distal_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
- // Hanger: victor 8
+ superstructure_writer.set_hanger_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(8)));
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));