Sequence the indexer
Wiggle it and make sure the right rollers are spinning while we are
out.
Change-Id: I1f0596735c214ed2a7696be298faf6049f610128
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index d16c692..5dfd2d1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -42,6 +42,30 @@
output != nullptr ? &(output_struct.hood_voltage) : nullptr,
status->fbb());
+ if (unsafe_goal != nullptr) {
+ if (unsafe_goal->shooting() &&
+ shooting_start_time_ == aos::monotonic_clock::min_time) {
+ shooting_start_time_ = position_timestamp;
+ }
+
+ if (unsafe_goal->shooting()) {
+ constexpr std::chrono::milliseconds kPeriod =
+ std::chrono::milliseconds(250);
+ if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
+ kPeriod) {
+ intake_joint_.set_min_position(-0.25);
+ } else {
+ intake_joint_.set_min_position(-0.75);
+ }
+ } else {
+ intake_joint_.clear_min_position();
+ }
+
+ if (!unsafe_goal->shooting()) {
+ shooting_start_time_ = aos::monotonic_clock::min_time;
+ }
+ }
+
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
intake_joint_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
@@ -115,13 +139,21 @@
if (output != nullptr) {
if (unsafe_goal) {
- output_struct.washing_machine_spinner_voltage = 6.0;
+ output_struct.washing_machine_spinner_voltage = 0.0;
if (unsafe_goal->shooting()) {
- output_struct.feeder_voltage = 6.0;
+ if (shooter_.ready() &&
+ unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
+ unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+ output_struct.feeder_voltage = 9.0;
+ } else {
+ output_struct.feeder_voltage = 0.0;
+ }
+ output_struct.washing_machine_spinner_voltage = 5.0;
+ output_struct.intake_roller_voltage = 3.0;
} else {
output_struct.feeder_voltage = 0.0;
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
}
- output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
} else {
output_struct.intake_roller_voltage = 0.0;
}
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 8f7cd8c..1526610 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -56,6 +56,9 @@
Climber climber_;
+ aos::monotonic_clock::time_point shooting_start_time_ =
+ aos::monotonic_clock::min_time;
+
double time_ = 0.0;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 75a4b26..026a50b 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -96,7 +96,13 @@
}
if (data.IsPressed(kIntakeExtend)) {
- intake_pos = 1.0;
+ intake_pos = 1.2;
+ roller_speed = 9.0f;
+ }
+
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->intake()->position() > -0.5) {
+ roller_speed = std::max(roller_speed, 6.0f);
}
if (data.IsPressed(kFeed)) {
@@ -119,7 +125,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), hood_pos,
- CreateProfileParameters(*builder.fbb(), 0.5, 1.0));
+ CreateProfileParameters(*builder.fbb(), 0.7, 3.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(