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/control_loops/drivetrain/drivetrain.q b/y2014/control_loops/drivetrain/drivetrain.q
index ddc2efc..96bd817 100644
--- a/y2014/control_loops/drivetrain/drivetrain.q
+++ b/y2014/control_loops/drivetrain/drivetrain.q
@@ -2,20 +2,37 @@
import "aos/common/controls/control_loops.q";
+// For logging information about what the code is doing with the shifters.
struct GearLogging {
+ // Which controller is being used.
int8_t controller_index;
+ // Whether the left loop is the high-gear one.
bool left_loop_high;
+ // Whether the right loop is the high-gear one.
bool right_loop_high;
+ // The state of the left shifter.
int8_t left_state;
+ // The state of the right shifter.
int8_t right_state;
};
+// For logging information about the state of the shifters.
struct CIMLogging {
+ // Whether the code thinks the left side is currently in gear.
bool left_in_gear;
+ // Whether the code thinks the right side is currently in gear.
bool right_in_gear;
+ // The velocity in rad/s (positive forward) the code thinks the left motor
+ // is currently spinning at.
double left_motor_speed;
+ // The velocity in rad/s (positive forward) the code thinks the right motor
+ // is currently spinning at.
double right_motor_speed;
+ // The velocity estimate for the left side of the robot in m/s (positive
+ // forward) used for shifting.
double left_velocity;
+ // The velocity estimate for the right side of the robot in m/s (positive
+ // forward) used for shifting.
double right_velocity;
};
@@ -23,21 +40,45 @@
implements aos.control_loops.ControlLoop;
message Goal {
+ // Position of the steering wheel (positive = turning left when going
+ // forwards).
double steering;
+ // Position of the throttle (positive forwards).
double throttle;
+ // True to shift into high, false to shift into low.
bool highgear;
+ // True to activate quickturn.
bool quickturn;
+ // True to have the closed-loop controller take over.
bool control_loop_driving;
+ // Position goal for the left side in meters when the closed-loop controller
+ // is active.
double left_goal;
+ // Velocity goal for the left side in m/s when the closed-loop controller
+ // is active.
double left_velocity_goal;
+ // Position goal for the right side in meters when the closed-loop
+ // controller is active.
double right_goal;
+ // Velocity goal for the right side in m/s when the closed-loop controller
+ // is active.
double right_velocity_goal;
};
message Position {
+ // Relative position of the left side in meters.
double left_encoder;
+ // Relative position of the right side in meters.
double right_encoder;
+ // The speed in m/s of the left side from the most recent encoder pulse,
+ // or 0 if there was no edge within the last 5ms.
+ double left_speed;
+ // The speed in m/s of the right side from the most recent encoder pulse,
+ // or 0 if there was no edge within the last 5ms.
+ double right_speed;
+ // Position of the left shifter (smaller = towards low gear).
double left_shifter_position;
+ // Position of the right shifter (smaller = towards low gear).
double right_shifter_position;
double low_left_hall;
double high_left_hall;
@@ -46,21 +87,33 @@
};
message Output {
+ // Voltage to send to the left motor(s).
double left_voltage;
+ // Voltage to send to the right motor(s).
double right_voltage;
+ // True to set the left shifter piston for high gear.
bool left_high;
+ // True to set the right shifter piston for high gear.
bool right_high;
};
message Status {
+ // Estimated speed of the center of the robot in m/s (positive forwards).
double robot_speed;
+ // Estimated relative position of the left side in meters.
double filtered_left_position;
+ // Estimated relative position of the right side in meters.
double filtered_right_position;
+ // Estimated velocity of the left side in m/s.
double filtered_left_velocity;
+ // Estimated velocity of the left side in m/s.
double filtered_right_velocity;
+ // The voltage we wanted to send to the left side last cycle.
double uncapped_left_voltage;
+ // The voltage we wanted to send to the right side last cycle.
double uncapped_right_voltage;
+ // True if the output voltage was capped last cycle.
bool output_was_capped;
};
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 855905c..8c37854 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -79,6 +79,12 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
+double drivetrain_velocity_translate(double in) {
+ return (1.0 / in) / 256.0 /*cpr*/ *
+ constants::GetValues().drivetrain_encoder_ratio *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
+}
+
float hall_translate(const constants::ShifterHallEffect &k, float in_low,
float in_high) {
const float low_ratio =
@@ -130,10 +136,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 set_high_left_drive_hall(::std::unique_ptr<AnalogInput> analog) {
@@ -276,6 +284,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->low_left_hall = low_left_drive_hall_->GetVoltage();
drivetrain_message->high_left_hall = high_left_drive_hall_->GetVoltage();