Merge "Log the pulse widths from the drivetrain encoders"
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 50dee34..0278ebc 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) {
@@ -274,6 +282,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();
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index cef50d0..f5f53bb 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -2,12 +2,23 @@
import "aos/common/controls/control_loops.q";
+// 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;
};
@@ -15,38 +26,72 @@
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;
};
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 right 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_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index 9632160..4fde0f5 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()() {
@@ -107,6 +115,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();
}
diff --git a/y2015/control_loops/drivetrain/drivetrain.q b/y2015/control_loops/drivetrain/drivetrain.q
index 0d804cb..824688c 100644
--- a/y2015/control_loops/drivetrain/drivetrain.q
+++ b/y2015/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,40 +40,76 @@
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;
};
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 right 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/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index 05ef948..0ea4b8d 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
@@ -252,6 +260,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();
}
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.q b/y2015_bot3/control_loops/drivetrain/drivetrain.q
index 977f79e..7ff0498 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2015_bot3/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,40 +40,76 @@
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;
};
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 right 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/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index 1eeac1f..6b5c3db 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.
@@ -147,6 +155,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();
}