Adjust the button behavior and arm locations.
Change-Id: Ide2e4d3a8b22602501547d721cca1c7a7e4f3e8f
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 6d7b628..aa91137 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -386,8 +386,8 @@
]
-tall_box_x = 0.401
-tall_box_y = 0.14
+tall_box_x = 0.411
+tall_box_y = 0.125
short_box_x = 0.431
short_box_y = 0.082
@@ -401,17 +401,19 @@
# TODO(austin): Drive the front/back off the same numbers a bit better.
front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(
+ 0.700, 2.125, circular_index=-1.000000)
front_middle2_box = to_theta_with_circular_index(
- 0.732, 2.268, circular_index=-1)
+ 0.700, 2.268, circular_index=-1)
front_middle1_box = to_theta_with_circular_index(
- 0.878, 1.885, circular_index=-1)
-front_low_box = to_theta_with_circular_index(0.926, 1.542, circular_index=-1)
+ 0.800, 1.915, circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
back_middle2_box = to_theta_with_circular_index(
- -0.732, 2.268, circular_index=0)
+ -0.700, 2.268, circular_index=0)
back_middle1_box = to_theta_with_circular_index(
- -0.878, 1.885, circular_index=0)
-back_low_box = to_theta_with_circular_index(-0.926, 1.542, circular_index=0)
+ -0.800, 1.915, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.87, 1.572, circular_index=0)
front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
@@ -420,15 +422,23 @@
up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+front_switch_auto = to_theta_with_circular_index(
+ 0.750, 2.20, circular_index=-1.000000)
+
+starting = numpy.array(
+ [numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array(
+ [numpy.pi / 2.0, -numpy.pi / 2.0])
+
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)])
+ [numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
below_hang = numpy.array(
- [numpy.pi / 2.0 - 0.329954, numpy.pi / 2.0 - (-0.534816)])
+ [numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
up_c1 = to_theta((0.63, 1.17), circular_index=-1)
up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -452,6 +462,7 @@
(tall_box_grab, "TallBoxGrab"),
(short_box_grab, "ShortBoxGrab"),
(front_high_box, "FrontHighBox"),
+ (front_middle3_box, "FrontMiddle3Box"),
(front_middle2_box, "FrontMiddle2Box"),
(front_middle1_box, "FrontMiddle1Box"),
(front_low_box, "FrontLowBox"),
@@ -467,6 +478,9 @@
(below_hang, "BelowHang"),
(self_hang, "SelfHang"),
(partner_hang, "PartnerHang"),
+ (front_switch_auto, "FrontSwitchAuto"),
+ (starting, "Starting"),
+ (vertical_starting, "VerticalStarting"),
] # yapf: disable
# We need to define critical points so we can create paths connecting them.
@@ -494,9 +508,20 @@
AngleSegment(neutral, back_switch),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+ AngleSegment(starting, vertical_starting),
+ AngleSegment(vertical_starting, neutral),
+
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
+
+ XYSegment(front_middle3_box, up),
+ XYSegment(front_middle3_box, front_high_box),
+ XYSegment(front_middle3_box, front_middle2_box),
+ XYSegment(front_middle3_box, front_middle1_box),
+
+ XYSegment(neutral, front_switch_auto),
+
XYSegment(up, front_middle1_box),
XYSegment(up, front_low_box),
XYSegment(front_high_box, front_middle2_box),
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 4609827..3f48cc9 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -71,7 +71,7 @@
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
- bool NearEnd(double threshold = 0.02) const {
+ bool NearEnd(double threshold = 0.03) const {
return ::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= threshold &&
::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= threshold &&
follower_.path_distance_to_go() < 1e-3;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 37fb4f9..e22219e 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -37,15 +37,15 @@
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));
+
intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
output != nullptr ? &(output->left_intake) : nullptr,
&(status->left_intake));
- const double right_intake_goal = ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
-
intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
&(position->right_intake),
output != nullptr ? &(output->right_intake) : nullptr,
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index ce46ec7..a828048 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -35,6 +35,7 @@
namespace arm = ::y2018::control_loops::superstructure::arm;
const ButtonLocation kIntakeClosed(4, 1);
+const ButtonLocation kSmallBox(4, 4);
const ButtonLocation kIntakeIn(3, 16);
const ButtonLocation kIntakeOut(4, 3);
@@ -56,7 +57,7 @@
const ButtonLocation kArmAboveHang(3, 7);
const ButtonLocation kArmBelowHang(3, 2);
-const ButtonLocation kWinch(3, 5);
+const ButtonLocation kWinch(3, 4);
const ButtonLocation kArmNeutral(3, 13);
const ButtonLocation kArmUp(3, 9);
@@ -65,9 +66,6 @@
const ButtonLocation kClawOpen(3, 1);
-const ButtonLocation kForkDeploy(3, 11);
-const ButtonLocation kForkStow(3, 10);
-
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
class Reader : public ::aos::input::JoystickInput {
@@ -129,26 +127,14 @@
return;
}
- if (data.IsPressed(kIntakeClosed)) {
- // Deploy the intake.
- if (superstructure_queue.position->box_back_beambreak_triggered) {
- intake_goal_ = 0.30;
- } else {
- intake_goal_ = 0.07;
- }
- } else {
- // Bring in the intake.
- intake_goal_ = -3.3;
- }
-
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->intake.left_intake_angle = intake_goal_;
new_superstructure_goal->intake.right_intake_angle = intake_goal_;
- if (data.IsPressed(kIntakeIn) || data.IsPressed(kArmPickupBoxFromIntake)) {
+ if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
- new_superstructure_goal->intake.roller_voltage = 8.0;
+ new_superstructure_goal->intake.roller_voltage = 7.5;
} else if (data.IsPressed(kIntakeOut)) {
// Turn off the rollers.
new_superstructure_goal->intake.roller_voltage = -12.0;
@@ -157,6 +143,52 @@
new_superstructure_goal->intake.roller_voltage = 0.0;
}
+ if (superstructure_queue.position->box_back_beambreak_triggered) {
+ SendColors(0.0, 0.5, 0.0);
+ } else if (superstructure_queue.position->box_distance < 0.2) {
+ SendColors(0.0, 0.0, 0.5);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+
+ if (data.IsPressed(kSmallBox)) {
+ // Deploy the intake.
+ if (superstructure_queue.position->box_back_beambreak_triggered) {
+ intake_goal_ = 0.30;
+ } else {
+ if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
+ superstructure_queue.position->box_distance < 0.15) {
+ intake_goal_ = 0.18;
+ } else {
+ intake_goal_ = -0.60;
+ }
+ }
+ } else if (data.IsPressed(kIntakeClosed)) {
+ // Deploy the intake.
+ if (superstructure_queue.position->box_back_beambreak_triggered) {
+ intake_goal_ = 0.30;
+ } else {
+ if (new_superstructure_goal->intake.roller_voltage > 0.1) {
+ if (superstructure_queue.position->box_distance < 0.15) {
+ intake_goal_ = 0.23;
+ } else if (superstructure_queue.position->box_distance < 0.20) {
+ intake_goal_ = 0.13;
+ } else if (superstructure_queue.position->box_distance < 0.25) {
+ intake_goal_ = -0.05;
+ } else if (superstructure_queue.position->box_distance < 0.28) {
+ intake_goal_ = -0.20;
+ } else {
+ intake_goal_ = -0.40;
+ }
+ } else {
+ intake_goal_ = -0.60;
+ }
+ }
+ } else {
+ // Bring in the intake.
+ intake_goal_ = -3.3;
+ }
+
// If we are disabled, stay at the node closest to where we start. This
// should remove long motions when enabled.
if (!data.GetControlBit(ControlBit::kEnabled)) {
@@ -173,19 +205,20 @@
arm_goal_position_ = arm::UpIndex();
} else if (data.IsPressed(kArmFrontSwitch)) {
arm_goal_position_ = arm::FrontSwitchIndex();
- } else if (data.IsPressed(kArmFrontHighBox) ||
- data.IsPressed(kArmFrontExtraHighBox)) {
+ } else if (data.IsPressed(kArmFrontExtraHighBox)) {
arm_goal_position_ = arm::FrontHighBoxIndex();
- } else if (data.IsPressed(kArmFrontMiddle2Box)) {
+ } else if (data.IsPressed(kArmFrontHighBox)) {
arm_goal_position_ = arm::FrontMiddle2BoxIndex();
+ } else if (data.IsPressed(kArmFrontMiddle2Box)) {
+ arm_goal_position_ = arm::FrontMiddle3BoxIndex();
} else if (data.IsPressed(kArmFrontMiddle1Box)) {
arm_goal_position_ = arm::FrontMiddle1BoxIndex();
} else if (data.IsPressed(kArmFrontLowBox)) {
arm_goal_position_ = arm::FrontLowBoxIndex();
- } else if (data.IsPressed(kArmBackHighBox) ||
- data.IsPressed(kArmBackExtraHighBox)) {
+ } else if (data.IsPressed(kArmBackExtraHighBox)) {
arm_goal_position_ = arm::BackHighBoxIndex();
- } else if (data.IsPressed(kArmBackMiddle2Box)) {
+ } else if (data.IsPressed(kArmBackHighBox) ||
+ data.IsPressed(kArmBackMiddle2Box)) {
arm_goal_position_ = arm::BackMiddle2BoxIndex();
} else if (data.IsPressed(kArmBackMiddle1Box)) {
arm_goal_position_ = arm::BackMiddle1BoxIndex();
@@ -228,11 +261,6 @@
new_superstructure_goal->open_claw = false;
}
- if (data.IsPressed(kForkDeploy)) {
- new_superstructure_goal->deploy_fork = true;
- } else if (data.IsPressed(kForkStow)) {
- new_superstructure_goal->deploy_fork = false;
- }
new_superstructure_goal->grab_box = grab_box;
LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);