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_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index 554e0a6..682cff5 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -73,6 +73,12 @@
(4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
+double drivetrain_velocity_translate(double in) {
+ return (1.0 / in) / 256.0 /*cpr*/ *
+ ::y2015_bot3::control_loops::kDrivetrainEncoderRatio *
+ (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
double elevator_translate(int32_t in) {
return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
::y2015_bot3::control_loops::kElevEncoderRatio * (2 * M_PI /*radians*/) *
@@ -98,10 +104,12 @@
// Drivetrain setters.
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);
}
// Elevator setters.
@@ -150,6 +158,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();
}