Update Hood to use AbsoluteAndAbsolute position
Change-Id: I3e88718560a6e3983681a3430dc31b5431c1a743
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index 9ecb98c..7ca61b7 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -197,6 +197,45 @@
estimator_state:frc971.AbsoluteEncoderEstimatorState (id: 14);
}
+table AbsoluteAndAbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool (id: 0);
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int (id: 1);
+
+ // If true, we have aborted.
+ estopped:bool (id: 2);
+
+ // Position of the joint.
+ position:float (id: 3);
+ // Velocity of the joint in units/second.
+ velocity:float (id: 4);
+ // Profiled goal position of the joint.
+ goal_position:float (id: 5);
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float (id: 6);
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float (id: 7);
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float (id: 8);
+
+ // The estimated voltage error.
+ voltage_error:float (id: 9);
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float (id: 10);
+
+ // Components of the control loop output
+ position_power:float (id: 11);
+ velocity_power:float (id: 12);
+ feedforwards_power:float (id: 13);
+
+ // State of the estimator.
+ estimator_state:frc971.AbsoluteAndAbsoluteEncoderEstimatorState (id: 14);
+}
+
table RelativeEncoderProfiledJointStatus {
// The state of the subsystem, if applicable. -1 otherwise.
// TODO(alex): replace with enum.
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index b86ad54..5089620 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -198,6 +198,36 @@
::std::unique_ptr<frc::AnalogInput> potentiometer_;
};
+// Class to hold two CTRE encoders, one with both index pulses and absolute
+// angle pwm, and another that can only turn once and only reports absolute
+// angle pwm.
+class AbsoluteAndAbsoluteEncoder {
+ public:
+ void set_single_turn_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> input) {
+ single_turn_duty_cycle_.set_input(::std::move(input));
+ }
+
+ void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
+ duty_cycle_.set_input(::std::move(input));
+ }
+
+ void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ encoder_ = ::std::move(encoder);
+ }
+
+ double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
+ double ReadSingleTurnAbsoluteEncoder() const {
+ return single_turn_duty_cycle_.Read();
+ }
+ int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
+
+ private:
+ DutyCycleReader duty_cycle_;
+ DutyCycleReader single_turn_duty_cycle_;
+ ::std::unique_ptr<frc::Encoder> encoder_;
+};
+
class AbsoluteEncoder {
public:
void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 25e7592..39b83fa 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -102,6 +102,29 @@
encoder_ratio * (2.0 * M_PI);
}
+ // Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
+ // with the correct unit and direction changes.
+ void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,
+ ::frc971::AbsoluteAndAbsolutePositionT *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ double single_turn_encoder_ratio, bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+
+ position->absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
+ : encoder.ReadAbsoluteEncoder()) *
+ encoder_ratio * (2.0 * M_PI);
+
+ position->single_turn_absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadSingleTurnAbsoluteEncoder())
+ : encoder.ReadSingleTurnAbsoluteEncoder()) *
+ single_turn_encoder_ratio * (2.0 * M_PI);
+ }
+
// Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
// and direction changes.
void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
@@ -128,11 +151,10 @@
// Copies a Absolute Encoder with the correct unit
// and direction changes.
- void CopyPosition(
- const ::frc971::wpilib::AbsoluteEncoder &encoder,
- ::frc971::AbsolutePositionT *position,
- double encoder_counts_per_revolution, double encoder_ratio,
- bool reverse) {
+ void CopyPosition(const ::frc971::wpilib::AbsoluteEncoder &encoder,
+ ::frc971::AbsolutePositionT *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
@@ -145,12 +167,11 @@
encoder_ratio * (2.0 * M_PI);
}
- void CopyPosition(
- const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
- ::frc971::PotAndIndexPositionT *position,
- ::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> potentiometer_translate, bool reverse,
- double pot_offset) {
+ void CopyPosition(const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
+ ::frc971::PotAndIndexPositionT *position,
+ ::std::function<double(int32_t)> encoder_translate,
+ ::std::function<double(double)> potentiometer_translate,
+ bool reverse, double pot_offset) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.polled_encoder_value());
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.cc b/frc971/zeroing/absolute_and_absolute_encoder.cc
index 54a5e78..0645d43 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder.cc
@@ -5,6 +5,7 @@
#include "glog/logging.h"
+#include "aos/logging/logging.h"
#include "frc971/zeroing/wrap.h"
namespace frc971 {
@@ -56,13 +57,15 @@
const AbsoluteAndAbsolutePosition &info) {
// Check for Abs Encoder NaN value that would mess up the rest of the zeroing
// code below. NaN values are given when the Absolute Encoder is disconnected.
- if (::std::isnan(info.absolute_encoder())) {
+ if (::std::isnan(info.absolute_encoder()) ||
+ ::std::isnan(info.single_turn_absolute_encoder())) {
if (zeroed_) {
- VLOG(1) << "NAN on absolute encoder.";
+ VLOG(1) << "NAN on one of the absolute encoders.";
error_ = true;
} else {
++nan_samples_;
- VLOG(1) << "NAN on absolute encoder while zeroing" << nan_samples_;
+ VLOG(1) << "NAN on one of the absolute encoders while zeroing"
+ << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
@@ -70,6 +73,8 @@
}
// Throw some dummy values in for now.
filtered_absolute_encoder_ = info.absolute_encoder();
+ filtered_single_turn_absolute_encoder_ =
+ info.single_turn_absolute_encoder();
filtered_position_ =
single_turn_to_relative_encoder_offset_ + info.encoder();
position_ = offset_ + info.encoder();
@@ -139,11 +144,11 @@
// Now compute the offset between the pot and relative encoder.
if (offset_samples_.size() < constants_.average_filter_size) {
- offset_samples_.push_back(adjusted_single_turn_absolute_encoder -
- sample.encoder);
+ offset_samples_.push_back(sample.encoder -
+ adjusted_single_turn_absolute_encoder);
} else {
offset_samples_[samples_idx_] =
- adjusted_single_turn_absolute_encoder - sample.encoder;
+ sample.encoder - adjusted_single_turn_absolute_encoder;
}
// Drop the oldest sample when we run this function the next time around.
@@ -153,7 +158,7 @@
::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
offset_samples_.size();
- offset_ = UnWrap(sample.encoder + single_turn_to_relative_encoder_offset_,
+ offset_ = UnWrap(sample.encoder - single_turn_to_relative_encoder_offset_,
average_relative_to_absolute_offset + sample.encoder,
constants_.one_revolution_distance) -
sample.encoder;
@@ -167,26 +172,36 @@
(adjusted_absolute_encoder -
(sample.absolute_encoder - constants_.measured_absolute_position))));
+ const double what_Unwrap_added =
+ (adjusted_single_turn_absolute_encoder -
+ (sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position));
+
// TODO(Ravago): this is impossible to read.
filtered_single_turn_absolute_encoder_ =
- ((sample.encoder + single_turn_to_relative_encoder_offset_) -
+ ((sample.encoder - single_turn_to_relative_encoder_offset_) -
(-constants_.single_turn_measured_absolute_position +
- (adjusted_single_turn_absolute_encoder -
- (sample.single_turn_absolute_encoder -
- constants_.single_turn_measured_absolute_position))));
+ what_Unwrap_added));
+
+ /*
+ filtered_single_turn_absolute_encoder_ =
+ sample.encoder - single_turn_to_relative_encoder_offset_;
+ */
+
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
if (offset_ready()) {
- if (!zeroed_) {
- first_offset_ = offset_;
- }
-
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- VLOG(1) << "Offset moved too far. Initial: " << first_offset_
- << ", current " << offset_ << ", allowable change: "
- << constants_.allowable_encoder_error *
- constants_.one_revolution_distance;
+ AOS_LOG(INFO,
+ "Offset moved too far. Initial: %f, current %f, allowable "
+ "change: %f ",
+ first_offset_, offset_,
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance);
error_ = true;
}
@@ -195,8 +210,7 @@
}
// Update the position.
- filtered_position_ = single_turn_to_relative_encoder_offset_ + info.encoder();
- position_ = offset_ + info.encoder();
+ position_ = first_offset_ + info.encoder();
}
flatbuffers::Offset<AbsoluteAndAbsoluteEncoderZeroingEstimator::State>
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.h b/frc971/zeroing/absolute_and_absolute_encoder.h
index 7d56f2b..dd2190d 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.h
+++ b/frc971/zeroing/absolute_and_absolute_encoder.h
@@ -33,7 +33,7 @@
bool zeroed() const override { return zeroed_; }
- double offset() const override { return offset_; }
+ double offset() const override { return first_offset_; }
bool error() const override { return error_; }
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 4d0c1cc..0e90e3f 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -171,6 +171,7 @@
// TODO(Brian): Actually split these targets apart. Need to convert all the
// reverse dependencies to #include what they actually need...
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
#include "frc971/zeroing/absolute_encoder.h"
#include "frc971/zeroing/hall_effect_and_position.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
diff --git a/y2020/constants.cc b/y2020/constants.cc
index d39e1cd..0e36f0f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -31,8 +31,8 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const hood =
- &r->hood;
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
+ *const hood = &r->hood;
// Hood constants.
hood->zeroing_voltage = 3.0;
@@ -43,12 +43,17 @@
hood->make_integral_loop =
control_loops::superstructure::hood::MakeIntegralHoodLoop;
hood->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
- hood->zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kHoodEncoderRatio();
hood->zeroing_constants.zeroing_threshold = 0.0005;
hood->zeroing_constants.moving_buffer_size = 20;
hood->zeroing_constants.allowable_encoder_error = 0.9;
- hood->zeroing_constants.middle_position = Values::kHoodRange().middle();
+ hood->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kHoodEncoderRatio();
+ hood->zeroing_constants.single_turn_middle_position =
+ Values::kHoodRange().middle();
+ hood->zeroing_constants.single_turn_one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kHoodSingleTurnEncoderRatio();
+ hood->zeroing_constants.measured_absolute_position = 0;
+ hood->zeroing_constants.single_turn_measured_absolute_position = 0;
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
@@ -93,7 +98,7 @@
turret_params->zeroing_constants.allowable_encoder_error = 0.9;
CHECK_LE(hood->range.range(),
- hood->zeroing_constants.one_revolution_distance);
+ hood->zeroing_constants.single_turn_one_revolution_distance);
CHECK_LE(intake->range.range(),
intake->zeroing_constants.one_revolution_distance);
@@ -113,6 +118,9 @@
0.0109413725126625 - 0.223719825811759;
turret_params->zeroing_constants.measured_absolute_position =
0.547478339799516;
+
+ hood->zeroing_constants.measured_absolute_position = 0.03207;
+ hood->zeroing_constants.single_turn_measured_absolute_position = 0.31055;
break;
case kPracticeTeamNumber:
diff --git a/y2020/constants.h b/y2020/constants.h
index f0e3564..81e6dd0 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -39,7 +39,20 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kHoodEncoderRatio() { return 8.0 / 72.0; }
+ static constexpr double kHoodEncoderRatio() {
+ // TODO: This math is not quite right
+ // 10.211 in of travel gets you 1 radian on the output
+ const double radians_per_in_travel = 1.0 / 10.211;
+
+ // one turn on the leadscrew gets you 12.0 mm travel
+ const double mm_travel_per_radian = 12.0 / (2.0 * M_PI);
+ const double in_travel_per_radian = mm_travel_per_radian / 25.4;
+
+ // units reduce; radians on the encoder * this number = radians on the hood
+ return in_travel_per_radian * radians_per_in_travel / 0.94816;
+ }
+
+ static constexpr double kHoodSingleTurnEncoderRatio() { return 8.0 / 72.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
return control_loops::superstructure::hood::kFreeSpeed / (2.0 * M_PI) *
@@ -50,14 +63,14 @@
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
0.00, // Back Hard
- 0.64, // Front Hard
- 0.01, // Back Soft
- 0.63 // Front Soft
+ 0.64, // Front Hard
+ 0.01, // Back Soft
+ 0.63 // Front Soft
};
}
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
hood;
// Intake
@@ -152,7 +165,6 @@
kFinisherEncoderRatio() * kFinisherEncoderCountsPerRevolution();
}
-
static constexpr double kAcceleratorEncoderCountsPerRevolution() {
return 2048.0;
}
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index d8118a1..db6b727 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -20,17 +20,19 @@
# Hood is an angular subsystem due to the mounting of the encoder on the hood
# joint. We are currently electing to ignore potential non-linearity.
-range_of_travel_radians = (37.0 * numpy.pi / 180.0)
-# 0.083 inches/turn
-# 6.38 inches of travel
-turns_of_leadscrew_per_range_of_travel = 6.38 / 0.083
+range_of_travel_radians = (38.0 * numpy.pi / 180.0)
+# 0.472441 inches/turn (12 mm)
+# 6.7725 inches of travel
+turns_of_leadscrew_per_range_of_travel = 6.7725 / 0.472441
radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
+radians_per_turn_of_motor = 12.0 / 60.0 * radians_per_turn
+
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- G=radians_per_turn / (2.0 * numpy.pi),
+ G=radians_per_turn_of_motor / (2.0 * numpy.pi),
J=4.0,
q_pos=0.15,
q_vel=5.0,
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);
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 4f39acf..e7b7b86 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -136,6 +136,11 @@
hood_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
}
+ void set_hood_single_turn_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ hood_encoder_.set_single_turn_absolute_pwm(::std::move(absolute_pwm));
+ }
+
// Intake
void set_intake_encoder(::std::unique_ptr<frc::Encoder> encoder) {
@@ -221,12 +226,13 @@
// TODO(alex): check new absolute encoder api.
// Hood
- frc971::AbsolutePositionT hood;
+ frc971::AbsoluteAndAbsolutePositionT hood;
CopyPosition(hood_encoder_, &hood,
Values::kHoodEncoderCountsPerRevolution(),
- Values::kHoodEncoderRatio(), true);
- flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
- frc971::AbsolutePosition::Pack(*builder.fbb(), &hood);
+ Values::kHoodEncoderRatio(),
+ Values::kHoodSingleTurnEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsoluteAndAbsolutePosition> hood_offset =
+ frc971::AbsoluteAndAbsolutePosition::Pack(*builder.fbb(), &hood);
// Intake
frc971::AbsolutePositionT intake_joint;
@@ -311,7 +317,9 @@
::frc971::wpilib::AbsoluteEncoderAndPotentiometer turret_encoder_;
- ::frc971::wpilib::AbsoluteEncoder hood_encoder_, intake_joint_encoder_;
+ ::frc971::wpilib::AbsoluteAndAbsoluteEncoder hood_encoder_;
+
+ ::frc971::wpilib::AbsoluteEncoder intake_joint_encoder_;
::std::unique_ptr<::frc::Encoder> finisher_encoder_,
left_accelerator_encoder_, right_accelerator_encoder_,
@@ -486,10 +494,13 @@
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
// TODO: pin numbers
+ // TODO(Ravago): Hood pin numbers
sensor_reader.set_hood_encoder(
make_unique<frc::Encoder>(22, 23, false, frc::Encoder::k4X));
- sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(24));
+ sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(25));
+ sensor_reader.set_hood_single_turn_absolute_pwm(
+ make_unique<frc::DigitalInput>(24));
sensor_reader.set_intake_encoder(make_encoder(5));
sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(1));