Drive code works on Tantrum.
Need to write the spring code. Drive now supports doubles... What a
pain.
Change-Id: Id589acdc443dcd81242a21e3b0c26f81d6974dc8
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 915f63d..90385e0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,221 +1,256 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools:environments.bzl", "mcu_cpus")
cc_binary(
- name = 'replay_drivetrain',
- srcs = [
- 'replay_drivetrain.cc',
- ],
- deps = [
- ':drivetrain_queue',
- '//aos/common/controls:replay_control_loop',
- '//aos/linux_code:init',
- '//frc971/queues:gyro',
- ],
+ name = "replay_drivetrain",
+ srcs = [
+ "replay_drivetrain.cc",
+ ],
+ deps = [
+ ":drivetrain_queue",
+ "//aos/common/controls:replay_control_loop",
+ "//aos/linux_code:init",
+ "//frc971/queues:gyro",
+ ],
)
queue_library(
- name = 'drivetrain_queue',
- srcs = [
- 'drivetrain.q',
- ],
- deps = [
- '//aos/common/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "drivetrain_queue",
+ srcs = [
+ "drivetrain.q",
+ ],
+ deps = [
+ "//aos/common/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
cc_library(
- name = 'drivetrain_config',
- hdrs = [
- 'drivetrain_config.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- '//frc971:shifter_hall_effect',
- ],
+ name = "drivetrain_config",
+ hdrs = [
+ "drivetrain_config.h",
+ ],
+ deps = [
+ "//frc971:shifter_hall_effect",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'gear',
- hdrs = [
- 'gear.h',
- ],
+ name = "gear",
+ hdrs = [
+ "gear.h",
+ ],
+ compatible_with = mcu_cpus,
)
cc_library(
- name = 'ssdrivetrain',
- srcs = [
- 'ssdrivetrain.cc',
- ],
- hdrs = [
- 'ssdrivetrain.h',
- ],
- deps = [
- ':drivetrain_queue',
- ':drivetrain_config',
- ':gear',
- '//aos/common/controls:control_loop',
- '//aos/common/controls:polytope',
- '//aos/common/logging:matrix_logging',
- '//aos/common/logging:queue_logging',
- '//aos/common/messages:robot_state',
- '//aos/common/util:log_interval',
- '//aos/common/util:trapezoid_profile',
- '//aos/common:math',
- '//frc971/control_loops:coerce_goal',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971:shifter_hall_effect',
- ],
+ name = "ssdrivetrain",
+ srcs = [
+ "ssdrivetrain.cc",
+ ],
+ hdrs = [
+ "ssdrivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":gear",
+ "//aos/common:math",
+ "//aos/common/controls:control_loop",
+ "//aos/common/controls:polytope",
+ "//aos/common/logging:matrix_logging",
+ "//aos/common/logging:queue_logging",
+ "//aos/common/messages:robot_state",
+ "//aos/common/util:log_interval",
+ "//aos/common/util:trapezoid_profile",
+ "//frc971:shifter_hall_effect",
+ "//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'polydrivetrain',
- srcs = [
- 'polydrivetrain.cc',
- ],
- hdrs = [
- 'polydrivetrain.h',
- ],
- deps = [
- ':drivetrain_queue',
- ':drivetrain_config',
- ':gear',
- '//aos/common/controls:polytope',
- '//aos/common:math',
- '//aos/common/messages:robot_state',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:coerce_goal',
- '//aos/common/util:log_interval',
- '//aos/common/logging:queue_logging',
- '//aos/common/logging:matrix_logging',
- ],
+ name = "polydrivetrain",
+ srcs = [
+ "polydrivetrain.cc",
+ ],
+ hdrs = [
+ "polydrivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":gear",
+ "//aos/common:math",
+ "//aos/common/controls:polytope",
+ "//aos/common/logging:matrix_logging",
+ "//aos/common/logging:queue_logging",
+ "//aos/common/messages:robot_state",
+ "//aos/common/util:log_interval",
+ "//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "drivetrain_config_uc",
+ hdrs = [
+ "drivetrain_config.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//frc971:shifter_hall_effect",
+ "//frc971/control_loops:state_feedback_loop_uc",
+ ],
+)
+
+cc_library(
+ name = "polydrivetrain_uc",
+ srcs = [
+ "drivetrain_uc.q.cc",
+ "polydrivetrain.cc",
+ ],
+ hdrs = [
+ "drivetrain_uc.q.h",
+ "polydrivetrain.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":drivetrain_config_uc",
+ ":gear",
+ "//aos/common:math",
+ "//aos/common/controls:polytope_uc",
+ "//frc971/control_loops:coerce_goal_uc",
+ "//frc971/control_loops:state_feedback_loop_uc",
+ ],
)
genrule(
- name = 'genrule_down_estimator',
- visibility = ['//visibility:private'],
- cmd = '$(location //frc971/control_loops/python:down_estimator) $(OUTS)',
- tools = [
- '//frc971/control_loops/python:down_estimator',
- ],
- outs = [
- 'down_estimator.h',
- 'down_estimator.cc',
- ],
+ name = "genrule_down_estimator",
+ outs = [
+ "down_estimator.h",
+ "down_estimator.cc",
+ ],
+ cmd = "$(location //frc971/control_loops/python:down_estimator) $(OUTS)",
+ tools = [
+ "//frc971/control_loops/python:down_estimator",
+ ],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'down_estimator',
- hdrs = [
- 'down_estimator.h',
- ],
- srcs = [
- 'down_estimator.cc',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "down_estimator",
+ srcs = [
+ "down_estimator.cc",
+ ],
+ hdrs = [
+ "down_estimator.h",
+ ],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'drivetrain_lib',
- srcs = [
- 'drivetrain.cc',
- ],
- hdrs = [
- 'drivetrain.h',
- ],
- deps = [
- ':down_estimator',
- ':drivetrain_queue',
- ':gear',
- ':polydrivetrain',
- ':ssdrivetrain',
- '//aos/common/controls:control_loop',
- '//frc971/queues:gyro',
- '//frc971/wpilib:imu_queue',
- '//aos/common/util:log_interval',
- '//aos/common/logging:queue_logging',
- '//aos/common/logging:matrix_logging',
- ],
+ name = "drivetrain_lib",
+ srcs = [
+ "drivetrain.cc",
+ ],
+ hdrs = [
+ "drivetrain.h",
+ ],
+ deps = [
+ ":down_estimator",
+ ":drivetrain_queue",
+ ":gear",
+ ":polydrivetrain",
+ ":ssdrivetrain",
+ "//aos/common/controls:control_loop",
+ "//aos/common/logging:matrix_logging",
+ "//aos/common/logging:queue_logging",
+ "//aos/common/util:log_interval",
+ "//frc971/queues:gyro",
+ "//frc971/wpilib:imu_queue",
+ ],
)
cc_test(
- name = 'drivetrain_lib_test',
- srcs = [
- 'drivetrain_lib_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':drivetrain_queue',
- ':drivetrain_lib',
- ':drivetrain_config',
- '//aos/common/controls:control_loop_test',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/queues:gyro',
- '//aos/common:queues',
- '//y2016:constants',
- '//y2016/control_loops/drivetrain:polydrivetrain_plants',
- ],
+ name = "drivetrain_lib_test",
+ srcs = [
+ "drivetrain_lib_test.cc",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_lib",
+ ":drivetrain_queue",
+ "//aos/common:queues",
+ "//aos/common/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/queues:gyro",
+ "//y2016:constants",
+ "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+ ],
)
genrule(
- name = 'genrule_haptic_wheel',
- visibility = ['//visibility:private'],
- cmd = '$(location //frc971/control_loops/python:haptic_wheel) $(OUTS)',
- tools = [
- '//frc971/control_loops/python:haptic_wheel',
- ],
- outs = [
- 'haptic_wheel.h',
- 'haptic_wheel.cc',
- 'integral_haptic_wheel.h',
- 'integral_haptic_wheel.cc',
- 'haptic_trigger.h',
- 'haptic_trigger.cc',
- 'integral_haptic_trigger.h',
- 'integral_haptic_trigger.cc',
- ],
- restricted_to = ["//tools:k8", "//tools:roborio", "//tools:armhf-debian", "//tools:cortex-m4f"],
+ name = "genrule_haptic_wheel",
+ outs = [
+ "haptic_wheel.h",
+ "haptic_wheel.cc",
+ "integral_haptic_wheel.h",
+ "integral_haptic_wheel.cc",
+ "haptic_trigger.h",
+ "haptic_trigger.cc",
+ "integral_haptic_trigger.h",
+ "integral_haptic_trigger.cc",
+ ],
+ cmd = "$(location //frc971/control_loops/python:haptic_wheel) $(OUTS)",
+ compatible_with = mcu_cpus,
+ tools = [
+ "//frc971/control_loops/python:haptic_wheel",
+ ],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'haptic_input_uc',
- hdrs = [
- 'haptic_wheel.h',
- 'integral_haptic_wheel.h',
- 'haptic_trigger.h',
- 'integral_haptic_trigger.h',
- ],
- srcs = [
- 'haptic_wheel.cc',
- 'integral_haptic_wheel.cc',
- 'haptic_trigger.cc',
- 'integral_haptic_trigger.cc',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop_uc',
- ],
- restricted_to = ["//tools:cortex-m4f"],
+ name = "haptic_input_uc",
+ srcs = [
+ "haptic_trigger.cc",
+ "haptic_wheel.cc",
+ "integral_haptic_trigger.cc",
+ "integral_haptic_wheel.cc",
+ ],
+ hdrs = [
+ "haptic_trigger.h",
+ "haptic_wheel.h",
+ "integral_haptic_trigger.h",
+ "integral_haptic_wheel.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//frc971/control_loops:state_feedback_loop_uc",
+ ],
)
cc_library(
- name = 'haptic_wheel',
- hdrs = [
- 'haptic_wheel.h',
- 'integral_haptic_wheel.h',
- 'haptic_trigger.h',
- 'integral_haptic_trigger.h',
- ],
- srcs = [
- 'haptic_wheel.cc',
- 'integral_haptic_wheel.cc',
- 'haptic_trigger.cc',
- 'integral_haptic_trigger.cc',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "haptic_wheel",
+ srcs = [
+ "haptic_trigger.cc",
+ "haptic_wheel.cc",
+ "integral_haptic_trigger.cc",
+ "integral_haptic_wheel.cc",
+ ],
+ hdrs = [
+ "haptic_trigger.h",
+ "haptic_wheel.h",
+ "integral_haptic_trigger.h",
+ "integral_haptic_wheel.h",
+ ],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b4a23c3..14c1a42 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -29,7 +29,7 @@
namespace drivetrain {
DrivetrainLoop::DrivetrainLoop(
- const DrivetrainConfig &dt_config,
+ const DrivetrainConfig<double> &dt_config,
::frc971::control_loops::DrivetrainQueue *my_drivetrain)
: aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
my_drivetrain),
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index fd3f076..83e6c52 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -22,7 +22,7 @@
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
- const DrivetrainConfig &dt_config,
+ const DrivetrainConfig<double> &dt_config,
::frc971::control_loops::DrivetrainQueue *my_drivetrain =
&::frc971::control_loops::drivetrain_queue);
@@ -40,10 +40,10 @@
double last_gyro_rate_ = 0.0;
- const DrivetrainConfig dt_config_;
+ const DrivetrainConfig<double> dt_config_;
StateFeedbackLoop<7, 2, 4> kf_;
- PolyDrivetrain dt_openloop_;
+ PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index c2c876e..6e546a3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -34,6 +34,7 @@
IMU_Y = 1, // Use the y-axis of the IMU.
};
+template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
ShifterType shifter_type;
@@ -48,18 +49,18 @@
IMUType imu_type;
// Polydrivetrain functions returning various controller loops with plants.
- ::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
- ::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
- ::std::function<StateFeedbackLoop<7, 2, 4>()> make_kf_drivetrain_loop;
+ ::std::function<StateFeedbackLoop<4, 2, 2, Scalar>()> make_drivetrain_loop;
+ ::std::function<StateFeedbackLoop<2, 2, 2, Scalar>()> make_v_drivetrain_loop;
+ ::std::function<StateFeedbackLoop<7, 2, 4, Scalar>()> make_kf_drivetrain_loop;
- double dt; // Control loop time step.
- double robot_radius; // Robot radius, in meters.
- double wheel_radius; // Wheel radius, in meters.
- double v; // Motor velocity constant.
+ Scalar dt; // Control loop time step.
+ Scalar robot_radius; // Robot radius, in meters.
+ Scalar wheel_radius; // Wheel radius, in meters.
+ Scalar v; // Motor velocity constant.
// Gear ratios, from wheel to motor shaft.
- double high_gear_ratio;
- double low_gear_ratio;
+ Scalar high_gear_ratio;
+ Scalar low_gear_ratio;
// Hall effect constants. Unused if not applicable to shifter type.
constants::ShifterHallEffect left_drive;
@@ -69,26 +70,26 @@
// (ie. true means high gear is default).
bool default_high_gear;
- double down_offset;
+ Scalar down_offset;
- double wheel_non_linearity;
+ Scalar wheel_non_linearity;
- double quickturn_wheel_multiplier;
+ Scalar quickturn_wheel_multiplier;
- double wheel_multiplier;
+ Scalar wheel_multiplier;
// Converts the robot state to a linear distance position, velocity.
- static Eigen::Matrix<double, 2, 1> LeftRightToLinear(
- const Eigen::Matrix<double, 7, 1> &left_right) {
- Eigen::Matrix<double, 2, 1> linear;
+ static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
+ const Eigen::Matrix<Scalar, 7, 1> &left_right) {
+ Eigen::Matrix<Scalar, 2, 1> linear;
linear(0, 0) = (left_right(0, 0) + left_right(2, 0)) / 2.0;
linear(1, 0) = (left_right(1, 0) + left_right(3, 0)) / 2.0;
return linear;
}
// Converts the robot state to an anglular distance, velocity.
- Eigen::Matrix<double, 2, 1> LeftRightToAngular(
- const Eigen::Matrix<double, 7, 1> &left_right) const {
- Eigen::Matrix<double, 2, 1> angular;
+ Eigen::Matrix<Scalar, 2, 1> LeftRightToAngular(
+ const Eigen::Matrix<Scalar, 7, 1> &left_right) const {
+ Eigen::Matrix<Scalar, 2, 1> angular;
angular(0, 0) =
(left_right(2, 0) - left_right(0, 0)) / (this->robot_radius * 2.0);
angular(1, 0) =
@@ -98,12 +99,12 @@
// Converts the linear and angular position, velocity to the top 4 states of
// the robot state.
- Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
- const Eigen::Matrix<double, 2, 1> &linear,
- const Eigen::Matrix<double, 2, 1> &angular) const {
- Eigen::Matrix<double, 2, 1> scaled_angle =
+ Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
+ const Eigen::Matrix<Scalar, 2, 1> &linear,
+ const Eigen::Matrix<Scalar, 2, 1> &angular) const {
+ Eigen::Matrix<Scalar, 2, 1> scaled_angle =
angular * this->robot_radius;
- Eigen::Matrix<double, 4, 1> state;
+ Eigen::Matrix<Scalar, 4, 1> state;
state(0, 0) = linear(0, 0) - scaled_angle(0, 0);
state(1, 0) = linear(1, 0) - scaled_angle(1, 0);
state(2, 0) = linear(0, 0) + scaled_angle(0, 0);
@@ -111,7 +112,6 @@
return state;
}
};
-
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index a791be2..06b4f21 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -34,8 +34,8 @@
const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
0.75};
-const DrivetrainConfig &GetDrivetrainConfig() {
- static DrivetrainConfig kDrivetrainConfig{
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+ static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
@@ -354,7 +354,7 @@
// Tests that converting from a left, right position to a distance, angle
// coordinate system and back returns the same answer.
TEST_F(DrivetrainTest, LinearToAngularAndBack) {
- const DrivetrainConfig dt_config = GetDrivetrainConfig();
+ const DrivetrainConfig<double> &dt_config = GetDrivetrainConfig();
const double width = dt_config.robot_radius * 2.0;
Eigen::Matrix<double, 7, 1> state;
@@ -560,7 +560,7 @@
R << /*[[*/ 1.5, 1.5 /*]]*/;
Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+ ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
EXPECT_EQ(R(0, 0), output(0, 0));
EXPECT_EQ(R(1, 0), output(1, 0));
@@ -576,7 +576,7 @@
R << 5, 5;
Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+ ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
EXPECT_EQ(2.0, output(0, 0));
EXPECT_EQ(2.0, output(1, 0));
@@ -592,7 +592,7 @@
R << 5, 5;
Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+ ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
EXPECT_EQ(3.0, output(0, 0));
EXPECT_EQ(2.0, output(1, 0));
@@ -608,7 +608,7 @@
R << 5, 5;
Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+ ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
EXPECT_EQ(2.0, output(0, 0));
EXPECT_EQ(2.0, output(1, 0));
@@ -624,7 +624,7 @@
R << 5, 5;
Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+ ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
EXPECT_EQ(1.0, output(0, 0));
EXPECT_EQ(1.0, output(1, 0));
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
new file mode 100644
index 0000000..aed0773
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
@@ -0,0 +1,111 @@
+#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#include <inttypes.h>
+
+namespace frc971 {
+namespace control_loops {
+
+GearLogging::GearLogging() { Zero(); }
+
+void GearLogging::Zero() {
+ controller_index = 0;
+ left_loop_high = false;
+ right_loop_high = false;
+ left_state = 0;
+ right_state = 0;
+}
+
+CIMLogging::CIMLogging() { Zero(); }
+
+void CIMLogging::Zero() {
+ left_in_gear = false;
+ right_in_gear = false;
+ left_motor_speed = 0.0f;
+ right_motor_speed = 0.0f;
+ left_velocity = 0.0f;
+ right_velocity = 0.0f;
+}
+
+void DrivetrainQueue_Goal::Zero() {
+ wheel = 0.0f;
+ wheel_velocity = 0.0f;
+ wheel_torque = 0.0f;
+ throttle = 0.0f;
+ throttle_velocity = 0.0f;
+ throttle_torque = 0.0f;
+ highgear = false;
+ quickturn = false;
+ control_loop_driving = false;
+ left_goal = 0.0f;
+ right_goal = 0.0f;
+ left_velocity_goal = 0.0f;
+ right_velocity_goal = 0.0f;
+ max_ss_voltage = 0.0f;
+ //linear.max_velocity = 0.0f;
+ //linear.max_acceleration = 0.0f;
+ //angular.max_velocity = 0.0f;
+ //angular.max_acceleration = 0.0f;
+}
+
+DrivetrainQueue_Goal::DrivetrainQueue_Goal() { Zero(); }
+
+void DrivetrainQueue_Position::Zero() {
+ left_encoder = 0.0f;
+ right_encoder = 0.0f;
+ left_speed = 0.0f;
+ right_speed = 0.0f;
+ left_shifter_position = 0.0f;
+ right_shifter_position = 0.0f;
+ low_left_hall = 0.0f;
+ high_left_hall = 0.0f;
+ low_right_hall = 0.0f;
+ high_right_hall = 0.0f;
+}
+
+DrivetrainQueue_Position::DrivetrainQueue_Position() { Zero(); }
+
+void DrivetrainQueue_Output::Zero() {
+ left_voltage = 0.0f;
+ right_voltage = 0.0f;
+ left_high = false;
+ right_high = false;
+}
+
+DrivetrainQueue_Output::DrivetrainQueue_Output() { Zero(); }
+
+void DrivetrainQueue_Status::Zero() {
+ robot_speed = 0.0f;
+ estimated_left_position = 0.0f;
+ estimated_right_position = 0.0f;
+ estimated_left_velocity = 0.0f;
+ estimated_right_velocity = 0.0f;
+ uncapped_left_voltage = 0.0f;
+ uncapped_right_voltage = 0.0f;
+ left_velocity_goal = 0.0f;
+ right_velocity_goal = 0.0f;
+ left_voltage_error = 0.0f;
+ right_voltage_error = 0.0f;
+ profiled_left_position_goal = 0.0f;
+ profiled_right_position_goal = 0.0f;
+ profiled_left_velocity_goal = 0.0f;
+ profiled_right_velocity_goal = 0.0f;
+ estimated_angular_velocity_error = 0.0f;
+ estimated_heading = 0.0f;
+ output_was_capped = false;
+ ground_angle = 0.0f;
+ gear_logging.controller_index = 0;
+ gear_logging.left_loop_high = false;
+ gear_logging.right_loop_high = false;
+ gear_logging.left_state = 0;
+ gear_logging.right_state = 0;
+ cim_logging.left_in_gear = false;
+ cim_logging.right_in_gear = false;
+ cim_logging.left_motor_speed = 0.0f;
+ cim_logging.right_motor_speed = 0.0f;
+ cim_logging.left_velocity = 0.0f;
+ cim_logging.right_velocity = 0.0f;
+}
+
+DrivetrainQueue_Status::DrivetrainQueue_Status() { Zero(); }
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.h b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
new file mode 100644
index 0000000..e93af26
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
@@ -0,0 +1,120 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
+#include <array>
+#include "aos/common/macros.h"
+
+namespace frc971 {
+namespace control_loops {
+
+struct GearLogging {
+ GearLogging();
+ void Zero();
+
+ int8_t controller_index;
+ bool left_loop_high;
+ bool right_loop_high;
+ int8_t left_state;
+ int8_t right_state;
+};
+
+struct CIMLogging {
+ CIMLogging();
+ void Zero();
+
+ bool left_in_gear;
+ bool right_in_gear;
+ float left_motor_speed;
+ float right_motor_speed;
+ float left_velocity;
+ float right_velocity;
+};
+
+struct DrivetrainQueue_Goal {
+ DrivetrainQueue_Goal();
+ void Zero();
+
+ float wheel;
+ float wheel_velocity;
+ float wheel_torque;
+ float throttle;
+ float throttle_velocity;
+ float throttle_torque;
+ bool highgear;
+ bool quickturn;
+ bool control_loop_driving;
+ float left_goal;
+ float right_goal;
+ float left_velocity_goal;
+ float right_velocity_goal;
+ float max_ss_voltage;
+};
+
+struct DrivetrainQueue_Position {
+ DrivetrainQueue_Position();
+ void Zero();
+
+ float left_encoder;
+ float right_encoder;
+ float left_speed;
+ float right_speed;
+ float left_shifter_position;
+ float right_shifter_position;
+ float low_left_hall;
+ float high_left_hall;
+ float low_right_hall;
+ float high_right_hall;
+};
+
+struct DrivetrainQueue_Output {
+ DrivetrainQueue_Output();
+ void Zero();
+
+ float left_voltage;
+ float right_voltage;
+ bool left_high;
+ bool right_high;
+};
+
+struct DrivetrainQueue_Status {
+ DrivetrainQueue_Status();
+ void Zero();
+
+ float robot_speed;
+ float estimated_left_position;
+ float estimated_right_position;
+ float estimated_left_velocity;
+ float estimated_right_velocity;
+ float uncapped_left_voltage;
+ float uncapped_right_voltage;
+ float left_velocity_goal;
+ float right_velocity_goal;
+ float left_voltage_error;
+ float right_voltage_error;
+ float profiled_left_position_goal;
+ float profiled_right_position_goal;
+ float profiled_left_velocity_goal;
+ float profiled_right_velocity_goal;
+ float estimated_angular_velocity_error;
+ float estimated_heading;
+ bool output_was_capped;
+ float ground_angle;
+ ::frc971::control_loops::GearLogging gear_logging;
+ ::frc971::control_loops::CIMLogging cim_logging;
+};
+
+class DrivetrainQueue {
+ public:
+ typedef DrivetrainQueue_Goal Goal;
+ DrivetrainQueue_Goal goal;
+ typedef DrivetrainQueue_Position Position;
+ DrivetrainQueue_Position position;
+ typedef DrivetrainQueue_Output Output;
+ DrivetrainQueue_Output output;
+ typedef DrivetrainQueue_Status Status;
+ DrivetrainQueue_Status status;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index db9f1c5..266acd5 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -2,340 +2,17 @@
#include "aos/common/commonmath.h"
#include "aos/common/controls/polytope.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/logging/matrix_logging.h"
-#include "aos/common/logging/queue_logging.h"
-
-#include "aos/common/messages/robot_state.q.h"
#include "frc971/control_loops/coerce_goal.h"
+#ifdef __linux__
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#endif // __linux__
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
-PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
- StateFeedbackLoop<7, 2, 4> *kf)
- : kf_(kf),
- U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
- /*[*/ -1, 0 /*]*/,
- /*[*/ 0, 1 /*]*/,
- /*[*/ 0, -1 /*]]*/)
- .finished(),
- (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
- /*[*/ 12 /*]*/,
- /*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/)
- .finished(),
- (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
- /*[*/ -12, 12, 12, -12 /*]*/)
- .finished()),
- loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
- ttrust_(1.1),
- wheel_(0.0),
- throttle_(0.0),
- quickturn_(false),
- left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
- right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
- counter_(0),
- dt_config_(dt_config) {
- last_position_.Zero();
- position_.Zero();
-}
-
-double PolyDrivetrain::MotorSpeed(
- const constants::ShifterHallEffect &hall_effect, double shifter_position,
- double velocity, Gear gear) {
- const double high_gear_speed =
- velocity / dt_config_.high_gear_ratio / dt_config_.wheel_radius;
- const double low_gear_speed =
- velocity / dt_config_.low_gear_ratio / dt_config_.wheel_radius;
-
- if (shifter_position < hall_effect.clear_low) {
- // We're in low gear, so return speed for that gear.
- return low_gear_speed;
- } else if (shifter_position > hall_effect.clear_high) {
- // We're in high gear, so return speed for that gear.
- return high_gear_speed;
- }
-
- // Not in gear, so speed-match to destination gear.
- switch (gear) {
- case Gear::HIGH:
- case Gear::SHIFTING_UP:
- return high_gear_speed;
- case Gear::LOW:
- case Gear::SHIFTING_DOWN:
- default:
- return low_gear_speed;
- break;
- }
-}
-
-Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear, Gear current_gear) {
- const Gear shift_up =
- (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
- ? Gear::SHIFTING_UP
- : Gear::HIGH;
- const Gear shift_down =
- (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
- ? Gear::SHIFTING_DOWN
- : Gear::LOW;
- if (current_gear != requested_gear) {
- if (IsInGear(current_gear)) {
- if (requested_gear == Gear::HIGH) {
- if (current_gear != Gear::HIGH) {
- current_gear = shift_up;
- }
- } else {
- if (current_gear != Gear::LOW) {
- current_gear = shift_down;
- }
- }
- } else {
- if (requested_gear == Gear::HIGH && current_gear == Gear::SHIFTING_DOWN) {
- current_gear = Gear::SHIFTING_UP;
- } else if (requested_gear == Gear::LOW &&
- current_gear == Gear::SHIFTING_UP) {
- current_gear = Gear::SHIFTING_DOWN;
- }
- }
- }
- return current_gear;
-}
-
-void PolyDrivetrain::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- const double wheel = goal.wheel;
- const double throttle = goal.throttle;
- const bool quickturn = goal.quickturn;
- const bool highgear = goal.highgear;
-
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI_2 * dt_config_.wheel_non_linearity;
-
- wheel_ = sin(angular_range * wheel) / sin(angular_range);
- wheel_ = sin(angular_range * wheel_) / sin(angular_range);
- wheel_ = 2.0 * wheel - wheel_;
- quickturn_ = quickturn;
-
- if (quickturn_) {
- wheel_ *= dt_config_.quickturn_wheel_multiplier;
- } else {
- wheel_ *= dt_config_.wheel_multiplier;
- }
-
- static const double kThrottleDeadband = 0.05;
- if (::std::abs(throttle) < kThrottleDeadband) {
- throttle_ = 0;
- } else {
- throttle_ = copysign(
- (::std::abs(throttle) - kThrottleDeadband) / (1.0 - kThrottleDeadband),
- throttle);
- }
-
- Gear requested_gear = highgear ? Gear::HIGH : Gear::LOW;
-
- left_gear_ = PolyDrivetrain::UpdateSingleGear(requested_gear, left_gear_);
- right_gear_ = PolyDrivetrain::UpdateSingleGear(requested_gear, right_gear_);
-}
-
-void PolyDrivetrain::SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
- Gear left_gear, Gear right_gear) {
- left_gear_ = left_gear;
- right_gear_ = right_gear;
- last_position_ = position_;
- position_ = *position;
-}
-
-double PolyDrivetrain::FilterVelocity(double throttle) const {
- const Eigen::Matrix<double, 2, 2> FF =
- loop_->plant().B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
-
- constexpr int kHighGearController = 3;
- const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->plant().coefficients(kHighGearController).B.inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->plant().coefficients(kHighGearController).A);
-
- ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
- int min_FF_sum_index;
- const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
- const double high_min_FF_sum = FF_high.col(0).sum();
-
- const double adjusted_ff_voltage =
- ::aos::Clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
- return (adjusted_ff_voltage +
- ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
- 2.0) /
- (ttrust_ * min_K_sum + min_FF_sum);
-}
-
-double PolyDrivetrain::MaxVelocity() {
- const Eigen::Matrix<double, 2, 2> FF =
- loop_->plant().B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
-
- constexpr int kHighGearController = 3;
- const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->plant().coefficients(kHighGearController).B.inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->plant().coefficients(kHighGearController).A);
-
- ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
- int min_FF_sum_index;
- const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- // const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
- const double high_min_FF_sum = FF_high.col(0).sum();
-
- const double adjusted_ff_voltage =
- ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
- return adjusted_ff_voltage / min_FF_sum;
-}
-
-void PolyDrivetrain::Update() {
- if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
- loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
- loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
- }
-
- // TODO(austin): Observer for the current velocity instead of difference
- // calculations.
- ++counter_;
-
- if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
- // FF * X = U (steady state)
- const Eigen::Matrix<double, 2, 2> FF =
- loop_->plant().B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
-
- // Invert the plant to figure out how the velocity filter would have to
- // work
- // out in order to filter out the forwards negative inertia.
- // This math assumes that the left and right power and velocity are
- // equals,
- // and that the plant is the same on the left and right.
- const double fvel = FilterVelocity(throttle_);
-
- const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
- double steering_velocity;
- if (quickturn_) {
- steering_velocity = wheel_ * MaxVelocity();
- } else {
- steering_velocity = ::std::abs(fvel) * wheel_;
- }
- const double left_velocity = fvel - steering_velocity;
- const double right_velocity = fvel + steering_velocity;
- goal_left_velocity_ = left_velocity;
- goal_right_velocity_ = right_velocity;
-
- // Integrate velocity to get the position.
- // This position is used to get integral control.
- loop_->mutable_R() << left_velocity, right_velocity;
-
- if (!quickturn_) {
- // K * R = w
- Eigen::Matrix<double, 1, 2> equality_k;
- equality_k << 1 + sign_svel, -(1 - sign_svel);
- const double equality_w = 0.0;
-
- // Construct a constraint on R by manipulating the constraint on U
- ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
- U_Poly_.static_H() * (loop_->controller().K() + FF),
- U_Poly_.static_k() +
- U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
- (loop_->controller().K() + FF).inverse() *
- ::aos::controls::ShiftPoints<2, 4>(
- U_Poly_.StaticVertices(),
- loop_->controller().K() * loop_->X_hat()));
-
- // Limit R back inside the box.
- loop_->mutable_R() =
- CoerceGoal(R_poly_hv, equality_k, equality_w, loop_->R());
- }
-
- const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
- const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
-
- for (int i = 0; i < 2; i++) {
- loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
- }
-
- if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
- loop_->mutable_X_hat() =
- loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
- }
-
- // Housekeeping: set the shifting logging values to zero, because we're not shifting
- left_motor_speed_ = 0.0;
- right_motor_speed_ = 0.0;
- current_left_velocity_ = 0.0;
- current_right_velocity_ = 0.0;
- } else {
- current_left_velocity_ =
- (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
- current_right_velocity_ =
- (position_.right_encoder - last_position_.right_encoder) /
- dt_config_.dt;
- left_motor_speed_ =
- MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
- current_left_velocity_, left_gear_);
- right_motor_speed_ =
- MotorSpeed(dt_config_.right_drive, position_.right_shifter_position,
- current_right_velocity_, right_gear_);
-
- goal_left_velocity_ = current_left_velocity_;
- goal_right_velocity_ = current_right_velocity_;
-
- // Any motor is not in gear. Speed match.
- ::Eigen::Matrix<double, 1, 1> R_left;
- ::Eigen::Matrix<double, 1, 1> R_right;
- R_left(0, 0) = left_motor_speed_;
- R_right(0, 0) = right_motor_speed_;
-
- const double wiggle =
- (static_cast<double>((counter_ % 30) / 15) - 0.5) * 8.0;
-
- loop_->mutable_U(0, 0) = ::aos::Clip(
- (R_left / dt_config_.v)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
- -12.0, 12.0);
- loop_->mutable_U(1, 0) = ::aos::Clip(
- (R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
- -12.0, 12.0);
- loop_->mutable_U() *= 12.0 / ::aos::robot_state->voltage_battery;
- }
-}
-
-void PolyDrivetrain::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
- if (output != NULL) {
- output->left_voltage = loop_->U(0, 0);
- output->right_voltage = loop_->U(1, 0);
- output->left_high = MaybeHigh(left_gear_);
- output->right_high = MaybeHigh(right_gear_);
- }
-}
-
-void PolyDrivetrain::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) {
- status->left_velocity_goal = goal_left_velocity_;
- status->right_velocity_goal = goal_right_velocity_;
-
- status->cim_logging.left_in_gear = IsInGear(left_gear_);
- status->cim_logging.left_motor_speed = left_motor_speed_;
- status->cim_logging.left_velocity = current_left_velocity_;
-
- status->cim_logging.right_in_gear = IsInGear(right_gear_);
- status->cim_logging.right_motor_speed = right_motor_speed_;
- status->cim_logging.right_velocity = current_right_velocity_;
-}
-
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index c9b0714..d16e8a1 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -3,8 +3,18 @@
#include "aos/common/controls/polytope.h"
+#include "aos/common/commonmath.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/gear.h"
+#ifdef __linux__
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
+#else
+#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#endif // __linux__
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -12,17 +22,18 @@
namespace control_loops {
namespace drivetrain {
+template <typename Scalar = double>
class PolyDrivetrain {
public:
- PolyDrivetrain(const DrivetrainConfig &dt_config,
- StateFeedbackLoop<7, 2, 4> *kf);
+ PolyDrivetrain(const DrivetrainConfig<Scalar> &dt_config,
+ StateFeedbackLoop<7, 2, 4, Scalar> *kf);
int controller_index() const { return loop_->index(); }
// Computes the speed of the motor given the hall effect position and the
// speed of the robot.
- double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
- double shifter_position, double velocity, Gear gear);
+ Scalar MotorSpeed(const constants::ShifterHallEffect &hall_effect,
+ Scalar shifter_position, Scalar velocity, Gear gear);
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
@@ -30,9 +41,9 @@
const ::frc971::control_loops::DrivetrainQueue::Position *position,
Gear left_gear, Gear right_gear);
- double FilterVelocity(double throttle) const;
+ Scalar FilterVelocity(Scalar throttle) const;
- double MaxVelocity();
+ Scalar MaxVelocity();
void Update();
@@ -44,16 +55,21 @@
// requested state.
Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
+ // Returns the current estimated velocity in m/s.
+ Scalar velocity() const {
+ return (loop_->mutable_X_hat()(0) + loop_->mutable_X_hat()(1)) / 2.0;
+ }
+
private:
- StateFeedbackLoop<7, 2, 4> *kf_;
+ StateFeedbackLoop<7, 2, 4, Scalar> *kf_;
- const ::aos::controls::HVPolytope<2, 4, 4> U_Poly_;
+ const ::aos::controls::HVPolytope<2, 4, 4, Scalar> U_Poly_;
- ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, Scalar>> loop_;
- const double ttrust_;
- double wheel_;
- double throttle_;
+ const Scalar ttrust_;
+ Scalar wheel_;
+ Scalar throttle_;
bool quickturn_;
Gear left_gear_;
@@ -62,18 +78,354 @@
::frc971::control_loops::DrivetrainQueue::Position last_position_;
::frc971::control_loops::DrivetrainQueue::Position position_;
int counter_;
- DrivetrainConfig dt_config_;
+ DrivetrainConfig<Scalar> dt_config_;
- double goal_left_velocity_ = 0.0;
- double goal_right_velocity_ = 0.0;
+ Scalar goal_left_velocity_ = 0.0;
+ Scalar goal_right_velocity_ = 0.0;
// Stored from the last iteration, for logging shifting logic.
- double left_motor_speed_ = 0.0;
- double right_motor_speed_ = 0.0;
- double current_left_velocity_ = 0.0;
- double current_right_velocity_ = 0.0;
+ Scalar left_motor_speed_ = 0.0;
+ Scalar right_motor_speed_ = 0.0;
+ Scalar current_left_velocity_ = 0.0;
+ Scalar current_right_velocity_ = 0.0;
};
+template <typename Scalar>
+PolyDrivetrain<Scalar>::PolyDrivetrain(
+ const DrivetrainConfig<Scalar> &dt_config,
+ StateFeedbackLoop<7, 2, 4, Scalar> *kf)
+ : kf_(kf),
+ U_Poly_((Eigen::Matrix<Scalar, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+ /*[*/ -1, 0 /*]*/,
+ /*[*/ 0, 1 /*]*/,
+ /*[*/ 0, -1 /*]]*/)
+ .finished(),
+ (Eigen::Matrix<Scalar, 4, 1>() << /*[[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]]*/)
+ .finished(),
+ (Eigen::Matrix<Scalar, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+ /*[*/ -12, 12, 12, -12 /*]*/)
+ .finished()),
+ loop_(new StateFeedbackLoop<2, 2, 2, Scalar>(
+ dt_config.make_v_drivetrain_loop())),
+ ttrust_(1.1),
+ wheel_(0.0),
+ throttle_(0.0),
+ quickturn_(false),
+ left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
+ right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
+ counter_(0),
+ dt_config_(dt_config) {
+ last_position_.Zero();
+ position_.Zero();
+}
+
+template <typename Scalar>
+Scalar PolyDrivetrain<Scalar>::MotorSpeed(
+ const constants::ShifterHallEffect &hall_effect, Scalar shifter_position,
+ Scalar velocity, Gear gear) {
+ const Scalar high_gear_speed =
+ velocity / dt_config_.high_gear_ratio / dt_config_.wheel_radius;
+ const Scalar low_gear_speed =
+ velocity / dt_config_.low_gear_ratio / dt_config_.wheel_radius;
+
+ if (shifter_position < hall_effect.clear_low) {
+ // We're in low gear, so return speed for that gear.
+ return low_gear_speed;
+ } else if (shifter_position > hall_effect.clear_high) {
+ // We're in high gear, so return speed for that gear.
+ return high_gear_speed;
+ }
+
+ // Not in gear, so speed-match to destination gear.
+ switch (gear) {
+ case Gear::HIGH:
+ case Gear::SHIFTING_UP:
+ return high_gear_speed;
+ case Gear::LOW:
+ case Gear::SHIFTING_DOWN:
+ default:
+ return low_gear_speed;
+ break;
+ }
+}
+
+template <typename Scalar>
+Gear PolyDrivetrain<Scalar>::UpdateSingleGear(Gear requested_gear,
+ Gear current_gear) {
+ const Gear shift_up =
+ (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
+ ? Gear::SHIFTING_UP
+ : Gear::HIGH;
+ const Gear shift_down =
+ (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
+ ? Gear::SHIFTING_DOWN
+ : Gear::LOW;
+ if (current_gear != requested_gear) {
+ if (IsInGear(current_gear)) {
+ if (requested_gear == Gear::HIGH) {
+ if (current_gear != Gear::HIGH) {
+ current_gear = shift_up;
+ }
+ } else {
+ if (current_gear != Gear::LOW) {
+ current_gear = shift_down;
+ }
+ }
+ } else {
+ if (requested_gear == Gear::HIGH && current_gear == Gear::SHIFTING_DOWN) {
+ current_gear = Gear::SHIFTING_UP;
+ } else if (requested_gear == Gear::LOW &&
+ current_gear == Gear::SHIFTING_UP) {
+ current_gear = Gear::SHIFTING_DOWN;
+ }
+ }
+ }
+ return current_gear;
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ const Scalar wheel = goal.wheel;
+ const Scalar throttle = goal.throttle;
+ const bool quickturn = goal.quickturn;
+ const bool highgear = goal.highgear;
+
+ // Apply a sin function that's scaled to make it feel better.
+ const Scalar angular_range = M_PI_2 * dt_config_.wheel_non_linearity;
+
+ wheel_ = sin(angular_range * wheel) / sin(angular_range);
+ wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+ wheel_ = 2.0 * wheel - wheel_;
+ quickturn_ = quickturn;
+
+ if (quickturn_) {
+ wheel_ *= dt_config_.quickturn_wheel_multiplier;
+ } else {
+ wheel_ *= dt_config_.wheel_multiplier;
+ }
+
+ static const Scalar kThrottleDeadband = 0.05;
+ if (::std::abs(throttle) < kThrottleDeadband) {
+ throttle_ = 0;
+ } else {
+ throttle_ = copysign(
+ (::std::abs(throttle) - kThrottleDeadband) / (1.0 - kThrottleDeadband),
+ throttle);
+ }
+
+ Gear requested_gear = highgear ? Gear::HIGH : Gear::LOW;
+
+ left_gear_ = UpdateSingleGear(requested_gear, left_gear_);
+ right_gear_ = UpdateSingleGear(requested_gear, right_gear_);
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::SetPosition(
+ const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ Gear left_gear, Gear right_gear) {
+ left_gear_ = left_gear;
+ right_gear_ = right_gear;
+ last_position_ = position_;
+ position_ = *position;
+}
+
+template <typename Scalar>
+Scalar PolyDrivetrain<Scalar>::FilterVelocity(Scalar throttle) const {
+ const Eigen::Matrix<Scalar, 2, 2> FF =
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<Scalar, 2, 2>::Identity() - loop_->plant().A());
+
+ constexpr int kHighGearController = 3;
+ const Eigen::Matrix<Scalar, 2, 2> FF_high =
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
+ (Eigen::Matrix<Scalar, 2, 2>::Identity() -
+ loop_->plant().coefficients(kHighGearController).A);
+
+ ::Eigen::Matrix<Scalar, 1, 2> FF_sum = FF.colwise().sum();
+ int min_FF_sum_index;
+ const Scalar min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+ const Scalar min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
+ const Scalar high_min_FF_sum = FF_high.col(0).sum();
+
+ const Scalar adjusted_ff_voltage =
+ ::aos::Clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ return (adjusted_ff_voltage +
+ ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+ 2.0) /
+ (ttrust_ * min_K_sum + min_FF_sum);
+}
+
+template <typename Scalar>
+Scalar PolyDrivetrain<Scalar>::MaxVelocity() {
+ const Eigen::Matrix<Scalar, 2, 2> FF =
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<Scalar, 2, 2>::Identity() - loop_->plant().A());
+
+ constexpr int kHighGearController = 3;
+ const Eigen::Matrix<Scalar, 2, 2> FF_high =
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
+ (Eigen::Matrix<Scalar, 2, 2>::Identity() -
+ loop_->plant().coefficients(kHighGearController).A);
+
+ ::Eigen::Matrix<Scalar, 1, 2> FF_sum = FF.colwise().sum();
+ int min_FF_sum_index;
+ const Scalar min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+ // const Scalar min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const Scalar high_min_FF_sum = FF_high.col(0).sum();
+
+ const Scalar adjusted_ff_voltage =
+ ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ return adjusted_ff_voltage / min_FF_sum;
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::Update() {
+ if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
+ loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
+ loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
+ }
+
+ // TODO(austin): Observer for the current velocity instead of difference
+ // calculations.
+ ++counter_;
+
+ if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+ // FF * X = U (steady state)
+ const Eigen::Matrix<Scalar, 2, 2> FF =
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<Scalar, 2, 2>::Identity() - loop_->plant().A());
+
+ // Invert the plant to figure out how the velocity filter would have to
+ // work
+ // out in order to filter out the forwards negative inertia.
+ // This math assumes that the left and right power and velocity are
+ // equals,
+ // and that the plant is the same on the left and right.
+ const Scalar fvel = FilterVelocity(throttle_);
+
+ const Scalar sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
+ Scalar steering_velocity;
+ if (quickturn_) {
+ steering_velocity = wheel_ * MaxVelocity();
+ } else {
+ steering_velocity = ::std::abs(fvel) * wheel_;
+ }
+ const Scalar left_velocity = fvel - steering_velocity;
+ const Scalar right_velocity = fvel + steering_velocity;
+ goal_left_velocity_ = left_velocity;
+ goal_right_velocity_ = right_velocity;
+
+ // Integrate velocity to get the position.
+ // This position is used to get integral control.
+ loop_->mutable_R() << left_velocity, right_velocity;
+
+ if (!quickturn_) {
+ // K * R = w
+ Eigen::Matrix<Scalar, 1, 2> equality_k;
+ equality_k << 1 + sign_svel, -(1 - sign_svel);
+ const Scalar equality_w = 0.0;
+
+ // Construct a constraint on R by manipulating the constraint on U
+ ::aos::controls::HVPolytope<2, 4, 4, Scalar> R_poly_hv(
+ U_Poly_.static_H() * (loop_->controller().K() + FF),
+ U_Poly_.static_k() +
+ U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+ (loop_->controller().K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4, Scalar>(
+ U_Poly_.StaticVertices(),
+ loop_->controller().K() * loop_->X_hat()));
+
+ // Limit R back inside the box.
+ loop_->mutable_R() =
+ CoerceGoal<Scalar>(R_poly_hv, equality_k, equality_w, loop_->R());
+ }
+
+ const Eigen::Matrix<Scalar, 2, 1> FF_volts = FF * loop_->R();
+ const Eigen::Matrix<Scalar, 2, 1> U_ideal =
+ loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+
+ for (int i = 0; i < 2; i++) {
+ loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
+ }
+
+ if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
+ loop_->mutable_X_hat() =
+ loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
+ }
+
+ // Housekeeping: set the shifting logging values to zero, because we're not shifting
+ left_motor_speed_ = 0.0;
+ right_motor_speed_ = 0.0;
+ current_left_velocity_ = 0.0;
+ current_right_velocity_ = 0.0;
+ } else {
+ current_left_velocity_ =
+ (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
+ current_right_velocity_ =
+ (position_.right_encoder - last_position_.right_encoder) /
+ dt_config_.dt;
+ left_motor_speed_ =
+ MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
+ current_left_velocity_, left_gear_);
+ right_motor_speed_ =
+ MotorSpeed(dt_config_.right_drive, position_.right_shifter_position,
+ current_right_velocity_, right_gear_);
+
+ goal_left_velocity_ = current_left_velocity_;
+ goal_right_velocity_ = current_right_velocity_;
+
+ // Any motor is not in gear. Speed match.
+ ::Eigen::Matrix<Scalar, 1, 1> R_left;
+ ::Eigen::Matrix<Scalar, 1, 1> R_right;
+ R_left(0, 0) = left_motor_speed_;
+ R_right(0, 0) = right_motor_speed_;
+
+ const Scalar wiggle =
+ (static_cast<Scalar>((counter_ % 30) / 15) - 0.5) * 8.0;
+
+ loop_->mutable_U(0, 0) = ::aos::Clip(
+ (R_left / dt_config_.v)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->mutable_U(1, 0) = ::aos::Clip(
+ (R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+#ifdef __linux__
+ loop_->mutable_U() *= 12.0 / ::aos::robot_state->voltage_battery;
+#endif // __linux__
+ }
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ if (output != NULL) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ output->left_high = MaybeHigh(left_gear_);
+ output->right_high = MaybeHigh(right_gear_);
+ }
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) {
+ status->left_velocity_goal = goal_left_velocity_;
+ status->right_velocity_goal = goal_right_velocity_;
+
+ status->cim_logging.left_in_gear = IsInGear(left_gear_);
+ status->cim_logging.left_motor_speed = left_motor_speed_;
+ status->cim_logging.left_velocity = current_left_velocity_;
+
+ status->cim_logging.right_in_gear = IsInGear(right_gear_);
+ status->cim_logging.right_motor_speed = right_motor_speed_;
+ status->cim_logging.right_velocity = current_right_velocity_;
+}
+
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 39c4ade..ad86b1f 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -59,7 +59,7 @@
(-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
(U_poly_.static_k() * max_voltage_),
(position_K * T_).inverse() *
- ::aos::controls::ShiftPoints<2, 4>(
+ ::aos::controls::ShiftPoints<2, 4, double>(
(U_poly_.StaticVertices() * max_voltage_),
-velocity_K * velocity_error + U_integral - kf_->ff_U()));
@@ -87,9 +87,9 @@
bool is_inside_h, is_inside_45;
const auto adjusted_pos_error_h =
- DoCoerceGoal(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
+ DoCoerceGoal<double>(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
const auto adjusted_pos_error_45 =
- DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+ DoCoerceGoal<double>(pos_poly_hv, L45, w45, intersection, &is_inside_45);
if (pos_poly_hv.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
@@ -116,21 +116,25 @@
}
}
-DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
- StateFeedbackLoop<7, 2, 4> *kf,
- double *integrated_kf_heading)
+DrivetrainMotorsSS::DrivetrainMotorsSS(
+ const DrivetrainConfig<double> &dt_config, StateFeedbackLoop<7, 2, 4> *kf,
+ double *integrated_kf_heading)
: dt_config_(dt_config),
kf_(kf),
- U_poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
- /*[*/ -1, 0 /*]*/,
- /*[*/ 0, 1 /*]*/,
- /*[*/ 0, -1 /*]]*/).finished(),
- (Eigen::Matrix<double, 4, 1>() << /*[[*/ 1.0 /*]*/,
- /*[*/ 1.0 /*]*/,
- /*[*/ 1.0 /*]*/,
- /*[*/ 1.0 /*]]*/).finished(),
- (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
- /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/).finished()),
+ U_poly_(
+ (Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+ /*[*/ -1, 0 /*]*/,
+ /*[*/ 0, 1 /*]*/,
+ /*[*/ 0, -1 /*]]*/)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << /*[[*/ 1.0 /*]*/,
+ /*[*/ 1.0 /*]*/,
+ /*[*/ 1.0 /*]*/,
+ /*[*/ 1.0 /*]]*/)
+ .finished(),
+ (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
+ /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/)
+ .finished()),
linear_profile_(::aos::controls::kLoopFrequency),
angular_profile_(::aos::controls::kLoopFrequency),
integrated_kf_heading_(integrated_kf_heading) {
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 9a9c0d8..5e261e9 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -18,7 +18,7 @@
class DrivetrainMotorsSS {
public:
- DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
+ DrivetrainMotorsSS(const DrivetrainConfig<double> &dt_config,
StateFeedbackLoop<7, 2, 4> *kf,
double *integrated_kf_heading);
@@ -42,7 +42,7 @@
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
- const DrivetrainConfig dt_config_;
+ const DrivetrainConfig<double> dt_config_;
StateFeedbackLoop<7, 2, 4> *kf_;
Eigen::Matrix<double, 7, 1> unprofiled_goal_;