updated drivetrain_lib_test and got rid of a confusing+buggy variable
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e755f3d..ca9c3c7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -136,8 +136,7 @@
left_goal_(0.0),
right_goal_(0.0),
raw_left_(0.0),
- raw_right_(0.0),
- control_loop_driving_(false) {
+ raw_right_(0.0) {
// Low gear on both.
loop_->set_controller_index(0);
}
@@ -155,14 +154,12 @@
Y << left + filtered_offset_, right - filtered_offset_;
loop_->Correct(Y);
}
- void SetPosition(
- double left, double right, double gyro, bool control_loop_driving) {
+ void SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
const double offset =
(right - left - gyro * constants::GetValues().turn_width) / 2.0;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
- control_loop_driving_ = control_loop_driving;
SetRawPosition(left, right);
}
@@ -170,8 +167,8 @@
loop_->U << left_voltage, right_voltage;
}
- void Update(bool stop_motors) {
- if (control_loop_driving_) {
+ void Update(bool stop_motors, bool enable_control_loop) {
+ if (enable_control_loop) {
loop_->Update(stop_motors);
} else {
if (stop_motors) {
@@ -221,7 +218,6 @@
double right_goal_;
double raw_left_;
double raw_right_;
- bool control_loop_driving_;
};
class PolyDrivetrain {
@@ -685,7 +681,7 @@
if (gyro_reading.FetchLatest()) {
LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
dt_closedloop.SetPosition(left_encoder, right_encoder,
- gyro_reading->angle, control_loop_driving);
+ gyro_reading->angle);
} else {
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
}
@@ -695,7 +691,7 @@
dt_openloop.Update();
if (control_loop_driving) {
- dt_closedloop.Update(output == NULL);
+ dt_closedloop.Update(output == NULL, true);
dt_closedloop.SendMotors(output);
} else {
dt_openloop.SendMotors(output);
@@ -703,10 +699,10 @@
dt_closedloop.SetExternalMotors(output->left_voltage,
output->right_voltage);
}
- dt_closedloop.Update(output == NULL);
+ dt_closedloop.Update(output == NULL, false);
}
- // set the output status of the controll loop state
+ // set the output status of the control loop state
if (status) {
bool done = false;
if (goal) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index bc52ab4..3854fe1 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -66,7 +66,7 @@
'<(EXTERNALS):gtest',
'drivetrain_loop',
'drivetrain_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/common.gyp:queues',
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 2d510f6..1983caa 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -4,9 +4,9 @@
#include "gtest/gtest.h"
#include "aos/common/network/team_number.h"
-#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
#include "aos/common/controls/polytope.h"
+#include "aos/common/controls/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
@@ -16,8 +16,6 @@
#include "frc971/queues/other_sensors.q.h"
-using ::aos::time::Time;
-
namespace frc971 {
namespace control_loops {
namespace testing {
@@ -105,10 +103,8 @@
double last_right_position_;
};
-class DrivetrainTest : public ::testing::Test {
+class DrivetrainTest : public ::aos::testing::ControlLoopTest {
protected:
- ::aos::common::testing::GlobalCoreInstance my_core;
-
// 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.
@@ -126,23 +122,7 @@
".frc971.control_loops.drivetrain.status"),
drivetrain_motor_(&my_drivetrain_loop_),
drivetrain_motor_plant_() {
- // Flush the robot state queue so we can use clean shared memory for this
- // test, also for the gyro.
- ::aos::robot_state.Clear();
- ::aos::controls::sensor_generation.Clear();
- ::aos::controls::sensor_generation.MakeWithBuilder()
- .reader_pid(254)
- .cape_resets(5)
- .Send();
::frc971::sensors::gyro_reading.Clear();
- SendDSPacket(true);
- }
-
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
- ::aos::robot_state.FetchLatest();
}
void VerifyNearGoal() {
@@ -157,9 +137,7 @@
}
virtual ~DrivetrainTest() {
- ::aos::robot_state.Clear();
::frc971::sensors::gyro_reading.Clear();
- ::aos::controls::sensor_generation.Clear();
}
};
@@ -172,7 +150,7 @@
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
VerifyNearGoal();
}
@@ -187,9 +165,9 @@
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
if (i > 20 && i < 200) {
- SendDSPacket(false);
+ SimulateTimestep(false);
} else {
- SendDSPacket(true);
+ SimulateTimestep(true);
}
}
VerifyNearGoal();
@@ -207,7 +185,7 @@
}
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
VerifyNearGoal();
}