Added tweak up and tweak down buttons.
Next up, add a bunch of extra nodes.
Change-Id: I4c2df145298567e61e8c36760be88f59a391588b
diff --git a/y2018/constants.cc b/y2018/constants.cc
index cef8979..8180bdc 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -122,9 +122,9 @@
arm_proximal->potentiometer_offset = -1.242 - 0.03 - 0.1;
arm_distal->zeroing.measured_absolute_position =
- 1.102987 - kDistalZeroingPosition + 0.12;
+ 1.102987 - kDistalZeroingPosition + 0.12 + 0.0095;
arm_distal->potentiometer_offset =
- 2.772210 + M_PI + 0.434 - 0.12 + 1.25 - 0.226;
+ 2.772210 + M_PI + 0.434 - 0.12 + 1.25 - 0.226 + 0.862067;
break;
default:
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 5559f7c..abe5ed3 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -117,6 +117,26 @@
% (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
h_file.append("}")
+ front_points = [
+ index_function_name(point[1]) + "()" for point in graph_generate.front_points
+ ]
+ h_file.append("")
+ h_file.append("constexpr ::std::array<uint32_t, %d> FrontPoints() {" %
+ len(front_points))
+ h_file.append(" return ::std::array<uint32_t, %d>{{%s}};" %
+ (len(front_points), ", ".join(front_points)))
+ h_file.append("}")
+
+ back_points = [
+ index_function_name(point[1]) + "()" for point in graph_generate.back_points
+ ]
+ h_file.append("")
+ h_file.append("constexpr ::std::array<uint32_t, %d> BackPoints() {" %
+ len(back_points))
+ h_file.append(" return ::std::array<uint32_t, %d>{{%s}};" %
+ (len(back_points), ", ".join(back_points)))
+ h_file.append("}")
+
# Add the Make*Path functions.
h_file.append("")
cc_file.append("")
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 0807c79..3b21e5d 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -556,19 +556,26 @@
front_switch_c1 = numpy.array([1.903841, -0.622351])
front_switch_c2 = numpy.array([1.903841, -0.622351])
+
+front_points = [
+ (front_high_box, "FrontHighBox"),
+ (front_middle2_box, "FrontMiddle2Box"),
+ (front_middle3_box, "FrontMiddle3Box"),
+ (front_middle1_box, "FrontMiddle1Box"),
+ (front_low_box, "FrontLowBox"),
+ (front_switch, "FrontSwitch"),
+] # yapf: disable
+
+back_points = [
+ (back_high_box, "BackHighBox"),
+ (back_middle2_box, "BackMiddle2Box"),
+ (back_middle1_box, "BackMiddle1Box"),
+ (back_low_box, "BackLowBox"),
+] # yapf: disable
+
points = [(ready_above_box, "ReadyAboveBox"),
(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"),
- (back_high_box, "BackHighBox"),
- (back_middle2_box, "BackMiddle2Box"),
- (back_middle1_box, "BackMiddle1Box"),
- (back_low_box, "BackLowBox"),
- (front_switch, "FrontSwitch"),
(back_switch, "BackSwitch"),
(neutral, "Neutral"),
(up, "Up"),
@@ -580,7 +587,7 @@
(starting, "Starting"),
(duck, "Duck"),
(vertical_starting, "VerticalStarting"),
-] # yapf: disable
+] + front_points + back_points # yapf: disable
duck_c1 = numpy.array([1.337111, -1.721008])
duck_c2 = numpy.array([1.283701, -1.795519])
@@ -588,6 +595,9 @@
ready_to_up_c1 = numpy.array([1.792962, 0.198329])
ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
+front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
+
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
@@ -611,6 +621,7 @@
]
unnamed_segments = [
+ SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2, front_switch_auto),
SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
@@ -623,15 +634,11 @@
XYSegment(ready_above_box, front_middle1_box),
XYSegment(ready_above_box, front_middle2_box),
XYSegment(ready_above_box, front_middle3_box),
- XYSegment(ready_above_box, front_high_box),
- #XYSegment(ready_above_box, up),
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, front_high_box),
AngleSegment(starting, vertical_starting),
AngleSegment(vertical_starting, neutral),
- # TODO(austin): Duck -> neutral with a theta spline.
- #AngleSegment(duck, vertical_starting),
-
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
@@ -641,8 +648,6 @@
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/joystick_reader.cc b/y2018/joystick_reader.cc
index ace4552..fcdf51f 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -28,6 +28,9 @@
using ::aos::input::driver_station::POVLocation;
using ::aos::input::DrivetrainInputReader;
+using ::y2018::control_loops::superstructure::arm::FrontPoints;
+using ::y2018::control_loops::superstructure::arm::BackPoints;
+
namespace y2018 {
namespace input {
namespace joysticks {
@@ -212,7 +215,51 @@
if (data.IsPressed(kArmPickupBoxFromIntake)) {
grab_box = true;
}
- if (data.PosEdge(kArmPickupBoxFromIntake)) {
+ const bool near_goal =
+ superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go < 1e-3;
+ if (data.PosEdge(kArmStepDown) && near_goal) {
+ uint32_t *front_point = ::std::find(
+ front_points_.begin(), front_points_.end(), arm_goal_position_);
+ uint32_t *back_point = ::std::find(
+ back_points_.begin(), back_points_.end(), arm_goal_position_);
+ LOG(INFO, "Step up\n");
+ if (front_point != front_points_.end()) {
+ LOG(INFO, "In the front list, %d\n",
+ static_cast<int>(
+ ::std::distance(front_points_.begin(), front_point)));
+ ++front_point;
+ if (front_point != front_points_.end()) {
+ LOG(INFO, "Incrementing front\n");
+ arm_goal_position_ = *front_point;
+ }
+ } else if (back_point != back_points_.end()) {
+ LOG(INFO, "In the back list, %d\n",
+ static_cast<int>(
+ ::std::distance(back_points_.begin(), back_point)));
+ ++back_point;
+ if (back_point != back_points_.end()) {
+ LOG(INFO, "Incrementing back\n");
+ arm_goal_position_ = *back_point;
+ }
+ }
+ } else if (data.PosEdge(kArmStepUp) && near_goal) {
+ const uint32_t *front_point = ::std::find(
+ front_points_.begin(), front_points_.end(), arm_goal_position_);
+ const uint32_t *back_point = ::std::find(
+ back_points_.begin(), back_points_.end(), arm_goal_position_);
+ if (front_point != front_points_.end()) {
+ if (front_point != front_points_.begin()) {
+ --front_point;
+ arm_goal_position_ = *front_point;
+ }
+ } else if (back_point != back_points_.end()) {
+ if (back_point != back_points_.begin()) {
+ --back_point;
+ arm_goal_position_ = *back_point;
+ }
+ }
+ } else if (data.PosEdge(kArmPickupBoxFromIntake)) {
arm_goal_position_ = arm::NeutralIndex();
} else if (data.IsPressed(kDuck)) {
arm_goal_position_ = arm::DuckIndex();
@@ -319,7 +366,10 @@
bool auto_running_ = false;
bool never_disabled_ = true;
- int arm_goal_position_ = 0;
+ uint32_t arm_goal_position_ = 0;
+
+ decltype(FrontPoints()) front_points_ = FrontPoints();
+ decltype(BackPoints()) back_points_ = BackPoints();
::aos::common::actions::ActionQueue action_queue_;
};