Log the pulse widths from the drivetrain encoders
We would like this data in some log files so we can see about using it
in the control loop's kalman filter.
Change-Id: I459ddd5015c6e9addc6ebd52c1a9979b0b28f24f
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index fa7d14a..84b921a 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -69,6 +69,12 @@
(4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
+double drivetrain_velocity_translate(double in) {
+ return (1.0 / in) / 256.0 /*cpr*/ *
+ constants::GetValues().drivetrain_encoder_ratio *
+ (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
double arm_translate(int32_t in) {
return -static_cast<double>(in) /
(512.0 /*cpr*/ * 4.0 /*4x*/) *
@@ -203,10 +209,12 @@
void set_left_encoder(::std::unique_ptr<Encoder> left_encoder) {
left_encoder_ = ::std::move(left_encoder);
+ left_encoder_->SetMaxPeriod(0.005);
}
void set_right_encoder(::std::unique_ptr<Encoder> right_encoder) {
right_encoder_ = ::std::move(right_encoder);
+ right_encoder_->SetMaxPeriod(0.005);
}
// All of the DMA-related set_* calls must be made before this, and it doesn't
@@ -254,6 +262,10 @@
-drivetrain_translate(right_encoder_->GetRaw());
drivetrain_message->left_encoder =
drivetrain_translate(left_encoder_->GetRaw());
+ drivetrain_message->left_speed =
+ drivetrain_velocity_translate(left_encoder_->GetPeriod());
+ drivetrain_message->right_speed =
+ drivetrain_velocity_translate(right_encoder_->GetPeriod());
drivetrain_message.Send();
}