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/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index 5dcb838..56a5aa1 100644
--- a/y2014_bot3/wpilib/wpilib_interface.cc
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -65,6 +65,12 @@
(4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
+double drivetrain_velocity_translate(double in) {
+ return (1.0 / in) / 256.0 /*cpr*/ *
+ ::y2014_bot3::control_loops::kDrivetrainEncoderRatio *
+ (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader {
public:
@@ -72,10 +78,12 @@
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
drivetrain_left_encoder_ = ::std::move(encoder);
+ drivetrain_left_encoder_->SetMaxPeriod(0.005);
}
void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
drivetrain_right_encoder_ = ::std::move(encoder);
+ drivetrain_right_encoder_->SetMaxPeriod(0.005);
}
void operator()() {
@@ -110,6 +118,10 @@
drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
-drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+ drivetrain_message->left_speed =
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ drivetrain_message->right_speed =
+ drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
drivetrain_message.Send();
}