Drive code works on Tantrum.
Need to write the spring code. Drive now supports doubles... What a
pain.
Change-Id: Id589acdc443dcd81242a21e3b0c26f81d6974dc8
diff --git a/aos/common/BUILD b/aos/common/BUILD
index 2ee374d..4de57b6 100644
--- a/aos/common/BUILD
+++ b/aos/common/BUILD
@@ -1,401 +1,402 @@
-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")
queue_library(
- name = 'test_queue',
- srcs = [
- 'test_queue.q',
- ],
+ name = "test_queue",
+ srcs = [
+ "test_queue.q",
+ ],
)
cc_library(
- name = 'math',
- hdrs = [
- 'commonmath.h',
- ],
+ name = "math",
+ hdrs = [
+ "commonmath.h",
+ ],
+ compatible_with = mcu_cpus,
)
cc_library(
- name = 'macros',
- hdrs = [
- 'macros.h',
- ],
- compatible_with = mcu_cpus,
+ name = "macros",
+ hdrs = [
+ "macros.h",
+ ],
+ compatible_with = mcu_cpus,
)
cc_library(
- name = 'type_traits',
- hdrs = [
- 'type_traits.h',
- ],
+ name = "type_traits",
+ hdrs = [
+ "type_traits.h",
+ ],
)
cc_library(
- name = 'time',
- srcs = [
- 'time.cc',
- ],
- hdrs = [
- 'time.h',
- ],
- deps = [
- '//aos/common/logging:logging',
- ':mutex',
- ':macros',
- '//aos/linux_code/ipc_lib:shared_mem',
- ],
+ name = "time",
+ srcs = [
+ "time.cc",
+ ],
+ hdrs = [
+ "time.h",
+ ],
+ deps = [
+ ":macros",
+ ":mutex",
+ "//aos/common/logging",
+ "//aos/linux_code/ipc_lib:shared_mem",
+ ],
)
genrule(
- name = 'gen_queue_primitives',
- visibility = ['//visibility:private'],
- tools = ['//aos/build/queues:queue_primitives'],
- outs = ['queue_primitives.h'],
- cmd = '$(location //aos/build/queues:queue_primitives) $@',
+ name = "gen_queue_primitives",
+ outs = ["queue_primitives.h"],
+ cmd = "$(location //aos/build/queues:queue_primitives) $@",
+ tools = ["//aos/build/queues:queue_primitives"],
+ visibility = ["//visibility:private"],
)
genrule(
- name = 'gen_print_field',
- visibility = ['//visibility:private'],
- tools = ['//aos/build/queues:print_field'],
- outs = ['print_field.cc'],
- cmd = '$(location //aos/build/queues:print_field) $@',
+ name = "gen_print_field",
+ outs = ["print_field.cc"],
+ cmd = "$(location //aos/build/queues:print_field) $@",
+ tools = ["//aos/build/queues:print_field"],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'generated_queue_headers',
- visibility = ['//aos/common/logging:__pkg__'],
- hdrs = [
- ':gen_queue_primitives',
- ],
+ name = "generated_queue_headers",
+ hdrs = [
+ ":gen_queue_primitives",
+ ],
+ visibility = ["//aos/common/logging:__pkg__"],
)
cc_library(
- name = 'event',
- hdrs = [
- 'event.h',
- ],
- srcs = [
- 'event.cc',
- ],
- deps = [
- '//aos/linux_code/ipc_lib:aos_sync',
- ':time',
- '//aos/common/logging:logging',
- ],
+ name = "event",
+ srcs = [
+ "event.cc",
+ ],
+ hdrs = [
+ "event.h",
+ ],
+ deps = [
+ ":time",
+ "//aos/common/logging",
+ "//aos/linux_code/ipc_lib:aos_sync",
+ ],
)
cc_library(
- name = 'unique_malloc_ptr',
- hdrs = [
- 'unique_malloc_ptr.h',
- ],
+ name = "unique_malloc_ptr",
+ hdrs = [
+ "unique_malloc_ptr.h",
+ ],
)
cc_library(
- name = 'queue_types',
- srcs = [
- 'queue_types.cc',
- ':gen_print_field',
- 'print_field_helpers.h',
- ],
- hdrs = [
- 'queue_types.h',
- ],
- deps = [
- ':generated_queue_headers',
- '//aos/linux_code/ipc_lib:shared_mem',
- '//aos/linux_code/ipc_lib:core_lib',
- ':mutex',
- '//aos/common/logging:printf_formats',
- ':time',
- ':byteorder'
- ],
+ name = "queue_types",
+ srcs = [
+ "print_field_helpers.h",
+ "queue_types.cc",
+ ":gen_print_field",
+ ],
+ hdrs = [
+ "queue_types.h",
+ ],
+ deps = [
+ ":byteorder",
+ ":generated_queue_headers",
+ ":mutex",
+ ":time",
+ "//aos/common/logging:printf_formats",
+ "//aos/linux_code/ipc_lib:core_lib",
+ "//aos/linux_code/ipc_lib:shared_mem",
+ ],
)
cc_test(
- name = 'queue_types_test',
- srcs = [
- 'queue_types_test.cc',
- ],
- deps = [
- ':queue_types',
- '//aos/testing:googletest',
- ':test_queue',
- '//aos/common/logging',
- '//aos/testing:test_logging',
- ],
+ name = "queue_types_test",
+ srcs = [
+ "queue_types_test.cc",
+ ],
+ deps = [
+ ":queue_types",
+ ":test_queue",
+ "//aos/common/logging",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ ],
)
cc_library(
- name = 'network_port',
- hdrs = [
- 'network_port.h',
- ],
+ name = "network_port",
+ hdrs = [
+ "network_port.h",
+ ],
)
cc_library(
- name = 'byteorder',
- hdrs = [
- 'byteorder.h',
- ],
+ name = "byteorder",
+ hdrs = [
+ "byteorder.h",
+ ],
)
cc_library(
- name = 'messages',
- srcs = [
- 'message.cc',
- ],
- hdrs = [
- 'message.h',
- ],
- deps = [
- ':time',
- ':macros',
- ':byteorder',
- ],
+ name = "messages",
+ srcs = [
+ "message.cc",
+ ],
+ hdrs = [
+ "message.h",
+ ],
+ deps = [
+ ":byteorder",
+ ":macros",
+ ":time",
+ ],
)
cc_library(
- name = 'queues',
- srcs = [],
- hdrs = [
- 'queue.h',
- ],
- deps = [
- '//aos/linux_code/ipc_lib:queue',
- '//aos/linux_code:queue',
- ':messages',
- ],
+ name = "queues",
+ srcs = [],
+ hdrs = [
+ "queue.h",
+ ],
+ deps = [
+ ":messages",
+ "//aos/linux_code:queue",
+ "//aos/linux_code/ipc_lib:queue",
+ ],
)
cc_library(
- name = 'scoped_fd',
- hdrs = [
- 'scoped_fd.h',
- ],
- deps = [
- '//aos/common/logging',
- ],
+ name = "scoped_fd",
+ hdrs = [
+ "scoped_fd.h",
+ ],
+ deps = [
+ "//aos/common/logging",
+ ],
)
cc_test(
- name = 'queue_test',
- srcs = [
- 'queue_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- '//aos/testing:test_shm',
- ':test_queue',
- '//aos/common/util:thread',
- ':die',
- ],
+ name = "queue_test",
+ srcs = [
+ "queue_test.cc",
+ ],
+ deps = [
+ ":die",
+ ":test_queue",
+ "//aos/common/util:thread",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ ],
)
cc_test(
- name = 'type_traits_test',
- srcs = [
- 'type_traits_test.cpp',
- ],
- deps = [
- '//aos/testing:googletest',
- ':type_traits',
- ],
+ name = "type_traits_test",
+ srcs = [
+ "type_traits_test.cpp",
+ ],
+ deps = [
+ ":type_traits",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'gtest_prod',
- hdrs = [
- 'gtest_prod.h',
- ],
+ name = "gtest_prod",
+ hdrs = [
+ "gtest_prod.h",
+ ],
)
cc_test(
- name = 'time_test',
- srcs = [
- 'time_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':time',
- '//aos/common/logging',
- '//aos/common/util:death_test_log_implementation',
- ],
+ name = "time_test",
+ srcs = [
+ "time_test.cc",
+ ],
+ deps = [
+ ":time",
+ "//aos/common/logging",
+ "//aos/common/util:death_test_log_implementation",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'die',
- srcs = [
- 'die.cc',
- ],
- hdrs = [
- 'die.h',
- ],
- deps = [
- ':macros',
- '//aos/common/libc:aos_strerror',
- ],
+ name = "die",
+ srcs = [
+ "die.cc",
+ ],
+ hdrs = [
+ "die.h",
+ ],
+ deps = [
+ ":macros",
+ "//aos/common/libc:aos_strerror",
+ ],
)
cc_test(
- name = 'mutex_test',
- srcs = [
- 'mutex_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':mutex',
- ':die',
- '//aos/common/logging',
- '//aos/common/util:death_test_log_implementation',
- '//aos/common/util:thread',
- '//aos/common:time',
- '//aos/testing:test_logging',
- '//aos/testing:test_shm',
- ],
+ name = "mutex_test",
+ srcs = [
+ "mutex_test.cc",
+ ],
+ deps = [
+ ":die",
+ ":mutex",
+ "//aos/common:time",
+ "//aos/common/logging",
+ "//aos/common/util:death_test_log_implementation",
+ "//aos/common/util:thread",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ "//aos/testing:test_shm",
+ ],
)
cc_test(
- name = 'event_test',
- srcs = [
- 'event_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':event',
- '//aos/testing:test_logging',
- ':time',
- ],
+ name = "event_test",
+ srcs = [
+ "event_test.cc",
+ ],
+ deps = [
+ ":event",
+ ":time",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ ],
)
cc_library(
- name = 'condition',
- hdrs = [
- 'condition.h',
- ],
- srcs = [
- 'condition.cc',
- ],
- deps = [
- ':mutex',
- '//aos/linux_code/ipc_lib:aos_sync',
- '//aos/common/logging:logging',
- ],
+ name = "condition",
+ srcs = [
+ "condition.cc",
+ ],
+ hdrs = [
+ "condition.h",
+ ],
+ deps = [
+ ":mutex",
+ "//aos/common/logging",
+ "//aos/linux_code/ipc_lib:aos_sync",
+ ],
)
cc_test(
- name = 'condition_test',
- srcs = [
- 'condition_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- '//aos/testing:prevent_exit',
- ':condition',
- '//aos/common/util:thread',
- ':time',
- ':mutex',
- '//aos/common/logging',
- '//aos/testing:test_shm',
- '//aos/linux_code/ipc_lib:core_lib',
- '//aos/linux_code/ipc_lib:aos_sync',
- ':die',
- ],
+ name = "condition_test",
+ srcs = [
+ "condition_test.cc",
+ ],
+ deps = [
+ ":condition",
+ ":die",
+ ":mutex",
+ ":time",
+ "//aos/common/logging",
+ "//aos/common/util:thread",
+ "//aos/linux_code/ipc_lib:aos_sync",
+ "//aos/linux_code/ipc_lib:core_lib",
+ "//aos/testing:googletest",
+ "//aos/testing:prevent_exit",
+ "//aos/testing:test_shm",
+ ],
)
cc_test(
- name = 'die_test',
- srcs = [
- 'die_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':die',
- ],
+ name = "die_test",
+ srcs = [
+ "die_test.cc",
+ ],
+ deps = [
+ ":die",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'stl_mutex',
- hdrs = [
- 'stl_mutex.h',
- ],
- deps = [
- '//aos/linux_code/ipc_lib:aos_sync',
- '//aos/common/logging',
- ],
+ name = "stl_mutex",
+ hdrs = [
+ "stl_mutex.h",
+ ],
+ deps = [
+ "//aos/common/logging",
+ "//aos/linux_code/ipc_lib:aos_sync",
+ ],
)
cc_library(
- name = 'mutex',
- hdrs = [
- 'mutex.h',
- ],
- srcs = [
- 'mutex.cc',
- ],
- deps = [
- '//aos/linux_code/ipc_lib:aos_sync',
- ':die',
- '//aos/common/logging:logging',
- ':type_traits',
- ],
+ name = "mutex",
+ srcs = [
+ "mutex.cc",
+ ],
+ hdrs = [
+ "mutex.h",
+ ],
+ deps = [
+ ":die",
+ ":type_traits",
+ "//aos/common/logging",
+ "//aos/linux_code/ipc_lib:aos_sync",
+ ],
)
cc_test(
- name = 'stl_mutex_test',
- srcs = [
- 'stl_mutex_test.cc',
- ],
- deps = [
- ':stl_mutex',
- '//aos/testing:googletest',
- '//aos/testing:test_logging',
- '//aos/common/util:thread',
- ':die',
- ],
+ name = "stl_mutex_test",
+ srcs = [
+ "stl_mutex_test.cc",
+ ],
+ deps = [
+ ":die",
+ ":stl_mutex",
+ "//aos/common/util:thread",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ ],
)
cc_library(
- name = 'transaction',
- hdrs = [
- 'transaction.h',
- ],
- deps = [
- '//aos/common/logging:logging',
- '//aos/common/util:compiler_memory_barrier',
- ],
+ name = "transaction",
+ hdrs = [
+ "transaction.h",
+ ],
+ deps = [
+ "//aos/common/logging",
+ "//aos/common/util:compiler_memory_barrier",
+ ],
)
cc_test(
- name = 'transaction_test',
- srcs = [
- 'transaction_test.cc',
- ],
- deps = [
- ':transaction',
- '//aos/testing:googletest',
- '//aos/common/logging',
- '//aos/common/util:death_test_log_implementation',
- ],
+ name = "transaction_test",
+ srcs = [
+ "transaction_test.cc",
+ ],
+ deps = [
+ ":transaction",
+ "//aos/common/logging",
+ "//aos/common/util:death_test_log_implementation",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'ring_buffer',
- hdrs = [
- 'ring_buffer.h',
- ],
+ name = "ring_buffer",
+ hdrs = [
+ "ring_buffer.h",
+ ],
)
cc_test(
- name = 'ring_buffer_test',
- srcs = [
- 'ring_buffer_test.cc',
- ],
- deps = [
- ':ring_buffer',
- '//aos/testing:googletest',
- ],
+ name = "ring_buffer_test",
+ srcs = [
+ "ring_buffer_test.cc",
+ ],
+ deps = [
+ ":ring_buffer",
+ "//aos/testing:googletest",
+ ],
)
diff --git a/aos/common/controls/BUILD b/aos/common/controls/BUILD
index 456cf44..46b5f24 100644
--- a/aos/common/controls/BUILD
+++ b/aos/common/controls/BUILD
@@ -1,89 +1,101 @@
-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_library(
- name = 'replay_control_loop',
- hdrs = [
- 'replay_control_loop.h',
- ],
- deps = [
- '//aos/common:queues',
- ':control_loop',
- '//aos/common/logging:replay',
- '//aos/common/logging:queue_logging',
- '//aos/common:time',
- ],
+ name = "replay_control_loop",
+ hdrs = [
+ "replay_control_loop.h",
+ ],
+ deps = [
+ ":control_loop",
+ "//aos/common:queues",
+ "//aos/common:time",
+ "//aos/common/logging:queue_logging",
+ "//aos/common/logging:replay",
+ ],
)
cc_library(
- name = 'control_loop_test',
- srcs = [
- 'control_loop_test.cc',
- ],
- hdrs = [
- 'control_loop_test.h',
- ],
- deps = [
- '//aos/common/logging:queue_logging',
- '//aos/common/messages:robot_state',
- '//aos/common:time',
- '//aos/testing:googletest',
- '//aos/testing:test_shm',
- ],
- testonly = True,
+ name = "control_loop_test",
+ testonly = True,
+ srcs = [
+ "control_loop_test.cc",
+ ],
+ hdrs = [
+ "control_loop_test.h",
+ ],
+ deps = [
+ "//aos/common:time",
+ "//aos/common/logging:queue_logging",
+ "//aos/common/messages:robot_state",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ ],
)
cc_library(
- name = 'polytope',
- hdrs = [
- 'polytope.h',
- ],
- deps = [
- '//third_party/eigen',
- '//third_party/cddlib',
- '//aos/common/logging',
- '//aos/common/logging:matrix_logging',
- ],
+ name = "polytope_uc",
+ hdrs = [
+ "polytope.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//third_party/eigen",
+ ],
+)
+
+cc_library(
+ name = "polytope",
+ hdrs = [
+ "polytope.h",
+ ],
+ deps = [
+ "//aos/common/logging",
+ "//aos/common/logging:matrix_logging",
+ "//third_party/cddlib",
+ "//third_party/eigen",
+ ],
)
cc_test(
- name = 'polytope_test',
- srcs = [
- 'polytope_test.cc',
- ],
- deps = [
- ':polytope',
- '//aos/testing:googletest',
- '//third_party/eigen',
- '//third_party/googletest:googlemock',
- '//aos/testing:test_logging',
- ],
+ name = "polytope_test",
+ srcs = [
+ "polytope_test.cc",
+ ],
+ deps = [
+ ":polytope",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ "//third_party/eigen",
+ "//third_party/googletest:googlemock",
+ ],
)
queue_library(
- name = 'control_loop_queues',
- srcs = [
- 'control_loops.q',
- ],
+ name = "control_loop_queues",
+ srcs = [
+ "control_loops.q",
+ ],
)
cc_library(
- name = 'control_loop',
- srcs = [
- 'control_loop.cc',
- 'control_loop-tmpl.h',
- ],
- hdrs = [
- 'control_loop.h',
- ],
- deps = [
- ':control_loop_queues',
- '//aos/common/logging',
- '//aos/common/logging:queue_logging',
- '//aos/common/messages:robot_state',
- '//aos/common/util:log_interval',
- '//aos/common:queues',
- '//aos/common:time',
- ],
+ name = "control_loop",
+ srcs = [
+ "control_loop.cc",
+ "control_loop-tmpl.h",
+ ],
+ hdrs = [
+ "control_loop.h",
+ ],
+ deps = [
+ ":control_loop_queues",
+ "//aos/common:queues",
+ "//aos/common:time",
+ "//aos/common/logging",
+ "//aos/common/logging:queue_logging",
+ "//aos/common/messages:robot_state",
+ "//aos/common/util:log_interval",
+ ],
)
diff --git a/aos/common/controls/polytope.h b/aos/common/controls/polytope.h
index 4399524..03d8fcd 100644
--- a/aos/common/controls/polytope.h
+++ b/aos/common/controls/polytope.h
@@ -2,11 +2,14 @@
#define AOS_COMMON_CONTROLS_POLYTOPE_H_
#include "Eigen/Dense"
+
+#ifdef __linux__
#include "third_party/cddlib/lib-src/setoper.h"
#include "third_party/cddlib/lib-src/cdd.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/matrix_logging.h"
+#endif // __linux__
namespace aos {
namespace controls {
@@ -18,18 +21,18 @@
// random-seeming polytopes it refuses to calculate the vertices completely. To
// avoid issues with that, using the "shifting" constructor is recommended
// whenever possible.
-template <int number_of_dimensions>
+template <int number_of_dimensions, typename Scalar = double>
class Polytope {
public:
virtual ~Polytope() {}
// Returns a reference to H.
virtual Eigen::Ref<
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ const Eigen::Matrix<Scalar, Eigen::Dynamic, number_of_dimensions>>
H() const = 0;
// Returns a reference to k.
- virtual Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ virtual Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
const = 0;
// Returns the number of dimensions in the polytope.
@@ -39,72 +42,74 @@
int num_constraints() const { return k().rows(); }
// Returns true if the point is inside the polytope.
- bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point) const;
+ bool IsInside(Eigen::Matrix<Scalar, number_of_dimensions, 1> point) const;
// Returns the list of vertices inside the polytope.
- virtual Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ virtual Eigen::Matrix<Scalar, number_of_dimensions, Eigen::Dynamic> Vertices()
const = 0;
};
-template <int number_of_dimensions, int num_vertices>
-Eigen::Matrix<double, number_of_dimensions, num_vertices> ShiftPoints(
- const Eigen::Matrix<double, number_of_dimensions, num_vertices> &vertices,
- const Eigen::Matrix<double, number_of_dimensions, 1> &offset) {
- Eigen::Matrix<double, number_of_dimensions, num_vertices> ans = vertices;
+template <int number_of_dimensions, int num_vertices, typename Scalar = double>
+Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> ShiftPoints(
+ const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> &vertices,
+ const Eigen::Matrix<Scalar, number_of_dimensions, 1> &offset) {
+ Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> ans = vertices;
for (int i = 0; i < num_vertices; ++i) {
ans.col(i) += offset;
}
return ans;
}
-template <int number_of_dimensions, int num_constraints, int num_vertices>
-class HVPolytope : public Polytope<number_of_dimensions> {
+template <int number_of_dimensions, int num_constraints, int num_vertices,
+ typename Scalar = double>
+class HVPolytope : public Polytope<number_of_dimensions, Scalar> {
public:
// Constructs a polytope given the H and k matrices.
- HVPolytope(Eigen::Ref<const Eigen::Matrix<double, num_constraints,
+ HVPolytope(Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints,
number_of_dimensions>> H,
- Eigen::Ref<const Eigen::Matrix<double, num_constraints, 1>> k,
- Eigen::Ref<const Eigen::Matrix<double, number_of_dimensions,
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> k,
+ Eigen::Ref<const Eigen::Matrix<Scalar, number_of_dimensions,
num_vertices>> vertices)
: H_(H), k_(k), vertices_(vertices) {}
- Eigen::Ref<const Eigen::Matrix<double, num_constraints, number_of_dimensions>>
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions>>
static_H() const {
return H_;
}
- Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, number_of_dimensions>>
H() const override {
return H_;
}
- Eigen::Ref<const Eigen::Matrix<double, num_constraints, 1>> static_k()
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> static_k()
const {
return k_;
}
- Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
const override {
return k_;
}
- Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ Eigen::Matrix<Scalar, number_of_dimensions, Eigen::Dynamic> Vertices()
const override {
return vertices_;
}
- Eigen::Matrix<double, number_of_dimensions, num_vertices>
+ Eigen::Matrix<Scalar, number_of_dimensions, num_vertices>
StaticVertices() const {
return vertices_;
}
private:
- const Eigen::Matrix<double, num_constraints, number_of_dimensions> H_;
- const Eigen::Matrix<double, num_constraints, 1> k_;
- const Eigen::Matrix<double, number_of_dimensions, num_vertices> vertices_;
+ const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions> H_;
+ const Eigen::Matrix<Scalar, num_constraints, 1> k_;
+ const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> vertices_;
};
+#ifdef __linux__
template <int number_of_dimensions>
class HPolytope : public Polytope<number_of_dimensions> {
public:
@@ -146,12 +151,14 @@
Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k);
};
-template <int number_of_dimensions>
-bool Polytope<number_of_dimensions>::IsInside(
- Eigen::Matrix<double, number_of_dimensions, 1> point) const {
- Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k_ptr = k();
+#endif // __linux__
+
+template <int number_of_dimensions, typename Scalar>
+bool Polytope<number_of_dimensions, Scalar>::IsInside(
+ Eigen::Matrix<Scalar, number_of_dimensions, 1> point) const {
+ Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k_ptr = k();
for (int i = 0; i < k_ptr.rows(); ++i) {
- double ev = (H().row(i) * point)(0, 0);
+ Scalar ev = (H().row(i) * point)(0, 0);
if (ev > k_ptr(i, 0)) {
return false;
}
@@ -159,6 +166,7 @@
return true;
}
+#ifdef __linux__
template <int number_of_dimensions>
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
HPolytope<number_of_dimensions>::CalculateVertices(
@@ -221,6 +229,7 @@
return vertices;
}
+#endif // __linux__
} // namespace controls
} // namespace aos
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 506e4b6..852b94e 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -246,7 +246,8 @@
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
InputType type,
- const ::frc971::control_loops::drivetrain::DrivetrainConfig &dt_config) {
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config) {
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
using InputType = DrivetrainInputReader::InputType;
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 86b0e10..e38320c 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -59,7 +59,8 @@
// Constructs the appropriate DrivetrainInputReader.
static std::unique_ptr<DrivetrainInputReader> Make(
InputType type,
- const ::frc971::control_loops::drivetrain::DrivetrainConfig &dt_config);
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
// Processes new joystick data and publishes drivetrain goal messages.
void HandleDrivetrain(const ::aos::input::driver_station::Data &data);
diff --git a/frc971/BUILD b/frc971/BUILD
index 4729bf6..46317d3 100644
--- a/frc971/BUILD
+++ b/frc971/BUILD
@@ -1,15 +1,18 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
+
+load("//tools:environments.bzl", "mcu_cpus")
cc_library(
- name = 'shifter_hall_effect',
- hdrs = [
- 'shifter_hall_effect.h',
- ],
+ name = "shifter_hall_effect",
+ hdrs = [
+ "shifter_hall_effect.h",
+ ],
+ compatible_with = mcu_cpus,
)
cc_library(
- name = 'constants',
- hdrs = [
- 'constants.h',
- ],
+ name = "constants",
+ hdrs = [
+ "constants.h",
+ ],
)
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 4493e3c..9b5e7f4 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -20,7 +20,7 @@
BaseAutonomousActor::BaseAutonomousActor(
AutonomousActionQueueGroup *s,
- const control_loops::drivetrain::DrivetrainConfig dt_config)
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
: aos::common::actions::ActorBase<AutonomousActionQueueGroup>(s),
dt_config_(dt_config),
initial_drivetrain_({0.0, 0.0}) {}
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 980be2c..c8f6398 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -17,7 +17,7 @@
public:
explicit BaseAutonomousActor(
AutonomousActionQueueGroup *s,
- const control_loops::drivetrain::DrivetrainConfig dt_config);
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
protected:
void ResetDrivetrain();
@@ -53,7 +53,7 @@
// Returns the distance left to go.
double DriveDistanceLeft();
- const control_loops::drivetrain::DrivetrainConfig dt_config_;
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
// Initial drivetrain positions.
struct InitialDrivetrain {
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 360097d..5998afc 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,6 +1,7 @@
package(default_visibility = ["//visibility:public"])
load("//aos/build:queues.bzl", "queue_library")
+load("//tools:environments.bzl", "mcu_cpus")
cc_library(
name = "team_number_test_environment",
@@ -89,6 +90,21 @@
)
cc_library(
+ name = "coerce_goal_uc",
+ srcs = [
+ "coerce_goal.cc",
+ ],
+ hdrs = [
+ "coerce_goal.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//aos/common/controls:polytope_uc",
+ "//third_party/eigen",
+ ],
+)
+
+cc_library(
name = "coerce_goal",
srcs = [
"coerce_goal.cc",
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 0c98560..d98df09 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -7,71 +7,5 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(
- const aos::controls::HVPolytope<2, 4, 4> ®ion,
- const Eigen::Matrix<double, 1, 2> &K, double w,
- const Eigen::Matrix<double, 2, 1> &R, bool *is_inside) {
- if (region.IsInside(R)) {
- if (is_inside) *is_inside = true;
- return R;
- }
- Eigen::Matrix<double, 2, 1> parallel_vector;
- Eigen::Matrix<double, 2, 1> perpendicular_vector;
- perpendicular_vector = K.transpose().normalized();
- parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
-
- Eigen::Matrix<double, 4, 1> projectedh = region.static_H() * parallel_vector;
- Eigen::Matrix<double, 4, 1> projectedk =
- region.static_k() - region.static_H() * perpendicular_vector * w;
-
- double min_boundary = ::std::numeric_limits<double>::lowest();
- double max_boundary = ::std::numeric_limits<double>::max();
- for (int i = 0; i < 4; ++i) {
- if (projectedh(i, 0) > 0) {
- max_boundary =
- ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
- } else {
- min_boundary =
- ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
- }
- }
-
- Eigen::Matrix<double, 1, 2> vertices;
- vertices << max_boundary, min_boundary;
-
- if (max_boundary > min_boundary) {
- double min_distance_sqr = 0;
- Eigen::Matrix<double, 2, 1> closest_point;
- for (int i = 0; i < vertices.innerSize(); i++) {
- Eigen::Matrix<double, 2, 1> point;
- point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
- const double length = (R - point).squaredNorm();
- if (i == 0 || length < min_distance_sqr) {
- closest_point = point;
- min_distance_sqr = length;
- }
- }
- if (is_inside) *is_inside = true;
- return closest_point;
- } else {
- Eigen::Matrix<double, 2, 4> region_vertices =
- region.StaticVertices();
- CHECK_GT(region_vertices.outerSize(), 0);
- double min_distance = INFINITY;
- int closest_i = 0;
- for (int i = 0; i < region_vertices.outerSize(); i++) {
- const double length = ::std::abs(
- (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
- if (i == 0 || length < min_distance) {
- closest_i = i;
- min_distance = length;
- }
- }
- if (is_inside) *is_inside = false;
- return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
- region_vertices(1, closest_i)).finished();
- }
-}
-
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 83c401d..b1d5c3c 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,22 +8,95 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(
- const aos::controls::HVPolytope<2, 4, 4> ®ion,
- const Eigen::Matrix<double, 1, 2> &K, double w,
- const Eigen::Matrix<double, 2, 1> &R, bool *is_inside);
+template <typename Scalar = double>
+Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4, Scalar> ®ion,
+ const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+ const Eigen::Matrix<Scalar, 2, 1> &R, bool *is_inside);
// Intersects a line with a region, and finds the closest point to R.
// Finds a point that is closest to R inside the region, and on the line
// defined by K X = w. If it is not possible to find a point on the line,
// finds a point that is inside the region and closest to the line.
-static inline Eigen::Matrix<double, 2, 1> CoerceGoal(
- const aos::controls::HVPolytope<2, 4, 4> ®ion,
- const Eigen::Matrix<double, 1, 2> &K, double w,
- const Eigen::Matrix<double, 2, 1> &R) {
+template <typename Scalar = double>
+static inline Eigen::Matrix<Scalar, 2, 1> CoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4, Scalar> ®ion,
+ const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+ const Eigen::Matrix<Scalar, 2, 1> &R) {
return DoCoerceGoal(region, K, w, R, nullptr);
}
+template <typename Scalar>
+Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4, Scalar> ®ion,
+ const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+ const Eigen::Matrix<Scalar, 2, 1> &R, bool *is_inside) {
+ if (region.IsInside(R)) {
+ if (is_inside) *is_inside = true;
+ return R;
+ }
+ Eigen::Matrix<Scalar, 2, 1> parallel_vector;
+ Eigen::Matrix<Scalar, 2, 1> perpendicular_vector;
+ perpendicular_vector = K.transpose().normalized();
+ parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
+
+ Eigen::Matrix<Scalar, 4, 1> projectedh = region.static_H() * parallel_vector;
+ Eigen::Matrix<Scalar, 4, 1> projectedk =
+ region.static_k() - region.static_H() * perpendicular_vector * w;
+
+ Scalar min_boundary = ::std::numeric_limits<Scalar>::lowest();
+ Scalar max_boundary = ::std::numeric_limits<Scalar>::max();
+ for (int i = 0; i < 4; ++i) {
+ if (projectedh(i, 0) > 0) {
+ max_boundary =
+ ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
+ } else {
+ min_boundary =
+ ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
+ }
+ }
+
+ Eigen::Matrix<Scalar, 1, 2> vertices;
+ vertices << max_boundary, min_boundary;
+
+ if (max_boundary > min_boundary) {
+ Scalar min_distance_sqr = 0;
+ Eigen::Matrix<Scalar, 2, 1> closest_point;
+ for (int i = 0; i < vertices.innerSize(); i++) {
+ Eigen::Matrix<Scalar, 2, 1> point;
+ point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+ const Scalar length = (R - point).squaredNorm();
+ if (i == 0 || length < min_distance_sqr) {
+ closest_point = point;
+ min_distance_sqr = length;
+ }
+ }
+ if (is_inside) *is_inside = true;
+ return closest_point;
+ } else {
+ Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
+#ifdef __linux__
+ CHECK_GT(region_vertices.outerSize(), 0);
+#else
+ assert(region_vertices.outerSize() > 0);
+#endif
+ Scalar min_distance = INFINITY;
+ int closest_i = 0;
+ for (int i = 0; i < region_vertices.outerSize(); i++) {
+ const Scalar length = ::std::abs(
+ (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+ if (i == 0 || length < min_distance) {
+ closest_i = i;
+ min_distance = length;
+ }
+ }
+ if (is_inside) *is_inside = false;
+ return (Eigen::Matrix<Scalar, 2, 1>() << region_vertices(0, closest_i),
+ region_vertices(1, closest_i))
+ .finished();
+ }
+}
+
} // namespace control_loops
} // namespace frc971
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_;
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e62bff9..2ef2ece 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -10,9 +10,13 @@
self.formatToType = {}
self.formatToType['%f'] = "double"
self.formatToType['%d'] = "int"
- def __str__ (self):
+
+ def Render(self, loop_type):
+ typestring = self.formatToType[self.formatt]
+ if loop_type == 'float' and typestring == 'double':
+ typestring = loop_type
return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (self.formatToType[self.formatt], self.name, self.value)
+ (typestring, self.name, self.value)
class ControlLoopWriter(object):
@@ -138,7 +142,7 @@
fd.write(self._namespace_start)
for const in self._constant_list:
- fd.write(str(const))
+ fd.write(const.Render(self._scalar_type))
fd.write('\n\n')
for loop in self._loops:
@@ -325,6 +329,8 @@
for y in xrange(matrix.shape[1]):
write_type = repr(matrix[x, y])
if scalar_type == 'float':
+ if '.' not in write_type:
+ write_type += '.0'
write_type += 'f'
ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 2380549..4c89fdc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -15,10 +15,10 @@
wheel_radius,
G_high,
G_low,
- q_pos_low,
- q_pos_high,
- q_vel_low,
- q_vel_high,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
efficiency=0.60,
has_imu=False,
force=False,
@@ -349,7 +349,7 @@
def WriteDrivetrain(drivetrain_files, kf_drivetrain_files, year_namespace,
- drivetrain_params):
+ drivetrain_params, scalar_type='double'):
# Write the generated constants out to a file.
drivetrain_low_low = Drivetrain(
@@ -394,13 +394,17 @@
right_low=False,
drivetrain_params=drivetrain_params)
- namespaces = [year_namespace, 'control_loops', 'drivetrain']
+ if isinstance(year_namespace, list):
+ namespaces = year_namespace
+ else:
+ namespaces = [year_namespace, 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [
drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
drivetrain_high_high
],
- namespaces=namespaces)
+ namespaces=namespaces,
+ scalar_type=scalar_type)
dog_loop_writer.AddConstant(
control_loop.Constant("kDt", "%f", drivetrain_low_low.dt))
dog_loop_writer.AddConstant(
@@ -447,7 +451,8 @@
kf_drivetrain_low_low, kf_drivetrain_low_high,
kf_drivetrain_high_low, kf_drivetrain_high_high
],
- namespaces=namespaces)
+ namespaces=namespaces,
+ scalar_type=scalar_type)
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index c9c9efe..29fdeef 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -407,19 +407,23 @@
self.right_gear, self.right_shifter_position)
def WritePolyDrivetrain(drivetrain_files, motor_files, year_namespace,
- drivetrain_params):
+ drivetrain_params, scalar_type='double'):
vdrivetrain = VelocityDrivetrain(drivetrain_params)
- namespaces = [year_namespace, 'control_loops', 'drivetrain']
+ if isinstance(year_namespace, list):
+ namespaces = year_namespace
+ else:
+ namespaces = [year_namespace, 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
- namespaces=namespaces)
+ namespaces=namespaces,
+ scalar_type=scalar_type)
dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], scalar_type=scalar_type)
cim_writer.Write(motor_files[0], motor_files[1])
diff --git a/motors/BUILD b/motors/BUILD
index 3b6433d..292a40f 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -174,6 +174,7 @@
"//motors/peripheral:can",
"//motors/usb",
"//motors/usb:cdc",
+ "//motors/seems_reasonable:drivetrain_lib",
],
)
diff --git a/motors/seems_reasonable/BUILD b/motors/seems_reasonable/BUILD
new file mode 100644
index 0000000..47ca065
--- /dev/null
+++ b/motors/seems_reasonable/BUILD
@@ -0,0 +1,76 @@
+load("//tools:environments.bzl", "mcu_cpus")
+
+py_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:drivetrain",
+ ],
+)
+
+py_binary(
+ name = "polydrivetrain",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:polydrivetrain",
+ ],
+)
+
+genrule(
+ name = "genrule_drivetrain",
+ outs = [
+ "drivetrain_dog_motor_plant.h",
+ "drivetrain_dog_motor_plant.cc",
+ "kalman_drivetrain_motor_plant.h",
+ "kalman_drivetrain_motor_plant.cc",
+ ],
+ cmd = "$(location :drivetrain) $(OUTS)",
+ restricted_to = mcu_cpus,
+ tools = [
+ ":drivetrain",
+ ],
+)
+
+genrule(
+ name = "genrule_polydrivetrain",
+ outs = [
+ "polydrivetrain_dog_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.cc",
+ "polydrivetrain_cim_plant.h",
+ "polydrivetrain_cim_plant.cc",
+ ],
+ cmd = "$(location :polydrivetrain) $(OUTS)",
+ restricted_to = mcu_cpus,
+ tools = [
+ ":polydrivetrain",
+ ],
+)
+
+cc_library(
+ name = "drivetrain_lib",
+ srcs = [
+ "drivetrain_dog_motor_plant.cc",
+ "polydrivetrain_dog_motor_plant.cc",
+ ],
+ hdrs = [
+ "drivetrain_dog_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop_uc",
+ "//frc971/control_loops/drivetrain:polydrivetrain_uc",
+ ],
+)
diff --git a/motors/seems_reasonable/drivetrain.py b/motors/seems_reasonable/drivetrain.py
new file mode 100644
index 0000000..dfa2f16
--- /dev/null
+++ b/motors/seems_reasonable/drivetrain.py
@@ -0,0 +1,38 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import drivetrain
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+kDrivetrain = drivetrain.DrivetrainParams(
+ J=8.0,
+ mass=100.0,
+ robot_radius=0.616 / 2.0,
+ wheel_radius=0.127 / 2.0 * 120.0 / 118.0,
+ G_low=46.0 / 60.0 * 20.0 / 48.0 * 14.0 / 62.0,
+ G_high=62.0 / 44.0 * 20.0 / 48.0 * 14.0 / 62.0,
+ controller_poles=[0.82, 0.82],
+)
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(
+ argv[1:3],
+ argv[3:5], ['motors', 'seems_reasonable'],
+ kDrivetrain,
+ scalar_type='float')
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/motors/seems_reasonable/polydrivetrain.py b/motors/seems_reasonable/polydrivetrain.py
new file mode 100644
index 0000000..4d3720e
--- /dev/null
+++ b/motors/seems_reasonable/polydrivetrain.py
@@ -0,0 +1,35 @@
+#!/usr/bin/python
+
+import sys
+from motors.seems_reasonable import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+def main(argv):
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(
+ argv[1:3],
+ argv[3:5], ['motors', 'seems_reasonable'],
+ drivetrain.kDrivetrain,
+ scalar_type='float')
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index 0467599..c6d70b8 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -13,25 +13,76 @@
#include "motors/usb/cdc.h"
#include "motors/usb/usb.h"
#include "motors/util.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "motors/seems_reasonable/drivetrain_dog_motor_plant.h"
+#include "motors/seems_reasonable/polydrivetrain_dog_motor_plant.h"
namespace frc971 {
namespace motors {
namespace {
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::PolyDrivetrain;
+using ::frc971::constants::ShifterHallEffect;
+using ::frc971::control_loops::DrivetrainQueue_Goal;
+using ::frc971::control_loops::DrivetrainQueue_Output;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<float> &GetDrivetrainConfig() {
+ static DrivetrainConfig<float> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+
+ ::motors::seems_reasonable::MakeDrivetrainLoop,
+ ::motors::seems_reasonable::MakeVelocityDrivetrainLoop,
+ ::std::function<StateFeedbackLoop<7, 2, 4, float>()>(),
+
+ ::motors::seems_reasonable::kDt, ::motors::seems_reasonable::kRobotRadius,
+ ::motors::seems_reasonable::kWheelRadius, ::motors::seems_reasonable::kV,
+
+ ::motors::seems_reasonable::kHighGearRatio,
+ ::motors::seems_reasonable::kLowGearRatio, kThreeStateDriveShifter,
+ kThreeStateDriveShifter, true /* default_high_gear */,
+ 0 /* down_offset if using constants use
+ constants::GetValues().down_error */, 0.8 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */, 1.5 /* wheel_multiplier */,
+ };
+
+ return kDrivetrainConfig;
+};
+
+
::std::atomic<teensy::AcmTty *> global_stdout{nullptr};
+::std::atomic<PolyDrivetrain<float> *> global_polydrivetrain{nullptr};
+
// Last width we received on each channel.
uint16_t pwm_input_widths[5];
// When we received a pulse on each channel in milliseconds.
uint32_t pwm_input_times[5];
+constexpr int kChannelTimeout = 100;
+
+bool lost_channel(int channel) {
+ DisableInterrupts disable_interrupts;
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
+ return true;
+ }
+ return false;
+}
+
// Returns the most recently captured value for the specified input channel
// scaled from -1 to 1, or 0 if it was captured over 100ms ago.
float convert_input_width(int channel) {
uint16_t width;
{
DisableInterrupts disable_interrupts;
- if (time_after(millis(), time_add(pwm_input_times[channel], 100))) {
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
return 0;
}
@@ -73,7 +124,9 @@
// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
// bandwidth.
void vesc_set_current(int vesc_id, float current) {
- const int32_t current_int = current * 1000.0f;
+ constexpr float kMaxCurrent = 80.0f;
+ const int32_t current_int =
+ ::std::max(-kMaxCurrent, ::std::min(kMaxCurrent, current)) * 1000.0f;
uint32_t id = CAN_EFF_FLAG;
id |= vesc_id;
id |= (0x01 /* SET_CURRENT */) << 8;
@@ -91,7 +144,9 @@
// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
// bandwidth.
void vesc_set_duty(int vesc_id, float duty) {
- const int32_t duty_int = duty * 100000.0f;
+ constexpr int32_t kMaxDuty = 99999;
+ const int32_t duty_int = ::std::max(
+ -kMaxDuty, ::std::min(kMaxDuty, static_cast<int32_t>(duty * 100000.0f)));
uint32_t id = CAN_EFF_FLAG;
id |= vesc_id;
id |= (0x00 /* SET_DUTY */) << 8;
@@ -316,7 +371,7 @@
const float sin = ConvertEncoderChannel(adc_readings.sin);
const float cos = ConvertEncoderChannel(adc_readings.cos);
- const float magnitude = ::std::sqrt(sin * sin + cos * cos);
+ const float magnitude = hypot(sin, cos);
const float magnitude_error = ::std::abs(magnitude - 1.0f);
valid = magnitude_error < 0.15f;
@@ -331,6 +386,8 @@
extern "C" void pit3_isr() {
PIT_TFLG3 = 1;
+ PolyDrivetrain<float> *polydrivetrain =
+ global_polydrivetrain.load(::std::memory_order_acquire);
salsa::SimpleAdcReadings adc_readings;
{
@@ -340,20 +397,69 @@
EncoderReading encoder(adc_readings);
- printf("TODO(Austin): 1kHz loop %d %d %d %d %d ADC %" PRIu16 " %" PRIu16
- " enc %d/1000 %s from %d\n",
- (int)(convert_input_width(0) * 1000),
- (int)(convert_input_width(1) * 1000),
- (int)(convert_input_width(2) * 1000),
- (int)(convert_input_width(3) * 1000),
- (int)(convert_input_width(4) * 1000), adc_readings.sin,
- adc_readings.cos, (int)(encoder.angle * 1000),
- encoder.valid ? "T" : "f",
- (int)(::std::sqrt(ConvertEncoderChannel(adc_readings.sin) *
- ConvertEncoderChannel(adc_readings.sin) +
- ConvertEncoderChannel(adc_readings.cos) *
- ConvertEncoderChannel(adc_readings.cos)) *
- 1000));
+ const bool lost_any_channel = lost_channel(0) || lost_channel(1) ||
+ lost_channel(2) || lost_channel(3) ||
+ lost_channel(4) ||
+ (::std::abs(convert_input_width(4)) < 0.05f);
+
+ if (polydrivetrain != nullptr) {
+ DrivetrainQueue_Goal goal;
+ goal.control_loop_driving = false;
+ if (lost_any_channel) {
+ goal.throttle = 0.0f;
+ goal.wheel = 0.0f;
+ } else {
+ goal.throttle = convert_input_width(1);
+ goal.wheel = convert_input_width(0);
+ }
+ goal.throttle = convert_input_width(1);
+ goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
+
+ DrivetrainQueue_Output output;
+
+ polydrivetrain->SetGoal(goal);
+ polydrivetrain->Update();
+ polydrivetrain->SetOutput(&output);
+
+ vesc_set_duty(0, -output.left_voltage / 12.0f);
+ vesc_set_duty(1, -output.left_voltage / 12.0f);
+
+ vesc_set_duty(2, output.right_voltage / 12.0f);
+ vesc_set_duty(3, output.right_voltage / 12.0f);
+
+ static int i = 0;
+ ++i;
+ if (i > 20) {
+ i = 0;
+ if (lost_any_channel) {
+ printf("200Hz loop, disabled %d %d %d %d %d\n",
+ (int)(convert_input_width(0) * 1000),
+ (int)(convert_input_width(1) * 1000),
+ (int)(convert_input_width(2) * 1000),
+ (int)(convert_input_width(3) * 1000),
+ (int)(convert_input_width(4) * 1000));
+ } else {
+ printf(
+ "TODO(Austin): 200Hz loop %d %d %d %d %d, lr, %d, %d velocity %d "
+ "ADC %" PRIu16 " %" PRIu16 " enc %d/1000 %s from %d\n",
+ (int)(convert_input_width(0) * 1000),
+ (int)(convert_input_width(1) * 1000),
+ (int)(convert_input_width(2) * 1000),
+ (int)(convert_input_width(3) * 1000),
+ (int)(convert_input_width(4) * 1000),
+ static_cast<int>(output.left_voltage * 100),
+ static_cast<int>(output.right_voltage * 100),
+ static_cast<int>(polydrivetrain->velocity() * 100),
+ adc_readings.sin, adc_readings.cos, (int)(encoder.angle * 1000),
+ encoder.valid ? "T" : "f",
+ (int)(::std::sqrt(ConvertEncoderChannel(adc_readings.sin) *
+ ConvertEncoderChannel(adc_readings.sin) +
+ ConvertEncoderChannel(adc_readings.cos) *
+ ConvertEncoderChannel(adc_readings.cos)) *
+ 1000));
+ }
+ }
+ }
}
} // namespace
@@ -434,7 +540,7 @@
// Workaround for errata e7914.
(void)PIT_MCR;
PIT_MCR = 0;
- PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 1000) - 1;
+ PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 200) - 1;
PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
can_init(0, 1);
@@ -442,6 +548,9 @@
SetupPwmFtm(FTM0);
SetupPwmFtm(FTM3);
+ PolyDrivetrain<float> polydrivetrain(GetDrivetrainConfig(), nullptr);
+ global_polydrivetrain.store(&polydrivetrain, ::std::memory_order_release);
+
// Leave the LEDs on for a bit longer.
delay(300);
printf("Done starting up\n");
@@ -453,7 +562,8 @@
NVIC_ENABLE_IRQ(IRQ_FTM3);
NVIC_ENABLE_IRQ(IRQ_PIT_CH3);
- DoReceiverTest2();
+ while (true) {
+ }
return 0;
}
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.cc b/y2012/control_loops/drivetrain/drivetrain_base.cc
index 1e0cfad..b94c4f0 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_base.cc
@@ -17,8 +17,8 @@
const 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::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.h b/y2012/control_loops/drivetrain/drivetrain_base.h
index 3d85af1..f5675a8 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.h
+++ b/y2012/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 86ec084..11dbd80 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -164,10 +164,10 @@
const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
bool is_inside_h;
- const auto adjusted_pos_error_h =
- DoCoerceGoal(hv_pos_poly, LH, wh, position_error, &is_inside_h);
+ const auto adjusted_pos_error_h = DoCoerceGoal<double>(
+ hv_pos_poly, LH, wh, position_error, &is_inside_h);
const auto adjusted_pos_error_45 =
- DoCoerceGoal(hv_pos_poly, L45, w45, intersection, nullptr);
+ DoCoerceGoal<double>(hv_pos_poly, L45, w45, intersection, nullptr);
if (pos_poly.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index 2083a24..c3357aa 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -12,9 +12,9 @@
namespace y2014 {
namespace control_loops {
-const DrivetrainConfig &GetDrivetrainConfig() {
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
// TODO(austin): Switch over to using the profile.
- static DrivetrainConfig kDrivetrainConfig{
+ 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,
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.h b/y2014/control_loops/drivetrain/drivetrain_base.h
index 3a9d70e..a47ff5c 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.h
+++ b/y2014/control_loops/drivetrain/drivetrain_base.h
@@ -3,12 +3,11 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-
namespace y2014 {
namespace control_loops {
-const DrivetrainConfig &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace control_loops
} // namespace y2014
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index e7786ee..e39a99a 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -17,8 +17,8 @@
const 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::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.h b/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
index d2e54f9..36f8ddf 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -10,8 +10,8 @@
const double kDrivetrainEncoderRatio =
(17.0 / 50.0) /*output reduction*/ * (24.0 / 64.0) /*encoder gears*/;
-const ::frc971::control_loops::drivetrain::DrivetrainConfig &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 41e6eec..b469f77 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -18,8 +18,8 @@
const 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,
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.h b/y2016/control_loops/drivetrain/drivetrain_base.h
index c67031c..4d5525a 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 0cff7fb..7b48512 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -483,7 +483,7 @@
::aos::common::actions::ActionQueue action_queue_;
- const ::frc971::control_loops::drivetrain::DrivetrainConfig dt_config_;
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
bool is_expanding_ = false;
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
index af3ff38..b351cbf 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -18,8 +18,8 @@
const 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::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.h b/y2017/control_loops/drivetrain/drivetrain_base.h
index 6ef519b..59ca4a9 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.h
+++ b/y2017/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
index 40e18c8..f4be260 100644
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -17,8 +17,8 @@
const 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::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.h b/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
index 88f93c9..4fff031 100644
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2018/control_loops/drivetrain/drivetrain_base.cc b/y2018/control_loops/drivetrain/drivetrain_base.cc
index 838b5ad..01a396d 100644
--- a/y2018/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_base.cc
@@ -17,8 +17,8 @@
const 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::IMU_Z_GYRO,
diff --git a/y2018/control_loops/drivetrain/drivetrain_base.h b/y2018/control_loops/drivetrain/drivetrain_base.h
index a7f40d8..a7ce43f 100644
--- a/y2018/control_loops/drivetrain/drivetrain_base.h
+++ b/y2018/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops