rename drivetrain queues to be consistent with everything else
Change-Id: Id79ada09bc12a53c0bca0e7b7654fb0384db7bd2
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index 0c42c36..bcebc2c 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -54,7 +54,7 @@
'action_client',
'action',
'<(EXTERNALS):eigen',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
],
'export_dependent_settings': [
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 352ec62..66e3d3c 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -44,9 +44,9 @@
// wait until next 10ms tick
::aos::time::PhasedLoop10MS(5000);
- control_loops::drivetrain.status.FetchLatest();
- if (control_loops::drivetrain.status.get()) {
- const auto &status = *control_loops::drivetrain.status;
+ control_loops::drivetrain_queue.status.FetchLatest();
+ if (control_loops::drivetrain_queue.status.get()) {
+ const auto& status = *control_loops::drivetrain_queue.status;
if (::std::abs(status.uncapped_left_voltage -
status.uncapped_right_voltage) >
24) {
@@ -110,7 +110,7 @@
LOG(DEBUG, "Driving left to %f, right to %f\n",
left_goal_state(0, 0) + action_q_->goal->left_initial_position,
right_goal_state(0, 0) + action_q_->goal->right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
//.highgear(false)
.left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
@@ -120,11 +120,11 @@
.Send();
}
if (ShouldCancel()) return;
- control_loops::drivetrain.status.FetchLatest();
- while (!control_loops::drivetrain.status.get()) {
+ control_loops::drivetrain_queue.status.FetchLatest();
+ while (!control_loops::drivetrain_queue.status.get()) {
LOG(WARNING,
"No previous drivetrain status packet, trying to fetch again\n");
- control_loops::drivetrain.status.FetchNextBlocking();
+ control_loops::drivetrain_queue.status.FetchNextBlocking();
if (ShouldCancel()) return;
}
while (true) {
@@ -132,13 +132,13 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- control_loops::drivetrain.status->filtered_left_position -
+ control_loops::drivetrain_queue.status->filtered_left_position -
(left_goal_state(0, 0) + action_q_->goal->left_initial_position));
const double right_error = ::std::abs(
- control_loops::drivetrain.status->filtered_right_position -
+ control_loops::drivetrain_queue.status->filtered_right_position -
(right_goal_state(0, 0) + action_q_->goal->right_initial_position));
const double velocity_error =
- ::std::abs(control_loops::drivetrain.status->robot_speed);
+ ::std::abs(control_loops::drivetrain_queue.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
velocity_error < 0.2) {
break;
@@ -146,7 +146,7 @@
LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
velocity_error);
}
- control_loops::drivetrain.status.FetchNextBlocking();
+ control_loops::drivetrain_queue.status.FetchNextBlocking();
}
LOG(INFO, "Done moving\n");
}
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e1cfddc..aa63eb8 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -34,7 +34,7 @@
void StopDrivetrain() {
LOG(INFO, "Stopping the drivetrain\n");
- control_loops::drivetrain.goal.MakeWithBuilder()
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.left_goal(left_initial_position)
.left_velocity_goal(0)
@@ -46,7 +46,7 @@
void ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
- control_loops::drivetrain.goal.MakeWithBuilder()
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
//.highgear(false)
.steering(0.0)
@@ -83,7 +83,7 @@
LOG(DEBUG, "Driving left to %f, right to %f\n",
left_initial_position - driveTrainState(0, 0),
right_initial_position + driveTrainState(0, 0));
- control_loops::drivetrain.goal.MakeWithBuilder()
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
//.highgear(false)
.left_goal(left_initial_position - driveTrainState(0, 0))
@@ -128,17 +128,16 @@
}
void InitializeEncoders() {
- control_loops::drivetrain.status.FetchLatest();
- while (!control_loops::drivetrain.status.get()) {
+ control_loops::drivetrain_queue.status.FetchLatest();
+ while (!control_loops::drivetrain_queue.status.get()) {
LOG(WARNING,
"No previous drivetrain position packet, trying to fetch again\n");
- control_loops::drivetrain.status.FetchNextBlocking();
+ control_loops::drivetrain_queue.status.FetchNextBlocking();
}
left_initial_position =
- control_loops::drivetrain.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->filtered_left_position;
right_initial_position =
- control_loops::drivetrain.status->filtered_right_position;
-
+ control_loops::drivetrain_queue.status->filtered_right_position;
}
void HandleAuto() {
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 98c1ee8..4dd0320 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -18,7 +18,7 @@
'dependencies': [
'auto_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/util/util.gyp:phased_loop',
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 218ab8c..c88169a 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -194,11 +194,11 @@
// lets just call the average of left and right velocities close enough
return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
}
-
+
double GetEstimatedLeftEncoder() const {
return loop_->X_hat(0, 0);
}
-
+
double GetEstimatedRightEncoder() const {
return loop_->X_hat(2, 0);
}
@@ -207,7 +207,7 @@
return loop_->output_was_capped();
}
- void SendMotors(Drivetrain::Output *output) const {
+ void SendMotors(DrivetrainQueue::Output *output) const {
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -394,7 +394,7 @@
}
}
}
- void SetPosition(const Drivetrain::Position *position) {
+ void SetPosition(const DrivetrainQueue::Position *position) {
const auto &values = constants::GetValues();
if (position == NULL) {
++stale_count_;
@@ -635,7 +635,7 @@
}
}
- void SendMotors(Drivetrain::Output *output) {
+ void SendMotors(DrivetrainQueue::Output *output) {
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -657,8 +657,8 @@
double position_time_delta_;
Gear left_gear_;
Gear right_gear_;
- Drivetrain::Position last_position_;
- Drivetrain::Position position_;
+ DrivetrainQueue::Position last_position_;
+ DrivetrainQueue::Position position_;
int counter_;
};
constexpr double PolyDrivetrain::kStallTorque;
@@ -674,10 +674,10 @@
constexpr double PolyDrivetrain::Kt;
-void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
- const Drivetrain::Position *position,
- Drivetrain::Output *output,
- Drivetrain::Status * status) {
+void DrivetrainLoop::RunIteration(const DrivetrainQueue::Goal *goal,
+ const DrivetrainQueue::Position *position,
+ DrivetrainQueue::Output *output,
+ DrivetrainQueue::Status * status) {
// TODO(aschuh): These should be members of the class.
static DrivetrainMotorsSS dt_closedloop;
static PolyDrivetrain dt_openloop;
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 8205a25..f1b28f3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -1,7 +1,7 @@
{
'targets': [
{
- 'target_name': 'drivetrain_loop',
+ 'target_name': 'drivetrain_queue',
'type': 'static_library',
'sources': ['drivetrain.q'],
'variables': {
@@ -37,7 +37,7 @@
'polydrivetrain_cim_plant.cc',
],
'dependencies': [
- 'drivetrain_loop',
+ 'drivetrain_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/controls/controls.gyp:polytope',
@@ -53,7 +53,7 @@
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(AOS)/common/controls/controls.gyp:control_loop',
- 'drivetrain_loop',
+ 'drivetrain_queue',
],
},
{
@@ -64,7 +64,7 @@
],
'dependencies': [
'<(EXTERNALS):gtest',
- 'drivetrain_loop',
+ 'drivetrain_queue',
'drivetrain_lib',
'<(AOS)/common/controls/controls.gyp:control_loop_test',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
@@ -81,7 +81,7 @@
'dependencies': [
'<(AOS)/linux_code/linux_code.gyp:init',
'drivetrain_lib',
- 'drivetrain_loop',
+ 'drivetrain_queue',
],
},
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f1b0453..decde09 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -13,23 +13,24 @@
namespace control_loops {
class DrivetrainLoop
- : public aos::controls::ControlLoop<control_loops::Drivetrain> {
+ : public aos::controls::ControlLoop<control_loops::DrivetrainQueue> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
- explicit DrivetrainLoop(
- control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::controls::ControlLoop<control_loops::Drivetrain>(my_drivetrain) {
+ explicit DrivetrainLoop(control_loops::DrivetrainQueue *my_drivetrain =
+ &control_loops::drivetrain_queue)
+ : aos::controls::ControlLoop<control_loops::DrivetrainQueue>(
+ my_drivetrain) {
::aos::controls::HPolytope<0>::Init();
}
protected:
// Executes one cycle of the control loop.
virtual void RunIteration(
- const control_loops::Drivetrain::Goal *goal,
- const control_loops::Drivetrain::Position *position,
- control_loops::Drivetrain::Output *output,
- control_loops::Drivetrain::Status *status);
+ const control_loops::DrivetrainQueue::Goal *goal,
+ const control_loops::DrivetrainQueue::Position *position,
+ control_loops::DrivetrainQueue::Output *output,
+ control_loops::DrivetrainQueue::Status *status);
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
SimpleLogInterval no_position_ = SimpleLogInterval(
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 45ced74..ea1324d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -19,7 +19,7 @@
double right_velocity;
};
-queue_group Drivetrain {
+queue_group DrivetrainQueue {
implements aos.control_loops.ControlLoop;
message Goal {
@@ -67,4 +67,4 @@
queue Status status;
};
-queue_group Drivetrain drivetrain;
+queue_group DrivetrainQueue drivetrain_queue;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b148faf..96583da 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -49,7 +49,7 @@
DrivetrainSimulation()
: drivetrain_plant_(
new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
- my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ my_drivetrain_queue_(".frc971.control_loops.drivetrain",
0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
".frc971.control_loops.drivetrain.output",
@@ -76,8 +76,8 @@
const double left_encoder = GetLeftPosition();
const double right_encoder = GetRightPosition();
- ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
- my_drivetrain_loop_.position.MakeMessage();
+ ::aos::ScopedMessagePtr<control_loops::DrivetrainQueue::Position> position =
+ my_drivetrain_queue_.position.MakeMessage();
position->left_encoder = left_encoder;
position->right_encoder = right_encoder;
position.Send();
@@ -87,15 +87,15 @@
void Simulate() {
last_left_position_ = drivetrain_plant_->Y(0, 0);
last_right_position_ = drivetrain_plant_->Y(1, 0);
- EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
- drivetrain_plant_->mutable_U() << my_drivetrain_loop_.output->left_voltage,
- my_drivetrain_loop_.output->right_voltage;
+ EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ drivetrain_plant_->mutable_U() << my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage;
drivetrain_plant_->Update();
}
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
private:
- Drivetrain my_drivetrain_loop_;
+ DrivetrainQueue my_drivetrain_queue_;
double last_left_position_;
double last_right_position_;
};
@@ -105,30 +105,30 @@
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory that
// is no longer valid.
- Drivetrain my_drivetrain_loop_;
+ DrivetrainQueue my_drivetrain_queue_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
- DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ DrivetrainTest() : my_drivetrain_queue_(".frc971.control_loops.drivetrain",
0x8a8dde77,
".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
".frc971.control_loops.drivetrain.output",
".frc971.control_loops.drivetrain.status"),
- drivetrain_motor_(&my_drivetrain_loop_),
+ drivetrain_motor_(&my_drivetrain_queue_),
drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
}
void VerifyNearGoal() {
- my_drivetrain_loop_.goal.FetchLatest();
- my_drivetrain_loop_.position.FetchLatest();
- EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+ my_drivetrain_queue_.goal.FetchLatest();
+ my_drivetrain_queue_.position.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
drivetrain_motor_plant_.GetLeftPosition(),
1e-2);
- EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+ EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
drivetrain_motor_plant_.GetRightPosition(),
1e-2);
}
@@ -140,7 +140,7 @@
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
.left_goal(-1.0)
.right_goal(1.0).Send();
for (int i = 0; i < 200; ++i) {
@@ -154,7 +154,7 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
.left_goal(-1.0)
.right_goal(1.0).Send();
for (int i = 0; i < 500; ++i) {
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index b2f9435..cca8066 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -31,7 +31,7 @@
'<(AOS)/common/util/util.gyp:log_interval',
'<(DEPTH)/frc971/queues/queues.gyp:gyro',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/frc971/actions/actions.gyp:action_client',
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 6d2f1d4..5ca4fa1 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -16,7 +16,7 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/actions/action_client.h"
-using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::drivetrain_queue;
using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
@@ -111,7 +111,7 @@
.Send();
} else if (!data.GetControlBit(ControlBit::kEnabled)) {
{
- auto goal = drivetrain.goal.MakeMessage();
+ auto goal = drivetrain_queue.goal.MakeMessage();
goal->Zero();
goal->control_loop_driving = false;
goal->left_goal = goal->right_goal = 0;
@@ -141,9 +141,10 @@
static double filtered_goal_distance = 0.0;
if (data.PosEdge(kDriveControlLoopEnable1) ||
data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && gyro_reading.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) /
+ if (drivetrain_queue.position.FetchLatest() &&
+ gyro_reading.FetchLatest()) {
+ distance = (drivetrain_queue.position->left_encoder +
+ drivetrain_queue.position->right_encoder) /
2.0 -
throttle * kThrottleGain / 2.0;
angle = gyro_reading->angle;
@@ -171,7 +172,7 @@
LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
}
- if (!drivetrain.goal.MakeWithBuilder()
+ if (!drivetrain_queue.goal.MakeWithBuilder()
.steering(wheel)
.throttle(throttle)
//.highgear(is_high_gear_)
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index fce7e55..699ecc9 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -11,7 +11,7 @@
'<(AOS)/common/common.gyp:stl_mutex',
'<(AOS)/build/aos.gyp:logging',
'<(EXTERNALS):WPILib',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(AOS)/common/controls/controls.gyp:sensor_generation',
'<(AOS)/common/util/util.gyp:log_interval',
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 42d5882..17bf67a 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -43,7 +43,7 @@
#endif
using ::aos::util::SimpleLogInterval;
-using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::drivetrain_queue;
using ::aos::util::WrappingCounter;
namespace frc971 {
@@ -97,7 +97,7 @@
message.Send();
}
- drivetrain.position.MakeWithBuilder()
+ drivetrain_queue.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(right_encoder_->GetRaw()))
.left_encoder(-drivetrain_translate(left_encoder_->GetRaw()))
.battery_voltage(ds->GetBatteryVoltage())
@@ -123,7 +123,7 @@
class SolenoidWriter {
public:
SolenoidWriter(const ::std::unique_ptr<BufferedPcm> &pcm)
- : pcm_(pcm), drivetrain_(".frc971.control_loops.drivetrain.output") {}
+ : pcm_(pcm), drivetrain_(".frc971.control_loops.drivetrain_queue.output") {}
void set_drivetrain_left(::std::unique_ptr<BufferedSolenoid> s) {
drivetrain_left_ = ::std::move(s);
@@ -160,7 +160,7 @@
::std::unique_ptr<BufferedSolenoid> drivetrain_left_;
::std::unique_ptr<BufferedSolenoid> drivetrain_right_;
- ::aos::Queue<::frc971::control_loops::Drivetrain::Output> drivetrain_;
+ ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
::std::atomic<bool> run_{true};
};
@@ -177,11 +177,11 @@
private:
virtual void Read() override {
- ::frc971::control_loops::drivetrain.output.FetchAnother();
+ ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
}
virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain.output;
+ auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
left_drivetrain_talon_->Set(-queue->left_voltage / 12.0);
right_drivetrain_talon_->Set(queue->right_voltage / 12.0);