Added intake rollers and test
Change-Id: Ie47b1e3d85b1693e2269bfd883bea03d0b887b92
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index efa6ccb..faf1c9a 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -38,11 +38,10 @@
output != nullptr ? &(output->wrist_voltage) : nullptr,
&(status->wrist));
- intake_.Iterate(
- unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake_joint),
+ output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+ &(status->intake));
stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
&(position->stilts),
@@ -55,6 +54,16 @@
status->estopped = status->elevator.estopped || status->wrist.estopped ||
status->intake.estopped || status->stilts.estopped;
+ if (output) {
+ if (status->intake.position > kMinIntakeAngleForRollers) {
+ output->intake_roller_voltage =
+ (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
+
+ } else {
+ output->intake_roller_voltage = 0.0;
+ }
+ }
+
// TODO(theo) move these up when Iterate() is split
// update the goals
collision_avoidance_.UpdateGoal(status, unsafe_goal);
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 22dbcd6..c3e53f2 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -28,18 +28,10 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
- const PotAndAbsoluteEncoderSubsystem &elevator() const {
- return elevator_;
- }
- const PotAndAbsoluteEncoderSubsystem &wrist() const {
- return wrist_;
- }
- const AbsoluteEncoderSubsystem &intake() const {
- return intake_;
- }
- const PotAndAbsoluteEncoderSubsystem &stilts() const {
- return stilts_;
- }
+ const PotAndAbsoluteEncoderSubsystem &elevator() const { return elevator_; }
+ const PotAndAbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+ const AbsoluteEncoderSubsystem &intake() const { return intake_; }
+ const PotAndAbsoluteEncoderSubsystem &stilts() const { return stilts_; }
protected:
virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
@@ -55,6 +47,8 @@
CollisionAvoidance collision_avoidance_;
+ static constexpr double kMinIntakeAngleForRollers = -0.7;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index de973c0..bd18b60 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -342,9 +342,9 @@
CheckCollisions();
}
- const double loop_time = chrono::duration_cast<chrono::duration<double>>(
- monotonic_clock::now() - loop_start_time)
- .count();
+ const double loop_time =
+ chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time).count();
const double elevator_acceleration =
(superstructure_plant_.elevator_velocity() -
@@ -657,6 +657,52 @@
VerifyNearGoal();
}
+// Tests that the rollers spin when allowed
+TEST_F(SuperstructureTest, IntakeRollerTest) {
+ WaitUntilZeroed();
+
+ // Get the elevator and wrist out of the way and set the Intake to where
+ // we should be able to spin and verify that they do
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage,
+ superstructure_queue_.goal->roller_voltage);
+ VerifyNearGoal();
+
+ // Move the intake where we oughtn't to spin the rollers and verify they don't
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage, 0.0);
+ VerifyNearGoal();
+}
+
+// Tests that running disabled, ya know, works
+TEST_F(SuperstructureTest, DiasableTest) {
+ RunForTime(chrono::seconds(2), false, false);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops