Convert drivetrain mode from a bool to a number
This lets us support more than 2 drivetrain controller types in
perparation for splines.
Change-Id: I943d6073e6c5facae7223cc6111551a57b039a04
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 523c9b3..62f31fa 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -242,9 +242,9 @@
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
- bool control_loop_driving = false;
+ int controller_type = 0;
if (goal) {
- control_loop_driving = goal->control_loop_driving;
+ controller_type = goal->controller_type;
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
@@ -252,13 +252,15 @@
dt_openloop_.Update();
- if (control_loop_driving) {
- dt_closedloop_.Update(output != NULL);
- dt_closedloop_.SetOutput(output);
- } else {
- dt_openloop_.SetOutput(output);
- // TODO(austin): Set profile to current spot.
- dt_closedloop_.Update(false);
+ dt_closedloop_.Update(output != NULL && controller_type == 1);
+
+ switch (controller_type) {
+ case 0:
+ dt_openloop_.SetOutput(output);
+ break;
+ case 1:
+ dt_closedloop_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index ed659c3..7fb859d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -55,8 +55,11 @@
// True to activate quickturn.
bool quickturn;
- // True to have the closed-loop controller take over.
- bool control_loop_driving;
+ // Type of controller in charge of the drivetrain.
+ // 0: polydrive
+ // 1: motion profiled position drive (statespace)
+ // 2: spline follower.
+ uint8_t controller_type;
// Position goals for each drivetrain side (in meters) when the
// closed-loop controller is active.
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index f6d5aa2..49022f6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -261,7 +261,7 @@
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.left_goal(-1.0)
.right_goal(1.0)
.Send();
@@ -273,7 +273,7 @@
// voltage offset/disturbance.
TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.left_goal(-1.0)
.right_goal(1.0)
.Send();
@@ -286,7 +286,7 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.left_goal(-1.0)
.right_goal(1.0)
.Send();
@@ -322,7 +322,7 @@
// This used to not work due to a U-capping bug.
TEST_F(DrivetrainTest, DriveStraightForward) {
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.left_goal(4.0)
.right_goal(4.0)
.Send();
@@ -341,7 +341,7 @@
// This used to fail in simulation due to libcdd issues with U-capping.
TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.left_goal(4.0)
.right_goal(3.9)
.Send();
@@ -385,7 +385,7 @@
{
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->control_loop_driving = true;
+ goal->controller_type = 1;
goal->left_goal = 4.0;
goal->right_goal = 4.0;
goal->left_velocity_goal = 0.0;
@@ -416,7 +416,7 @@
{
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->control_loop_driving = true;
+ goal->controller_type = 1;
goal->left_goal = -1.0;
goal->right_goal = 1.0;
goal->left_velocity_goal = 0.0;
@@ -447,7 +447,7 @@
{
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->control_loop_driving = true;
+ goal->controller_type = 1;
goal->left_goal = 5.0;
goal->right_goal = 4.0;
goal->left_velocity_goal = 0.0;
@@ -471,7 +471,7 @@
// drive profiles nicely.
TEST_F(DrivetrainTest, OpenLoopThenClosed) {
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(false)
+ .controller_type(0)
.wheel(0.0)
.throttle(1.0)
.highgear(true)
@@ -481,7 +481,7 @@
RunForTime(chrono::seconds(1));
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(false)
+ .controller_type(0)
.wheel(0.0)
.throttle(-0.3)
.highgear(true)
@@ -491,7 +491,7 @@
RunForTime(chrono::seconds(1));
my_drivetrain_queue_.goal.MakeWithBuilder()
- .control_loop_driving(false)
+ .controller_type(0)
.wheel(0.0)
.throttle(0.0)
.highgear(true)
@@ -503,7 +503,7 @@
{
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->control_loop_driving = true;
+ goal->controller_type = 1;
goal->left_goal = 5.0;
goal->right_goal = 4.0;
goal->left_velocity_goal = 0.0;