Update Hood to use AbsoluteAndAbsolute position
Change-Id: I3e88718560a6e3983681a3430dc31b5431c1a743
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 4c02a18..e6c1375 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -6,6 +6,7 @@
namespace control_loops {
namespace superstructure {
+using frc971::control_loops::AbsoluteAndAbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
@@ -65,11 +66,12 @@
OutputT output_struct;
- flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
- hood_.Iterate(unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
- position->hood(),
- output != nullptr ? &(output_struct.hood_voltage) : nullptr,
- status->fbb());
+ flatbuffers::Offset<AbsoluteAndAbsoluteEncoderProfiledJointStatus>
+ hood_status_offset = hood_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+ position->hood(),
+ output != nullptr ? &(output_struct.hood_voltage) : nullptr,
+ status->fbb());
if (unsafe_goal != nullptr) {
if (unsafe_goal->shooting() &&
@@ -121,7 +123,7 @@
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
- const AbsoluteEncoderProfiledJointStatus *const hood_status =
+ const AbsoluteAndAbsoluteEncoderProfiledJointStatus *const hood_status =
GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 7b87bca..c0f3798 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -6,12 +6,12 @@
#include "aos/robot_state/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/climber.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2020/control_loops/superstructure/climber.h"
#include "y2020/control_loops/superstructure/turret/aiming.h"
namespace y2020 {
@@ -40,8 +40,13 @@
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::
+ AbsoluteAndAbsoluteEncoderProfiledJointStatus>;
- const AbsoluteEncoderSubsystem &hood() const { return hood_; }
+ const AbsoluteAndAbsoluteEncoderSubsystem &hood() const { return hood_; }
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
const shooter::Shooter &shooter() const { return shooter_; }
@@ -53,7 +58,7 @@
aos::Sender<Status>::Builder *status) override;
private:
- AbsoluteEncoderSubsystem hood_;
+ AbsoluteAndAbsoluteEncoderSubsystem hood_;
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
shooter::Shooter shooter_;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index ac1e4a0..cb4ea64 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -38,6 +38,8 @@
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
typedef ::frc971::control_loops::drivetrain::Status DrivetrainStatus;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteAndAbsoluteEncoderSubsystem
+ AbsoluteAndAbsoluteEncoderSubsystem;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
@@ -88,8 +90,11 @@
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
- hood_encoder_(constants::GetValues()
- .hood.zeroing_constants.one_revolution_distance),
+ hood_encoder_(
+ constants::GetValues()
+ .hood.zeroing_constants.one_revolution_distance,
+ constants::GetValues()
+ .hood.zeroing_constants.single_turn_one_revolution_distance),
intake_plant_(new CappedTestPlant(intake::MakeIntakePlant())),
intake_encoder_(constants::GetValues()
.intake.zeroing_constants.one_revolution_distance),
@@ -129,7 +134,9 @@
hood_encoder_.Initialize(
start_pos, kNoiseScalar, 0.0,
constants::GetValues()
- .hood.zeroing_constants.measured_absolute_position);
+ .hood.zeroing_constants.measured_absolute_position,
+ constants::GetValues()
+ .hood.zeroing_constants.single_turn_measured_absolute_position);
}
void InitializeIntakePosition(double start_pos) {
@@ -165,9 +172,9 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
- frc971::AbsolutePosition::Builder hood_builder =
- builder.MakeBuilder<frc971::AbsolutePosition>();
- flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
+ frc971::AbsoluteAndAbsolutePosition::Builder hood_builder =
+ builder.MakeBuilder<frc971::AbsoluteAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsoluteAndAbsolutePosition> hood_offset =
hood_encoder_.GetSensorValues(&hood_builder);
frc971::AbsolutePosition::Builder intake_builder =
@@ -224,9 +231,9 @@
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
const double voltage_check_hood =
- (static_cast<AbsoluteEncoderSubsystem::State>(
+ (static_cast<AbsoluteAndAbsoluteEncoderSubsystem::State>(
superstructure_status_fetcher_->hood()->state()) ==
- AbsoluteEncoderSubsystem::State::RUNNING)
+ AbsoluteAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().hood.operating_voltage
: constants::GetValues().hood.zeroing_voltage;
@@ -826,7 +833,7 @@
SetEnabled(true);
WaitUntilZeroed();
RunFor(chrono::seconds(2));
- EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
+ EXPECT_EQ(AbsoluteAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.hood().state());
EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
index f139381..1e9f81a 100644
--- a/y2020/control_loops/superstructure/superstructure_position.fbs
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -14,7 +14,7 @@
table Position {
// Zero is at the horizontal, positive towards the front (radians).
- hood:frc971.AbsolutePosition (id: 0);
+ hood:frc971.AbsoluteAndAbsolutePosition (id: 0);
// Position of the intake. 0 when four-bar is vertical, positive extended.
intake_joint:frc971.AbsolutePosition (id: 1);
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index fb94d32..83ee607 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -51,7 +51,7 @@
estopped:bool (id: 1);
// Subsystem status.
- hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 2);
+ hood:frc971.control_loops.AbsoluteAndAbsoluteEncoderProfiledJointStatus (id: 2);
intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);