Merge "Add some new boards"
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/analysis/drivetrain_plot_stripped b/frc971/analysis/drivetrain_plot_stripped
index d42952e..3c24485 100644
--- a/frc971/analysis/drivetrain_plot_stripped
+++ b/frc971/analysis/drivetrain_plot_stripped
@@ -13,3 +13,8 @@
drivetrain.stripped goal left_goal
drivetrain.stripped goal right_goal
+
+drivetrain.stripped status left_voltage_error -m 0.1
+drivetrain.stripped status right_voltage_error -m 0.1
+
+drivetrain.stripped using accelerometer_y -m -10
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 d42e6c6..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),
@@ -139,6 +139,16 @@
const double angle = ::std::atan2(::frc971::imu_values->accelerometer_x,
::frc971::imu_values->accelerometer_z) +
0.008;
+
+ switch (dt_config_.imu_type) {
+ case IMUType::IMU_X:
+ last_accel_ = -::frc971::imu_values->accelerometer_x;
+ break;
+ case IMUType::IMU_Y:
+ last_accel_ = -::frc971::imu_values->accelerometer_y;
+ break;
+ }
+
if (accel_squared > 1.03 || accel_squared < 0.97) {
LOG(DEBUG, "New IMU value, rejecting reading\n");
} else {
@@ -206,8 +216,9 @@
}
{
- Eigen::Matrix<double, 3, 1> Y;
- Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
+ Eigen::Matrix<double, 4, 1> Y;
+ Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
+ last_accel_;
kf_.Correct(Y);
integrated_kf_heading_ += dt_config_.dt *
(kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) /
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index d641ab2..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, 3> kf_;
- PolyDrivetrain dt_openloop_;
+ StateFeedbackLoop<7, 2, 4> kf_;
+ PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
@@ -64,6 +64,8 @@
bool right_high_requested_;
bool has_been_enabled_ = false;
+
+ double last_accel_ = 0.0;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 6de9883..6e546a3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -29,6 +29,12 @@
FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
};
+enum class IMUType : int32_t {
+ IMU_X = 0, // Use the x-axis of the IMU.
+ IMU_Y = 1, // Use the y-axis of the IMU.
+};
+
+template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
ShifterType shifter_type;
@@ -39,19 +45,22 @@
// Type of gyro to use.
GyroType gyro_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, 3>()> make_kf_drivetrain_loop;
+ // Type of IMU to use.
+ IMUType imu_type;
- 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.
+ // Polydrivetrain functions returning various controller loops with plants.
+ ::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;
+
+ 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;
@@ -61,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) =
@@ -90,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);
@@ -103,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 5faae63..06b4f21 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -34,12 +34,12 @@
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,
-
+ IMUType::IMU_X,
::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
@@ -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 97df2cf..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, 3> *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 070b7eb..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, 3> *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, 3> *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 ed99034..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, 3> *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 db255d7..5e261e9 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -18,8 +18,8 @@
class DrivetrainMotorsSS {
public:
- DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
- StateFeedbackLoop<7, 2, 3> *kf,
+ DrivetrainMotorsSS(const DrivetrainConfig<double> &dt_config,
+ StateFeedbackLoop<7, 2, 4> *kf,
double *integrated_kf_heading);
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
@@ -42,8 +42,8 @@
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
- const DrivetrainConfig dt_config_;
- StateFeedbackLoop<7, 2, 3> *kf_;
+ const DrivetrainConfig<double> dt_config_;
+ StateFeedbackLoop<7, 2, 4> *kf_;
Eigen::Matrix<double, 7, 1> unprofiled_goal_;
double last_gyro_to_wheel_offset_ = 0;
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 5665fd9..4c89fdc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -8,409 +8,535 @@
import glog
class DrivetrainParams(object):
- def __init__(self, J, mass, robot_radius, wheel_radius, G_high, G_low,
- q_pos_low, q_pos_high, q_vel_low, q_vel_high,
- motor_type = control_loop.CIM(), num_motors = 2, dt = 0.00505,
- controller_poles=[0.90, 0.90], observer_poles=[0.02, 0.02]):
- """Defines all constants of a drivetrain.
+ def __init__(self,
+ J,
+ mass,
+ robot_radius,
+ wheel_radius,
+ G_high,
+ G_low,
+ 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,
+ kf_q_voltage=10.0,
+ motor_type=control_loop.CIM(),
+ num_motors=2,
+ dt=0.00505,
+ controller_poles=[0.90, 0.90],
+ observer_poles=[0.02, 0.02]):
+ """Defines all constants of a drivetrain.
- Args:
- J: float, Moment of inertia of drivetrain in kg m^2
- mass: float, Mass of the robot in kg.
- robot_radius: float, Radius of the robot, in meters (requires tuning by
- hand).
- wheel_radius: float, Radius of the wheels, in meters.
- G_high: float, Gear ratio for high gear.
- G_low: float, Gear ratio for low gear.
- dt: float, Control loop time step.
- q_pos_low: float, q position low gear.
- q_pos_high: float, q position high gear.
- q_vel_low: float, q velocity low gear.
- q_vel_high: float, q velocity high gear.
- motor_type: object, class of values defining the motor in drivetrain.
- num_motors: int, number of motors on one side of drivetrain.
- controller_poles: array, An array of poles. (See control_loop.py)
- observer_poles: array, An array of poles. (See control_loop.py)
- """
+ Args:
+ J: float, Moment of inertia of drivetrain in kg m^2
+ mass: float, Mass of the robot in kg.
+ robot_radius: float, Radius of the robot, in meters (requires tuning by
+ hand).
+ wheel_radius: float, Radius of the wheels, in meters.
+ G_high: float, Gear ratio for high gear.
+ G_low: float, Gear ratio for low gear.
+ dt: float, Control loop time step.
+ q_pos_low: float, q position low gear.
+ q_pos_high: float, q position high gear.
+ q_vel_low: float, q velocity low gear.
+ q_vel_high: float, q velocity high gear.
+ efficiency: float, gear box effiency.
+ has_imu: bool, true if imu is present.
+ force: bool, true if force.
+ kf_q_voltage: float
+ motor_type: object, class of values defining the motor in drivetrain.
+ num_motors: int, number of motors on one side of drivetrain.
+ controller_poles: array, An array of poles. (See control_loop.py)
+ observer_poles: array, An array of poles. (See control_loop.py)
+ """
- self.J = J
- self.mass = mass
- self.robot_radius = robot_radius
- self.wheel_radius = wheel_radius
- self.G_high = G_high
- self.G_low = G_low
- self.dt = dt
- self.q_pos_low = q_pos_low
- self.q_pos_high = q_pos_high
- self.q_vel_low = q_vel_low
- self.q_vel_high = q_vel_high
- self.motor_type = motor_type
- self.num_motors = num_motors
- self.controller_poles = controller_poles
- self.observer_poles = observer_poles
+ self.J = J
+ self.mass = mass
+ self.robot_radius = robot_radius
+ self.wheel_radius = wheel_radius
+ self.G_high = G_high
+ self.G_low = G_low
+ self.dt = dt
+ self.q_pos_low = q_pos_low
+ self.q_pos_high = q_pos_high
+ self.q_vel_low = q_vel_low
+ self.q_vel_high = q_vel_high
+ self.efficiency = efficiency
+ self.has_imu = has_imu
+ self.kf_q_voltage = kf_q_voltage
+ self.motor_type = motor_type
+ self.force = force
+ self.num_motors = num_motors
+ self.controller_poles = controller_poles
+ self.observer_poles = observer_poles
+
class Drivetrain(control_loop.ControlLoop):
- def __init__(self, drivetrain_params, name="Drivetrain", left_low=True,
- right_low=True):
- """Defines a base drivetrain for a robot.
+ def __init__(self,
+ drivetrain_params,
+ name="Drivetrain",
+ left_low=True,
+ right_low=True):
+ """Defines a base drivetrain for a robot.
- Args:
- drivetrain_params: DrivetrainParams, class of values defining the drivetrain.
- name: string, Name of this drivetrain.
- left_low: bool, Whether the left is in high gear.
- right_low: bool, Whether the right is in high gear.
- """
- super(Drivetrain, self).__init__(name)
+ Args:
+ drivetrain_params: DrivetrainParams, class of values defining the drivetrain.
+ name: string, Name of this drivetrain.
+ left_low: bool, Whether the left is in high gear.
+ right_low: bool, Whether the right is in high gear.
+ """
+ super(Drivetrain, self).__init__(name)
- # Moment of inertia of the drivetrain in kg m^2
- self.J = drivetrain_params.J
- # Mass of the robot, in kg.
- self.mass = drivetrain_params.mass
- # Radius of the robot, in meters (requires tuning by hand)
- self.robot_radius = drivetrain_params.robot_radius
- # Radius of the wheels, in meters.
- self.r = drivetrain_params.wheel_radius
+ # Moment of inertia of the drivetrain in kg m^2
+ self.J = drivetrain_params.J
+ # Mass of the robot, in kg.
+ self.mass = drivetrain_params.mass
+ # Radius of the robot, in meters (requires tuning by hand)
+ self.robot_radius = drivetrain_params.robot_radius
+ # Radius of the wheels, in meters.
+ self.r = drivetrain_params.wheel_radius
+ self.has_imu = drivetrain_params.has_imu
- # Gear ratios
- self.G_low = drivetrain_params.G_low
- self.G_high = drivetrain_params.G_high
- if left_low:
- self.Gl = self.G_low
- else:
- self.Gl = self.G_high
- if right_low:
- self.Gr = self.G_low
- else:
- self.Gr = self.G_high
+ # Gear ratios
+ self.G_low = drivetrain_params.G_low
+ self.G_high = drivetrain_params.G_high
+ if left_low:
+ self.Gl = self.G_low
+ else:
+ self.Gl = self.G_high
+ if right_low:
+ self.Gr = self.G_low
+ else:
+ self.Gr = self.G_high
- # Control loop time step
- self.dt = drivetrain_params.dt
+ # Control loop time step
+ self.dt = drivetrain_params.dt
- self.BuildDrivetrain(drivetrain_params.motor_type, drivetrain_params.num_motors);
+ self.efficiency = drivetrain_params.efficiency
+ self.force = drivetrain_params.force
- if left_low or right_low:
- q_pos = drivetrain_params.q_pos_low
- q_vel = drivetrain_params.q_vel_low
- else:
- q_pos = drivetrain_params.q_pos_high
- q_vel = drivetrain_params.q_vel_high
+ self.BuildDrivetrain(drivetrain_params.motor_type,
+ drivetrain_params.num_motors)
- self.BuildDrivetrainController(q_pos, q_vel)
+ if left_low or right_low:
+ q_pos = drivetrain_params.q_pos_low
+ q_vel = drivetrain_params.q_vel_low
+ else:
+ q_pos = drivetrain_params.q_pos_high
+ q_vel = drivetrain_params.q_vel_high
- self.InitializeState()
+ self.BuildDrivetrainController(q_pos, q_vel)
- def BuildDrivetrain(self, motor, num_motors_per_side):
- self.motor = motor
- # Number of motors per side
- self.num_motors = num_motors_per_side
- # Stall Torque in N m
- self.stall_torque = motor.stall_torque * self.num_motors * 0.60
- # Stall Current in Amps
- self.stall_current = motor.stall_current * self.num_motors
- # Free Speed in rad/s
- self.free_speed = motor.free_speed
- # Free Current in Amps
- self.free_current = motor.free_current * self.num_motors
+ self.InitializeState()
- # Effective motor resistance in ohms.
- self.resistance = 12.0 / self.stall_current
+ def BuildDrivetrain(self, motor, num_motors_per_side):
+ self.motor = motor
+ # Number of motors per side
+ self.num_motors = num_motors_per_side
+ # Stall Torque in N m
+ self.stall_torque = motor.stall_torque * self.num_motors * self.efficiency
+ # Stall Current in Amps
+ self.stall_current = motor.stall_current * self.num_motors
+ # Free Speed in rad/s
+ self.free_speed = motor.free_speed
+ # Free Current in Amps
+ self.free_current = motor.free_current * self.num_motors
- # Resistance of the motor, divided by the number of motors.
- # Motor velocity constant
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
+ # Effective motor resistance in ohms.
+ self.resistance = 12.0 / self.stall_current
- # These describe the way that a given side of a robot will be influenced
- # by the other side. Units of 1 / kg.
- self.msp = 1.0 / self.mass + self.robot_radius * self.robot_radius / self.J
- self.msn = 1.0 / self.mass - self.robot_radius * self.robot_radius / self.J
- # The calculations which we will need for A and B.
- self.tcl = self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
- self.tcr = self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
- self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
- self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
+ # Resistance of the motor, divided by the number of motors.
+ # Motor velocity constant
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
- # State feedback matrices
- # X will be of the format
- # [[positionl], [velocityl], [positionr], velocityr]]
- self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -self.msp * self.tcl, 0, -self.msn * self.tcr],
- [0, 0, 0, 1],
- [0, -self.msn * self.tcl, 0, -self.msp * self.tcr]])
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [self.msp * self.mpl, self.msn * self.mpr],
- [0, 0],
- [self.msn * self.mpl, self.msp * self.mpr]])
- self.C = numpy.matrix([[1, 0, 0, 0],
- [0, 0, 1, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.mass + self.robot_radius * self.robot_radius / self.J
+ self.msn = 1.0 / self.mass - self.robot_radius * self.robot_radius / self.J
+ # The calculations which we will need for A and B.
+ self.tcl = self.Kt / self.Kv / (
+ self.Gl * self.Gl * self.resistance * self.r * self.r)
+ self.tcr = self.Kt / self.Kv / (
+ self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+ self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # X will be of the format
+ # [[positionl], [velocityl], [positionr], velocityr]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0], [0, -self.msp * self.tcl, 0, -self.msn * self.tcr],
+ [0, 0, 0, 1], [0, -self.msn * self.tcl, 0, -self.msp * self.tcr]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0], [self.msp * self.mpl, self.msn * self.mpr], [0, 0],
+ [self.msn * self.mpl, self.msp * self.mpr]])
+ self.C = numpy.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- def BuildDrivetrainController(self, q_pos, q_vel):
- # Tune the LQR controller
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ def BuildDrivetrainController(self, q_pos, q_vel):
+ # Tune the LQR controller
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0,
+ 0.0], [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos**2.0)),
+ 0.0], [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
- glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, self._name)
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- glog.debug('K %s', repr(self.K))
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0)), 0.0],
+ [0.0, (1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.hlp = 0.3
- self.llp = 0.4
- self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, self._name)
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.hlp = 0.3
+ self.llp = 0.4
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
class KFDrivetrain(Drivetrain):
- def __init__(self, drivetrain_params, name="KFDrivetrain",
- left_low=True, right_low=True):
- """Kalman filter values of a drivetrain.
+ def __init__(self,
+ drivetrain_params,
+ name="KFDrivetrain",
+ left_low=True,
+ right_low=True):
+ """Kalman filter values of a drivetrain.
- Args:
- drivetrain_params: DrivetrainParams, class of values defining the drivetrain.
- name: string, Name of this drivetrain.
- left_low: bool, Whether the left is in high gear.
- right_low: bool, Whether the right is in high gear.
- """
- super(KFDrivetrain, self).__init__(drivetrain_params, name, left_low, right_low)
+ Args:
+ drivetrain_params: DrivetrainParams, class of values defining the drivetrain.
+ name: string, Name of this drivetrain.
+ left_low: bool, Whether the left is in high gear.
+ right_low: bool, Whether the right is in high gear.
+ """
+ super(KFDrivetrain, self).__init__(drivetrain_params, name, left_low,
+ right_low)
- self.unaugmented_A_continuous = self.A_continuous
- self.unaugmented_B_continuous = self.B_continuous
+ self.unaugmented_A_continuous = self.A_continuous
+ self.unaugmented_B_continuous = self.B_continuous
- # The practical voltage applied to the wheels is
- # V_left = U_left + left_voltage_error
- #
- # The states are
- # [left position, left velocity, right position, right velocity,
- # left voltage error, right voltage error, angular_error]
- #
- # The left and right positions are filtered encoder positions and are not
- # adjusted for heading error.
- # The turn velocity as computed by the left and right velocities is
- # adjusted by the gyro velocity.
- # The angular_error is the angular velocity error between the wheel speed
- # and the gyro speed.
- self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
- self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
- self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
- self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
- self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
- self.A_continuous[0,6] = 1
- self.A_continuous[2,6] = -1
+ # The practical voltage applied to the wheels is
+ # V_left = U_left + left_voltage_error
+ #
+ # The states are
+ # [left position, left velocity, right position, right velocity,
+ # left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
+ self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+ self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+ self.A_continuous[0:4, 0:4] = self.unaugmented_A_continuous
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ if self.force:
+ self.A_continuous[0:4, 4:6] = numpy.matrix(
+ [[0.0, 0.0], [self.msp, self.msn], [0.0, 0.0],
+ [self.msn, self.msp]])
+ q_voltage = drivetrain_params.kf_q_voltage * self.mpl
+ else:
+ self.A_continuous[0:4, 4:6] = self.unaugmented_B_continuous
+ q_voltage = drivetrain_params.kf_q_voltage
- self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
- [0, 0, 1, 0, 0, 0, 0],
- [0, -0.5 / drivetrain_params.robot_radius, 0, 0.5 / drivetrain_params.robot_radius, 0, 0, 0]])
+ self.B_continuous[0:4, 0:2] = self.unaugmented_B_continuous
+ self.A_continuous[0, 6] = 1
+ self.A_continuous[2, 6] = -1
- self.D = numpy.matrix([[0, 0],
- [0, 0],
- [0, 0]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- q_pos = 0.05
- q_vel = 1.00
- q_voltage = 10.0
- q_encoder_uncertainty = 2.00
+ if self.has_imu:
+ self.C = numpy.matrix(
+ [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
+ 0, -0.5 / drivetrain_params.robot_radius, 0,
+ 0.5 / drivetrain_params.robot_radius, 0, 0, 0
+ ], [0, 0, 0, 0, 0, 0, 0]])
+ gravity = 9.8
+ self.C[3, 0:6] = 0.5 * (
+ self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]
+ ) / gravity
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
+ self.D = numpy.matrix([[0, 0], [0, 0], [0, 0], [
+ 0.5 *
+ (self.B_continuous[1, 0] + self.B_continuous[3, 0]) / gravity,
+ 0.5 *
+ (self.B_continuous[1, 1] + self.B_continuous[3, 1]) / gravity
+ ]])
+ else:
+ self.C = numpy.matrix(
+ [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
+ 0, -0.5 / drivetrain_params.robot_radius, 0,
+ 0.5 / drivetrain_params.robot_radius, 0, 0, 0
+ ]])
- r_pos = 0.0001
- r_gyro = 0.000001
- self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
- [0.0, (r_pos ** 2.0), 0.0],
- [0.0, 0.0, (r_gyro ** 2.0)]])
+ self.D = numpy.matrix([[0, 0], [0, 0], [0, 0]])
- # Solving for kf gains.
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ q_pos = 0.05
+ q_vel = 1.00
+ q_encoder_uncertainty = 2.00
- self.L = self.A * self.KalmanGain
+ self.Q = numpy.matrix(
+ [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0], [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0,
+ 0.0], [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0,
+ 0.0], [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0,
+ 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty**2.0)]])
- unaug_K = self.K
+ r_pos = 0.0001
+ r_gyro = 0.000001
+ if self.has_imu:
+ r_accelerometer = 7.0
+ self.R = numpy.matrix([[(r_pos**2.0), 0.0, 0.0, 0.0],
+ [0.0, (r_pos**2.0), 0.0, 0.0],
+ [0.0, 0.0, (r_gyro**2.0), 0.0],
+ [0.0, 0.0, 0.0, (r_accelerometer**2.0)]])
+ else:
+ self.R = numpy.matrix([[(r_pos**2.0), 0.0, 0.0],
+ [0.0, (r_pos**2.0), 0.0],
+ [0.0, 0.0, (r_gyro**2.0)]])
- # Implement a nice closed loop controller for use by the closed loop
- # controller.
- self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
- self.K[0:2, 0:4] = unaug_K
- self.K[0, 4] = 1.0
- self.K[1, 5] = 1.0
+ # Solving for kf gains.
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.Qff = numpy.matrix(numpy.zeros((4, 4)))
- qff_pos = 0.005
- qff_vel = 1.00
- self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
- self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
- self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
- self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
- self.Kff = numpy.matrix(numpy.zeros((2, 7)))
- self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+ if not self.has_imu:
+ self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
+ self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
+ self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
- self.InitializeState()
+ self.L = self.A * self.KalmanGain
+
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
+ self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ if self.force:
+ self.K[0, 4] = 1.0 / self.mpl
+ self.K[1, 5] = 1.0 / self.mpr
+ else:
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos**2.0
+ self.Qff[1, 1] = 1.0 / qff_vel**2.0
+ self.Qff[2, 2] = 1.0 / qff_pos**2.0
+ self.Qff[3, 3] = 1.0 / qff_vel**2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(
+ self.B[0:4, :], self.Qff)
+
+ self.InitializeState()
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(name="DrivetrainLowLow",
- left_low=True, right_low=True, drivetrain_params=drivetrain_params)
- drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh",
- left_low=True, right_low=False, drivetrain_params=drivetrain_params)
- drivetrain_high_low = Drivetrain(name="DrivetrainHighLow",
- left_low=False, right_low=True, drivetrain_params=drivetrain_params)
- drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh",
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ # Write the generated constants out to a file.
+ drivetrain_low_low = Drivetrain(
+ name="DrivetrainLowLow",
+ left_low=True,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ drivetrain_low_high = Drivetrain(
+ name="DrivetrainLowHigh",
+ left_low=True,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ drivetrain_high_low = Drivetrain(
+ name="DrivetrainHighLow",
+ left_low=False,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ drivetrain_high_high = Drivetrain(
+ name="DrivetrainHighHigh",
+ left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
- kf_drivetrain_low_low = KFDrivetrain(name="KFDrivetrainLowLow",
- left_low=True, right_low=True, drivetrain_params=drivetrain_params)
- kf_drivetrain_low_high = KFDrivetrain(name="KFDrivetrainLowHigh",
- left_low=True, right_low=False, drivetrain_params=drivetrain_params)
- kf_drivetrain_high_low = KFDrivetrain(name="KFDrivetrainHighLow",
- left_low=False, right_low=True, drivetrain_params=drivetrain_params)
- kf_drivetrain_high_high = KFDrivetrain(name="KFDrivetrainHighHigh",
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ kf_drivetrain_low_low = KFDrivetrain(
+ name="KFDrivetrainLowLow",
+ left_low=True,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_low_high = KFDrivetrain(
+ name="KFDrivetrainLowHigh",
+ left_low=True,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_high_low = KFDrivetrain(
+ name="KFDrivetrainHighLow",
+ left_low=False,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_high_high = KFDrivetrain(
+ name="KFDrivetrainHighHigh",
+ left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
- 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)
- dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
- drivetrain_low_low.dt))
- dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
- drivetrain_low_low.stall_torque))
- dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
- drivetrain_low_low.stall_current))
- dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeed", "%f",
- drivetrain_low_low.free_speed))
- dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
- drivetrain_low_low.free_current))
- dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
- drivetrain_low_low.J))
- dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
- drivetrain_low_low.mass))
- dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
- drivetrain_low_low.robot_radius))
- dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
- drivetrain_low_low.r))
- dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
- drivetrain_low_low.resistance))
- dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
- drivetrain_low_low.Kv))
- dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
- drivetrain_low_low.Kt))
- dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
- drivetrain_low_low.G_low))
- dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
- drivetrain_high_high.G_high))
- dog_loop_writer.AddConstant(control_loop.Constant("kHighOutputRatio", "%f",
- drivetrain_high_high.G_high * drivetrain_high_high.r))
+ 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,
+ scalar_type=scalar_type)
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kDt", "%f", drivetrain_low_low.dt))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kStallTorque", "%f",
+ drivetrain_low_low.stall_torque))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kStallCurrent", "%f",
+ drivetrain_low_low.stall_current))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kFreeSpeed", "%f",
+ drivetrain_low_low.free_speed))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kFreeCurrent", "%f",
+ drivetrain_low_low.free_current))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kJ", "%f", drivetrain_low_low.J))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kMass", "%f", drivetrain_low_low.mass))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kRobotRadius", "%f",
+ drivetrain_low_low.robot_radius))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kWheelRadius", "%f", drivetrain_low_low.r))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kR", "%f", drivetrain_low_low.resistance))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kV", "%f", drivetrain_low_low.Kv))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kT", "%f", drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kLowGearRatio", "%f", drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
+ dog_loop_writer.AddConstant(
+ control_loop.Constant(
+ "kHighOutputRatio", "%f",
+ drivetrain_high_high.G_high * drivetrain_high_high.r))
- dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
+ dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- kf_loop_writer = control_loop.ControlLoopWriter(
- "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
- kf_drivetrain_high_low, kf_drivetrain_high_high],
- namespaces = namespaces)
- kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "KFDrivetrain", [
+ kf_drivetrain_low_low, kf_drivetrain_low_high,
+ kf_drivetrain_high_low, kf_drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
+ kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
+
def PlotDrivetrainMotions(drivetrain_params):
- # Simulate the response of the system to a step input.
- drivetrain = Drivetrain(left_low=False, right_low=False, drivetrain_params=drivetrain_params)
- simulated_left = []
- simulated_right = []
- for _ in xrange(100):
- drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
- simulated_left.append(drivetrain.X[0, 0])
- simulated_right.append(drivetrain.X[2, 0])
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain(
+ left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
- pylab.rc('lines', linewidth=4)
- pylab.plot(range(100), simulated_left, label='left position')
- pylab.plot(range(100), simulated_right, 'r--', label='right position')
- pylab.suptitle('Acceleration Test\n12 Volt Step Input')
- pylab.legend(loc='lower right')
- pylab.show()
+ pylab.rc('lines', linewidth=4)
+ pylab.plot(range(100), simulated_left, label='left position')
+ pylab.plot(range(100), simulated_right, 'r--', label='right position')
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input')
+ pylab.legend(loc='lower right')
+ pylab.show()
- # Simulate forwards motion.
- drivetrain = Drivetrain(left_low=False, right_low=False, drivetrain_params=drivetrain_params)
- close_loop_left = []
- close_loop_right = []
- left_power = []
- right_power = []
- R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
- for _ in xrange(300):
- U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
- drivetrain.U_min, drivetrain.U_max)
- drivetrain.UpdateObserver(U)
- drivetrain.Update(U)
- close_loop_left.append(drivetrain.X[0, 0])
- close_loop_right.append(drivetrain.X[2, 0])
- left_power.append(U[0, 0])
- right_power.append(U[1, 0])
+ # Simulate forwards motion.
+ drivetrain = Drivetrain(
+ left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ close_loop_left = []
+ close_loop_right = []
+ left_power = []
+ right_power = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
+ drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
- pylab.plot(range(300), close_loop_left, label='left position')
- pylab.plot(range(300), close_loop_right, 'm--', label='right position')
- pylab.plot(range(300), left_power, label='left power')
- pylab.plot(range(300), right_power, '--', label='right power')
- pylab.suptitle('Linear Move\nLeft and Right Position going to 1')
- pylab.legend()
- pylab.show()
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, 'm--', label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, '--', label='right power')
+ pylab.suptitle('Linear Move\nLeft and Right Position going to 1')
+ pylab.legend()
+ pylab.show()
- # Try turning in place
- drivetrain = Drivetrain(drivetrain_params=drivetrain_params)
- close_loop_left = []
- close_loop_right = []
- R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
- for _ in xrange(200):
- U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
- drivetrain.U_min, drivetrain.U_max)
- drivetrain.UpdateObserver(U)
- drivetrain.Update(U)
- close_loop_left.append(drivetrain.X[0, 0])
- close_loop_right.append(drivetrain.X[2, 0])
+ # Try turning in place
+ drivetrain = Drivetrain(drivetrain_params=drivetrain_params)
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(200):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
+ drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
- pylab.plot(range(200), close_loop_left, label='left position')
- pylab.plot(range(200), close_loop_right, label='right position')
- pylab.suptitle('Angular Move\nLeft position going to -1 and right position going to 1')
- pylab.legend(loc='center right')
- pylab.show()
+ pylab.plot(range(200), close_loop_left, label='left position')
+ pylab.plot(range(200), close_loop_right, label='right position')
+ pylab.suptitle(
+ 'Angular Move\nLeft position going to -1 and right position going to 1'
+ )
+ pylab.legend(loc='center right')
+ pylab.show()
- # Try turning just one side.
- drivetrain = Drivetrain(drivetrain_params=drivetrain_params)
- close_loop_left = []
- close_loop_right = []
- R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
- for _ in xrange(300):
- U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
- drivetrain.U_min, drivetrain.U_max)
- drivetrain.UpdateObserver(U)
- drivetrain.Update(U)
- close_loop_left.append(drivetrain.X[0, 0])
- close_loop_right.append(drivetrain.X[2, 0])
+ # Try turning just one side.
+ drivetrain = Drivetrain(drivetrain_params=drivetrain_params)
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
+ drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
- pylab.plot(range(300), close_loop_left, label='left position')
- pylab.plot(range(300), close_loop_right, label='right position')
- pylab.suptitle('Pivot\nLeft position not changing and right position going to 1')
- pylab.legend(loc='center right')
- pylab.show()
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.suptitle(
+ 'Pivot\nLeft position not changing and right position going to 1')
+ pylab.legend(loc='center right')
+ pylab.show()
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/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 5271cc2..96315c7 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -1,73 +1,75 @@
-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 = 'averager',
- hdrs = [
- 'averager.h',
- ],
+ name = "averager",
+ hdrs = [
+ "averager.h",
+ ],
)
cc_test(
- name = 'averager_test',
- srcs = [
- 'averager_test.cc',
- ],
- deps = [
- ':averager',
- '//aos/testing:googletest',
- ],
+ name = "averager_test",
+ srcs = [
+ "averager_test.cc",
+ ],
+ deps = [
+ ":averager",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'zeroing',
- srcs = [
- 'zeroing.cc',
- ],
- hdrs = [
- 'zeroing.h',
- ],
- deps = [
- ':wrap',
- '//frc971/control_loops:queues',
- '//frc971:constants',
- ],
+ name = "zeroing",
+ srcs = [
+ "zeroing.cc",
+ ],
+ hdrs = [
+ "zeroing.h",
+ ],
+ deps = [
+ ":wrap",
+ "//frc971:constants",
+ "//frc971/control_loops:queues",
+ ],
)
cc_test(
- name = 'zeroing_test',
- srcs = [
- 'zeroing_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- '//aos/testing:test_shm',
- ':zeroing',
- '//aos/common/util:thread',
- '//aos/common:die',
- '//frc971/control_loops:position_sensor_sim',
- '//frc971/control_loops:queues',
- ],
+ name = "zeroing_test",
+ srcs = [
+ "zeroing_test.cc",
+ ],
+ deps = [
+ ":zeroing",
+ "//aos/common:die",
+ "//aos/common/util:thread",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:queues",
+ ],
)
cc_library(
- name = 'wrap',
- srcs = [
- 'wrap.cc',
- ],
- hdrs = [
- 'wrap.h',
- ],
+ name = "wrap",
+ srcs = [
+ "wrap.cc",
+ ],
+ hdrs = [
+ "wrap.h",
+ ],
+ compatible_with = mcu_cpus,
)
cc_test(
- name = 'wrap_test',
- srcs = [
- 'wrap_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':wrap',
- ],
+ name = "wrap_test",
+ srcs = [
+ "wrap_test.cc",
+ ],
+ deps = [
+ ":wrap",
+ "//aos/testing:googletest",
+ ],
)
diff --git a/frc971/zeroing/wrap.cc b/frc971/zeroing/wrap.cc
index 2698928..593ac97 100644
--- a/frc971/zeroing/wrap.cc
+++ b/frc971/zeroing/wrap.cc
@@ -5,8 +5,12 @@
namespace frc971 {
namespace zeroing {
+float Wrap(float nearest, float value, float period) {
+ return remainderf(value - nearest, period) + nearest;
+}
+
double Wrap(double nearest, double value, double period) {
- return ::std::remainder(value - nearest, period) + nearest;
+ return remainder(value - nearest, period) + nearest;
}
} // namespace zeroing
diff --git a/frc971/zeroing/wrap.h b/frc971/zeroing/wrap.h
index 84b049d..c6ec085 100644
--- a/frc971/zeroing/wrap.h
+++ b/frc971/zeroing/wrap.h
@@ -7,6 +7,7 @@
// Returns a modified value which has been wrapped such that it is +- period/2
// away from nearest.
double Wrap(double nearest, double value, double period);
+float Wrap(float nearest, float value, float period);
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/wrap_test.cc b/frc971/zeroing/wrap_test.cc
index e195385..b5f9100 100644
--- a/frc971/zeroing/wrap_test.cc
+++ b/frc971/zeroing/wrap_test.cc
@@ -1,6 +1,7 @@
-#include <random>
-
#include "frc971/zeroing/wrap.h"
+
+#include <cmath>
+
#include "gtest/gtest.h"
namespace frc971 {
@@ -39,6 +40,18 @@
}
}
+// Tests some various positive and negative values for wrap (with floats).
+TEST(WrapTest, TestFloatWrap) {
+ EXPECT_NEAR(1.0f, Wrap(0.0f, 1.0f, 10.0f), 1e-6f);
+ EXPECT_NEAR(-1.0f, Wrap(0.0f, -1.0f, 10.0f), 1e-6f);
+
+ EXPECT_NEAR(1.0f, Wrap(5.0f, 1.0f, 10.0f), 1e-6f);
+ EXPECT_NEAR(9.0f, Wrap(5.0f, -1.0f, 10.0f), 1e-6f);
+
+ EXPECT_NEAR(10.0f, Wrap(5.0f, 10.0f, 10.0f), 1e-6f);
+ EXPECT_NEAR(1.0f, Wrap(5.0f, -9.0f, 10.0f), 1e-6f);
+}
+
} // namespace testing
} // namespace zeroing
} // namespace frc971
diff --git a/motors/BUILD b/motors/BUILD
index 3b6433d..0a63da0 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -172,6 +172,8 @@
"//motors/core",
"//motors/peripheral:adc",
"//motors/peripheral:can",
+ "//motors/seems_reasonable:drivetrain_lib",
+ "//motors/seems_reasonable:spring",
"//motors/usb",
"//motors/usb:cdc",
],
diff --git a/motors/seems_reasonable/BUILD b/motors/seems_reasonable/BUILD
new file mode 100644
index 0000000..a1613eb
--- /dev/null
+++ b/motors/seems_reasonable/BUILD
@@ -0,0 +1,96 @@
+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",
+ ],
+)
+
+cc_library(
+ name = "spring",
+ srcs = ["spring.cc"],
+ hdrs = ["spring.h"],
+ compatible_with = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/zeroing:wrap"],
+)
+
+cc_test(
+ name = "spring_test",
+ srcs = [
+ "spring_test.cc",
+ ],
+ deps = [
+ ":spring",
+ "//aos/testing:googletest",
+ ],
+)
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/seems_reasonable/spring.cc b/motors/seems_reasonable/spring.cc
new file mode 100644
index 0000000..d66e950
--- /dev/null
+++ b/motors/seems_reasonable/spring.cc
@@ -0,0 +1,152 @@
+#include "motors/seems_reasonable/spring.h"
+
+#include "frc971/zeroing/wrap.h"
+
+#include <cmath>
+
+namespace motors {
+namespace seems_reasonable {
+namespace {
+
+constexpr float kTwoPi = 2.0 * M_PI;
+
+} // namespace
+
+float NextGoal(float current_goal, float goal) {
+ float remainder = remainderf(current_goal - goal, kTwoPi);
+ if (remainder >= 0.0f) {
+ remainder -= kTwoPi;
+ }
+ return -remainder + current_goal;
+}
+
+float PreviousGoal(float current_goal, float goal) {
+ float remainder = remainderf(current_goal - goal, kTwoPi);
+ if (remainder <= 0.0f) {
+ remainder += kTwoPi;
+ }
+ return -remainder + current_goal;
+}
+
+void Spring::Iterate(bool unload, bool prime, bool fire, bool force_reset,
+ bool encoder_valid, float angle) {
+ // Angle is +- M_PI. So, we need to find the nearest angle to the previous
+ // one, and that's our new angle.
+ angle_ = ::frc971::zeroing::Wrap(angle_, angle, kTwoPi);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Go to the previous unload from where we are.
+ goal_ = angle_;
+ goal_ = PreviousGoal(kUnloadGoal);
+ if (prime && fire) {
+ Unload();
+ }
+ break;
+ case State::UNLOAD:
+ if (!encoder_valid) {
+ state_ = State::STUCK_UNLOAD;
+ } else if (!unload && prime && fire) {
+ // Go to the next goal from the current location. This handles if we
+ // fired or didn't on the previous cycle.
+ goal_ = angle_;
+ goal_ = NextGoal(kLoadGoal);
+ Load();
+ }
+ case State::STUCK_UNLOAD:
+ if (force_reset && encoder_valid && state_ == State::STUCK_UNLOAD) {
+ state_ = State::UNINITIALIZED;
+ } else if (timeout_ > 0) {
+ --timeout_;
+ }
+ break;
+ case State::LOAD:
+ if (!encoder_valid) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ StuckUnload();
+ } else if (unload) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ Unload();
+ } else if (!Near()) {
+ if (timeout_ > 0) {
+ --timeout_;
+ } else {
+ StuckUnload();
+ }
+ } else if (prime) {
+ goal_ = NextGoal(kPrimeGoal);
+ Prime();
+ }
+ break;
+ case State::PRIME:
+ if (!encoder_valid) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ StuckUnload();
+ } else if (unload) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ Unload();
+ } else if (!prime) {
+ goal_ = PreviousGoal(kLoadGoal);
+ Load();
+ } else if (!Near()) {
+ if (timeout_ > 0) {
+ --timeout_;
+ } else {
+ StuckUnload();
+ }
+ } else if (fire) {
+ goal_ = NextGoal(kFireGoal);
+ Fire();
+ }
+ break;
+
+ case State::FIRE:
+ if (!encoder_valid) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ StuckUnload();
+ } else if (!Near()) {
+ if (timeout_ > 0) {
+ --timeout_;
+ } else {
+ StuckUnload();
+ }
+ } else {
+ // TODO(austin): Maybe have a different timeout for success.
+ if (timeout_ > 0) {
+ timeout_--;
+ } else {
+ Load();
+ goal_ = NextGoal(kLoadGoal);
+ }
+ }
+ break;
+ }
+ const float error = goal_ - angle_;
+ const float derror = (error - last_error_) * 200.0f;
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ output_ = 0.0f;
+ break;
+ case State::STUCK_UNLOAD:
+ case State::UNLOAD:
+ if (timeout_ > 0) {
+ output_ = -0.1f;
+ } else {
+ output_ = 0.0f;
+ }
+ break;
+
+ case State::LOAD:
+ case State::PRIME:
+ case State::FIRE: {
+ constexpr float kP = 3.00f;
+ constexpr float kD = 0.00f;
+ output_ = kP * error + kD * derror;
+ } break;
+ }
+ last_error_ = error;
+}
+
+} // namespace seems_reasonable
+} // namespace motors
diff --git a/motors/seems_reasonable/spring.h b/motors/seems_reasonable/spring.h
new file mode 100644
index 0000000..9f8f879
--- /dev/null
+++ b/motors/seems_reasonable/spring.h
@@ -0,0 +1,102 @@
+#ifndef MOTORS_SEEMS_REASONABLE_SPRING_H_
+#define MOTORS_SEEMS_REASONABLE_SPRING_H_
+
+#include <cmath>
+
+namespace motors {
+namespace seems_reasonable {
+
+float NextGoal(float current_goal, float goal);
+float PreviousGoal(float current_goal, float goal);
+
+class Spring {
+ public:
+ Spring() = default;
+ Spring(const Spring &) = delete;
+ Spring &operator=(const Spring &) = delete;
+
+ // Iterates the loop.
+ // If unload is true, unload.
+ // If the encoder isn't valid, unload.
+ // If prime is true, go to primed state.
+ // If prime and fire are true, fire.
+ void Iterate(bool unload, bool prime, bool fire, bool force_reset,
+ bool encoder_valid, float angle);
+
+ enum class State {
+ UNINITIALIZED = 0,
+ STUCK_UNLOAD = 1,
+ UNLOAD = 2,
+ LOAD = 3,
+ PRIME = 4,
+ FIRE = 5,
+ };
+
+ // Returns the current to output to the spring motors.
+ float output() const { return output_; }
+
+ // Returns true if the motor is near the goal.
+ bool Near() { return ::std::abs(angle_ - goal_) < 0.2f; }
+
+ State state() const { return state_; }
+
+ float angle() const { return angle_; }
+ float goal() const { return goal_; }
+
+ int timeout() const { return timeout_; }
+
+ private:
+ void Load() {
+ timeout_ = 5 * 200;
+ state_ = State::LOAD;
+ }
+
+ void Prime() {
+ timeout_ = 1 * 200;
+ state_ = State::PRIME;
+ }
+
+ void Unload() {
+ timeout_ = 10 * 200;
+ state_ = State::UNLOAD;
+ }
+
+ void StuckUnload() {
+ timeout_ = 10 * 200;
+ state_ = State::STUCK_UNLOAD;
+ }
+
+ void Fire() {
+ timeout_ = 100;
+ state_ = State::FIRE;
+ }
+
+ float NextGoal(float goal) {
+ return ::motors::seems_reasonable::NextGoal(goal_, goal);
+ }
+
+ float PreviousGoal(float goal) {
+ return ::motors::seems_reasonable::PreviousGoal(goal_, goal);
+ }
+
+ State state_ = State::UNINITIALIZED;
+
+ // Note, these need to be (-M_PI, M_PI]
+ constexpr static float kLoadGoal = -0.345f;
+ constexpr static float kPrimeGoal = -0.269f;
+ constexpr static float kFireGoal = -0.163f;
+ constexpr static float kUnloadGoal = kFireGoal;
+
+ float angle_ = 0.0f;
+ float goal_ = 0.0f;
+
+ int timeout_ = 0;
+
+ float output_ = 0.0f;
+ float last_error_ = 0.0f;
+};
+
+} // namespace seems_reasonable
+} // namespace motors
+
+#endif // MOTORS_SEEMS_REASONABLE_SPRING_H_
diff --git a/motors/seems_reasonable/spring_test.cc b/motors/seems_reasonable/spring_test.cc
new file mode 100644
index 0000000..bcc2a8a
--- /dev/null
+++ b/motors/seems_reasonable/spring_test.cc
@@ -0,0 +1,35 @@
+#include "motors/seems_reasonable/spring.h"
+
+#include "gtest/gtest.h"
+
+namespace motors {
+namespace seems_reasonable {
+namespace testing {
+
+// Tests that NextGoal always returns the next goal.
+TEST(GoalTest, TestNextGoal) {
+ EXPECT_NEAR(1.0, NextGoal(0.0, 1.0), 1e-6);
+
+ EXPECT_NEAR(2.0 * M_PI + 1.0, NextGoal(1.1, 1.0), 1e-6);
+
+ EXPECT_NEAR(6.0 * M_PI + 1.0, NextGoal(1.1 + 4.0 * M_PI, 1.0), 1e-6);
+
+ EXPECT_NEAR(2.0 * M_PI + 1.0, NextGoal(1.0, 1.0), 1e-6);
+
+ EXPECT_NEAR(2.0 * M_PI + 6.0, NextGoal(6.1, 6.0), 1e-6);
+}
+
+// Tests that PreviousGoal always returns the previous goal.
+TEST(GoalTest, TestPreviousGoal) {
+ EXPECT_NEAR(-2.0 * M_PI + 1.0, PreviousGoal(0.0, 1.0), 1e-6);
+
+ EXPECT_NEAR(1.0, PreviousGoal(1.1, 1.0), 1e-6);
+
+ EXPECT_NEAR(4.0 * M_PI + 1.0, PreviousGoal(1.1 + 4.0 * M_PI, 1.0), 1e-6);
+
+ EXPECT_NEAR(-2.0 * M_PI + 1.0, PreviousGoal(1.0, 1.0), 1e-6);
+}
+
+} // namespace testing
+} // namespace seems_reasonable
+} // namespace motors
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index 0467599..322cc06 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -5,11 +5,15 @@
#include <atomic>
#include <cmath>
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "motors/core/kinetis.h"
#include "motors/core/time.h"
#include "motors/peripheral/adc.h"
#include "motors/peripheral/can.h"
#include "motors/peripheral/configuration.h"
+#include "motors/seems_reasonable/drivetrain_dog_motor_plant.h"
+#include "motors/seems_reasonable/polydrivetrain_dog_motor_plant.h"
+#include "motors/seems_reasonable/spring.h"
#include "motors/usb/cdc.h"
#include "motors/usb/usb.h"
#include "motors/util.h"
@@ -18,20 +22,70 @@
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;
+using ::motors::seems_reasonable::Spring;
+
+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};
+::std::atomic<Spring *> global_spring{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 +127,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 +147,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;
@@ -241,6 +299,98 @@
ftm->MODE &= ~FTM_MODE_WPDIS;
}
+struct AccelerometerResult {
+ uint16_t result;
+ bool success;
+};
+
+// Does a transfer on the accelerometer. Returns the resulting frame, or a
+// failure if it takes until after end_micros.
+AccelerometerResult AccelerometerTransfer(uint16_t data, uint32_t end_micros) {
+ SPI0_SR = SPI_SR_RFDF;
+ SPI0_PUSHR = SPI_PUSHR_PCS(1) | data;
+
+ while (!(SPI0_SR & SPI_SR_RFDF)) {
+ if (time_after(micros(), end_micros)) {
+ return {0, false};
+ }
+ }
+ const uint32_t popr = SPI0_POPR;
+ SPI0_SR = SPI_SR_RFDF;
+ return {static_cast<uint16_t>(popr & 0xFFFF), true};
+}
+
+constexpr uint32_t kAccelerometerTimeout = 500;
+
+bool AccelerometerWrite(uint8_t address, uint8_t data, uint32_t end_micros) {
+ const AccelerometerResult r = AccelerometerTransfer(
+ (static_cast<uint16_t>(address) << 8) | static_cast<uint16_t>(data),
+ end_micros);
+ return r.success;
+}
+
+AccelerometerResult AccelerometerRead(uint8_t address, uint32_t end_micros) {
+ AccelerometerResult r = AccelerometerTransfer(
+ (static_cast<uint16_t>(address) << 8) | UINT16_C(0x8000), end_micros);
+ r.result = r.result & UINT16_C(0xFF);
+ return r;
+}
+
+bool accelerometer_inited = false;
+
+void AccelerometerInit() {
+ accelerometer_inited = false;
+ const uint32_t end_micros = time_add(micros(), kAccelerometerTimeout);
+ {
+ const auto who_am_i = AccelerometerRead(0xF, end_micros);
+ if (!who_am_i.success) {
+ return;
+ }
+ if (who_am_i.result != 0x32) {
+ return;
+ }
+ }
+ if (!AccelerometerWrite(
+ 0x20, (1 << 5) /* Normal mode */ | (1 << 3) /* 100 Hz */ |
+ (1 << 2) /* Z enabled */ | (1 << 1) /* Y enabled */ |
+ (1 << 0) /* X enabled */,
+ end_micros)) {
+ }
+ // If want to read LSB, need to enable BDU to avoid splitting reads.
+ if (!AccelerometerWrite(0x23, (0 << 6) /* Data LSB at lower address */ |
+ (3 << 4) /* 400g full scale */ |
+ (0 << 0) /* 4-wire interface */,
+ end_micros)) {
+ }
+ accelerometer_inited = true;
+}
+
+float AccelerometerConvert(uint16_t value) {
+ return static_cast<float>(400.0 / 65536.0) * static_cast<float>(value);
+}
+
+// Returns the total acceleration (in any direction) or 0 if there's an error.
+float ReadAccelerometer() {
+ if (!accelerometer_inited) {
+ AccelerometerInit();
+ return 0;
+ }
+
+ const uint32_t end_micros = time_add(micros(), kAccelerometerTimeout);
+ const auto x = AccelerometerRead(0x29, end_micros);
+ const auto y = AccelerometerRead(0x2B, end_micros);
+ const auto z = AccelerometerRead(0x2D, end_micros);
+ if (!x.success || !y.success || !z.success) {
+ accelerometer_inited = false;
+ return 0;
+ }
+
+ const float x_g = AccelerometerConvert(x.result);
+ const float y_g = AccelerometerConvert(y.result);
+ const float z_g = AccelerometerConvert(z.result);
+ return ::std::sqrt(x_g * x_g + y_g * y_g + z_g * z_g);
+}
+
extern "C" void ftm0_isr() {
while (true) {
const uint32_t status = FTM0->STATUS;
@@ -316,9 +466,9 @@
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;
+ valid = magnitude_error < 0.30f;
angle = ::std::atan2(sin, cos);
}
@@ -331,6 +481,9 @@
extern "C" void pit3_isr() {
PIT_TFLG3 = 1;
+ PolyDrivetrain<float> *polydrivetrain =
+ global_polydrivetrain.load(::std::memory_order_acquire);
+ Spring *spring = global_spring.load(::std::memory_order_acquire);
salsa::SimpleAdcReadings adc_readings;
{
@@ -339,21 +492,114 @@
}
EncoderReading encoder(adc_readings);
+ static float last_good_encoder = 0.0f;
+ static int invalid_encoder_count = 0;
+ if (encoder.valid) {
+ last_good_encoder = encoder.angle;
+ invalid_encoder_count = 0;
+ } else {
+ ++invalid_encoder_count;
+ }
- 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.5f);
+
+ if (polydrivetrain != nullptr && spring != 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.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);
+
+ bool prime = convert_input_width(2) > 0.1f;
+ bool fire = convert_input_width(3) > 0.1f;
+
+ bool unload = lost_any_channel;
+ static bool was_lost = true;
+ bool force_reset = !lost_any_channel && was_lost;
+ was_lost = lost_any_channel;
+
+ spring->Iterate(unload, prime, fire, force_reset, invalid_encoder_count <= 2,
+ last_good_encoder);
+
+ float spring_output = spring->output();
+
+ vesc_set_duty(4, -spring_output);
+ vesc_set_duty(5, spring_output);
+
+ const float accelerometer = ReadAccelerometer();
+ (void)accelerometer;
+
+ /*
+ // Massive debug. Turn on for useful bits.
+ printf("acc %d/1000\n", (int)(accelerometer / 1000));
+ if (!encoder.valid) {
+ printf("Stuck encoder: ADC %" PRIu16 " %" PRIu16
+ " enc %d/1000 %s mag %d\n",
+ adc_readings.sin, adc_readings.cos, (int)(encoder.angle * 1000),
+ encoder.valid ? "T" : "f",
+ (int)(hypot(ConvertEncoderChannel(adc_readings.sin),
+ ConvertEncoderChannel(adc_readings.cos)) *
+ 1000));
+ }
+ 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 "
+ " state: %d, near %d angle %d goal %d to: %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),
+ static_cast<int>(spring->state()), static_cast<int>(spring->Near()),
+ static_cast<int>(spring->angle() * 1000),
+ static_cast<int>(spring->goal() * 1000),
+ static_cast<int>(spring->timeout()), 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
@@ -420,6 +666,36 @@
// FTM0_CH2
PORTC_PCR3 = PORT_PCR_MUX(4);
+ // SPI0
+ // ACC_CS PCS0
+ PORTA_PCR14 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SCK
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // MOSI
+ PORTA_PCR16 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // MISO
+ PORTA_PCR17 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ SIM_SCGC6 |= SIM_SCGC6_SPI0;
+ SPI0_MCR = SPI_MCR_MSTR | SPI_MCR_PCSIS(1) | SPI_MCR_CLR_TXF |
+ SPI_MCR_CLR_RXF | SPI_MCR_HALT;
+ // 60 MHz "protocol clock"
+ // 6ns CS setup
+ // 8ns CS hold
+ SPI0_CTAR0 = SPI_CTAR_FMSZ(15) | SPI_CTAR_CPOL /* Clock idles high */ |
+ SPI_CTAR_CPHA /* Data captured on trailing edge */ |
+ 0 /* !LSBFE MSB first */ |
+ SPI_CTAR_PCSSCK(0) /* PCS->SCK prescaler = 1 */ |
+ SPI_CTAR_PASC(0) /* SCK->PCS prescaler = 1 */ |
+ SPI_CTAR_PDT(0) /* PCS->PCS prescaler = 1 */ |
+ SPI_CTAR_PBR(0) /* baud prescaler = 1 */ |
+ SPI_CTAR_CSSCK(0) /* PCS->SCK 2/60MHz = 33.33ns */ |
+ SPI_CTAR_ASC(0) /* SCK->PCS 2/60MHz = 33.33ns */ |
+ SPI_CTAR_DT(2) /* PCS->PSC 8/60MHz = 133.33ns */ |
+ SPI_CTAR_BR(2) /* BR 60MHz/6 = 10MHz */;
+
+ SPI0_MCR &= ~SPI_MCR_HALT;
+
delay(100);
teensy::UsbDevice usb_device(0, 0x16c0, 0x0492);
@@ -434,7 +710,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,10 +718,18 @@
SetupPwmFtm(FTM0);
SetupPwmFtm(FTM3);
+ PolyDrivetrain<float> polydrivetrain(GetDrivetrainConfig(), nullptr);
+ global_polydrivetrain.store(&polydrivetrain, ::std::memory_order_release);
+ Spring spring;
+ global_spring.store(&spring, ::std::memory_order_release);
+
// Leave the LEDs on for a bit longer.
delay(300);
printf("Done starting up\n");
+ AccelerometerInit();
+ printf("Accelerometer init %s\n", accelerometer_inited ? "success" : "fail");
+
// Done starting up, now turn the LED off.
PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0;
@@ -453,7 +737,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 6f817f8..b94c4f0 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_base.cc
@@ -17,11 +17,12 @@
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,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
::y2012::control_loops::drivetrain::MakeDrivetrainLoop,
::y2012::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
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/y2012/control_loops/python/drivetrain.py b/y2012/control_loops/python/drivetrain.py
index 919ce82..6ae809e 100755
--- a/y2012/control_loops/python/drivetrain.py
+++ b/y2012/control_loops/python/drivetrain.py
@@ -20,6 +20,7 @@
q_pos_high = 0.14,
q_vel_low = 1.0,
q_vel_high = 0.95,
+ has_imu = False,
dt = 0.005,
controller_poles = [0.8, 0.8])
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 b43323b..c3357aa 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -12,12 +12,13 @@
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,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
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/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index 77e8254..e8fdb56 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -20,6 +20,7 @@
q_pos_high = 0.12,
q_vel_low = 1.0,
q_vel_high = 1.0,
+ has_imu = False,
dt = 0.005,
controller_poles = [0.7, 0.7])
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index f05af98..e39a99a 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -17,11 +17,12 @@
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,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
::y2014_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
::y2014_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
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/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index 830dcd8..93993c3 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -20,6 +20,7 @@
q_pos_high = 0.14,
q_vel_low = 1.0,
q_vel_high = 0.95,
+ has_imu = False,
dt = 0.005,
controller_poles = [0.67, 0.67])
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 7013c52..b469f77 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -18,11 +18,12 @@
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,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
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/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index e8b2c73..7417ffb 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -20,6 +20,7 @@
q_pos_high = 0.14,
q_vel_low = 1.0,
q_vel_high = 0.95,
+ has_imu = False,
dt = 0.005,
controller_poles = [0.67, 0.67])
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 bb22c0f..b351cbf 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -18,11 +18,12 @@
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,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
::y2017::control_loops::drivetrain::MakeDrivetrainLoop,
::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
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/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
index b4635ee..74b114d 100755
--- a/y2017/control_loops/python/drivetrain.py
+++ b/y2017/control_loops/python/drivetrain.py
@@ -19,7 +19,8 @@
q_pos_low = 0.12,
q_pos_high = 0.14,
q_vel_low = 1.0,
- q_vel_high = 0.95)
+ q_vel_high = 0.95,
+ has_imu = False)
def main(argv):
argv = FLAGS(argv)
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
index e450315..f4be260 100644
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -17,11 +17,12 @@
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,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
::y2017_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
::y2017_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
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/y2017_bot3/control_loops/python/drivetrain.py b/y2017_bot3/control_loops/python/drivetrain.py
index 6b5ea65..0272232 100755
--- a/y2017_bot3/control_loops/python/drivetrain.py
+++ b/y2017_bot3/control_loops/python/drivetrain.py
@@ -20,7 +20,8 @@
q_pos_low = 0.12,
q_pos_high = 0.14,
q_vel_low = 1.0,
- q_vel_high = 0.95)
+ q_vel_high = 0.95,
+ has_imu = False)
def main(argv):
argv = FLAGS(argv)
diff --git a/y2018/constants.cc b/y2018/constants.cc
index 907b97f..cef8979 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -89,20 +89,21 @@
r->vision_error = 0.0;
left_intake->zeroing.measured_absolute_position = 0.213653;
- left_intake->potentiometer_offset = -5.45258;
- left_intake->spring_offset = -0.25;
+ left_intake->potentiometer_offset = -5.45258 + 1.299206;
+ left_intake->spring_offset = -0.25 - 0.009 + 0.029;
- right_intake->zeroing.measured_absolute_position = 0.056773;
- right_intake->potentiometer_offset = 3.739919;
- right_intake->spring_offset = 0.25 - 0.014;
+ right_intake->zeroing.measured_absolute_position = 0.37022 - 0.04;
+ right_intake->potentiometer_offset = 3.739919 + 1.087098 + 0.825;
+ right_intake->spring_offset = 0.25 + 0.015;
arm_proximal->zeroing.measured_absolute_position =
- 0.067941 + 1.047 - 0.116 + 0.06 - 0.004;
- arm_proximal->potentiometer_offset = 1.047 - 3.653298 + -0.078 + 0.9455;
+ 0.067941 + 1.047 - 0.116 + 0.06 - 0.004 + 0.009;
+ arm_proximal->potentiometer_offset =
+ 1.047 - 3.653298 + -0.078 + 0.9455 + 0.265;
arm_distal->zeroing.measured_absolute_position =
- -0.870445 + 5.209807817203074 + 0.118 - 0.004;
- arm_distal->potentiometer_offset = 5.209807817203074 + 1.250476;
+ -0.870445 + 5.209807817203074 + 0.118 - 0.004 + 0.407;
+ arm_distal->potentiometer_offset = 5.209807817203074 + 1.250476 + 0.110;
break;
case kPracticeTeamNumber:
@@ -115,7 +116,7 @@
right_intake->zeroing.measured_absolute_position = 0.351376;
right_intake->potentiometer_offset = 9.59 + 1.530320 - 3.620648;
- right_intake->spring_offset = 0.255 + 0.008;
+ right_intake->spring_offset = 0.255 + 0.008 - 0.09;
arm_proximal->zeroing.measured_absolute_position = 0.1877 + 0.02 + 0.1;
arm_proximal->potentiometer_offset = -1.242 - 0.03 - 0.1;
diff --git a/y2018/control_loops/drivetrain/drivetrain_base.cc b/y2018/control_loops/drivetrain/drivetrain_base.cc
index d589bc1..01a396d 100644
--- a/y2018/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_base.cc
@@ -17,11 +17,12 @@
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,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_Y,
::y2018::control_loops::drivetrain::MakeDrivetrainLoop,
::y2018::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
@@ -35,7 +36,7 @@
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.1 /* wheel_multiplier */,
+ 1.5 /* wheel_multiplier */,
};
return kDrivetrainConfig;
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
diff --git a/y2018/control_loops/python/drivetrain.py b/y2018/control_loops/python/drivetrain.py
index 988ac74..64217a0 100644
--- a/y2018/control_loops/python/drivetrain.py
+++ b/y2018/control_loops/python/drivetrain.py
@@ -15,16 +15,20 @@
#J needs updating
kDrivetrain = drivetrain.DrivetrainParams(
- J=2.5,
+ J=6.0,
mass=68.0,
robot_radius=0.616 / 2.0,
- wheel_radius=0.127 / 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,
q_pos_low=0.12,
q_pos_high=0.14,
q_vel_low=1.0,
q_vel_high=0.95,
+ efficiency=0.70,
+ has_imu=True,
+ force=True,
+ kf_q_voltage=13.0,
controller_poles=[0.82, 0.82],
)
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index d5dbe12..5559f7c 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -14,13 +14,19 @@
def add_edge(cc_file, name, segment, index, reverse):
cc_file.append(" // Adding edge %d" % index)
+ vmax = "vmax"
+ if segment.vmax:
+ vmax = "::std::min(vmax, %f)" % segment.vmax
+ cc_file.append( " trajectories->emplace_back(%s," % (vmax))
+ cc_file.append( " alpha_unitizer,")
if reverse:
cc_file.append(
- " trajectories->emplace_back(Path::Reversed(%s()), 0.005);" %
- (path_function_name(str(name))))
+ " Trajectory(Path::Reversed(%s()), 0.005));"
+ % (path_function_name(str(name))))
else:
- cc_file.append(" trajectories->emplace_back(%s(), 0.005);" %
- (path_function_name(str(name))))
+ cc_file.append(
+ " Trajectory(%s(), 0.005));"
+ % (path_function_name(str(name))))
start_index = None
end_index = None
@@ -37,11 +43,13 @@
(index_function_name(start_index),
index_function_name(end_index)))
cc_file.append(
- " (trajectories->back().path().length() + 0.2)});")
+ " (trajectories->back().trajectory.path().length() + 1.0)});")
# TODO(austin): Allow different vmaxes for different paths.
cc_file.append(
- " trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);")
+ " trajectories->back().trajectory.OptimizeTrajectory(")
+ cc_file.append(" trajectories->back().alpha_unitizer,")
+ cc_file.append(" trajectories->back().vmax);")
cc_file.append("")
@@ -81,6 +89,22 @@
h_file.append("namespace superstructure {")
h_file.append("namespace arm {")
+ h_file.append("")
+ h_file.append("struct TrajectoryAndParams {")
+ h_file.append(" TrajectoryAndParams(double new_vmax,")
+ h_file.append(
+ " const ::Eigen::Matrix<double, 2, 2> &new_alpha_unitizer,"
+ )
+ h_file.append(" Trajectory &&new_trajectory)")
+ h_file.append(" : vmax(new_vmax),")
+ h_file.append(" alpha_unitizer(new_alpha_unitizer),")
+ h_file.append(" trajectory(::std::move(new_trajectory)) {}")
+ h_file.append(" double vmax;")
+ h_file.append(" ::Eigen::Matrix<double, 2, 2> alpha_unitizer;")
+ h_file.append(" Trajectory trajectory;")
+ h_file.append("};")
+ h_file.append("")
+
# Now dump out the vertices and associated constexpr vertex name functions.
for index, point in enumerate(graph_generate.points):
h_file.append("")
@@ -130,12 +154,12 @@
h_file.append("")
h_file.append("// Builds a search graph.")
h_file.append("SearchGraph MakeSearchGraph("
- "::std::vector<Trajectory> *trajectories,")
+ "::std::vector<TrajectoryAndParams> *trajectories,")
h_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
- "::std::vector<Trajectory> *trajectories,")
+ "::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
cc_file.append(" " "double vmax) {")
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index aa91137..0807c79 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -195,7 +195,7 @@
# Segment in angle space.
class AngleSegment:
- def __init__(self, start, end, name=None):
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
"""Creates an angle segment.
Args:
@@ -207,6 +207,8 @@
self.start = start
self.end = end
self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
def __repr__(self):
return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
@@ -245,7 +247,7 @@
class XYSegment:
"""Straight line in XY space."""
- def __init__(self, start, end, name=None):
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
"""Creates an XY segment.
Args:
@@ -257,6 +259,8 @@
self.start = start
self.end = end
self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
def __repr__(self):
return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
@@ -329,21 +333,30 @@
class SplineSegment:
- def __init__(self, start, control1, control2, end, name=None):
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
self.start = start
self.control1 = control1
self.control2 = control2
self.end = end
self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
def __repr__(self):
- return "XYSegment(%s, %s, &s, %s)" % (repr(self.start),
- repr(self.control1),
- repr(self.control2),
- repr(self.end))
+ return "SplineSegment(%s, %s, %s, %s)" % (repr(self.start),
+ repr(self.control1),
+ repr(self.control2),
+ repr(self.end))
def DrawTo(self, cr, theta_version):
- if (theta_version):
+ if theta_version:
c_i_select = get_circular_index(self.start)
start = get_xy(self.start)
control1 = get_xy(self.control1)
@@ -356,17 +369,27 @@
c_i_select)
for alpha in subdivide_spline(start, control1, control2, end)
])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
else:
start = get_xy(self.start)
control1 = get_xy(self.control1)
control2 = get_xy(self.control2)
end = get_xy(self.end)
- #cr.move_to(start[0], start[1])
+
draw_lines(cr, [
spline_eval(start, control1, control2, end, alpha)
for alpha in subdivide_spline(start, control1, control2, end)
])
- # cr.spline_to(control1[0], control1[1], control2[0], control2[1], end[0], end[1])
+
+ cr.move_to(self.start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
def ToThetaPoints(self):
t1, t2 = self.start
@@ -386,6 +409,76 @@
]
+def get_derivs(t_prev, t, t_next):
+ c, a, b = t_prev, t, t_next
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+class ThetaSplineSegment:
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(self.start),
+ repr(self.control1),
+ repr(self.control2),
+ repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if (theta_version):
+ draw_lines(cr, [
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha)
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ get_xy(
+ spline_eval(self.start, self.control1, self.control2,
+ self.end, alpha))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ return [
+ get_derivs(
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha - 0.00001),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha + 0.00001))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ]
+
+
tall_box_x = 0.411
tall_box_y = 0.125
@@ -425,6 +518,9 @@
front_switch_auto = to_theta_with_circular_index(
0.750, 2.20, circular_index=-1.000000)
+duck = numpy.array(
+ [numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
+
starting = numpy.array(
[numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
vertical_starting = numpy.array(
@@ -452,8 +548,10 @@
front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
-ready_above_box_c1 = to_theta((0.38, 0.33), circular_index=-1)
-ready_above_box_c2 = to_theta((0.42, 0.51), circular_index=-1)
+#c1: (1.421433, -1.070254)
+#c2: (1.434384, -1.057803
+ready_above_box_c1 = numpy.array([1.480802, -1.081218])
+ready_above_box_c2 = numpy.array([1.391449, -1.060331])
front_switch_c1 = numpy.array([1.903841, -0.622351])
front_switch_c2 = numpy.array([1.903841, -0.622351])
@@ -480,17 +578,25 @@
(partner_hang, "PartnerHang"),
(front_switch_auto, "FrontSwitchAuto"),
(starting, "Starting"),
+ (duck, "Duck"),
(vertical_starting, "VerticalStarting"),
] # yapf: disable
+duck_c1 = numpy.array([1.337111, -1.721008])
+duck_c2 = numpy.array([1.283701, -1.795519])
+
+ready_to_up_c1 = numpy.array([1.792962, 0.198329])
+ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+
+
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
named_segments = [
- XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox"),
- XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox"),
- XYSegment(tall_box_grab, short_box_grab, "TallToShortBox"),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
ready_above_box, "ReadyToNeutral"),
+ XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
+ XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
+ XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
tall_box_grab, "TallToNeutral"),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
@@ -505,12 +611,27 @@
]
unnamed_segments = [
- AngleSegment(neutral, back_switch),
+ SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+ SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
+ ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+ AngleSegment(neutral, back_switch),
+
+ XYSegment(ready_above_box, front_low_box),
+ XYSegment(ready_above_box, front_switch),
+ XYSegment(ready_above_box, front_middle1_box),
+ XYSegment(ready_above_box, front_middle2_box),
+ XYSegment(ready_above_box, front_middle3_box),
+ XYSegment(ready_above_box, front_high_box),
+ #XYSegment(ready_above_box, up),
AngleSegment(starting, vertical_starting),
AngleSegment(vertical_starting, neutral),
+ # TODO(austin): Duck -> neutral with a theta spline.
+ #AngleSegment(duck, vertical_starting),
+
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 4912359..d3997ca 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -31,7 +31,7 @@
int i = 0;
for (const auto &trajectory : trajectories_) {
LOG(INFO, "trajectory length for edge node %d: %f\n", i,
- trajectory.path().length());
+ trajectory.trajectory.path().length());
++i;
}
}
@@ -330,7 +330,8 @@
LOG(INFO, "Switching from node %d to %d along edge %d\n",
static_cast<int>(current_node_), static_cast<int>(next_edge.end),
static_cast<int>(min_edge));
- follower_.SwitchTrajectory(&trajectories_[min_edge]);
+ vmax_ = trajectories_[min_edge].vmax;
+ follower_.SwitchTrajectory(&trajectories_[min_edge].trajectory);
current_node_ = next_edge.end;
}
}
@@ -340,7 +341,7 @@
close_enough_for_full_power_
? kOperatingVoltage()
: (state_ == State::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
- follower_.Update(arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
status->goal_theta0 = follower_.theta(0);
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 840ce53..d86b95b 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -5,6 +5,7 @@
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
@@ -23,15 +24,15 @@
// The operating voltage.
static constexpr double kOperatingVoltage() {
- return kGrannyMode() ? 4.0 : 12.0;
+ return kGrannyMode() ? 5.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
- static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 10.0; }
- static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 10.0; }
+ static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
+ static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
static constexpr double kPathlessVMax() { return 5.0; }
- static constexpr double kGotoPathVMax() { return 12.0; }
+ static constexpr double kGotoPathVMax() { return 6.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
@@ -96,7 +97,9 @@
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
- ::std::vector<Trajectory> trajectories_;
+ double vmax_ = kVMax();
+
+ ::std::vector<TrajectoryAndParams> trajectories_;
SearchGraph search_graph_;
bool close_enough_for_full_power_ = false;