Merge "Fix color labels"
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 2df9a33..fbdcfbb 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -115,6 +115,18 @@
(-point[3], -point[4], -point[5]))
cc_file.append(" }));")
cc_file.append("}")
+
+ # Matrix of nodes
+ h_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList();")
+
+ cc_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
+ cc_file.append(" ::std::vector<::Eigen::Matrix<double, 2, 1>> points;")
+ for index, point in enumerate(graph_generate.points):
+ cc_file.append(
+ " points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());" % (
+ numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+ cc_file.append(" return points;")
+ cc_file.append("}")
# Now create the MakeSearchGraph function.
h_file.append("")
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 1b7c1ce..480715d 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -18,19 +18,23 @@
public:
Arm();
+ // If true, tune down all the constants for testing.
+ static constexpr bool kGrannyMode() { return true; }
+
// The operating voltage.
- static constexpr double kOperatingVoltage() { return 12.0; }
+ static constexpr double kOperatingVoltage() {
+ return kGrannyMode() ? 6.0 : 12.0;
+ }
static constexpr double kDt() { return 0.00505; }
- static constexpr double kAlpha0Max() { return 25.0; }
- static constexpr double kAlpha1Max() { return 25.0; }
- static constexpr double kVMax() { return 11.0; }
+ static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 25.0; }
+ static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 25.0; }
+ static constexpr double kVMax() { return kGrannyMode() ? 4.0 : 11.5; }
static constexpr double kPathlessVMax() { return 4.0; }
void Iterate(const uint32_t *unsafe_goal,
const control_loops::ArmPosition *position,
double *proximal_output, double *distal_output,
- bool *release_arm_brake,
- control_loops::ArmStatus *status);
+ bool *release_arm_brake, control_loops::ArmStatus *status);
void Reset();
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 75198fa..d44b6b2 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -160,6 +160,9 @@
// Value of the beam breaker sensor. This value is true if the beam is
// broken, false if the beam isn't broken.
bool claw_beambreak_triggered;
+ // Value of the beambreak sensor detecting when the box has hit the frame
+ // cutout.
+ bool box_back_beambreak_triggered;
};
message Output {
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 6ccc6f9..a239cc2 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -263,6 +263,14 @@
right_intake_cube_detector_ = ::std::move(input);
}
+ void set_claw_beambreak(::std::unique_ptr<DigitalInput> input) {
+ claw_beambreak_ = ::std::move(input);
+ }
+
+ void set_box_back_beambreak(::std::unique_ptr<DigitalInput> input) {
+ box_back_beambreak_ = ::std::move(input);
+ }
+
// Auto mode switches.
void set_autonomous_mode(int i, ::std::unique_ptr<DigitalInput> sensor) {
autonomous_modes_.at(i) = ::std::move(sensor);
@@ -461,6 +469,10 @@
superstructure_message->right_intake.beam_break =
right_intake_cube_detector_->Get();
+ superstructure_message->claw_beambreak_triggered = claw_beambreak_->Get();
+ superstructure_message->box_back_beambreak_triggered =
+ !box_back_beambreak_->Get();
+
superstructure_message.Send();
}
@@ -537,6 +549,9 @@
::std::unique_ptr<DigitalInput> left_intake_cube_detector_,
right_intake_cube_detector_;
+ ::std::unique_ptr<DigitalInput> claw_beambreak_;
+ ::std::unique_ptr<DigitalInput> box_back_beambreak_;
+
::std::unique_ptr<DigitalInput> pwm_trigger_;
::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
@@ -803,8 +818,8 @@
reader.set_left_intake_spring_angle(make_unique<AnalogInput>(4));
reader.set_left_intake_cube_detector(make_unique<DigitalInput>(0));
- reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
- reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
+ reader.set_claw_beambreak(make_unique<DigitalInput>(8));
+ reader.set_box_back_beambreak(make_unique<DigitalInput>(9));
reader.set_pwm_trigger(make_unique<DigitalInput>(25));