Update sensor reading for new turret
Change-Id: Id2b8465f78ed25c91bd9477c416da5500a83e9b8
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index cdbff1d..d49b05d 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -43,6 +43,10 @@
public:
DMAEdgeCounter(Encoder *encoder, DigitalInput *input)
: encoder_(encoder), input_(input) {}
+ DMAEdgeCounter() = default;
+
+ void set_encoder(Encoder *encoder) { encoder_ = encoder; }
+ void set_input(DigitalInput *input) { input_ = input; }
int positive_count() const { return pos_edge_count_; }
int negative_count() const { return neg_edge_count_; }
@@ -74,8 +78,8 @@
bool ExtractValue(const DMASample &sample) const;
- Encoder *const encoder_;
- DigitalInput *const input_;
+ Encoder *encoder_ = nullptr;
+ DigitalInput *input_ = nullptr;
// The last DMA reading we got.
DMASample prev_sample_;
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 9822b0a..008e08b 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -47,12 +47,9 @@
constexpr ::frc971::constants::Range Values::kHoodRange;
constexpr double Values::kTurretEncoderCountsPerRevolution,
- Values::kTurretEncoderRatio, Values::kTurretPotRatio,
- Values::kTurretEncoderIndexDifference,
- Values::kMaxTurretEncoderPulsesPerSecond;
-constexpr ::frc971::constants::Range Values::kTurretRange;
+ Values::kTurretEncoderRatio, Values::kMaxTurretEncoderPulsesPerSecond;
-constexpr double Values::kMaxIndexerEncoderCountsPerRevolution,
+constexpr double Values::kIndexerEncoderCountsPerRevolution,
Values::kIndexerEncoderRatio, Values::kIndexerEncoderIndexDifference,
Values::kMaxIndexerEncoderPulsesPerSecond;
@@ -64,7 +61,6 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
Values::Intake *const intake = &r->intake;
- Values::Turret *const turret = &r->turret;
Values::Hood *const hood = &r->hood;
r->drivetrain_max_speed = 5;
@@ -75,13 +71,6 @@
intake->zeroing.moving_buffer_size = 20;
intake->zeroing.allowable_encoder_error = 0.3;
- turret->zeroing.average_filter_size = Values::kZeroingSampleSize;
- turret->zeroing.one_revolution_distance =
- Values::kTurretEncoderIndexDifference;
- turret->zeroing.zeroing_threshold = 0.001;
- turret->zeroing.moving_buffer_size = 9;
- turret->zeroing.allowable_encoder_error = 0.3;
-
hood->zeroing.index_pulse_count = 2;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
hood->zeroing.known_index_pulse = 0;
@@ -92,9 +81,6 @@
intake->pot_offset = 0;
intake->zeroing.measured_absolute_position = 0;
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
-
hood->pot_offset = 0.1;
hood->zeroing.measured_index_position = 0.05;
@@ -106,9 +92,6 @@
intake->pot_offset = 0.26712;
intake->zeroing.measured_absolute_position = 0.008913;
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
-
hood->zeroing.measured_index_position = 0.652898 - 0.488117;
r->down_error = 0;
@@ -119,9 +102,6 @@
intake->pot_offset = 0.2921 + 0.00039 + 0.012236 - 0.023602;
intake->zeroing.measured_absolute_position = 0.031437;
- turret->pot_offset = -5.45 - 0.026111;
- turret->zeroing.measured_absolute_position = 0.2429;
-
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
r->down_error = 0;
diff --git a/y2017/constants.h b/y2017/constants.h
index e011501..ec8de5d 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -89,21 +89,15 @@
-0.39 * M_PI / 180.0, 37.11 * M_PI / 180.0, (-0.39 + 1.0) * M_PI / 180.0,
(37.11 - 1.0) * M_PI / 180.0};
- static constexpr double kTurretEncoderCountsPerRevolution = 1024 * 4;
- static constexpr double kTurretEncoderRatio = 16.0 / 92.0;
- static constexpr double kTurretPotRatio = 16.0 / 92.0;
- static constexpr double kTurretEncoderIndexDifference =
- 2.0 * M_PI * kTurretEncoderRatio;
+ static constexpr double kTurretEncoderCountsPerRevolution = 256 * 4;
+ static constexpr double kTurretEncoderRatio = 11.0 / 94.0;
static constexpr double kMaxTurretEncoderPulsesPerSecond =
control_loops::superstructure::turret::kFreeSpeed *
control_loops::superstructure::turret::kOutputRatio /
constants::Values::kTurretEncoderRatio *
kTurretEncoderCountsPerRevolution;
- static constexpr ::frc971::constants::Range kTurretRange{
- -460.0 / 2.0 * M_PI / 180.0, 460.0 / 2.0 * M_PI / 180.0,
- -450.0 / 2.0 * M_PI / 180.0, 450.0 / 2.0 * M_PI / 180.0};
- static constexpr double kMaxIndexerEncoderCountsPerRevolution = 256 * 4;
+ static constexpr double kIndexerEncoderCountsPerRevolution = 256 * 4;
static constexpr double kIndexerEncoderRatio = (18.0 / 36.0) * (12.0 / 84.0);
static constexpr double kIndexerEncoderIndexDifference =
2.0 * M_PI * kIndexerEncoderRatio;
@@ -111,14 +105,12 @@
control_loops::superstructure::indexer::kFreeSpeed *
control_loops::superstructure::indexer::kOutputRatio /
constants::Values::kIndexerEncoderRatio *
- kMaxIndexerEncoderCountsPerRevolution;
+ kIndexerEncoderCountsPerRevolution;
double drivetrain_max_speed;
Intake intake;
- Turret turret;
-
Hood hood;
double down_error;
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 1343640..09d36a6 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -104,11 +104,6 @@
(2 * M_PI /*radians*/);
}
-double turret_pot_translate(double voltage) {
- return -voltage * Values::kTurretPotRatio * (10.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
-}
-
constexpr double kMaxFastEncoderPulsesPerSecond =
max(Values::kMaxDrivetrainEncoderPulsesPerSecond,
Values::kMaxShooterEncoderPulsesPerSecond);
@@ -124,6 +119,8 @@
Values::kMaxHoodEncoderPulsesPerSecond;
static_assert(kMaxSlowEncoderPulsesPerSecond <= 100000,
"slow encoders are too fast");
+static_assert(kMaxSlowEncoderPulsesPerSecond < kMaxMediumEncoderPulsesPerSecond,
+ "slow encoders are faster than medium?");
// Handles reading the duty cycle on a DigitalInput.
class DutyCycleReader {
@@ -203,10 +200,7 @@
static_cast<int>(1 / 4.0 /* built-in tolerance */ /
kMaxMediumEncoderPulsesPerSecond * 1e9 +
0.5));
- slow_encoder_filter_.SetPeriodNanoSeconds(
- static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxSlowEncoderPulsesPerSecond * 1e9 +
- 0.5));
+ hall_filter_.SetPeriodNanoSeconds(100000);
}
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
@@ -239,29 +233,35 @@
void set_indexer_encoder(::std::unique_ptr<Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
+ indexer_counter_.set_encoder(encoder.get());
indexer_encoder_ = ::std::move(encoder);
}
+ void set_indexer_hall(::std::unique_ptr<DigitalInput> input) {
+ hall_filter_.Add(input.get());
+ indexer_counter_.set_input(input.get());
+ indexer_hall_ = ::std::move(input);
+ }
+
void set_turret_encoder(::std::unique_ptr<Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
- turret_encoder_.set_encoder(::std::move(encoder));
+ turret_counter_.set_encoder(encoder.get());
+ turret_encoder_ = ::std::move(encoder);
}
- void set_turret_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
- turret_encoder_.set_potentiometer(::std::move(potentiometer));
- }
-
- void set_turret_absolute(::std::unique_ptr<DigitalInput> input) {
- turret_encoder_.set_absolute_pwm(::std::move(input));
+ void set_turret_hall(::std::unique_ptr<DigitalInput> input) {
+ hall_filter_.Add(input.get());
+ turret_counter_.set_input(input.get());
+ turret_hall_ = ::std::move(input);
}
void set_hood_encoder(::std::unique_ptr<Encoder> encoder) {
- slow_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
hood_encoder_.set_encoder(::std::move(encoder));
}
void set_hood_index(::std::unique_ptr<DigitalInput> index) {
- slow_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
hood_encoder_.set_index(::std::move(index));
}
@@ -274,7 +274,9 @@
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+ dma_synchronizer_->Add(&indexer_counter_);
dma_synchronizer_->Add(&hood_encoder_);
+ dma_synchronizer_->Add(&turret_counter_);
}
void operator()() {
@@ -330,10 +332,9 @@
Values::kIntakeEncoderRatio, intake_pot_translate, true,
values.intake.pot_offset);
- superstructure_message->theta_indexer =
- -encoder_translate(indexer_encoder_->GetRaw(),
- Values::kMaxIndexerEncoderCountsPerRevolution,
- Values::kIndexerEncoderRatio);
+ CopyPosition(indexer_counter_, &superstructure_message->column.indexer,
+ Values::kIndexerEncoderCountsPerRevolution,
+ Values::kIndexerEncoderRatio, true);
superstructure_message->theta_shooter =
encoder_translate(shooter_encoder_->GetRaw(),
@@ -344,10 +345,9 @@
Values::kHoodEncoderCountsPerRevolution,
Values::kHoodEncoderRatio, true);
- CopyPosition(turret_encoder_, &superstructure_message->turret,
+ CopyPosition(turret_counter_, &superstructure_message->column.turret,
Values::kTurretEncoderCountsPerRevolution,
- Values::kTurretEncoderRatio, turret_pot_translate, true,
- values.turret.pot_offset);
+ Values::kTurretEncoderRatio, true);
superstructure_message.Send();
}
@@ -410,13 +410,35 @@
encoder_ratio * (2.0 * M_PI);
}
+ void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
+ ::frc971::HallEffectAndPosition *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->position =
+ multiplier * encoder_translate(counter.polled_encoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->current = !counter.polled_value();
+ position->posedge_count = counter.negative_count();
+ position->negedge_count = counter.positive_count();
+ position->posedge_value =
+ multiplier * encoder_translate(counter.last_negative_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->negedge_value =
+ multiplier * encoder_translate(counter.last_positive_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ }
+
int32_t my_pid_;
DriverStation *ds_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
- slow_encoder_filter_;
+ hall_filter_;
::std::unique_ptr<Encoder> drivetrain_left_encoder_,
drivetrain_right_encoder_;
@@ -424,9 +446,13 @@
AbsoluteEncoderAndPotentiometer intake_encoder_;
::std::unique_ptr<Encoder> indexer_encoder_;
- ::std::unique_ptr<AnalogInput> indexer_hall_;
+ ::std::unique_ptr<DigitalInput> indexer_hall_;
+ ::frc971::wpilib::DMAEdgeCounter indexer_counter_;
- AbsoluteEncoderAndPotentiometer turret_encoder_;
+ ::std::unique_ptr<Encoder> turret_encoder_;
+ ::std::unique_ptr<DigitalInput> turret_hall_;
+ ::frc971::wpilib::DMAEdgeCounter turret_counter_;
+
::frc971::wpilib::DMAEncoder hood_encoder_;
::std::unique_ptr<Encoder> shooter_encoder_;
@@ -559,10 +585,10 @@
reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
reader.set_indexer_encoder(make_encoder(5));
+ reader.set_indexer_hall(make_unique<DigitalInput>(4));
reader.set_turret_encoder(make_encoder(6));
- reader.set_turret_absolute(make_unique<DigitalInput>(2));
- reader.set_turret_potentiometer(make_unique<AnalogInput>(5));
+ reader.set_turret_hall(make_unique<DigitalInput>(2));
reader.set_hood_encoder(make_encoder(4));
reader.set_hood_index(make_unique<DigitalInput>(1));