Merge "Disable stupid WPILib messages about joysticks"
diff --git a/debian/usr.BUILD b/debian/usr.BUILD
index ac354cc..c66b30d 100644
--- a/debian/usr.BUILD
+++ b/debian/usr.BUILD
@@ -44,6 +44,7 @@
],
copts = [
'-Wno-error',
+ '-Wno-parentheses-equality',
],
includes = [
'lib/python3/dist-packages/numpy/f2py/src/',
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
index 658b064..310b3e3 100644
--- a/frc971/wpilib/loop_output_handler.cc
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -15,8 +15,8 @@
: watchdog_(this, timeout) {}
void LoopOutputHandler::operator()() {
- ::aos::SetCurrentThreadName("OutputHandler");
::std::thread watchdog_thread(::std::ref(watchdog_));
+ ::aos::SetCurrentThreadName("OutputHandler");
::aos::SetCurrentThreadRealtimePriority(30);
while (run_) {
@@ -51,11 +51,11 @@
if (timerfd_.get() == -1) {
PLOG(FATAL, "timerfd_create(Time::kDefaultClock, 0)");
}
- ::aos::SetCurrentThreadRealtimePriority(35);
- ::aos::SetCurrentThreadName("OutputWatchdog");
}
void LoopOutputHandler::Watchdog::operator()() {
+ ::aos::SetCurrentThreadRealtimePriority(35);
+ ::aos::SetCurrentThreadName("OutputWatchdog");
uint8_t buf[8];
while (run_) {
PCHECK(read(timerfd_.get(), buf, sizeof(buf)));
diff --git a/tools/build_rules/fortran.bzl b/tools/build_rules/fortran.bzl
index c67884a..ff940af 100644
--- a/tools/build_rules/fortran.bzl
+++ b/tools/build_rules/fortran.bzl
@@ -18,6 +18,7 @@
'-Werror',
'-Wextra',
'-Wno-builtin-macro-redefined',
+ '-Wunused-local-typedefs',
'-D__has_feature(x)=0',
'-fmacro-backtrace-limit=0']
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/control_loops/drivetrain/replay_drivetrain.cc b/y2014/control_loops/drivetrain/replay_drivetrain.cc
index 346ca69..b0ac33f 100644
--- a/y2014/control_loops/drivetrain/replay_drivetrain.cc
+++ b/y2014/control_loops/drivetrain/replay_drivetrain.cc
@@ -15,14 +15,22 @@
::aos::InitNRT();
- ::aos::controls::ControlLoopReplayer<::y2014::control_loops::DrivetrainQueue>
- replayer(&::y2014::control_loops::drivetrain_queue, "drivetrain");
+ {
+ ::aos::controls::ControlLoopReplayer<
+ ::y2014::control_loops::DrivetrainQueue>
+ replayer(&::y2014::control_loops::drivetrain_queue, "drivetrain");
- replayer.AddDirectQueueSender("wpilib_interface.Gyro", "sending",
- ::frc971::sensors::gyro_reading);
- for (int i = 1; i < argc; ++i) {
- replayer.ProcessFile(argv[i]);
+ replayer.AddDirectQueueSender("wpilib_interface.Gyro", "sending",
+ ::frc971::sensors::gyro_reading);
+ for (int i = 1; i < argc; ++i) {
+ replayer.ProcessFile(argv[i]);
+ }
}
+ ::frc971::sensors::gyro_reading.Clear();
+ ::y2014::control_loops::drivetrain_queue.goal.Clear();
+ ::y2014::control_loops::drivetrain_queue.status.Clear();
+ ::y2014::control_loops::drivetrain_queue.position.Clear();
+ ::y2014::control_loops::drivetrain_queue.output.Clear();
::aos::Cleanup();
}
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 855905c..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) {
@@ -239,7 +247,6 @@
void operator()() {
::aos::SetCurrentThreadName("SensorReader");
- LOG(INFO, "In sensor reader thread\n");
my_pid_ = getpid();
ds_ =
@@ -253,7 +260,6 @@
top_reader_.Start();
bottom_reader_.Start();
dma_synchronizer_->Start();
- LOG(INFO, "Things are now started\n");
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
@@ -276,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();
@@ -682,7 +692,6 @@
::std::thread joystick_thread(::std::ref(joystick_sender));
SensorReader reader;
- LOG(INFO, "Creating the reader\n");
// Create this first to make sure it ends up in one of the lower-numbered
// FPGA slots so we can use it with DMA.
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 5dcb838..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,15 +78,16 @@
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()() {
::aos::SetCurrentThreadName("SensorReader");
- LOG(INFO, "In sensor reader thread\n");
my_pid_ = getpid();
ds_ =
@@ -91,8 +98,6 @@
#endif
pdp_.reset(new PowerDistributionPanel());
- LOG(INFO, "Things are now started\n");
-
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
::aos::time::PhasedLoopXMS(5, 4000);
@@ -110,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();
}
@@ -339,7 +348,6 @@
// Sensors
SensorReader reader;
- LOG(INFO, "Creating the reader\n");
reader.set_drivetrain_left_encoder(make_encoder(4));
reader.set_drivetrain_right_encoder(make_encoder(5));
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 fa7d14a..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
@@ -220,7 +228,6 @@
}
void operator()() {
- LOG(INFO, "In sensor reader thread\n");
::aos::SetCurrentThreadName("SensorReader");
my_pid_ = getpid();
@@ -234,7 +241,6 @@
wrist_encoder_.Start();
dma_synchronizer_->Start();
- LOG(INFO, "Things are now started\n");
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
@@ -254,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();
}
@@ -638,7 +648,6 @@
// TODO(austin): Compressor needs to use a spike.
SensorReader reader;
- LOG(INFO, "Creating the reader\n");
reader.set_arm_left_encoder(encoder(1));
reader.set_arm_left_index(make_unique<DigitalInput>(1));
reader.set_arm_left_potentiometer(make_unique<AnalogInput>(1));
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 554e0a6..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.
@@ -119,7 +127,6 @@
}
void operator()() {
- LOG(INFO, "In sensor reader thread\n");
::aos::SetCurrentThreadName("SensorReader");
my_pid_ = getpid();
@@ -131,8 +138,6 @@
#endif
pdp_.reset(new PowerDistributionPanel());
- LOG(INFO, "Things are now started\n");
-
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
::aos::time::PhasedLoopXMS(5, 4000);
@@ -150,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();
}
@@ -455,7 +464,6 @@
::std::thread joystick_thread(::std::ref(joystick_sender));
SensorReader reader;
- LOG(INFO, "Creating the reader\n");
reader.set_elevator_encoder(encoder(6));
reader.set_elevator_zeroing_hall_effect(make_unique<DigitalInput>(6));