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/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 23bb8c1..1ba8bf8 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -61,7 +61,7 @@
new_drivetrain_goal->throttle_torque = throttle_torque;
new_drivetrain_goal->highgear = high_gear;
new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
- new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
+ new_drivetrain_goal->controller_type = is_control_loop_driving ? 1 : 0;
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
new_drivetrain_goal->left_velocity_goal = 0;
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index df558ab..04ee334 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -29,7 +29,7 @@
LOG(INFO, "resetting the drivetrain\n");
max_drivetrain_voltage_ = 12.0;
drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(false)
+ .controller_type(0)
.highgear(true)
.wheel(0.0)
.throttle(0.0)
@@ -58,7 +58,7 @@
}
auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
- drivetrain_message->control_loop_driving = true;
+ drivetrain_message->controller_type = 1;
drivetrain_message->highgear = true;
drivetrain_message->wheel = 0.0;
drivetrain_message->throttle = 0.0;
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;
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 6aec414..9bf98a2 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -125,7 +125,7 @@
left_goal_state(0, 0) + params.left_initial_position,
right_goal_state(0, 0) + params.right_initial_position);
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.highgear(true)
.left_goal(left_goal_state(0, 0) + params.left_initial_position)
.right_goal(right_goal_state(0, 0) + params.right_initial_position)
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index e5bd9bc..3ed2cd1 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -52,7 +52,7 @@
void StopDrivetrain() {
LOG(INFO, "Stopping the drivetrain\n");
frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.highgear(true)
.left_goal(left_initial_position)
.left_velocity_goal(0)
@@ -65,7 +65,7 @@
void ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(false)
+ .controller_type(0)
.highgear(true)
.wheel(0.0)
.throttle(0.0)
@@ -99,7 +99,7 @@
double right_goal = (right_initial_position + distance +
theta * control_loops::drivetrain::kRobotRadius);
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
+ .controller_type(1)
.highgear(true)
.left_goal(left_goal)
.right_goal(right_goal)
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index bc13be2..72c76ef 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -229,7 +229,7 @@
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
+ .controller_type(is_control_loop_driving ? 1 : 0)
.left_goal(left_goal)
.right_goal(right_goal)
.left_velocity_goal(0)
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index f921c8e..0716a97 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -64,7 +64,7 @@
.throttle(0.0)
.highgear(false)
.quickturn(false)
- .control_loop_driving(true)
+ .controller_type(1)
.left_goal(left_current + side_distance_change)
.right_goal(right_current - side_distance_change)
.left_velocity_goal(0)
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 99bfb68..8f99e4c 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -163,7 +163,7 @@
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
+ .controller_type(is_control_loop_driving ? 1 : 0)
.left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
.right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
.left_velocity_goal(0)