Merge changes I6c5ebe37,I43f710f6,Ibb232c1e
* changes:
Draw Cargo Ship
Draw Rocket
Drawing the HAB
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index c139195..3e24fab 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -1,5 +1,7 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "ring_buffer",
hdrs = [
@@ -41,6 +43,7 @@
hdrs = [
"sized_array.h",
],
+ compatible_with = mcu_cpus,
)
cc_test(
diff --git a/aos/containers/priority_queue.h b/aos/containers/priority_queue.h
index 0bc94e7..8fb5e5c 100644
--- a/aos/containers/priority_queue.h
+++ b/aos/containers/priority_queue.h
@@ -78,8 +78,17 @@
bool empty() const { return size_ == 0; }
bool full() const { return size_ == buffer_size; }
+ // Removes all the elements from the queue:
+ void clear() {
+ size_ = 0;
+ bottom_ = buffer_size;
+ top_ = buffer_size;
+ }
+
Data &top() { return list_[top_].data; }
+ const Data &top() const { return list_[top_].data; }
Data &get(size_t idx) { return list_[idx].data; }
+ const Data &get(size_t idx) const { return list_[idx].data; }
iterator begin() { return iterator(this, bottom_); }
iterator end() { return iterator(this, buffer_size); }
diff --git a/aos/containers/priority_queue_test.cc b/aos/containers/priority_queue_test.cc
index 2c45990..c05d827 100644
--- a/aos/containers/priority_queue_test.cc
+++ b/aos/containers/priority_queue_test.cc
@@ -52,7 +52,7 @@
ASSERT_EQ(4u, queue_.size());
ASSERT_FALSE(queue_.full());
::std::vector<int> reverse_expected{20, 15, 11};
- EXPECT_EQ(20, queue_.top());
+ EXPECT_EQ(10, *queue_.begin());
for (; it != queue_.end(); ++it) {
EXPECT_EQ(reverse_expected.back(), *it);
reverse_expected.pop_back();
@@ -60,6 +60,30 @@
ASSERT_TRUE(reverse_expected.empty());
}
+// Tests that the clear() method works properly.
+TEST_F(PriorityQueueTest, ClearQueue) {
+ queue_.PushFromBottom(10);
+ queue_.PushFromBottom(20);
+ queue_.PushFromBottom(15);
+ queue_.PushFromBottom(11);
+ ASSERT_EQ(4u, queue_.size());
+ ASSERT_FALSE(queue_.full());
+ queue_.clear();
+ ASSERT_TRUE(queue_.empty());
+ ASSERT_EQ(0u, queue_.size());
+
+ queue_.PushFromBottom(1);
+ queue_.PushFromBottom(3);
+ queue_.PushFromBottom(2);
+ ASSERT_EQ(3u, queue_.size());
+ ::std::vector<int> reverse_expected{3, 2, 1};
+ for (const int val : queue_) {
+ EXPECT_EQ(reverse_expected.back(), val);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
TEST_F(PriorityQueueTest, FullBufferInsertTop) {
for (int ii = 0; ii < 10; ++ii) {
queue_.PushFromBottom(ii);
diff --git a/aos/time/BUILD b/aos/time/BUILD
index b76cd99..6570490 100644
--- a/aos/time/BUILD
+++ b/aos/time/BUILD
@@ -1,4 +1,4 @@
-package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
cc_library(
name = "time",
@@ -8,11 +8,31 @@
hdrs = [
"time.h",
],
+ visibility = ["//visibility:public"],
deps = [
"//aos:macros",
- "//aos/mutex:mutex",
- "//aos/logging",
"//aos/ipc_lib:shared_mem",
+ "//aos/logging",
+ "//aos/mutex",
+ "//aos/type_traits",
+ ],
+)
+
+# TODO(Brian): Remove this hack once bazel chases deps through selects correctly.
+cc_library(
+ name = "time_mcu",
+ srcs = [
+ "time.cc",
+ ],
+ hdrs = [
+ "time.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:macros",
+ "//aos/type_traits",
+ "//motors/core",
],
)
@@ -24,7 +44,7 @@
deps = [
":time",
"//aos/logging",
- "//aos/util:death_test_log_implementation",
"//aos/testing:googletest",
+ "//aos/util:death_test_log_implementation",
],
)
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 0d7295c..06f4b12 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -6,6 +6,8 @@
#include <atomic>
#include <chrono>
+#ifdef __linux__
+
// We only use global_core from here, which is weak, so we don't really have a
// dependency on it.
#include "aos/ipc_lib/shared_mem.h"
@@ -13,8 +15,19 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
+#else // __linux__
+
+#include "motors/core/kinetis.h"
+
+// The systick interrupt increments this every 1ms.
+extern "C" volatile uint32_t systick_millis_count;
+
+#endif // __linux__
+
namespace chrono = ::std::chrono;
+#ifdef __linux__
+
namespace std {
namespace this_thread {
template <>
@@ -42,10 +55,13 @@
} // namespace this_thread
} // namespace std
+#endif // __linux__
namespace aos {
namespace time {
+#ifdef __linux__
+
// State required to enable and use mock time.
namespace {
// True if mock time is enabled.
@@ -98,6 +114,8 @@
chrono::duration_cast<chrono::nanoseconds>(offset).count();
}
+#endif // __linux__
+
struct timespec to_timespec(
const ::aos::monotonic_clock::duration duration) {
struct timespec time_timespec;
@@ -119,11 +137,11 @@
constexpr monotonic_clock::time_point monotonic_clock::min_time;
monotonic_clock::time_point monotonic_clock::now() noexcept {
- {
- if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
- MutexLocker time_mutex_locker(&time::time_mutex);
- return time::current_mock_time;
- }
+#ifdef __linux__
+
+ if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
+ MutexLocker time_mutex_locker(&time::time_mutex);
+ return time::current_mock_time;
}
struct timespec current_time;
@@ -139,7 +157,45 @@
return time_point(::std::chrono::seconds(current_time.tv_sec) +
::std::chrono::nanoseconds(current_time.tv_nsec)) + offset;
+
+#else // __linux__
+
+ __disable_irq();
+ const uint32_t current_counter = SYST_CVR;
+ uint32_t ms_count = systick_millis_count;
+ const uint32_t istatus = SCB_ICSR;
+ __enable_irq();
+ // If the interrupt is pending and the timer has already wrapped from 0 back
+ // up to its max, then add another ms.
+ if ((istatus & SCB_ICSR_PENDSTSET) && current_counter > 50) {
+ ++ms_count;
+ }
+
+ // It counts down, but everything we care about counts up.
+ const uint32_t counter_up = ((F_CPU / 1000) - 1) - current_counter;
+
+ // "3.2.1.2 System Tick Timer" in the TRM says "The System Tick Timer's clock
+ // source is always the core clock, FCLK".
+ using systick_duration =
+ std::chrono::duration<uint32_t, std::ratio<1, F_CPU>>;
+
+ return time_point(aos::time::round<std::chrono::nanoseconds>(
+ std::chrono::milliseconds(ms_count) + systick_duration(counter_up)));
+
+#endif // __linux__
}
+#ifdef __linux__
+realtime_clock::time_point realtime_clock::now() noexcept {
+ struct timespec current_time;
+ if (clock_gettime(CLOCK_REALTIME, ¤t_time) != 0) {
+ PLOG(FATAL, "clock_gettime(%jd, %p) failed",
+ static_cast<uintmax_t>(CLOCK_REALTIME), ¤t_time);
+ }
+
+ return time_point(::std::chrono::seconds(current_time.tv_sec) +
+ ::std::chrono::nanoseconds(current_time.tv_nsec));
+}
+#endif // __linux__
} // namespace aos
diff --git a/aos/time/time.h b/aos/time/time.h
index 9bef741..136629c 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -24,7 +24,32 @@
typedef ::std::chrono::time_point<monotonic_clock> time_point;
static monotonic_clock::time_point now() noexcept;
- static constexpr bool is_steady = true;
+ // This clock is still subject to rate adjustments based on adjtime, so it is
+ // not steady.
+ static constexpr bool is_steady = false;
+
+ // Returns the epoch (0).
+ static constexpr monotonic_clock::time_point epoch() {
+ return time_point(zero());
+ }
+
+ static constexpr monotonic_clock::duration zero() { return duration(0); }
+
+ static constexpr time_point min_time{
+ time_point(duration(::std::numeric_limits<duration::rep>::min()))};
+};
+
+class realtime_clock {
+ public:
+ typedef ::std::chrono::nanoseconds::rep rep;
+ typedef ::std::chrono::nanoseconds::period period;
+ typedef ::std::chrono::nanoseconds duration;
+ typedef ::std::chrono::time_point<monotonic_clock> time_point;
+
+#ifdef __linux__
+ static monotonic_clock::time_point now() noexcept;
+#endif // __linux__
+ static constexpr bool is_steady = false;
// Returns the epoch (0).
static constexpr monotonic_clock::time_point epoch() {
@@ -39,6 +64,8 @@
namespace time {
+#ifdef __linux__
+
// Enables returning the mock time value for Now instead of checking the system
// clock.
void EnableMockTime(monotonic_clock::time_point now);
@@ -78,6 +105,8 @@
DISALLOW_COPY_AND_ASSIGN(TimeFreezer);
};
+#endif // __linux__
+
// Converts a monotonic_clock::duration into a timespec object.
struct timespec to_timespec(::aos::monotonic_clock::duration duration);
@@ -85,9 +114,72 @@
// epoch.
struct timespec to_timespec(::aos::monotonic_clock::time_point time);
+namespace time_internal {
+
+template <class T>
+struct is_duration : std::false_type {};
+template <class Rep, class Period>
+struct is_duration<std::chrono::duration<Rep, Period>> : std::true_type {};
+
+} // namespace time_internal
+
+// Returns the greatest duration t representable in ToDuration that is less or
+// equal to d.
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/duration/floor.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Rep, class Period,
+ class = std::enable_if_t<time_internal::is_duration<To>{}>>
+constexpr To floor(const std::chrono::duration<Rep, Period> &d) {
+ To t = std::chrono::duration_cast<To>(d);
+ if (t > d) return t - To{1};
+ return t;
+}
+
+// Returns the value t representable in ToDuration that is the closest to d. If
+// there are two such values, returns the even value (that is, the value t such
+// that t % 2 == 0).
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/duration/round.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Rep, class Period,
+ class = std::enable_if_t<
+ time_internal::is_duration<To>{} &&
+ !std::chrono::treat_as_floating_point<typename To::rep>{}>>
+constexpr To round(const std::chrono::duration<Rep, Period> &d) {
+ To t0 = aos::time::floor<To>(d);
+ To t1 = t0 + To{1};
+ auto diff0 = d - t0;
+ auto diff1 = t1 - d;
+ if (diff0 == diff1) {
+ if (t0.count() & 1) return t1;
+ return t0;
+ } else if (diff0 < diff1) {
+ return t0;
+ }
+ return t1;
+}
+
+// Returns the nearest time point to tp representable in ToDuration, rounding to
+// even in halfway cases, like std::chrono::round in C++17.
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/time_point/round.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Clock, class FromDuration,
+ class = std::enable_if_t<
+ time_internal::is_duration<To>{} &&
+ !std::chrono::treat_as_floating_point<typename To::rep>{}>>
+constexpr std::chrono::time_point<Clock, To> round(
+ const std::chrono::time_point<Clock, FromDuration> &tp) {
+ return std::chrono::time_point<Clock, To>{
+ aos::time::round<To>(tp.time_since_epoch())};
+}
+
} // namespace time
} // namespace aos
+#ifdef __linux__
+
namespace std {
namespace this_thread {
// Template specialization for monotonic_clock, since we can use clock_nanosleep
@@ -98,5 +190,6 @@
} // namespace this_thread
} // namespace std
+#endif // __linux__
#endif // AOS_TIME_H_
diff --git a/aos/type_traits/BUILD b/aos/type_traits/BUILD
index 171bb8d..e5590b0 100644
--- a/aos/type_traits/BUILD
+++ b/aos/type_traits/BUILD
@@ -1,10 +1,13 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "type_traits",
hdrs = [
"type_traits.h",
],
+ compatible_with = mcu_cpus,
)
cc_test(
diff --git a/aos/type_traits/type_traits.h b/aos/type_traits/type_traits.h
index f0a2e72..437cb3e 100644
--- a/aos/type_traits/type_traits.h
+++ b/aos/type_traits/type_traits.h
@@ -1,8 +1,6 @@
#ifndef AOS_TYPE_TRAITS_
#define AOS_TYPE_TRAITS_
-#include <features.h>
-
#include <type_traits>
namespace aos {
diff --git a/aos/vision/debug/jpeg_list-source.cc b/aos/vision/debug/jpeg_list-source.cc
index d3e1006..027acc9 100644
--- a/aos/vision/debug/jpeg_list-source.cc
+++ b/aos/vision/debug/jpeg_list-source.cc
@@ -1,5 +1,7 @@
#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/image/image_dataset.h"
+
#include <gdk/gdk.h>
#include <fstream>
#include <string>
@@ -7,77 +9,12 @@
namespace aos {
namespace vision {
-namespace {
-std::string GetFileContents(const std::string &filename) {
- std::ifstream in(filename, std::ios::in | std::ios::binary);
- if (in) {
- std::string contents;
- in.seekg(0, std::ios::end);
- contents.resize(in.tellg());
- in.seekg(0, std::ios::beg);
- in.read(&contents[0], contents.size());
- in.close();
- return (contents);
- }
- fprintf(stderr, "Could not read file: %s\n", filename.c_str());
- exit(-1);
-}
-
-std::vector<std::string> Split(DataRef inp, char delim) {
- size_t i = 0;
- std::vector<size_t> pos;
- while (i < inp.size()) {
- i = inp.find(delim, i);
- if (i == std::string::npos) break;
- // fprintf(stderr, "k=%d, i=%d\n", k, (int)i);
- pos.emplace_back(i);
- i = i + 1;
- }
- std::vector<std::string> res;
- res.reserve(pos.size() + 1);
- i = 0;
- for (auto p : pos) {
- res.emplace_back(inp.substr(i, p - i).to_string());
- i = p + 1;
- }
- res.emplace_back(inp.substr(i).to_string());
- return res;
-}
-} // namespace
-
class JpegListImageSource : public ImageSource {
public:
void Init(const std::string &jpeg_list_filename,
DebugFrameworkInterface *interface) override {
interface_ = interface;
- auto contents = GetFileContents(jpeg_list_filename);
-
- std::string basename;
- auto it = jpeg_list_filename.find_last_of('/');
- if (it != std::string::npos) {
- basename = jpeg_list_filename.substr(0, it + 1);
- }
-
- for (const auto &jpeg_filename : Split(contents, '\n')) {
- [&]() {
- if (jpeg_filename.empty()) return;
- for (std::size_t i = 0; i < jpeg_filename.size(); ++i) {
- if (jpeg_filename[i] == '#') return;
- if (jpeg_filename[i] != ' ') break;
- }
- bool is_jpeg = true;
- size_t l = jpeg_filename.size();
- if (l > 4 && jpeg_filename[l - 1] == 'v') {
- is_jpeg = false;
- }
- if (jpeg_filename[0] == '/') {
- images_.emplace_back(Frame{is_jpeg, GetFileContents(jpeg_filename)});
- } else {
- images_.emplace_back(
- Frame{is_jpeg, GetFileContents(basename + jpeg_filename)});
- }
- }();
- }
+ images_ = LoadDataset(jpeg_list_filename);
fprintf(stderr, "loaded %lu items\n", images_.size());
if (!images_.empty()) {
SetCurrentFrame();
@@ -123,11 +60,7 @@
private:
DebugFrameworkInterface *interface_ = nullptr;
- struct Frame {
- bool is_jpeg = true;
- std::string data;
- };
- std::vector<Frame> images_;
+ std::vector<DatasetFrame> images_;
size_t idx_ = 0;
};
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
index 9bc3c19..be702d7 100644
--- a/aos/vision/image/BUILD
+++ b/aos/vision/image/BUILD
@@ -42,6 +42,15 @@
)
cc_library(
+ name = "image_dataset",
+ srcs = ["image_dataset.cc"],
+ hdrs = ["image_dataset.h"],
+ deps = [
+ ":image_types",
+ ],
+)
+
+cc_library(
name = "image_stream",
hdrs = ["image_stream.h"],
deps = [
diff --git a/aos/vision/image/image_dataset.cc b/aos/vision/image/image_dataset.cc
new file mode 100644
index 0000000..cd3ec46
--- /dev/null
+++ b/aos/vision/image/image_dataset.cc
@@ -0,0 +1,84 @@
+#include "aos/vision/image/image_dataset.h"
+
+#include <fstream>
+
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+namespace {
+std::string GetFileContents(const std::string &filename) {
+ std::ifstream in(filename, std::ios::in | std::ios::binary);
+ if (in) {
+ std::string contents;
+ in.seekg(0, std::ios::end);
+ contents.resize(in.tellg());
+ in.seekg(0, std::ios::beg);
+ in.read(&contents[0], contents.size());
+ in.close();
+ return (contents);
+ }
+ fprintf(stderr, "Could not read file: %s\n", filename.c_str());
+ exit(-1);
+}
+
+std::vector<std::string> Split(DataRef inp, char delim) {
+ size_t i = 0;
+ std::vector<size_t> pos;
+ while (i < inp.size()) {
+ i = inp.find(delim, i);
+ if (i == std::string::npos) break;
+ // fprintf(stderr, "k=%d, i=%d\n", k, (int)i);
+ pos.emplace_back(i);
+ i = i + 1;
+ }
+ std::vector<std::string> res;
+ res.reserve(pos.size() + 1);
+ i = 0;
+ for (auto p : pos) {
+ res.emplace_back(inp.substr(i, p - i).to_string());
+ i = p + 1;
+ }
+ res.emplace_back(inp.substr(i).to_string());
+ return res;
+}
+} // namespace
+
+std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename) {
+ std::vector<DatasetFrame> images;
+ auto contents = GetFileContents(jpeg_list_filename);
+
+ std::string basename;
+ auto it = jpeg_list_filename.find_last_of('/');
+ if (it != std::string::npos) {
+ basename = jpeg_list_filename.substr(0, it + 1);
+ }
+
+ for (const auto &jpeg_filename : Split(contents, '\n')) {
+ [&]() {
+ if (jpeg_filename.empty()) return;
+ for (std::size_t i = 0; i < jpeg_filename.size(); ++i) {
+ if (jpeg_filename[i] == '#') return;
+ if (jpeg_filename[i] != ' ') break;
+ }
+ bool is_jpeg = true;
+ size_t l = jpeg_filename.size();
+ if (l > 4 && jpeg_filename[l - 1] == 'v') {
+ is_jpeg = false;
+ }
+ if (jpeg_filename[0] == '/') {
+ images.emplace_back(
+ DatasetFrame{is_jpeg, GetFileContents(jpeg_filename)});
+ } else {
+ images.emplace_back(
+ DatasetFrame{is_jpeg, GetFileContents(basename + jpeg_filename)});
+ }
+ }();
+ }
+
+ return images;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/image/image_dataset.h b/aos/vision/image/image_dataset.h
new file mode 100644
index 0000000..ce8c841
--- /dev/null
+++ b/aos/vision/image/image_dataset.h
@@ -0,0 +1,21 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_DATASET_H_
+#define _AOS_VISION_IMAGE_IMAGE_DATASET_H_
+
+#include <string>
+#include <vector>
+
+namespace aos {
+namespace vision {
+
+struct DatasetFrame {
+ // TODO: These should be V4L formats ideally.
+ bool is_jpeg = true;
+ std::string data;
+};
+
+std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_IMAGE_DATASET_H_
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 44eb262..18ed29e 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -188,3 +188,33 @@
// Maximum acceleration for the profile.
float max_acceleration;
};
+
+// Definition of a constraint on a trajectory
+struct Constraint {
+ // Type of constraint
+ // 0: Null constraint. Ignore and all following
+ // 1: longitual acceleration
+ // 2: lateral acceleration
+ // 3: voltage
+ // 4: velocity
+ uint8_t constraint_type;
+ float value;
+ // start and end distance are only checked for velocity limits.
+ float start_distance;
+ float end_distance;
+};
+
+// Parameters for computing a trajectory using a chain of splines and
+// constraints.
+struct MultiSpline {
+ // index of the spline. Zero indicates the spline should not be computed.
+ uint32_t spline_idx;
+ // Number of splines. The spline point arrays will be expected to have
+ // 6 + 5 * (n - 1) points in them. The endpoints are shared between
+ // neighboring splines.
+ uint8_t spline_count;
+ float[36] spline_x;
+ float[36] spline_y;
+
+ Constraint[6] constraints;
+};
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index aee3a37..d95e101 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -49,6 +49,23 @@
)
cc_library(
+ name = "splinedrivetrain",
+ srcs = [
+ "splinedrivetrain.cc",
+ ],
+ hdrs = [
+ "splinedrivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":spline",
+ ":distance_spline",
+ ":trajectory",
+ ]
+)
+
+cc_library(
name = "ssdrivetrain",
srcs = [
"ssdrivetrain.cc",
@@ -170,6 +187,7 @@
":gear",
":polydrivetrain",
":ssdrivetrain",
+ ":splinedrivetrain",
"//aos/controls:control_loop",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a50b429..168f81d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -38,6 +38,7 @@
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_spline_(dt_config_),
down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -329,12 +330,15 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
+ dt_spline_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
dt_closedloop_.Update(output != NULL && controller_type == 1);
+ dt_spline_.Update(output != NULL && controller_type == 2);
+
switch (controller_type) {
case 0:
dt_openloop_.SetOutput(output);
@@ -342,6 +346,9 @@
case 1:
dt_closedloop_.SetOutput(output);
break;
+ case 2:
+ dt_spline_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a357eab..05bf711 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
namespace frc971 {
namespace control_loops {
@@ -50,6 +51,7 @@
StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ SplineDrivetrain dt_spline_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index c66b03e..f63f685 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -72,6 +72,14 @@
// The control loop will profile if these are all non-zero.
.frc971.ProfileParameters linear;
.frc971.ProfileParameters angular;
+
+ // Parameters for a spline to follow. This just contains info on a spline to
+ // compute. Each time this is sent, spline drivetrain will compute a new
+ // spline.
+ .frc971.MultiSpline spline;
+
+ // Which spline to follow.
+ uint32_t spline_handle;
};
message Position {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
new file mode 100644
index 0000000..0d6b6ae
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -0,0 +1,101 @@
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+const int kMaxSplineConstraints = 6;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config) {}
+
+// TODO(alex): put in another thread to avoid malloc in RT.
+void SplineDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ current_spline_handle_ = goal.spline_handle;
+ const ::frc971::MultiSpline &multispline = goal.spline;
+ if (multispline.spline_idx) {
+ current_spline_idx_ = multispline.spline_idx;
+ auto x = multispline.spline_x;
+ auto y = multispline.spline_y;
+ ::std::vector<Spline> splines = ::std::vector<Spline>();
+ for (int i = 0; i < multispline.spline_count; ++i) {
+ ::Eigen::Matrix<double, 2, 6> points =
+ ::Eigen::Matrix<double, 2, 6>::Zero();
+ for (int j = 0; j < 6; ++j) {
+ points(0, j) = x[i * 5 + j];
+ points(1, j) = y[i * 5 + j];
+ }
+ splines.emplace_back(Spline(points));
+ }
+
+ distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+ new DistanceSpline(::std::move(splines)));
+
+ current_trajectory_ = ::std::unique_ptr<Trajectory>(
+ new Trajectory(distance_spline_.get(), dt_config_));
+
+ for (int i = 0; i < kMaxSplineConstraints; ++i) {
+ const ::frc971::Constraint &constraint = multispline.constraints[i];
+ switch (constraint.constraint_type) {
+ case 0:
+ break;
+ case 1:
+ current_trajectory_->set_longitudal_acceleration(constraint.value);
+ break;
+ case 2:
+ current_trajectory_->set_lateral_acceleration(constraint.value);
+ break;
+ case 3:
+ current_trajectory_->set_voltage_limit(constraint.value);
+ break;
+ case 4:
+ current_trajectory_->LimitVelocity(constraint.start_distance,
+ constraint.end_distance,
+ constraint.value);
+ break;
+ }
+ }
+
+ current_trajectory_->Plan();
+ current_xva_ = current_trajectory_->FFAcceleration(0);
+ current_xva_(1) = 0.0;
+ }
+}
+
+void SplineDrivetrain::Update(bool enable) {
+ if (enable && current_trajectory_ &&
+ !current_trajectory_->is_at_end(current_state_)) {
+ next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, ¤t_state_);
+ }
+}
+
+// TODO(alex): Handle drift.
+void SplineDrivetrain::SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ if (!output) {
+ return;
+ }
+ if (!current_trajectory_) {
+ return;
+ }
+ if (current_spline_handle_ == current_spline_idx_) {
+ if (!current_trajectory_->is_at_end(current_state_)) {
+ double current_distance = current_xva_(0);
+ ::Eigen::Matrix<double, 2, 1> FFVoltage =
+ current_trajectory_->FFVoltage(current_distance);
+ output->left_voltage = FFVoltage(0);
+ output->right_voltage = FFVoltage(1);
+ current_xva_ = next_xva_;
+ }
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
new file mode 100644
index 0000000..4bb7ac9
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -0,0 +1,44 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class SplineDrivetrain {
+ public:
+ SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
+
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+
+ void Update(bool enabled);
+
+ void SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output);
+ // TODO(alex): What status do we need?
+ void PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ private:
+ const DrivetrainConfig<double> dt_config_;
+
+ uint32_t current_spline_handle_; // Current spline told to excecute.
+ uint32_t current_spline_idx_; // Current excecuting spline.
+ ::std::unique_ptr<DistanceSpline> distance_spline_;
+ ::std::unique_ptr<Trajectory> current_trajectory_;
+ ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
+ ::Eigen::Matrix<double, 2, 1> current_state_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 6b9a936..2228231 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -9,6 +9,7 @@
class AngularSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -35,6 +36,7 @@
class AngularSystem(control_loop.ControlLoop):
+
def __init__(self, params, name="AngularSystem"):
super(AngularSystem, self).__init__(name)
self.params = params
@@ -45,15 +47,15 @@
self.G = params.G
# Moment of inertia in kg m^2
- self.J = params.J + self.motor.motor_inertia / (self.G ** 2.0)
+ self.J = params.J + self.motor.motor_inertia / (self.G**2.0)
# Control loop time step
self.dt = params.dt
# State is [position, velocity]
# Input is [Voltage]
- C1 = self.motor.Kt / (self.G * self.G * self.motor.resistance *
- self.J * self.motor.Kv)
+ C1 = self.motor.Kt / (
+ self.G * self.G * self.motor.resistance * self.J * self.motor.Kv)
C2 = self.motor.Kt / (self.G * self.J * self.motor.resistance)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -116,6 +118,7 @@
class IntegralAngularSystem(AngularSystem):
+
def __init__(self, params, name="IntegralAngularSystem"):
super(IntegralAngularSystem, self).__init__(params, name=name)
@@ -136,10 +139,9 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
- self.Q = numpy.matrix(
- [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
- [0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+ self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+ [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+ [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -197,8 +199,8 @@
vbat = 12.0
- goal = numpy.concatenate(
- (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
profile = TrapezoidProfile(plant.dt)
profile.set_maximum_acceleration(max_acceleration)
@@ -257,8 +259,7 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -387,7 +388,8 @@
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', angular_system.G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', angular_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ angular_system.motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
integral_angular_system = IntegralAngularSystem(params,
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index fc39eb9..8c13eb0 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -3,44 +3,46 @@
from frc971.control_loops.python import control_loop
import numpy
+
class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ def __init__(self):
+ super(CIM, self).__init__("CIM")
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the CIM in kg m^2
+ self.J = 0.0001
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.resistance)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.resistance)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceControllerPoles([0.01])
+ self.PlaceObserverPoles([0.01])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e3e00b7..ac22821 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,548 +2,591 @@
import numpy
import os
-class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double"
- self.formatToType['%d'] = "int"
- 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") % \
- (typestring, self.name, self.value)
+class Constant(object):
+
+ def __init__(self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double"
+ self.formatToType['%d'] = "int"
+
+ 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") % \
+ (typestring, self.name, self.value)
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None,
- write_constants=False, plant_type='StateFeedbackPlant',
- observer_type='StateFeedbackObserver',
- scalar_type='double'):
- """Constructs a control loop writer.
- Args:
- gain_schedule_name: string, Name of the overall controller.
- loops: array[ControlLoop], a list of control loops to gain schedule
- in order.
- namespaces: array[string], a list of names of namespaces to nest in
- order. If None, the default will be used.
- plant_type: string, The C++ type of the plant.
- observer_type: string, The C++ type of the observer.
- scalar_type: string, The C++ type of the base scalar.
- """
- self._gain_schedule_name = gain_schedule_name
- self._loops = loops
- if namespaces:
- self._namespaces = namespaces
- else:
- self._namespaces = ['frc971', 'control_loops']
+ def __init__(self,
+ gain_schedule_name,
+ loops,
+ namespaces=None,
+ write_constants=False,
+ plant_type='StateFeedbackPlant',
+ observer_type='StateFeedbackObserver',
+ scalar_type='double'):
+ """Constructs a control loop writer.
- self._namespace_start = '\n'.join(
- ['namespace %s {' % name for name in self._namespaces])
+ Args:
+ gain_schedule_name: string, Name of the overall controller.
+ loops: array[ControlLoop], a list of control loops to gain schedule
+ in order.
+ namespaces: array[string], a list of names of namespaces to nest in
+ order. If None, the default will be used.
+ plant_type: string, The C++ type of the plant.
+ observer_type: string, The C++ type of the observer.
+ scalar_type: string, The C++ type of the base scalar.
+ """
+ self._gain_schedule_name = gain_schedule_name
+ self._loops = loops
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'control_loops']
- self._namespace_end = '\n'.join(
- ['} // namespace %s' % name for name in reversed(self._namespaces)])
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
- self._constant_list = []
- self._plant_type = plant_type
- self._observer_type = observer_type
- self._scalar_type = scalar_type
+ self._namespace_end = '\n'.join([
+ '} // namespace %s' % name for name in reversed(self._namespaces)
+ ])
- def AddConstant(self, constant):
- """Adds a constant to write.
+ self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
+ self._scalar_type = scalar_type
- Args:
- constant: Constant, the constant to add to the header.
- """
- self._constant_list.append(constant)
+ def AddConstant(self, constant):
+ """Adds a constant to write.
- def _TopDirectory(self):
- return self._namespaces[0]
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
- def _HeaderGuard(self, header_file):
- return ('_'.join([namespace.upper() for namespace in self._namespaces]) + '_' +
- os.path.basename(header_file).upper()
- .replace('.', '_').replace('/', '_') + '_')
+ def _TopDirectory(self):
+ return self._namespaces[0]
- def Write(self, header_file, cc_file):
- """Writes the loops to the specified files."""
- self.WriteHeader(header_file)
- self.WriteCC(os.path.basename(header_file), cc_file)
+ def _HeaderGuard(self, header_file):
+ return ('_'.join([namespace.upper() for namespace in self._namespaces])
+ + '_' + os.path.basename(header_file).upper().replace(
+ '.', '_').replace('/', '_') + '_')
- def _GenericType(self, typename, extra_args=None):
- """Returns a loop template using typename for the type."""
- num_states = self._loops[0].A.shape[0]
- num_inputs = self._loops[0].B.shape[1]
- num_outputs = self._loops[0].C.shape[0]
- if extra_args is not None:
- extra_args = ', ' + extra_args
- else:
- extra_args = ''
- if self._scalar_type != 'double':
- extra_args += ', ' + self._scalar_type
- return '%s<%d, %d, %d%s>' % (
- typename, num_states, num_inputs, num_outputs, extra_args)
+ def Write(self, header_file, cc_file):
+ """Writes the loops to the specified files."""
+ self.WriteHeader(header_file)
+ self.WriteCC(os.path.basename(header_file), cc_file)
- def _ControllerType(self):
- """Returns a template name for StateFeedbackController."""
- return self._GenericType('StateFeedbackController')
+ def _GenericType(self, typename, extra_args=None):
+ """Returns a loop template using typename for the type."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ if extra_args is not None:
+ extra_args = ', ' + extra_args
+ else:
+ extra_args = ''
+ if self._scalar_type != 'double':
+ extra_args += ', ' + self._scalar_type
+ return '%s<%d, %d, %d%s>' % (typename, num_states, num_inputs,
+ num_outputs, extra_args)
- def _ObserverType(self):
- """Returns a template name for StateFeedbackObserver."""
- return self._GenericType(self._observer_type)
+ def _ControllerType(self):
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
- def _LoopType(self):
- """Returns a template name for StateFeedbackLoop."""
- num_states = self._loops[0].A.shape[0]
- num_inputs = self._loops[0].B.shape[1]
- num_outputs = self._loops[0].C.shape[0]
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType(self._observer_type)
- return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
- num_states,
- num_inputs,
- num_outputs, self._scalar_type,
- self._PlantType(), self._ObserverType())
+ def _LoopType(self):
+ """Returns a template name for StateFeedbackLoop."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
+ num_states, num_inputs, num_outputs, self._scalar_type,
+ self._PlantType(), self._ObserverType())
- def _PlantType(self):
- """Returns a template name for StateFeedbackPlant."""
- return self._GenericType(self._plant_type)
+ def _PlantType(self):
+ """Returns a template name for StateFeedbackPlant."""
+ return self._GenericType(self._plant_type)
- def _PlantCoeffType(self):
- """Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType(self._plant_type + 'Coefficients')
+ def _PlantCoeffType(self):
+ """Returns a template name for StateFeedbackPlantCoefficients."""
+ return self._GenericType(self._plant_type + 'Coefficients')
- def _ControllerCoeffType(self):
- """Returns a template name for StateFeedbackControllerCoefficients."""
- return self._GenericType('StateFeedbackControllerCoefficients')
+ def _ControllerCoeffType(self):
+ """Returns a template name for StateFeedbackControllerCoefficients."""
+ return self._GenericType('StateFeedbackControllerCoefficients')
- def _ObserverCoeffType(self):
- """Returns a template name for StateFeedbackObserverCoefficients."""
- return self._GenericType(self._observer_type + 'Coefficients')
+ def _ObserverCoeffType(self):
+ """Returns a template name for StateFeedbackObserverCoefficients."""
+ return self._GenericType(self._observer_type + 'Coefficients')
- def WriteHeader(self, header_file):
- """Writes the header file to the file named header_file."""
- with open(header_file, 'w') as fd:
- header_guard = self._HeaderGuard(header_file)
- fd.write('#ifndef %s\n'
- '#define %s\n\n' % (header_guard, header_guard))
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- if (self._plant_type == 'StateFeedbackHybridPlant' or
- self._observer_type == 'HybridKalman'):
- fd.write('#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n')
+ def WriteHeader(self, header_file):
+ """Writes the header file to the file named header_file."""
+ with open(header_file, 'w') as fd:
+ header_guard = self._HeaderGuard(header_file)
+ fd.write('#ifndef %s\n'
+ '#define %s\n\n' % (header_guard, header_guard))
+ fd.write(
+ '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ if (self._plant_type == 'StateFeedbackHybridPlant' or
+ self._observer_type == 'HybridKalman'):
+ fd.write(
+ '#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n'
+ )
- fd.write('\n')
+ fd.write('\n')
- fd.write(self._namespace_start)
+ fd.write(self._namespace_start)
- for const in self._constant_list:
- fd.write(const.Render(self._scalar_type))
+ for const in self._constant_list:
+ fd.write(const.Render(self._scalar_type))
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
- fd.write('\n')
- fd.write(loop.DumpControllerHeader(self._scalar_type))
- fd.write('\n')
- fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
- fd.write('\n')
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
+ fd.write('\n')
+ fd.write(loop.DumpControllerHeader(self._scalar_type))
+ fd.write('\n')
+ fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
+ fd.write('\n')
- fd.write('%s Make%sPlant();\n\n' %
- (self._PlantType(), self._gain_schedule_name))
+ fd.write('%s Make%sPlant();\n\n' % (self._PlantType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sController();\n\n' %
- (self._ControllerType(), self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' % (self._ControllerType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sObserver();\n\n' %
- (self._ObserverType(), self._gain_schedule_name))
+ fd.write('%s Make%sObserver();\n\n' % (self._ObserverType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sLoop();\n\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write('%s Make%sLoop();\n\n' % (self._LoopType(),
+ self._gain_schedule_name))
- fd.write(self._namespace_end)
- fd.write('\n\n')
- fd.write("#endif // %s\n" % header_guard)
+ fd.write(self._namespace_end)
+ fd.write('\n\n')
+ fd.write("#endif // %s\n" % header_guard)
- def WriteCC(self, header_file_name, cc_file):
- """Writes the cc file to the file named cc_file."""
- with open(cc_file, 'w') as fd:
- fd.write('#include \"%s/%s\"\n' %
- (os.path.join(*self._namespaces), header_file_name))
- fd.write('\n')
- fd.write('#include <vector>\n')
- fd.write('\n')
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- fd.write('\n')
- fd.write(self._namespace_start)
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
- fd.write('\n')
+ def WriteCC(self, header_file_name, cc_file):
+ """Writes the cc file to the file named cc_file."""
+ with open(cc_file, 'w') as fd:
+ fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
+ header_file_name))
+ fd.write('\n')
+ fd.write('#include <vector>\n')
+ fd.write('\n')
+ fd.write(
+ '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(
+ loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
+ fd.write('\n')
- for loop in self._loops:
- fd.write(loop.DumpController(self._scalar_type))
- fd.write('\n')
+ for loop in self._loops:
+ fd.write(loop.DumpController(self._scalar_type))
+ fd.write('\n')
- for loop in self._loops:
- fd.write(loop.DumpObserver(self._ObserverCoeffType(), self._scalar_type))
- fd.write('\n')
+ for loop in self._loops:
+ fd.write(
+ loop.DumpObserver(self._ObserverCoeffType(),
+ self._scalar_type))
+ fd.write('\n')
- fd.write('%s Make%sPlant() {\n' %
- (self._PlantType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._PlantCoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._PlantCoeffType(), self._PlantCoeffType(),
- loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
- fd.write('}\n\n')
+ fd.write('%s Make%sPlant() {\n' % (self._PlantType(),
+ self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' %
+ (self._PlantCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
+ loop.PlantFunction()))
+ fd.write(' return %s(&plants);\n' % self._PlantType())
+ fd.write('}\n\n')
- fd.write('%s Make%sController() {\n' %
- (self._ControllerType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerCoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
- loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._ControllerType())
- fd.write('}\n\n')
+ fd.write('%s Make%sController() {\n' % (self._ControllerType(),
+ self._gain_schedule_name))
+ fd.write(
+ ' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' %
+ (self._ControllerCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(
+ ' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ControllerCoeffType(),
+ self._ControllerCoeffType(), loop.ControllerFunction()))
+ fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write('}\n\n')
- fd.write('%s Make%sObserver() {\n' %
- (self._ObserverType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
- self._ObserverCoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ObserverCoeffType(), self._ObserverCoeffType(),
- loop.ObserverFunction()))
- fd.write(' return %s(&observers);\n' % self._ObserverType())
- fd.write('}\n\n')
+ fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
+ self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n'
+ % (self._ObserverCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(
+ ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
+ % (index, self._ObserverCoeffType(),
+ self._ObserverCoeffType(), loop.ObserverFunction()))
+ fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
- fd.write(' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
- (self._LoopType(), self._gain_schedule_name,
- self._gain_schedule_name, self._gain_schedule_name))
- fd.write('}\n\n')
+ fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
+ self._gain_schedule_name))
+ fd.write(
+ ' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n'
+ % (self._LoopType(), self._gain_schedule_name,
+ self._gain_schedule_name, self._gain_schedule_name))
+ fd.write('}\n\n')
- fd.write(self._namespace_end)
- fd.write('\n')
+ fd.write(self._namespace_end)
+ fd.write('\n')
class ControlLoop(object):
- def __init__(self, name):
- """Constructs a control loop object.
- Args:
- name: string, The name of the loop to use when writing the C++ files.
- """
- self._name = name
+ def __init__(self, name):
+ """Constructs a control loop object.
- @property
- def name(self):
- """Returns the name"""
- return self._name
+ Args:
+ name: string, The name of the loop to use when writing the C++ files.
+ """
+ self._name = name
- def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
- """Calculates the discrete time values for A and B.
+ @property
+ def name(self):
+ """Returns the name"""
+ return self._name
- Args:
- A_continuous: numpy.matrix, The continuous time A matrix
- B_continuous: numpy.matrix, The continuous time B matrix
- dt: float, The time step of the control loop
+ def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+ """Calculates the discrete time values for A and B.
- Returns:
- (A, B), numpy.matrix, the control matricies.
- """
- return controls.c2d(A_continuous, B_continuous, dt)
+ Args:
+ A_continuous: numpy.matrix, The continuous time A matrix
+ B_continuous: numpy.matrix, The continuous time B matrix
+ dt: float, The time step of the control loop
- def InitializeState(self):
- """Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
- self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ Returns:
+ (A, B), numpy.matrix, the control matricies.
+ """
+ return controls.c2d(A_continuous, B_continuous, dt)
- def PlaceControllerPoles(self, poles):
- """Places the controller poles.
+ def InitializeState(self):
+ """Sets X, Y, and X_hat to zero defaults."""
+ self.X = numpy.zeros((self.A.shape[0], 1))
+ self.Y = self.C * self.X
+ self.X_hat = numpy.zeros((self.A.shape[0], 1))
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.K = controls.dplace(self.A, self.B, poles)
+ def PlaceControllerPoles(self, poles):
+ """Places the controller poles.
- def PlaceObserverPoles(self, poles):
- """Places the observer poles.
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.K = controls.dplace(self.A, self.B, poles)
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.L = controls.dplace(self.A.T, self.C.T, poles).T
+ def PlaceObserverPoles(self, poles):
+ """Places the observer poles.
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.L = controls.dplace(self.A.T, self.C.T, poles).T
- def Update(self, U):
- """Simulates one time step with the provided U."""
- #U = numpy.clip(U, self.U_min, self.U_max)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
+ def Update(self, U):
+ """Simulates one time step with the provided U."""
+ #U = numpy.clip(U, self.U_min, self.U_max)
+ self.X = self.A * self.X + self.B * U
+ self.Y = self.C * self.X + self.D * U
- def PredictObserver(self, U):
- """Runs the predict step of the observer update."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
+ def PredictObserver(self, U):
+ """Runs the predict step of the observer update."""
+ self.X_hat = (self.A * self.X_hat + self.B * U)
- def CorrectObserver(self, U):
- """Runs the correct step of the observer update."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
+ def CorrectObserver(self, U):
+ """Runs the correct step of the observer update."""
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat = (self.A * self.X_hat + self.B * U +
- self.A * KalmanGain * (self.Y - self.C * self.X_hat - self.D * U))
+ def UpdateObserver(self, U):
+ """Updates the observer given the provided U."""
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ self.X_hat = (self.A * self.X_hat + self.B * U + self.A * KalmanGain *
+ (self.Y - self.C * self.X_hat - self.D * U))
- def _DumpMatrix(self, matrix_name, matrix, scalar_type):
- """Dumps the provided matrix into a variable called matrix_name.
+ def _DumpMatrix(self, matrix_name, matrix, scalar_type):
+ """Dumps the provided matrix into a variable called matrix_name.
- Args:
- matrix_name: string, The variable name to save the matrix to.
- matrix: The matrix to dump.
- scalar_type: The C++ type to use for the scalar in the matrix.
+ Args:
+ matrix_name: string, The variable name to save the matrix to.
+ matrix: The matrix to dump.
+ scalar_type: The C++ type to use for the scalar in the matrix.
- Returns:
- string, The C++ commands required to populate a variable named matrix_name
- with the contents of matrix.
- """
- ans = [' Eigen::Matrix<%s, %d, %d> %s;\n' % (
- scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)]
- for x in xrange(matrix.shape[0]):
- 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))
+ Returns:
+ string, The C++ commands required to populate a variable named matrix_name
+ with the contents of matrix.
+ """
+ ans = [
+ ' Eigen::Matrix<%s, %d, %d> %s;\n' % (scalar_type, matrix.shape[0],
+ matrix.shape[1], matrix_name)
+ ]
+ for x in xrange(matrix.shape[0]):
+ 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))
- return ''.join(ans)
+ return ''.join(ans)
- def DumpPlantHeader(self, plant_coefficient_type):
- """Writes out a c++ header declaration which will create a Plant object.
+ def DumpPlantHeader(self, plant_coefficient_type):
+ """Writes out a c++ header declaration which will create a Plant object.
- Returns:
- string, The header declaration for the function.
- """
- return '%s Make%sPlantCoefficients();\n' % (
- plant_coefficient_type, self._name)
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s Make%sPlantCoefficients();\n' % (plant_coefficient_type,
+ self._name)
- def DumpPlant(self, plant_coefficient_type, scalar_type):
- """Writes out a c++ function which will create a PlantCoefficients object.
+ def DumpPlant(self, plant_coefficient_type, scalar_type):
+ """Writes out a c++ function which will create a PlantCoefficients object.
- Returns:
- string, The function which will create the object.
- """
- ans = ['%s Make%sPlantCoefficients() {\n' % (
- plant_coefficient_type, self._name)]
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = [
+ '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
+ self._name)
+ ]
- ans.append(self._DumpMatrix('C', self.C, scalar_type))
- ans.append(self._DumpMatrix('D', self.D, scalar_type))
- ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
- ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
+ ans.append(self._DumpMatrix('C', self.C, scalar_type))
+ ans.append(self._DumpMatrix('D', self.D, scalar_type))
+ ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
+ ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
- if plant_coefficient_type.startswith('StateFeedbackPlant'):
- ans.append(self._DumpMatrix('A', self.A, scalar_type))
- ans.append(self._DumpMatrix('B', self.B, scalar_type))
- ans.append(' return %s'
- '(A, B, C, D, U_max, U_min);\n' % (
- plant_coefficient_type))
- elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous, scalar_type))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous, scalar_type))
- ans.append(' return %s'
- '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
- plant_coefficient_type))
- else:
- glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+ if plant_coefficient_type.startswith('StateFeedbackPlant'):
+ ans.append(self._DumpMatrix('A', self.A, scalar_type))
+ ans.append(self._DumpMatrix('B', self.B, scalar_type))
+ ans.append(
+ ' return %s'
+ '(A, B, C, D, U_max, U_min);\n' % (plant_coefficient_type))
+ elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
+ ans.append(
+ self._DumpMatrix('A_continuous', self.A_continuous,
+ scalar_type))
+ ans.append(
+ self._DumpMatrix('B_continuous', self.B_continuous,
+ scalar_type))
+ ans.append(' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min);\n' %
+ (plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
- ans.append('}\n')
- return ''.join(ans)
+ ans.append('}\n')
+ return ''.join(ans)
- def PlantFunction(self):
- """Returns the name of the plant coefficient function."""
- return 'Make%sPlantCoefficients()' % self._name
+ def PlantFunction(self):
+ """Returns the name of the plant coefficient function."""
+ return 'Make%sPlantCoefficients()' % self._name
- def ControllerFunction(self):
- """Returns the name of the controller function."""
- return 'Make%sControllerCoefficients()' % self._name
+ def ControllerFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sControllerCoefficients()' % self._name
- def ObserverFunction(self):
- """Returns the name of the controller function."""
- return 'Make%sObserverCoefficients()' % self._name
+ def ObserverFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sObserverCoefficients()' % self._name
- def DumpControllerHeader(self, scalar_type):
- """Writes out a c++ header declaration which will create a Controller object.
+ def DumpControllerHeader(self, scalar_type):
+ """Writes out a c++ header declaration which will create a Controller object.
- Returns:
- string, The header declaration for the function.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
- num_states, num_inputs, num_outputs, scalar_type,
- self.ControllerFunction())
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
+ num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())
- def DumpController(self, scalar_type):
- """Returns a c++ function which will create a Controller object.
+ def DumpController(self, scalar_type):
+ """Returns a c++ function which will create a Controller object.
- Returns:
- string, The function which will create the object.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % (
- num_states, num_inputs, num_outputs, scalar_type,
- self.ControllerFunction())]
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = [
+ 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' %
+ (num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())
+ ]
- ans.append(self._DumpMatrix('K', self.K, scalar_type))
- if not hasattr(self, 'Kff'):
- self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
+ ans.append(self._DumpMatrix('K', self.K, scalar_type))
+ if not hasattr(self, 'Kff'):
+ self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
- ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
+ ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
- ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
- '(K, Kff);\n' % (
- num_states, num_inputs, num_outputs, scalar_type))
- ans.append('}\n')
- return ''.join(ans)
+ ans.append(
+ ' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
+ '(K, Kff);\n' % (num_states, num_inputs, num_outputs, scalar_type))
+ ans.append('}\n')
+ return ''.join(ans)
- def DumpObserverHeader(self, observer_coefficient_type):
- """Writes out a c++ header declaration which will create a Observer object.
+ def DumpObserverHeader(self, observer_coefficient_type):
+ """Writes out a c++ header declaration which will create a Observer object.
- Returns:
- string, The header declaration for the function.
- """
- return '%s %s;\n' % (
- observer_coefficient_type, self.ObserverFunction())
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s %s;\n' % (observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self, observer_coefficient_type, scalar_type):
- """Returns a c++ function which will create a Observer object.
+ def DumpObserver(self, observer_coefficient_type, scalar_type):
+ """Returns a c++ function which will create a Observer object.
- Returns:
- string, The function which will create the object.
- """
- ans = ['%s %s {\n' % (
- observer_coefficient_type, self.ObserverFunction())]
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = [
+ '%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
+ ]
- if observer_coefficient_type.startswith('StateFeedbackObserver'):
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- Q = self.Q
- R = self.R
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- Q = numpy.zeros(self.A.shape)
- R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
- ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
- ans.append(self._DumpMatrix('Q', Q, scalar_type))
- ans.append(self._DumpMatrix('R', R, scalar_type))
- ans.append(' return %s(KalmanGain, Q, R);\n'
- % (observer_coefficient_type,))
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ Q = self.Q
+ R = self.R
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ Q = numpy.zeros(self.A.shape)
+ R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
+ ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
+ ans.append(self._DumpMatrix('Q', Q, scalar_type))
+ ans.append(self._DumpMatrix('R', R, scalar_type))
+ ans.append(' return %s(KalmanGain, Q, R);\n' %
+ (observer_coefficient_type,))
- elif observer_coefficient_type.startswith('HybridKalman'):
- ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
- ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
- ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state, scalar_type))
- ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
- observer_coefficient_type,))
- else:
- glog.fatal('Unsupported observer type %s', observer_coefficient_type)
+ elif observer_coefficient_type.startswith('HybridKalman'):
+ ans.append(
+ self._DumpMatrix('Q_continuous', self.Q_continuous,
+ scalar_type))
+ ans.append(
+ self._DumpMatrix('R_continuous', self.R_continuous,
+ scalar_type))
+ ans.append(
+ self._DumpMatrix('P_steady_state', self.P_steady_state,
+ scalar_type))
+ ans.append(
+ ' return %s(Q_continuous, R_continuous, P_steady_state);\n' %
+ (observer_coefficient_type,))
+ else:
+ glog.fatal('Unsupported observer type %s',
+ observer_coefficient_type)
- ans.append('}\n')
- return ''.join(ans)
+ ans.append('}\n')
+ return ''.join(ans)
+
class HybridControlLoop(ControlLoop):
- def __init__(self, name):
- super(HybridControlLoop, self).__init__(name=name)
- def Discretize(self, dt):
- [self.A, self.B, self.Q, self.R] = \
- controls.kalmd(self.A_continuous, self.B_continuous,
- self.Q_continuous, self.R_continuous, dt)
+ def __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
- def PredictHybridObserver(self, U, dt):
- self.Discretize(dt)
- self.X_hat = self.A * self.X_hat + self.B * U
- self.P = (self.A * self.P * self.A.T + self.Q)
+ def Discretize(self, dt):
+ [self.A, self.B, self.Q, self.R] = \
+ controls.kalmd(self.A_continuous, self.B_continuous,
+ self.Q_continuous, self.R_continuous, dt)
- def CorrectHybridObserver(self, U):
- Y_bar = self.Y - self.C * self.X_hat
- C_t = self.C.T
- S = self.C * self.P * C_t + self.R
- self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
- self.X_hat = self.X_hat + self.KalmanGain * Y_bar
- self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+ def PredictHybridObserver(self, U, dt):
+ self.Discretize(dt)
+ self.X_hat = self.A * self.X_hat + self.B * U
+ self.P = (self.A * self.P * self.A.T + self.Q)
- def InitializeState(self):
- super(HybridControlLoop, self).InitializeState()
- if hasattr(self, 'Q_steady_state'):
- self.P = self.Q_steady_state
- else:
- self.P = numpy.matrix(numpy.zeros((self.A.shape[0], self.A.shape[0])))
+ def CorrectHybridObserver(self, U):
+ Y_bar = self.Y - self.C * self.X_hat
+ C_t = self.C.T
+ S = self.C * self.P * C_t + self.R
+ self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
+ self.X_hat = self.X_hat + self.KalmanGain * Y_bar
+ self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+
+ def InitializeState(self):
+ super(HybridControlLoop, self).InitializeState()
+ if hasattr(self, 'Q_steady_state'):
+ self.P = self.Q_steady_state
+ else:
+ self.P = numpy.matrix(
+ numpy.zeros((self.A.shape[0], self.A.shape[0])))
class CIM(object):
- def __init__(self):
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133.0
- # Free Speed in rad/s
- self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
- # Free Current in Amps
- self.free_current = 4.7
- # Resistance of the motor
- self.resistance = 12.0 / self.stall_current
- # 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
+
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133.0
+ # Free Speed in rad/s
+ self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in Amps
+ self.free_current = 4.7
+ # Resistance of the motor
+ self.resistance = 12.0 / self.stall_current
+ # 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
class MiniCIM(object):
- def __init__(self):
- # Stall Torque in N m
- self.stall_torque = 1.41
- # Stall Current in Amps
- self.stall_current = 89.0
- # Free Speed in rad/s
- self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
- # Free Current in Amps
- self.free_current = 3.0
- # Resistance of the motor
- self.resistance = 12.0 / self.stall_current
- # 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
+
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 1.41
+ # Stall Current in Amps
+ self.stall_current = 89.0
+ # Free Speed in rad/s
+ self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in Amps
+ self.free_current = 3.0
+ # Resistance of the motor
+ self.resistance = 12.0 / self.stall_current
+ # 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
class NMotor(object):
+
def __init__(self, motor, n):
"""Gangs together n motors."""
self.motor = motor
@@ -558,6 +601,7 @@
class Vex775Pro(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 0.71
@@ -570,7 +614,8 @@
# Resistance of the motor
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Motor inertia in kg m^2
@@ -578,37 +623,40 @@
class BAG(object):
- # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
- def __init__(self):
- # Stall Torque in (N m)
- self.stall_torque = 0.43
- # Stall Current in (Amps)
- self.stall_current = 53.0
- # Free Speed in (rad/s)
- self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
- # Free Current in (Amps)
- self.free_current = 1.8
- # Resistance of the motor (Ohms)
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant (radians / (sec * volt))
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
- # Torque constant (N * m / A)
- self.Kt = self.stall_torque / self.stall_current
- # Motor inertia in kg m^2
- self.motor_inertia = 0.000006
+ # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
+ def __init__(self):
+ # Stall Torque in (N m)
+ self.stall_torque = 0.43
+ # Stall Current in (Amps)
+ self.stall_current = 53.0
+ # Free Speed in (rad/s)
+ self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in (Amps)
+ self.free_current = 1.8
+ # Resistance of the motor (Ohms)
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant (radians / (sec * volt))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
+ # Torque constant (N * m / A)
+ self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.000006
+
class MN3510(object):
- def __init__(self):
- # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
- # Free Current in Amps
- self.free_current = 0.0
- # Resistance of the motor
- self.resistance = 0.188
- # Stall Current in Amps
- self.stall_current = 14.0 / self.resistance
- # Motor velocity constant
- self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
- # Torque constant Nm / A
- self.Kt = 1.0 / self.Kv
- # Stall Torque in N m
- self.stall_torque = self.Kt * self.stall_current
+
+ def __init__(self):
+ # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
+ # Free Current in Amps
+ self.free_current = 0.0
+ # Resistance of the motor
+ self.resistance = 0.188
+ # Stall Current in Amps
+ self.stall_current = 14.0 / self.resistance
+ # Motor velocity constant
+ self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
+ # Torque constant Nm / A
+ self.Kt = 1.0 / self.Kv
+ # Stall Torque in N m
+ self.stall_torque = self.Kt * self.stall_current
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 6db8f34..8efcff6 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -16,106 +16,108 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class DownEstimator(control_loop.ControlLoop):
- def __init__(self, name='DownEstimator'):
- super(DownEstimator, self).__init__(name)
- self.dt = 0.005
- # State is [gyro_angle, bias].
- # U is [gyro_x_velocity].
+ def __init__(self, name='DownEstimator'):
+ super(DownEstimator, self).__init__(name)
+ self.dt = 0.005
- self.A_continuous = numpy.matrix([[0, 1],
- [0, 0]])
+ # State is [gyro_angle, bias].
+ # U is [gyro_x_velocity].
- self.B_continuous = numpy.matrix([[1],
- [0]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, 0]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.B_continuous = numpy.matrix([[1], [0]])
- q_gyro_noise = 1e-6
- q_gyro_bias_noise = 1e-3
- self.Q = numpy.matrix([[1.0 / (q_gyro_noise ** 2.0), 0.0],
- [0.0, 1.0 / (q_gyro_bias_noise ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_accelerometer_angle_noise = 5e+7
- self.R = numpy.matrix([[(r_accelerometer_angle_noise ** 2.0)]])
+ q_gyro_noise = 1e-6
+ q_gyro_bias_noise = 1e-3
+ self.Q = numpy.matrix([[1.0 / (q_gyro_noise**2.0), 0.0],
+ [0.0, 1.0 / (q_gyro_bias_noise**2.0)]])
- self.C = numpy.matrix([[1.0, 0.0]])
- self.D = numpy.matrix([[0]])
+ r_accelerometer_angle_noise = 5e+7
+ self.R = numpy.matrix([[(r_accelerometer_angle_noise**2.0)]])
- self.U_max = numpy.matrix([[numpy.pi]])
- self.U_min = numpy.matrix([[-numpy.pi]])
- self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.U_max = numpy.matrix([[numpy.pi]])
+ self.U_min = numpy.matrix([[-numpy.pi]])
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.InitializeState()
+ self.L = self.A * self.KalmanGain
- def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
- acceleration = math.sqrt(
- accelerometer_x**2 + accelerometer_y**2 + accelerometer_z**2)
- if acceleration < 0.9 or acceleration > 1.1:
- glog.error('bad total acceleration %f' % acceleration)
- # TODO(Brian): Tune this?
- return
- accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
- Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+ self.InitializeState()
- Y = Z - self.H * self.X_hat
- S = self.H * self.P * self.H.transpose() + self.R
- K = self.P * self.H.transpose() * numpy.linalg.inv(S)
- self.X_hat += K * Y
- self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+ def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
+ acceleration = math.sqrt(accelerometer_x**2 + accelerometer_y**2 +
+ accelerometer_z**2)
+ if acceleration < 0.9 or acceleration > 1.1:
+ glog.error('bad total acceleration %f' % acceleration)
+ # TODO(Brian): Tune this?
+ return
+ accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
+ Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+
+ Y = Z - self.H * self.X_hat
+ S = self.H * self.P * self.H.transpose() + self.R
+ K = self.P * self.H.transpose() * numpy.linalg.inv(S)
+ self.X_hat += K * Y
+ self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- estimator = DownEstimator()
+ estimator = DownEstimator()
- if FLAGS.plot:
- real_angles = [0]
- real_velocities = [0]
- estimated_angles = [0]
- estimated_velocities = [0]
- for _ in xrange(100):
- estimator.Predict(0)
- estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
- real_angles.append(math.pi / 2)
- real_velocities.append(0)
- estimated_angles.append(estimator.X_hat[0, 0])
- estimated_velocities.append(estimator.X_hat[1, 0])
- angle = math.pi / 2
- velocity = 1
- for i in xrange(100):
- measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
- estimator.Predict(measured_velocity)
- estimator.Update(math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
- math.cos(angle) + (random.random() - 0.5) * 0.02,
- measured_velocity)
- real_angles.append(angle)
- real_velocities.append(velocity)
- estimated_angles.append(estimator.X_hat[0, 0])
- estimated_velocities.append(estimator.X_hat[1, 0])
- angle += velocity * estimator.dt
- pylab.plot(range(len(real_angles)), real_angles)
- pylab.plot(range(len(real_velocities)), real_velocities)
- pylab.plot(range(len(estimated_angles)), estimated_angles)
- pylab.plot(range(len(estimated_velocities)), estimated_velocities)
- pylab.show()
+ if FLAGS.plot:
+ real_angles = [0]
+ real_velocities = [0]
+ estimated_angles = [0]
+ estimated_velocities = [0]
+ for _ in xrange(100):
+ estimator.Predict(0)
+ estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
+ real_angles.append(math.pi / 2)
+ real_velocities.append(0)
+ estimated_angles.append(estimator.X_hat[0, 0])
+ estimated_velocities.append(estimator.X_hat[1, 0])
+ angle = math.pi / 2
+ velocity = 1
+ for i in xrange(100):
+ measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
+ estimator.Predict(measured_velocity)
+ estimator.Update(
+ math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
+ math.cos(angle) + (random.random() - 0.5) * 0.02,
+ measured_velocity)
+ real_angles.append(angle)
+ real_velocities.append(velocity)
+ estimated_angles.append(estimator.X_hat[0, 0])
+ estimated_velocities.append(estimator.X_hat[1, 0])
+ angle += velocity * estimator.dt
+ pylab.plot(range(len(real_angles)), real_angles)
+ pylab.plot(range(len(real_velocities)), real_velocities)
+ pylab.plot(range(len(estimated_angles)), estimated_angles)
+ pylab.plot(range(len(estimated_velocities)), estimated_velocities)
+ pylab.show()
- if len(argv) != 3:
- print "Expected .h file name and .cc file name"
- else:
- namespaces = ['frc971', 'control_loops', 'drivetrain']
- kf_loop_writer = control_loop.ControlLoopWriter(
- "DownEstimator", [estimator],
- namespaces = namespaces)
- kf_loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "DownEstimator", [estimator], namespaces=namespaces)
+ kf_loop_writer.Write(argv[1], argv[2])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c59cbab..1da85bc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -7,7 +7,9 @@
from matplotlib import pylab
import glog
+
class DrivetrainParams(object):
+
def __init__(self,
J,
mass,
@@ -107,6 +109,7 @@
class Drivetrain(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
name="Drivetrain",
@@ -190,8 +193,8 @@
# 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))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -215,14 +218,11 @@
# X will be of the format
# [[positionl], [velocityl], [positionr], velocityr]]
self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
- [0, 0, 0, 1],
- [0, -self.msnl * self.tcl, 0, -self.mspr * self.tcr]])
+ [[0, 1, 0, 0], [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
+ [0, 0, 0, 1], [0, -self.msnl * self.tcl, 0,
+ -self.mspr * self.tcr]])
self.B_continuous = numpy.matrix(
- [[0, 0],
- [self.mspl * self.mpl, self.msnr * self.mpr],
- [0, 0],
+ [[0, 0], [self.mspl * self.mpl, self.msnr * self.mpr], [0, 0],
[self.msnl * self.mpl, self.mspr * self.mpr]])
self.C = numpy.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
@@ -232,10 +232,10 @@
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.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.R = numpy.matrix([[(1.0 / (12.0**2.0)), 0.0],
[0.0, (1.0 / (12.0**2.0))]])
@@ -254,6 +254,7 @@
class KFDrivetrain(Drivetrain):
+
def __init__(self,
drivetrain_params,
name="KFDrivetrain",
@@ -291,9 +292,10 @@
self.A_continuous[0:4, 0:4] = self.unaugmented_A_continuous
if self.force:
- self.A_continuous[0:4, 4:6] = numpy.matrix(
- [[0.0, 0.0], [self.mspl, self.msnl], [0.0, 0.0],
- [self.msnr, self.mspr]])
+ self.A_continuous[0:4, 4:6] = numpy.matrix([[0.0, 0.0],
+ [self.mspl, self.msnl],
+ [0.0, 0.0],
+ [self.msnr, self.mspr]])
q_voltage = drivetrain_params.kf_q_voltage * self.mpl
else:
self.A_continuous[0:4, 4:6] = self.unaugmented_B_continuous
@@ -307,28 +309,31 @@
self.B_continuous, self.dt)
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]])
+ 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.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]) / gravity
- 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
- ]])
+ 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
- ]])
+ 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.D = numpy.matrix([[0, 0], [0, 0], [0, 0]])
@@ -337,12 +342,12 @@
q_encoder_uncertainty = 2.00
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],
+ [[(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)]])
r_pos = 0.0001
@@ -365,7 +370,8 @@
# If we don't have an IMU, pad various matrices with zeros so that
# we can still have 4 measurement outputs.
if not self.has_imu:
- self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
+ 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)))))
Rtmp = numpy.zeros((4, 4))
@@ -401,8 +407,11 @@
self.InitializeState()
-def WriteDrivetrain(drivetrain_files, kf_drivetrain_files, year_namespace,
- drivetrain_params, scalar_type='double'):
+def WriteDrivetrain(drivetrain_files,
+ kf_drivetrain_files,
+ year_namespace,
+ drivetrain_params,
+ scalar_type='double'):
# Write the generated constants out to a file.
drivetrain_low_low = Drivetrain(
@@ -569,8 +578,7 @@
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'
- )
+ 'Angular Move\nLeft position going to -1 and right position going to 1')
pylab.legend(loc='center right')
pylab.show()
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 6c88e15..088b204 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -15,392 +15,443 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
gflags.DEFINE_string('data', None, 'If defined, plot the provided CAN data')
-gflags.DEFINE_bool('rerun_kf', False, 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.')
+gflags.DEFINE_bool(
+ 'rerun_kf', False,
+ 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.'
+)
+
class SystemParams(object):
- def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
- q_torque, current_limit):
- self.J = J
- self.G = G
- self.q_pos = q_pos
- self.q_vel = q_vel
- self.q_torque = q_torque
- self.kP = kP
- self.kD = kD
- self.kCompensationTimeconstant = kCompensationTimeconstant
- self.r_pos = 0.001
- self.current_limit = current_limit
- #[15.0, 0.25],
- #[10.0, 0.2],
- #[5.0, 0.13],
- #[3.0, 0.10],
- #[2.0, 0.08],
- #[1.0, 0.06],
- #[0.5, 0.05],
- #[0.25, 0.025],
+ def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
+ q_torque, current_limit):
+ self.J = J
+ self.G = G
+ self.q_pos = q_pos
+ self.q_vel = q_vel
+ self.q_torque = q_torque
+ self.kP = kP
+ self.kD = kD
+ self.kCompensationTimeconstant = kCompensationTimeconstant
+ self.r_pos = 0.001
+ self.current_limit = current_limit
-kWheel = SystemParams(J=0.0008,
- G=(1.25 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.20,
- q_torque=0.005,
- kP=7.0,
- kD=0.0,
- kCompensationTimeconstant=0.95,
- current_limit=4.5)
-kTrigger = SystemParams(J=0.00025,
- G=(0.925 * 2.0 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.1,
- q_torque=0.005,
- kP=120.0,
- kD=1.8,
- kCompensationTimeconstant=0.95,
- current_limit=3.0)
+ #[15.0, 0.25],
+ #[10.0, 0.2],
+ #[5.0, 0.13],
+ #[3.0, 0.10],
+ #[2.0, 0.08],
+ #[1.0, 0.06],
+ #[0.5, 0.05],
+ #[0.25, 0.025],
+
+
+kWheel = SystemParams(
+ J=0.0008,
+ G=(1.25 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.20,
+ q_torque=0.005,
+ kP=7.0,
+ kD=0.0,
+ kCompensationTimeconstant=0.95,
+ current_limit=4.5)
+kTrigger = SystemParams(
+ J=0.00025,
+ G=(0.925 * 2.0 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.1,
+ q_torque=0.005,
+ kP=120.0,
+ kD=1.8,
+ kCompensationTimeconstant=0.95,
+ current_limit=3.0)
+
class HapticInput(control_loop.ControlLoop):
- def __init__(self, params=None, name='HapticInput'):
- # The defaults are for the steering wheel.
- super(HapticInput, self).__init__(name)
- motor = self.motor = control_loop.MN3510()
- # Moment of inertia of the wheel in kg m^2
- self.J = params.J
+ def __init__(self, params=None, name='HapticInput'):
+ # The defaults are for the steering wheel.
+ super(HapticInput, self).__init__(name)
+ motor = self.motor = control_loop.MN3510()
- # Control loop time step
- self.dt = 0.001
+ # Moment of inertia of the wheel in kg m^2
+ self.J = params.J
- # Gear ratio from the motor to the input.
- self.G = params.G
+ # Control loop time step
+ self.dt = 0.001
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1, 1] = 0
- self.A_continuous[0, 1] = 1
+ # Gear ratio from the motor to the input.
+ self.G = params.G
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1, 0] = motor.Kt * self.G / self.J
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1, 1] = 0
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1, 0] = motor.Kt * self.G / self.J
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- self.U_max = numpy.matrix([[2.5]])
- self.U_min = numpy.matrix([[-2.5]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.L = numpy.matrix([[0.0], [0.0]])
- self.K = numpy.matrix([[0.0, 0.0]])
+ self.U_max = numpy.matrix([[2.5]])
+ self.U_min = numpy.matrix([[-2.5]])
- self.InitializeState()
+ self.L = numpy.matrix([[0.0], [0.0]])
+ self.K = numpy.matrix([[0.0, 0.0]])
+
+ self.InitializeState()
class IntegralHapticInput(HapticInput):
- def __init__(self, params=None, name="IntegralHapticInput"):
- super(IntegralHapticInput, self).__init__(name=name, params=params)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, params=None, name="IntegralHapticInput"):
+ super(IntegralHapticInput, self).__init__(name=name, params=params)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[1, 2] = (1 / self.J)
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[1, 2] = (1 / self.J)
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- self.Q = numpy.matrix([[(params.q_pos ** 2.0), 0.0, 0.0],
- [0.0, (params.q_vel ** 2.0), 0.0],
- [0.0, 0.0, (params.q_torque ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix([[(params.r_pos ** 2.0)]])
+ self.Q = numpy.matrix([[(params.q_pos**2.0), 0.0, 0.0],
+ [0.0, (params.q_vel**2.0), 0.0],
+ [0.0, 0.0, (params.q_torque**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ self.R = numpy.matrix([[(params.r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+
+ self.InitializeState()
+
def ReadCan(filename):
- """Reads the candump in filename and returns the 4 fields."""
- trigger = []
- trigger_velocity = []
- trigger_torque = []
- trigger_current = []
- wheel = []
- wheel_velocity = []
- wheel_torque = []
- wheel_current = []
+ """Reads the candump in filename and returns the 4 fields."""
+ trigger = []
+ trigger_velocity = []
+ trigger_torque = []
+ trigger_current = []
+ wheel = []
+ wheel_velocity = []
+ wheel_torque = []
+ wheel_current = []
- trigger_request_time = [0.0]
- trigger_request_current = [0.0]
- wheel_request_time = [0.0]
- wheel_request_current = [0.0]
+ trigger_request_time = [0.0]
+ trigger_request_current = [0.0]
+ wheel_request_time = [0.0]
+ wheel_request_current = [0.0]
- with open(filename, 'r') as fd:
- for line in fd:
- data = line.split()
- can_id = int(data[1], 16)
- if can_id == 0:
- data = [int(d, 16) for d in data[3:]]
- trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
- trigger_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
- trigger_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- trigger_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
- elif can_id == 1:
- data = [int(d, 16) for d in data[3:]]
- wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
- wheel_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
- wheel_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- wheel_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
- elif can_id == 2:
- data = [int(d, 16) for d in data[3:]]
- trigger_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- trigger_request_time.append(len(trigger) * 0.001)
- elif can_id == 3:
- data = [int(d, 16) for d in data[3:]]
- wheel_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- wheel_request_time.append(len(wheel) * 0.001)
+ with open(filename, 'r') as fd:
+ for line in fd:
+ data = line.split()
+ can_id = int(data[1], 16)
+ if can_id == 0:
+ data = [int(d, 16) for d in data[3:]]
+ trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ trigger_velocity.append(
+ ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ trigger_torque.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_current.append(
+ ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 1:
+ data = [int(d, 16) for d in data[3:]]
+ wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ wheel_velocity.append(
+ ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ wheel_torque.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_current.append(
+ ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 2:
+ data = [int(d, 16) for d in data[3:]]
+ trigger_request_current.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_request_time.append(len(trigger) * 0.001)
+ elif can_id == 3:
+ data = [int(d, 16) for d in data[3:]]
+ wheel_request_current.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_request_time.append(len(wheel) * 0.001)
- trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
- wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
+ trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
+ wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
- # Extend out the data in the interpolation table.
- trigger_request_time.append(trigger_data_time[-1])
- trigger_request_current.append(trigger_request_current[-1])
- wheel_request_time.append(wheel_data_time[-1])
- wheel_request_current.append(wheel_request_current[-1])
+ # Extend out the data in the interpolation table.
+ trigger_request_time.append(trigger_data_time[-1])
+ trigger_request_current.append(trigger_request_current[-1])
+ wheel_request_time.append(wheel_data_time[-1])
+ wheel_request_current.append(wheel_request_current[-1])
- return (trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity,
- wheel_velocity, trigger_torque, wheel_torque, trigger_current,
- wheel_current, trigger_request_time, trigger_request_current,
- wheel_request_time, wheel_request_current)
+ return (trigger_data_time, wheel_data_time, trigger, wheel,
+ trigger_velocity, wheel_velocity, trigger_torque, wheel_torque,
+ trigger_current, wheel_current, trigger_request_time,
+ trigger_request_current, wheel_request_time, wheel_request_current)
-def rerun_and_plot_kf(data_time, data_radians, data_current, data_request_current,
- params, run_correct=True):
- kf_velocity = []
- dt_velocity = []
- kf_position = []
- adjusted_position = []
- last_angle = None
- haptic_observer = IntegralHapticInput(params=params)
- # Parameter sweep J.
- num_kf = 1
- min_J = max_J = params.J
+def rerun_and_plot_kf(data_time,
+ data_radians,
+ data_current,
+ data_request_current,
+ params,
+ run_correct=True):
+ kf_velocity = []
+ dt_velocity = []
+ kf_position = []
+ adjusted_position = []
+ last_angle = None
+ haptic_observer = IntegralHapticInput(params=params)
- # J = 0.0022
- #num_kf = 15
- #min_J = min_J / 2.0
- #max_J = max_J * 2.0
- initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+ # Parameter sweep J.
+ num_kf = 1
+ min_J = max_J = params.J
- def DupParamsWithJ(params, J):
- p = copy.copy(params)
- p.J = J
- return p
- haptic_observers = [IntegralHapticInput(params=DupParamsWithJ(params, j)) for j in
- numpy.logspace(numpy.log10(min_J),
- numpy.log10(max_J), num=num_kf)]
- # Initialize all the KF's.
- haptic_observer.X_hat[1, 0] = initial_velocity
- haptic_observer.X_hat[0, 0] = data_radians[0]
- for observer in haptic_observers:
- observer.X_hat[1, 0] = initial_velocity
- observer.X_hat[0, 0] = data_radians[0]
+ # J = 0.0022
+ #num_kf = 15
+ #min_J = min_J / 2.0
+ #max_J = max_J * 2.0
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
- last_request_current = data_request_current[0]
- kf_torques = [[] for i in xrange(num_kf)]
- for angle, current, request_current in zip(data_radians, data_current,
- data_request_current):
- # Predict and correct all the parameter swept observers.
- for i, observer in enumerate(haptic_observers):
- observer.Y = numpy.matrix([[angle]])
- if run_correct:
- observer.CorrectObserver(numpy.matrix([[current]]))
- kf_torques[i].append(-observer.X_hat[2, 0])
- observer.PredictObserver(numpy.matrix([[current]]))
- observer.PredictObserver(numpy.matrix([[current]]))
+ def DupParamsWithJ(params, J):
+ p = copy.copy(params)
+ p.J = J
+ return p
- # Predict and correct the main observer.
- haptic_observer.Y = numpy.matrix([[angle]])
- if run_correct:
- haptic_observer.CorrectObserver(numpy.matrix([[current]]))
- kf_position.append(haptic_observer.X_hat[0, 0])
- adjusted_position.append(kf_position[-1] - last_request_current / params.kP)
- last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (1.0 - params.kCompensationTimeconstant)
- kf_velocity.append(haptic_observer.X_hat[1, 0])
- if last_angle is None:
- last_angle = angle
- dt_velocity.append((angle - last_angle) / 0.001)
+ haptic_observers = [
+ IntegralHapticInput(params=DupParamsWithJ(params, j))
+ for j in numpy.logspace(
+ numpy.log10(min_J), numpy.log10(max_J), num=num_kf)
+ ]
+ # Initialize all the KF's.
+ haptic_observer.X_hat[1, 0] = initial_velocity
+ haptic_observer.X_hat[0, 0] = data_radians[0]
+ for observer in haptic_observers:
+ observer.X_hat[1, 0] = initial_velocity
+ observer.X_hat[0, 0] = data_radians[0]
- haptic_observer.PredictObserver(numpy.matrix([[current]]))
- last_angle = angle
+ last_request_current = data_request_current[0]
+ kf_torques = [[] for i in xrange(num_kf)]
+ for angle, current, request_current in zip(data_radians, data_current,
+ data_request_current):
+ # Predict and correct all the parameter swept observers.
+ for i, observer in enumerate(haptic_observers):
+ observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_torques[i].append(-observer.X_hat[2, 0])
+ observer.PredictObserver(numpy.matrix([[current]]))
+ observer.PredictObserver(numpy.matrix([[current]]))
- # Plot the wheel observers.
- fig, ax1 = pylab.subplots()
- ax1.plot(data_time, data_radians, '.', label='wheel')
- ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
- ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
- ax1.plot(data_time, kf_position, '.', label='kf_position')
- ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
+ # Predict and correct the main observer.
+ haptic_observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ haptic_observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_position.append(haptic_observer.X_hat[0, 0])
+ adjusted_position.append(kf_position[-1] -
+ last_request_current / params.kP)
+ last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (
+ 1.0 - params.kCompensationTimeconstant)
+ kf_velocity.append(haptic_observer.X_hat[1, 0])
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
- ax2 = ax1.twinx()
- ax2.plot(data_time, data_current, label='data_current')
- ax2.plot(data_time, data_request_current, label='request_current')
+ haptic_observer.PredictObserver(numpy.matrix([[current]]))
+ last_angle = angle
- for i, kf_torque in enumerate(kf_torques):
- ax2.plot(data_time, kf_torque,
- label='-kf_torque[%f]' % haptic_observers[i].J)
- fig.tight_layout()
- ax1.legend(loc=3)
- ax2.legend(loc=4)
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='wheel')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+ ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
+ ax1.plot(data_time, kf_position, '.', label='kf_position')
+ ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
-def plot_input(data_time, data_radians, data_velocity, data_torque,
- data_current, params, run_correct=True):
- dt_velocity = []
- last_angle = None
- initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_current, label='data_current')
+ ax2.plot(data_time, data_request_current, label='request_current')
- for angle in data_radians:
- if last_angle is None:
- last_angle = angle
- dt_velocity.append((angle - last_angle) / 0.001)
+ for i, kf_torque in enumerate(kf_torques):
+ ax2.plot(
+ data_time,
+ kf_torque,
+ label='-kf_torque[%f]' % haptic_observers[i].J)
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
- last_angle = angle
- # Plot the wheel observers.
- fig, ax1 = pylab.subplots()
- ax1.plot(data_time, data_radians, '.', label='angle')
- ax1.plot(data_time, data_velocity, '-', label='velocity')
- ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+def plot_input(data_time,
+ data_radians,
+ data_velocity,
+ data_torque,
+ data_current,
+ params,
+ run_correct=True):
+ dt_velocity = []
+ last_angle = None
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
- ax2 = ax1.twinx()
- ax2.plot(data_time, data_torque, label='data_torque')
- ax2.plot(data_time, data_current, label='data_current')
- fig.tight_layout()
- ax1.legend(loc=3)
- ax2.legend(loc=4)
+ for angle in data_radians:
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
+
+ last_angle = angle
+
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='angle')
+ ax1.plot(data_time, data_velocity, '-', label='velocity')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_torque, label='data_torque')
+ ax2.plot(data_time, data_current, label='data_current')
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
+
def main(argv):
- if FLAGS.plot:
- if FLAGS.data is None:
- haptic_wheel = HapticInput()
- haptic_wheel_controller = IntegralHapticInput()
- observer_haptic_wheel = IntegralHapticInput()
- observer_haptic_wheel.X_hat[2, 0] = 0.01
+ if FLAGS.plot:
+ if FLAGS.data is None:
+ haptic_wheel = HapticInput()
+ haptic_wheel_controller = IntegralHapticInput()
+ observer_haptic_wheel = IntegralHapticInput()
+ observer_haptic_wheel.X_hat[2, 0] = 0.01
- R = numpy.matrix([[0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [0.0]])
- control_loop.TestSingleIntegralAxisSquareWave(
- R, 1.0, haptic_wheel, haptic_wheel_controller, observer_haptic_wheel)
+ control_loop.TestSingleIntegralAxisSquareWave(
+ R, 1.0, haptic_wheel, haptic_wheel_controller,
+ observer_haptic_wheel)
+ else:
+ # Read the CAN trace in.
+ trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
+ wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
+ wheel_current, trigger_request_time, trigger_request_current, \
+ wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+
+ wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
+ wheel_velocity = [w * 50.0 for w in wheel_velocity]
+ wheel_torque = [w / 2.0 for w in wheel_torque]
+ wheel_current = [w * 10.0 for w in wheel_current]
+ wheel_request_current = [w * 2.0 for w in wheel_request_current]
+ resampled_wheel_request_current = scipy.interpolate.interp1d(
+ wheel_request_time, wheel_request_current,
+ kind="zero")(wheel_data_time)
+
+ trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
+ trigger_velocity = [t * 50.0 for t in trigger_velocity]
+ trigger_torque = [t / 2.0 for t in trigger_torque]
+ trigger_current = [t * 10.0 for t in trigger_current]
+ trigger_request_current = [t * 2.0 for t in trigger_request_current]
+ resampled_trigger_request_current = scipy.interpolate.interp1d(
+ trigger_request_time, trigger_request_current,
+ kind="zero")(trigger_data_time)
+
+ if FLAGS.rerun_kf:
+ rerun_and_plot_kf(
+ trigger_data_time,
+ trigger_radians,
+ trigger_current,
+ resampled_trigger_request_current,
+ kTrigger,
+ run_correct=True)
+ rerun_and_plot_kf(
+ wheel_data_time,
+ wheel_radians,
+ wheel_current,
+ resampled_wheel_request_current,
+ kWheel,
+ run_correct=True)
+ else:
+ plot_input(trigger_data_time, trigger_radians, trigger_velocity,
+ trigger_torque, trigger_current, kTrigger)
+ plot_input(wheel_data_time, wheel_radians, wheel_velocity,
+ wheel_torque, wheel_current, kWheel)
+ pylab.show()
+
+ return
+
+ if len(argv) != 9:
+ glog.fatal('Expected .h file name and .cc file name')
else:
- # Read the CAN trace in.
- trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
- wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
- wheel_current, trigger_request_time, trigger_request_current, \
- wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
+ ('HapticTrigger', kTrigger, argv[5:9])]:
+ haptic_input = HapticInput(params=params, name=name)
+ loop_writer = control_loop.ControlLoopWriter(
+ name, [haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
+ loop_writer.Write(filenames[0], filenames[1])
- wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
- wheel_velocity = [w * 50.0 for w in wheel_velocity]
- wheel_torque = [w / 2.0 for w in wheel_torque]
- wheel_current = [w * 10.0 for w in wheel_current]
- wheel_request_current = [w * 2.0 for w in wheel_request_current]
- resampled_wheel_request_current = scipy.interpolate.interp1d(
- wheel_request_time, wheel_request_current, kind="zero")(wheel_data_time)
+ integral_haptic_input = IntegralHapticInput(
+ params=params, name='Integral' + name)
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, [integral_haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
- trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
- trigger_velocity = [t * 50.0 for t in trigger_velocity]
- trigger_torque = [t / 2.0 for t in trigger_torque]
- trigger_current = [t * 10.0 for t in trigger_current]
- trigger_request_current = [t * 2.0 for t in trigger_request_current]
- resampled_trigger_request_current = scipy.interpolate.interp1d(
- trigger_request_time, trigger_request_current, kind="zero")(trigger_data_time)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "Dt", "%f",
+ integral_haptic_input.dt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "FreeCurrent", "%f",
+ integral_haptic_input.motor.free_current))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "StallTorque", "%f",
+ integral_haptic_input.motor.stall_torque))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "J", "%f",
+ integral_haptic_input.J))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "R", "%f",
+ integral_haptic_input.motor.resistance))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "T", "%f",
+ integral_haptic_input.motor.Kt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "V", "%f",
+ integral_haptic_input.motor.Kv))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "P", "%f", params.kP))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "D", "%f", params.kD))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "G", "%f", params.G))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "CurrentLimit", "%f",
+ params.current_limit))
- if FLAGS.rerun_kf:
- rerun_and_plot_kf(trigger_data_time, trigger_radians, trigger_current,
- resampled_trigger_request_current, kTrigger, run_correct=True)
- rerun_and_plot_kf(wheel_data_time, wheel_radians, wheel_current,
- resampled_wheel_request_current, kWheel, run_correct=True)
- else:
- plot_input(trigger_data_time, trigger_radians, trigger_velocity,
- trigger_torque, trigger_current, kTrigger)
- plot_input(wheel_data_time, wheel_radians, wheel_velocity, wheel_torque,
- wheel_current, kWheel)
- pylab.show()
-
- return
-
- if len(argv) != 9:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['frc971', 'control_loops', 'drivetrain']
- for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
- ('HapticTrigger', kTrigger, argv[5:9])]:
- haptic_input = HapticInput(params=params, name=name)
- loop_writer = control_loop.ControlLoopWriter(name, [haptic_input],
- namespaces=namespaces,
- scalar_type='float')
- loop_writer.Write(filenames[0], filenames[1])
-
- integral_haptic_input = IntegralHapticInput(params=params,
- name='Integral' + name)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name, [integral_haptic_input], namespaces=namespaces,
- scalar_type='float')
-
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "Dt", "%f",
- integral_haptic_input.dt))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "FreeCurrent", "%f",
- integral_haptic_input.motor.free_current))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "StallTorque", "%f",
- integral_haptic_input.motor.stall_torque))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "J", "%f",
- integral_haptic_input.J))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "R", "%f",
- integral_haptic_input.motor.resistance))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "T", "%f",
- integral_haptic_input.motor.Kt))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "V", "%f",
- integral_haptic_input.motor.Kv))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "P", "%f",
- params.kP))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "D", "%f",
- params.kD))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "G", "%f",
- params.G))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "CurrentLimit", "%f",
- params.current_limit))
-
- integral_loop_writer.Write(filenames[2], filenames[3])
+ integral_loop_writer.Write(filenames[2], filenames[3])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index a457183..042bd5c 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -9,6 +9,7 @@
class LinearSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -37,6 +38,7 @@
class LinearSystem(control_loop.ControlLoop):
+
def __init__(self, params, name='LinearSystem'):
super(LinearSystem, self).__init__(name)
self.params = params
@@ -57,10 +59,9 @@
# State is [position, velocity]
# Input is [Voltage]
C1 = self.motor.Kt / (self.G * self.G * self.radius * self.radius *
- self.motor.resistance * self.mass *
- self.motor.Kv)
- C2 = self.motor.Kt / (self.G * self.radius * self.motor.resistance *
- self.mass)
+ self.motor.resistance * self.mass * self.motor.Kv)
+ C2 = self.motor.Kt / (
+ self.G * self.radius * self.motor.resistance * self.mass)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -79,9 +80,10 @@
glog.debug('Controllability of %d',
numpy.linalg.matrix_rank(controllability))
glog.debug('Mass: %f', self.mass)
- glog.debug('Stall force: %f', self.motor.stall_torque / self.G / self.radius)
- glog.debug('Stall acceleration: %f', self.motor.stall_torque / self.G /
- self.radius / self.mass)
+ glog.debug('Stall force: %f',
+ self.motor.stall_torque / self.G / self.radius)
+ glog.debug('Stall acceleration: %f',
+ self.motor.stall_torque / self.G / self.radius / self.mass)
glog.debug('Free speed is %f',
-self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
@@ -122,6 +124,7 @@
class IntegralLinearSystem(LinearSystem):
+
def __init__(self, params, name='IntegralLinearSystem'):
super(IntegralLinearSystem, self).__init__(params, name=name)
@@ -142,10 +145,9 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
- self.Q = numpy.matrix(
- [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
- [0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+ self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+ [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+ [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -203,8 +205,8 @@
vbat = 12.0
- goal = numpy.concatenate(
- (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
profile = TrapezoidProfile(plant.dt)
profile.set_maximum_acceleration(max_acceleration)
@@ -263,8 +265,7 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -391,11 +392,11 @@
loop_writer = control_loop.ControlLoopWriter(
linear_system.name, [linear_system], namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', linear_system.motor.
- free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ linear_system.motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f', linear_system.G *
- linear_system.radius))
+ control_loop.Constant('kOutputRatio', '%f',
+ linear_system.G * linear_system.radius))
loop_writer.AddConstant(
control_loop.Constant('kRadius', '%f', linear_system.radius))
loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 91dba3c..06a182e 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -10,516 +10,565 @@
import glog
+
def CoerceGoal(region, K, w, R):
- """Intersects a line with a region, and finds the closest point to R.
+ """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. This
- function assumes that
+ 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. This
+ function assumes that
- Args:
- region: HPolytope, the valid goal region.
- K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
- w: float, the offset in the equation above.
- R: numpy.matrix (2 x 1), the point to be closest to.
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
- Returns:
- numpy.matrix (2 x 1), the point.
- """
- return DoCoerceGoal(region, K, w, R)[0]
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
def DoCoerceGoal(region, K, w, R):
- if region.IsInside(R):
- return (R, True)
+ if region.IsInside(R):
+ return (R, True)
- perpendicular_vector = K.T / numpy.linalg.norm(K)
- parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
- [-perpendicular_vector[0, 0]]])
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
- # We want to impose the constraint K * X = w on the polytope H * X <= k.
- # We do this by breaking X up into parallel and perpendicular components to
- # the half plane. This gives us the following equation.
- #
- # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
- #
- # Then, substitute this into the polytope.
- #
- # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
- #
- # Substitute K * X = w
- #
- # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
- #
- # Move all the knowns to the right side.
- #
- # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
- #
- # Let t = parallel.T \dot X, the component parallel to the surface.
- #
- # H * parallel * t <= k - H * perpendicular * w
- #
- # This is a polytope which we can solve, and use to figure out the range of X
- # that we care about!
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
- t_poly = polytope.HPolytope(
- region.H * parallel_vector,
- region.k - region.H * perpendicular_vector * w)
+ t_poly = polytope.HPolytope(region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
- vertices = t_poly.Vertices()
+ vertices = t_poly.Vertices()
- if vertices.shape[0]:
- # The region exists!
- # Find the closest vertex
- min_distance = numpy.infty
- closest_point = None
- for vertex in vertices:
- point = parallel_vector * vertex + perpendicular_vector * w
- length = numpy.linalg.norm(R - point)
- if length < min_distance:
- min_distance = length
- closest_point = point
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
- return (closest_point, True)
- else:
- # Find the vertex of the space that is closest to the line.
- region_vertices = region.Vertices()
- min_distance = numpy.infty
- closest_point = None
- for vertex in region_vertices:
- point = vertex.T
- length = numpy.abs((perpendicular_vector.T * point)[0, 0])
- if length < min_distance:
- min_distance = length
- closest_point = point
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
- return (closest_point, False)
+ return (closest_point, False)
+
class VelocityDrivetrainModel(control_loop.ControlLoop):
- def __init__(self, drivetrain_params, left_low=True, right_low=True,
- name="VelocityDrivetrainModel"):
- super(VelocityDrivetrainModel, self).__init__(name)
- self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
- left_low=left_low, right_low=right_low,
- drivetrain_params=drivetrain_params)
- self.dt = drivetrain_params.dt
- self.A_continuous = numpy.matrix(
- [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
- [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
- self.B_continuous = numpy.matrix(
- [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
- [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
- self.C = numpy.matrix(numpy.eye(2))
- self.D = numpy.matrix(numpy.zeros((2, 2)))
+ def __init__(self,
+ drivetrain_params,
+ left_low=True,
+ right_low=True,
+ name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
+ left_low=left_low,
+ right_low=right_low,
+ drivetrain_params=drivetrain_params)
+ self.dt = drivetrain_params.dt
+ self.A_continuous = numpy.matrix(
+ [[
+ self._drivetrain.A_continuous[1, 1],
+ self._drivetrain.A_continuous[1, 3]
+ ],
+ [
+ self._drivetrain.A_continuous[3, 1],
+ self._drivetrain.A_continuous[3, 3]
+ ]])
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
+ self.B_continuous = numpy.matrix(
+ [[
+ self._drivetrain.B_continuous[1, 0],
+ self._drivetrain.B_continuous[1, 1]
+ ],
+ [
+ self._drivetrain.B_continuous[3, 0],
+ self._drivetrain.B_continuous[3, 1]
+ ]])
+ self.C = numpy.matrix(numpy.eye(2))
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
- # FF * X = U (steady state)
- self.FF = self.B.I * (numpy.eye(2) - self.A)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceControllerPoles(drivetrain_params.controller_poles)
- # Build a kalman filter for the velocity. We don't care what the gains
- # are, but the hybrid kalman filter that we want to write to disk to get
- # access to A_continuous and B_continuous needs this for completeness.
- self.Q_continuous = numpy.matrix([[(0.5 ** 2.0), 0.0], [0.0, (0.5 ** 2.0)]])
- self.R_continuous = numpy.matrix([[(0.1 ** 2.0), 0.0], [0.0, (0.1 ** 2.0)]])
- self.PlaceObserverPoles(drivetrain_params.observer_poles)
- _, _, self.Q, self.R = controls.kalmd(
- A_continuous=self.A_continuous, B_continuous=self.B_continuous,
- Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
- dt=self.dt)
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.PlaceControllerPoles(drivetrain_params.controller_poles)
+ # Build a kalman filter for the velocity. We don't care what the gains
+ # are, but the hybrid kalman filter that we want to write to disk to get
+ # access to A_continuous and B_continuous needs this for completeness.
+ self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0, (0.5**2.0)]])
+ self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0, (0.1**2.0)]])
+ self.PlaceObserverPoles(drivetrain_params.observer_poles)
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
- self.G_high = self._drivetrain.G_high
- self.G_low = self._drivetrain.G_low
- self.resistance = self._drivetrain.resistance
- self.r = self._drivetrain.r
- self.Kv = self._drivetrain.Kv
- self.Kt = self._drivetrain.Kt
+ self.KalmanGain, self.P_steady_state = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.U_max = self._drivetrain.U_max
- self.U_min = self._drivetrain.U_min
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.resistance = self._drivetrain.resistance
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
- @property
- def robot_radius_l(self):
- return self._drivetrain.robot_radius_l
- @property
- def robot_radius_r(self):
- return self._drivetrain.robot_radius_r
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+ @property
+ def robot_radius_l(self):
+ return self._drivetrain.robot_radius_l
+
+ @property
+ def robot_radius_r(self):
+ return self._drivetrain.robot_radius_r
+
class VelocityDrivetrain(object):
- HIGH = 'high'
- LOW = 'low'
- SHIFTING_UP = 'up'
- SHIFTING_DOWN = 'down'
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
- def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
- self.drivetrain_low_low = VelocityDrivetrainModel(
- left_low=True, right_low=True, name=name + 'LowLow',
- drivetrain_params=drivetrain_params)
- self.drivetrain_low_high = VelocityDrivetrainModel(
- left_low=True, right_low=False, name=name + 'LowHigh',
- drivetrain_params=drivetrain_params)
- self.drivetrain_high_low = VelocityDrivetrainModel(
- left_low=False, right_low=True, name = name + 'HighLow',
- drivetrain_params=drivetrain_params)
- self.drivetrain_high_high = VelocityDrivetrainModel(
- left_low=False, right_low=False, name = name + 'HighHigh',
- drivetrain_params=drivetrain_params)
+ def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True,
+ right_low=True,
+ name=name + 'LowLow',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_low_high = VelocityDrivetrainModel(
+ left_low=True,
+ right_low=False,
+ name=name + 'LowHigh',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_high_low = VelocityDrivetrainModel(
+ left_low=False,
+ right_low=True,
+ name=name + 'HighLow',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_high_high = VelocityDrivetrainModel(
+ left_low=False,
+ right_low=False,
+ name=name + 'HighHigh',
+ drivetrain_params=drivetrain_params)
- # X is [lvel, rvel]
- self.X = numpy.matrix(
- [[0.0],
- [0.0]])
+ # X is [lvel, rvel]
+ self.X = numpy.matrix([[0.0], [0.0]])
- self.U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
+ numpy.matrix([[12], [12], [12], [12]]))
- self.U_max = numpy.matrix(
- [[12.0],
- [12.0]])
- self.U_min = numpy.matrix(
- [[-12.0000000000],
- [-12.0000000000]])
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0000000000], [-12.0000000000]])
- self.dt = 0.00505
+ self.dt = 0.00505
- self.R = numpy.matrix(
- [[0.0],
- [0.0]])
+ self.R = numpy.matrix([[0.0], [0.0]])
- self.U_ideal = numpy.matrix(
- [[0.0],
- [0.0]])
+ self.U_ideal = numpy.matrix([[0.0], [0.0]])
- # ttrust is the comprimise between having full throttle negative inertia,
- # and having no throttle negative inertia. A value of 0 is full throttle
- # inertia. A value of 1 is no throttle negative inertia.
- self.ttrust = 1.0
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
- self.left_gear = VelocityDrivetrain.LOW
- self.right_gear = VelocityDrivetrain.LOW
- self.left_shifter_position = 0.0
- self.right_shifter_position = 0.0
- self.left_cim = CIM()
- self.right_cim = CIM()
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = CIM()
+ self.right_cim = CIM()
- def IsInGear(self, gear):
- return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
- def MotorRPM(self, shifter_position, velocity):
- if shifter_position > 0.5:
- return (velocity / self.CurrentDrivetrain().G_high /
- self.CurrentDrivetrain().r)
- else:
- return (velocity / self.CurrentDrivetrain().G_low /
- self.CurrentDrivetrain().r)
-
- def CurrentDrivetrain(self):
- if self.left_shifter_position > 0.5:
- if self.right_shifter_position > 0.5:
- return self.drivetrain_high_high
- else:
- return self.drivetrain_high_low
- else:
- if self.right_shifter_position > 0.5:
- return self.drivetrain_low_high
- else:
- return self.drivetrain_low_low
-
- def SimShifter(self, gear, shifter_position):
- if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
- shifter_position = min(shifter_position + 0.5, 1.0)
- else:
- shifter_position = max(shifter_position - 0.5, 0.0)
-
- if shifter_position == 1.0:
- gear = VelocityDrivetrain.HIGH
- elif shifter_position == 0.0:
- gear = VelocityDrivetrain.LOW
-
- return gear, shifter_position
-
- def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
- high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
- self.CurrentDrivetrain().r)
- low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
- self.CurrentDrivetrain().r)
- #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
- high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
- low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
- high_power = high_torque * high_omega
- low_power = low_torque * low_omega
- #if should_print:
- # print gear_name, "High omega", high_omega, "Low omega", low_omega
- # print gear_name, "High torque", high_torque, "Low torque", low_torque
- # print gear_name, "High power", high_power, "Low power", low_power
-
- # Shift algorithm improvements.
- # TODO(aschuh):
- # It takes time to shift. Shifting down for 1 cycle doesn't make sense
- # because you will end up slower than without shifting. Figure out how
- # to include that info.
- # If the driver is still in high gear, but isn't asking for the extra power
- # from low gear, don't shift until he asks for it.
- goal_gear_is_high = high_power > low_power
- #goal_gear_is_high = True
-
- if not self.IsInGear(current_gear):
- glog.debug('%s Not in gear.', gear_name)
- return current_gear
- else:
- is_high = current_gear is VelocityDrivetrain.HIGH
- if is_high != goal_gear_is_high:
- if goal_gear_is_high:
- glog.debug('%s Shifting up.', gear_name)
- return VelocityDrivetrain.SHIFTING_UP
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
else:
- glog.debug('%s Shifting down.', gear_name)
- return VelocityDrivetrain.SHIFTING_DOWN
- else:
- return current_gear
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
- def FilterVelocity(self, throttle):
- # 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 equal.
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
- # The throttle filter should filter such that the motor in the highest gear
- # should be controlling the time constant.
- # Do this by finding the index of FF that has the lowest value, and computing
- # the sums using that index.
- FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
- min_FF_sum_index = numpy.argmin(FF_sum)
- min_FF_sum = FF_sum[min_FF_sum_index, 0]
- min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
- # Compute the FF sum for high gear.
- high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
- # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
- # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
- # - self.K[0, :].sum() * x_avg
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
- # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
- # (self.K[0, :].sum() + self.FF[0, :].sum())
+ return gear, shifter_position
- # U = (K + FF) * R - K * X
- # (K + FF) ^-1 * (U + K * X) = R
+ def ComputeGear(self,
+ wheel_velocity,
+ should_print=False,
+ current_gear=False,
+ gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = (
+ (12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ low_torque = (
+ (12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
- # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
- # have the same velocity goal as high gear, and so that the robot will hold
- # the same speed for the same throttle for all gears.
- adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
- return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
- / (self.ttrust * min_K_sum + min_FF_sum))
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
- def Update(self, throttle, steering):
- # Shift into the gear which sends the most power to the floor.
- # This is the same as sending the most torque down to the floor at the
- # wheel.
+ if not self.IsInGear(current_gear):
+ glog.debug('%s Not in gear.', gear_name)
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ glog.debug('%s Shifting up.', gear_name)
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
- self.left_gear = self.right_gear = True
- if True:
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ def FilterVelocity(self, throttle):
+ # 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 equal.
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
- # Filter the throttle to provide a nicer response.
- fvel = self.FilterVelocity(throttle)
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
- # Constant radius means that angualar_velocity / linear_velocity = constant.
- # Compute the left and right velocities.
- steering_velocity = numpy.abs(fvel) * steering
- left_velocity = fvel - steering_velocity
- right_velocity = fvel + steering_velocity
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
- # Write this constraint in the form of K * R = w
- # angular velocity / linear velocity = constant
- # (left - right) / (left + right) = constant
- # left - right = constant * left + constant * right
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
- # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
- # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
- # constant
- # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
- # (-steering * sign(fvel)) = constant
- # (-steering * sign(fvel)) * (left + right) = left - right
- # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(
+ throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum *
+ (self.X[0, 0] + self.X[1, 0]) / 2.0) /
+ (self.ttrust * min_K_sum + min_FF_sum))
- equality_k = numpy.matrix(
- [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
- equality_w = 0.0
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
- self.R[0, 0] = left_velocity
- self.R[1, 0] = right_velocity
+ self.left_gear = self.right_gear = True
+ if True:
+ self.left_gear = self.ComputeGear(
+ self.X[0, 0],
+ should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(
+ self.X[1, 0],
+ should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(
+ self.left_shifter_position, self.X[0, 0])
- # Construct a constraint on R by manipulating the constraint on U
- # Start out with H * U <= k
- # U = FF * R + K * (R - X)
- # H * (FF * R + K * R - K * X) <= k
- # H * (FF + K) * R <= k + H * K * X
- R_poly = polytope.HPolytope(
- self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
- self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(
+ self.right_shifter_position, self.X[0, 0])
- # Limit R back inside the box.
- self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
- FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
- self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
- else:
- glog.debug('Not all in gear')
- if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
- # TODO(austin): Use battery volts here.
- R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- self.U_ideal[0, 0] = numpy.clip(
- self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
- self.left_cim.U_min, self.left_cim.U_max)
- self.left_cim.Update(self.U_ideal[0, 0])
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
- R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
- self.U_ideal[1, 0] = numpy.clip(
- self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
- self.right_cim.U_min, self.right_cim.U_max)
- self.right_cim.Update(self.U_ideal[1, 0])
- else:
- assert False
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
- self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
- # TODO(austin): Model the robot as not accelerating when you shift...
- # This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
- self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+ equality_k = numpy.matrix([[
+ 1 + steering * numpy.sign(fvel),
+ -(1 - steering * numpy.sign(fvel))
+ ]])
+ equality_w = 0.0
- self.left_gear, self.left_shifter_position = self.SimShifter(
- self.left_gear, self.left_shifter_position)
- self.right_gear, self.right_shifter_position = self.SimShifter(
- self.right_gear, self.right_shifter_position)
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
- glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
- glog.debug('Left shifter %s %d Right shifter %s %d',
- self.left_gear, self.left_shifter_position,
- self.right_gear, self.right_shifter_position)
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H *
+ (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k +
+ self.U_poly.H * self.CurrentDrivetrain().K * self.X)
-def WritePolyDrivetrain(drivetrain_files, motor_files, hybrid_files,
- year_namespace, drivetrain_params,
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (
+ self.boxed_R - self.X) + FF_volts
+ else:
+ glog.debug('Not all in gear')
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(
+ self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) +
+ R_left / self.left_cim.Kv, self.left_cim.U_min,
+ self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position,
+ self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) +
+ R_right / self.right_cim.Kv, self.right_cim.U_min,
+ self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain(
+ ).A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d', self.left_gear,
+ self.left_shifter_position, self.right_gear,
+ self.right_shifter_position)
+
+
+def WritePolyDrivetrain(drivetrain_files,
+ motor_files,
+ hybrid_files,
+ year_namespace,
+ drivetrain_params,
scalar_type='double'):
- vdrivetrain = VelocityDrivetrain(drivetrain_params)
- hybrid_vdrivetrain = VelocityDrivetrain(drivetrain_params,
- name="HybridVelocityDrivetrain")
- 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,
- scalar_type=scalar_type)
+ vdrivetrain = VelocityDrivetrain(drivetrain_params)
+ hybrid_vdrivetrain = VelocityDrivetrain(
+ drivetrain_params, name="HybridVelocityDrivetrain")
+ 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,
+ scalar_type=scalar_type)
- dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
+ dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- hybrid_loop_writer = control_loop.ControlLoopWriter(
- "HybridVelocityDrivetrain", [hybrid_vdrivetrain.drivetrain_low_low,
- hybrid_vdrivetrain.drivetrain_low_high,
- hybrid_vdrivetrain.drivetrain_high_low,
- hybrid_vdrivetrain.drivetrain_high_high],
- namespaces=namespaces,
- scalar_type=scalar_type,
- plant_type='StateFeedbackHybridPlant',
- observer_type='HybridKalman')
+ hybrid_loop_writer = control_loop.ControlLoopWriter(
+ "HybridVelocityDrivetrain", [
+ hybrid_vdrivetrain.drivetrain_low_low,
+ hybrid_vdrivetrain.drivetrain_low_high,
+ hybrid_vdrivetrain.drivetrain_high_low,
+ hybrid_vdrivetrain.drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
- hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
+ hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], scalar_type=scalar_type)
+ cim_writer = control_loop.ControlLoopWriter(
+ "CIM", [CIM()], scalar_type=scalar_type)
- cim_writer.Write(motor_files[0], motor_files[1])
+ cim_writer.Write(motor_files[0], motor_files[1])
+
def PlotPolyDrivetrainMotions(drivetrain_params):
- vdrivetrain = VelocityDrivetrain(drivetrain_params)
- vl_plot = []
- vr_plot = []
- ul_plot = []
- ur_plot = []
- radius_plot = []
- t_plot = []
- left_gear_plot = []
- right_gear_plot = []
- vdrivetrain.left_shifter_position = 0.0
- vdrivetrain.right_shifter_position = 0.0
- vdrivetrain.left_gear = VelocityDrivetrain.LOW
- vdrivetrain.right_gear = VelocityDrivetrain.LOW
+ vdrivetrain = VelocityDrivetrain(drivetrain_params)
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
- glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
- if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
- glog.debug('Left is high')
- else:
- glog.debug('Left is low')
- if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
- glog.debug('Right is high')
- else:
- glog.debug('Right is low')
-
- for t in numpy.arange(0, 1.7, vdrivetrain.dt):
- if t < 0.5:
- vdrivetrain.Update(throttle=0.00, steering=1.0)
- elif t < 1.2:
- vdrivetrain.Update(throttle=0.5, steering=1.0)
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Left is high')
else:
- vdrivetrain.Update(throttle=0.00, steering=1.0)
- t_plot.append(t)
- vl_plot.append(vdrivetrain.X[0, 0])
- vr_plot.append(vdrivetrain.X[1, 0])
- ul_plot.append(vdrivetrain.U[0, 0])
- ur_plot.append(vdrivetrain.U[1, 0])
- left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
- right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
-
- fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
- turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
- if abs(fwd_velocity) < 0.0000001:
- radius_plot.append(turn_velocity)
+ glog.debug('Left is low')
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Right is high')
else:
- radius_plot.append(turn_velocity / fwd_velocity)
+ glog.debug('Right is low')
- # TODO(austin):
- # Shifting compensation.
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
+ else:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append(
+ (vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append(
+ (vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
- # Tighten the turn.
- # Closed loop drive.
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
- pylab.plot(t_plot, vl_plot, label='left velocity')
- pylab.plot(t_plot, vr_plot, label='right velocity')
- pylab.plot(t_plot, ul_plot, label='left voltage')
- pylab.plot(t_plot, ur_plot, label='right voltage')
- pylab.plot(t_plot, radius_plot, label='radius')
- pylab.plot(t_plot, left_gear_plot, label='left gear high')
- pylab.plot(t_plot, right_gear_plot, label='right gear high')
- pylab.legend()
- pylab.show()
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
diff --git a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
index caa3082..839677e 100644
--- a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
+++ b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
@@ -39,11 +39,9 @@
# Write the generated constants out to a file.
if len(argv) != 5:
- glog.fatal(
- 'Expected .h file name and .cc file name for the \
+ glog.fatal('Expected .h file name and .cc file name for the \
static_zeroing_single_dof_profiled_subsystem_test and integral \
- static_zeroing_single_dof_profiled_subsystem_test.'
- )
+ static_zeroing_single_dof_profiled_subsystem_test.')
else:
namespaces = ['frc971', 'control_loops']
linear_system.WriteLinearSystem(kIntake, argv[1:3], argv[3:5],
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index fc3ff34..0da8bd6 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -3,10 +3,10 @@
#include <assert.h>
+#include <chrono>
#include <memory>
#include <utility>
#include <vector>
-#include <chrono>
#include "Eigen/Dense"
#include "unsupported/Eigen/MatrixFunctions"
@@ -72,8 +72,7 @@
Reset();
}
- StateFeedbackPlant(StateFeedbackPlant &&other)
- : index_(other.index_) {
+ StateFeedbackPlant(StateFeedbackPlant &&other) : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
@@ -279,8 +278,8 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &
- KalmanGain,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs>
+ &KalmanGain,
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R)
: KalmanGain(KalmanGain), Q(Q), R(R) {}
@@ -294,7 +293,8 @@
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
- number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
+ *observers)
: coefficients_(::std::move(*observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
@@ -302,7 +302,8 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain()
+ const {
return coefficients().KalmanGain;
}
Scalar KalmanGain(int i, int j) const { return KalmanGain()(i, j); }
@@ -328,8 +329,7 @@
number_of_outputs, Scalar> &plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
- mutable_X_hat() +=
- KalmanGain() * (Y - plant.C() * X_hat() - plant.D() * U);
+ mutable_X_hat() += KalmanGain() * (Y - plant.C() * X_hat() - plant.D() * U);
}
// Sets the current controller to be index, clamped to be within range.
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 7b7b370..635db59 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -110,7 +110,8 @@
};
template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Reset() {
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
+ ProfiledJointStatus>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
@@ -118,10 +119,11 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Iterate(
- const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position, double *output,
- ProfiledJointStatus *status) {
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
+ ProfiledJointStatus>::
+ Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ ProfiledJointStatus *status) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 9a9c092..66b1ab9 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -53,13 +53,7 @@
template <>
const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
- kZeroingSampleSize,
- kEncoderIndexDifference,
- 0.0,
- 0.2,
- 0.0005,
- 20,
- 1.9};
+ kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20, 1.9};
template <typename ZeroingEstimator>
const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
@@ -94,8 +88,7 @@
TestIntakeSystemSimulation()
: subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
- subsystem_sensor_sim_(
- kEncoderIndexDifference) {
+ subsystem_sensor_sim_(kEncoderIndexDifference) {
// Start the subsystem out in the middle by default.
InitializeSubsystemPosition((kRange.lower + kRange.upper) / 2.0);
}
diff --git a/motors/core/time.h b/motors/core/time.h
index 247de2c..d03cb10 100644
--- a/motors/core/time.h
+++ b/motors/core/time.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+// This whole file is deprecated. Use //aos/time instead.
+
#ifdef __cplusplus
extern "C"
{
diff --git a/motors/peripheral/BUILD b/motors/peripheral/BUILD
index ef2e833..2c71973 100644
--- a/motors/peripheral/BUILD
+++ b/motors/peripheral/BUILD
@@ -71,6 +71,7 @@
visibility = ["//visibility:public"],
deps = [
":uart_buffer",
+ "//aos/containers:sized_array",
"//motors:util",
"//motors/core",
"//third_party/GSL",
diff --git a/motors/peripheral/uart.cc b/motors/peripheral/uart.cc
index 3f7ddaf..9cd4014 100644
--- a/motors/peripheral/uart.cc
+++ b/motors/peripheral/uart.cc
@@ -39,18 +39,24 @@
// M_UART_RIE /* Enable RX interrupt or DMA */
// Also set in C5: M_UART_TDMAS /* Do DMA for TX */ |
// M_UART_RDMAS /* Do DMA for RX */
- c2_value_ = M_UART_TE;
+ c2_value_ = 0;
module_->C2 = c2_value_;
module_->PFIFO =
M_UART_TXFE /* Enable TX FIFO */ | M_UART_RXFE /* Enable RX FIFO */;
module_->CFIFO =
M_UART_TXFLUSH /* Flush TX FIFO */ | M_UART_RXFLUSH /* Flush RX FIFO */;
+ c2_value_ = M_UART_TE | M_UART_RE;
+ module_->C2 = c2_value_;
// TODO(Brian): Adjust for DMA?
module_->TWFIFO = tx_fifo_size_ - 1;
- module_->RWFIFO = rx_fifo_size_ - 1;
+ module_->RWFIFO = 1;
}
void Uart::DoWrite(gsl::span<const char> data) {
+ // In theory, we could be more efficient about this by writing the number of
+ // bytes we know there's space for and only calling SpaceAvailable() (or
+ // otherwise reading S1) before the final one. In practice, the FIFOs are so
+ // short on this part it probably won't help anything.
for (int i = 0; i < data.size(); ++i) {
while (!SpaceAvailable()) {
}
@@ -58,38 +64,91 @@
}
}
-Uart::~Uart() { DisableTransmitInterrupt(); }
+aos::SizedArray<char, 4> Uart::DoRead() {
+ // In theory, we could be more efficient about this by reading the number of
+ // bytes we know to be accessible and only calling DataAvailable() (or
+ // otherwise reading S1) before the final one. In practice, the FIFOs are so
+ // short on this part it probably won't help anything.
+ aos::SizedArray<char, 4> result;
+ while (DataAvailable() && !result.full()) {
+ result.push_back(ReadCharacter());
+ }
+ return result;
+}
+
+Uart::~Uart() {
+ DoDisableTransmitInterrupt();
+ DoDisableReceiveInterrupt();
+}
+
+InterruptBufferedUart::~InterruptBufferedUart() {
+ uart_.DisableReceiveInterrupt(DisableInterrupts());
+}
void InterruptBufferedUart::Initialize(int baud_rate) {
uart_.Initialize(baud_rate);
+ {
+ DisableInterrupts disable_interrupts;
+ uart_.EnableReceiveInterrupt(disable_interrupts);
+ }
}
void InterruptBufferedUart::Write(gsl::span<const char> data) {
DisableInterrupts disable_interrupts;
- uart_.EnableTransmitInterrupt();
- static_assert(buffer_.size() >= 8,
- "Need enough space to not turn the interrupt off each time");
+ uart_.EnableTransmitInterrupt(disable_interrupts);
while (!data.empty()) {
- const int bytes_written = buffer_.PushSpan(data);
+ const int bytes_written = transmit_buffer_.PushSpan(data);
data = data.subspan(bytes_written);
WriteCharacters(data.empty(), disable_interrupts);
ReenableInterrupts{&disable_interrupts};
}
}
-void InterruptBufferedUart::WriteCharacters(bool disable_empty,
- const DisableInterrupts &) {
+gsl::span<char> InterruptBufferedUart::Read(gsl::span<char> buffer) {
+ size_t bytes_read = 0;
+ {
+ DisableInterrupts disable_interrupts;
+ const gsl::span<const char> read_data =
+ receive_buffer_.PopSpan(buffer.size());
+ std::copy(read_data.begin(), read_data.end(), buffer.begin());
+ bytes_read += read_data.size();
+ }
+ {
+ DisableInterrupts disable_interrupts;
+ const gsl::span<const char> read_data =
+ receive_buffer_.PopSpan(buffer.size() - bytes_read);
+ std::copy(read_data.begin(), read_data.end(),
+ buffer.subspan(bytes_read).begin());
+ bytes_read += read_data.size();
+ }
+ return buffer.subspan(0, bytes_read);
+}
+
+void InterruptBufferedUart::WriteCharacters(
+ bool disable_empty, const DisableInterrupts &disable_interrupts) {
while (true) {
- if (buffer_.empty()) {
+ if (transmit_buffer_.empty()) {
if (disable_empty) {
- uart_.DisableTransmitInterrupt();
+ uart_.DisableTransmitInterrupt(disable_interrupts);
}
return;
}
if (!uart_.SpaceAvailable()) {
return;
}
- uart_.WriteCharacter(buffer_.PopSingle());
+ uart_.WriteCharacter(transmit_buffer_.PopSingle());
+ }
+}
+
+void InterruptBufferedUart::ReadCharacters(const DisableInterrupts &) {
+ while (true) {
+ if (receive_buffer_.full()) {
+ return;
+ }
+ if (!uart_.DataAvailable()) {
+ return;
+ }
+ receive_buffer_.PushSingle(uart_.ReadCharacter());
}
}
diff --git a/motors/peripheral/uart.h b/motors/peripheral/uart.h
index ca40ab7..ffb1529 100644
--- a/motors/peripheral/uart.h
+++ b/motors/peripheral/uart.h
@@ -1,6 +1,7 @@
#ifndef MOTORS_PERIPHERAL_UART_H_
#define MOTORS_PERIPHERAL_UART_H_
+#include "aos/containers/sized_array.h"
#include "motors/core/kinetis.h"
#include "motors/peripheral/uart_buffer.h"
#include "motors/util.h"
@@ -25,22 +26,59 @@
DoWrite(data);
}
+ // Returns all the data which is currently available.
+ aos::SizedArray<char, 4> Read(const DisableInterrupts &) {
+ return DoRead();
+ }
+
bool SpaceAvailable() const { return module_->S1 & M_UART_TDRE; }
// Only call this if SpaceAvailable() has just returned true.
void WriteCharacter(char c) { module_->D = c; }
- void EnableTransmitInterrupt() {
+ bool DataAvailable() const { return module_->S1 & M_UART_RDRF; }
+ // Only call this if DataAvailable() has just returned true.
+ char ReadCharacter() { return module_->D; }
+
+ // TODO(Brian): These APIs for enabling/disabling interrupts aren't quite
+ // right. Redo them some time. Some issues:
+ // * They get called during initialization/destruction time, which means
+ // interrupts don't really need to be disabled because everything is
+ // singlethreaded.
+ // * Often, several C2 modifications are made in a single
+ // interrupts-disabled section. These could be batched to reduce
+ // peripheral writes. Sometimes, no modifications are made at all, in
+ // which case there doesn't even need to be a single write.
+
+ void EnableTransmitInterrupt(const DisableInterrupts &) {
c2_value_ |= M_UART_TIE;
module_->C2 = c2_value_;
}
- void DisableTransmitInterrupt() {
- c2_value_ &= ~M_UART_TIE;
+ void DisableTransmitInterrupt(const DisableInterrupts &) {
+ DoDisableTransmitInterrupt();
+ }
+
+ void EnableReceiveInterrupt(const DisableInterrupts &) {
+ c2_value_ |= M_UART_RIE;
module_->C2 = c2_value_;
}
+ void DisableReceiveInterrupt(const DisableInterrupts &) {
+ DoDisableReceiveInterrupt();
+ }
+
private:
+ void DoDisableTransmitInterrupt() {
+ c2_value_ &= ~M_UART_TIE;
+ module_->C2 = c2_value_;
+ }
+ void DoDisableReceiveInterrupt() {
+ c2_value_ &= ~M_UART_RIE;
+ module_->C2 = c2_value_;
+ }
+
void DoWrite(gsl::span<const char> data);
+ aos::SizedArray<char, 4> DoRead();
KINETISK_UART_t *const module_;
const int module_clock_frequency_;
@@ -51,27 +89,37 @@
};
// Interrupt-based buffered interface to a UART.
+// TODO(Brian): Move DisableInterrupts calls up to the caller of this.
class InterruptBufferedUart {
public:
InterruptBufferedUart(KINETISK_UART_t *module, int module_clock_frequency)
: uart_(module, module_clock_frequency) {}
+ ~InterruptBufferedUart();
void Initialize(int baud_rate);
+ // Queues up the given data for immediate writing. Blocks only if the queue
+ // fills up before all of data is enqueued.
void Write(gsl::span<const char> data);
+ // Reads currently available data.
+ // Returns all the data which is currently available (possibly none);
+ // buffer is where to store the result. The return value will be a subspan of
+ // this.
+ gsl::span<char> Read(gsl::span<char> buffer);
+
// Should be called as the body of the interrupt handler.
void HandleInterrupt(const DisableInterrupts &disable_interrupts) {
WriteCharacters(true, disable_interrupts);
+ ReadCharacters(disable_interrupts);
}
private:
void WriteCharacters(bool disable_empty, const DisableInterrupts &);
+ void ReadCharacters(const DisableInterrupts &);
Uart uart_;
- UartBuffer<1024> buffer_;
-
- bool interrupt_enabled_ = false;
+ UartBuffer<1024> transmit_buffer_, receive_buffer_;
};
} // namespace teensy
diff --git a/motors/peripheral/uart_buffer.h b/motors/peripheral/uart_buffer.h
index 63dd70d..879ab5c 100644
--- a/motors/peripheral/uart_buffer.h
+++ b/motors/peripheral/uart_buffer.h
@@ -15,15 +15,25 @@
// Returns the number of characters added.
__attribute__((warn_unused_result)) int PushSpan(gsl::span<const char> data);
+ // max is the maximum size the returned spans should be.
+ // The data in the result is only valid until another method is called.
+ // Note that this may not return all available data when doing so would
+ // require wrapping around, but it will always return a non-empty span if any
+ // data is available.
+ gsl::span<const char> PopSpan(int max);
+
bool empty() const { return size_ == 0; }
+ bool full() const { return size_ == kSize; }
// This may only be called when !empty().
char PopSingle();
+ // This may only be called when !full().
+ void PushSingle(char c);
static constexpr int size() { return kSize; }
private:
- // The index at which we will push the next character.
+ // The index at which we will pop the next character.
int start_ = 0;
// How many characters we currently have.
int size_ = 0;
@@ -31,7 +41,7 @@
::std::array<char, kSize> data_;
};
-template<int kSize>
+template <int kSize>
int UartBuffer<kSize>::PushSpan(gsl::span<const char> data) {
const int end_location = (start_ + size_) % kSize;
const int remaining_end = ::std::min(kSize - size_, kSize - end_location);
@@ -52,7 +62,16 @@
return on_end + on_start;
}
-template<int kSize>
+template <int kSize>
+gsl::span<const char> UartBuffer<kSize>::PopSpan(int max) {
+ const size_t result_size = std::min(max, std::min(kSize - start_, size_));
+ const auto result = gsl::span<const char>(data_).subspan(start_, result_size);
+ start_ = (start_ + result_size) % kSize;
+ size_ -= result_size;
+ return result;
+}
+
+template <int kSize>
char UartBuffer<kSize>::PopSingle() {
const char r = data_[start_];
--size_;
@@ -60,6 +79,13 @@
return r;
}
+template <int kSize>
+void UartBuffer<kSize>::PushSingle(char c) {
+ const int end_location = (start_ + size_) % kSize;
+ data_[end_location] = c;
+ ++size_;
+}
+
} // namespace teensy
} // namespace frc971
diff --git a/motors/peripheral/uart_buffer_test.cc b/motors/peripheral/uart_buffer_test.cc
index 5ab7c98..c464f8a 100644
--- a/motors/peripheral/uart_buffer_test.cc
+++ b/motors/peripheral/uart_buffer_test.cc
@@ -181,6 +181,106 @@
ASSERT_TRUE(buffer.empty());
}
+// Tests that using PopSpan with single characters works correctly.
+TEST(UartBufferTest, PopSpanSingle) {
+ UartBuffer<3> buffer;
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(1);
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(2);
+ ASSERT_FALSE(buffer.full());
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(1u, result[0]);
+ }
+
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(3);
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(4);
+ ASSERT_TRUE(buffer.full());
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(2u, result[0]);
+ }
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(3u, result[0]);
+ }
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(4u, result[0]);
+ }
+}
+
+// Tests that using PopSpan with multiple characters works correctly.
+TEST(UartBufferTest, PopSpanMultiple) {
+ UartBuffer<1024> buffer;
+ for (int i = 0; i < 10; ++i) {
+ buffer.PushSingle(i);
+ }
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+ {
+ const auto result = buffer.PopSpan(5);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i), result[i]);
+ }
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 5), result[i]);
+ }
+ }
+ ASSERT_TRUE(buffer.PopSpan(5).empty());
+ ASSERT_TRUE(buffer.PopSpan(3000).empty());
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+}
+
+// Tests that using PopSpan with multiple characters works correctly when
+// wrapping.
+TEST(UartBufferTest, PopSpanWrapMultiple) {
+ UartBuffer<10> buffer;
+ for (int i = 0; i < 10; ++i) {
+ buffer.PushSingle(i);
+ }
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+ {
+ const auto result = buffer.PopSpan(5);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i), result[i]);
+ }
+ }
+ for (int i = 0; i < 5; ++i) {
+ buffer.PushSingle(20 + i);
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 5), result[i]);
+ }
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 20), result[i]);
+ }
+ }
+}
+
} // namespace testing
} // namespace teensy
} // namespace frc971
diff --git a/third_party/google-glog/BUILD b/third_party/google-glog/BUILD
index 37fb27b..0c41228 100644
--- a/third_party/google-glog/BUILD
+++ b/third_party/google-glog/BUILD
@@ -1,5 +1,5 @@
-licenses(['notice'])
+licenses(["notice"])
-load(':bazel/glog.bzl', 'glog_library')
+load(":bazel/glog.bzl", "glog_library")
glog_library()
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 91fbb2d..93e5d09 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -9,4 +9,4 @@
bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
-bazel build -c opt --curses=no --color=no //motors/... --cpu=cortex-m4f
+bazel build -c opt --curses=no --color=no //motors/... //y2019/jevois/... --cpu=cortex-m4f
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 3ae9fe9..cade03d 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -13,493 +13,500 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Claw(control_loop.ControlLoop):
- def __init__(self, name="RawClaw"):
- super(Claw, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 5500.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the claw in kg m^2
- self.J_top = 2.8
- self.J_bottom = 3.0
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (13.5 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
- # Control loop time step
- self.dt = 0.005
+ def __init__(self, name="RawClaw"):
+ super(Claw, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 5500.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the claw in kg m^2
+ self.J_top = 2.8
+ self.J_bottom = 3.0
- # State is [bottom position, bottom velocity, top - bottom position,
- # top - bottom velocity]
- # Input is [bottom power, top power - bottom power * J_top / J_bottom]
- # Motor time constants. difference_bottom refers to the constant for how the
- # bottom velocity affects the difference of the top and bottom velocities.
- self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
- self.bottom_bottom = self.common_motor_constant / self.J_bottom
- self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
- - 1 / self.J_top)
- self.difference_difference = self.common_motor_constant / self.J_top
- # State feedback matrices
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
+ # Control loop time step
+ self.dt = 0.005
- self.A_continuous = numpy.matrix(
- [[0, 0, 1, 0],
- [0, 0, 0, 1],
- [0, 0, self.bottom_bottom, 0],
- [0, 0, self.difference_bottom, self.difference_difference]])
+ # State is [bottom position, bottom velocity, top - bottom position,
+ # top - bottom velocity]
+ # Input is [bottom power, top power - bottom power * J_top / J_bottom]
+ # Motor time constants. difference_bottom refers to the constant for how the
+ # bottom velocity affects the difference of the top and bottom velocities.
+ self.common_motor_constant = -self.Kt / self.Kv / (
+ self.G * self.G * self.R)
+ self.bottom_bottom = self.common_motor_constant / self.J_bottom
+ self.difference_bottom = -self.common_motor_constant * (
+ 1 / self.J_bottom - 1 / self.J_top)
+ self.difference_difference = self.common_motor_constant / self.J_top
+ # State feedback matrices
- self.A_bottom_cont = numpy.matrix(
- [[0, 1],
- [0, self.bottom_bottom]])
+ self.A_continuous = numpy.matrix(
+ [[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, self.bottom_bottom, 0],
+ [0, 0, self.difference_bottom, self.difference_difference]])
- self.A_diff_cont = numpy.matrix(
- [[0, 1],
- [0, self.difference_difference]])
+ self.A_bottom_cont = numpy.matrix([[0, 1], [0, self.bottom_bottom]])
- self.motor_feedforward = self.Kt / (self.G * self.R)
- self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
- self.motor_feedforward_difference = self.motor_feedforward / self.J_top
- self.motor_feedforward_difference_bottom = (
- self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [0, 0],
- [self.motor_feedforward_bottom, 0],
- [-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
+ self.A_diff_cont = numpy.matrix([[0, 1],
+ [0, self.difference_difference]])
- glog.debug('Cont X_ss %f', self.motor_feedforward / -self.common_motor_constant)
+ self.motor_feedforward = self.Kt / (self.G * self.R)
+ self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
+ self.motor_feedforward_difference = self.motor_feedforward / self.J_top
+ self.motor_feedforward_difference_bottom = (
+ self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
+ self.B_continuous = numpy.matrix([[0, 0], [0, 0],
+ [self.motor_feedforward_bottom, 0],
+ [
+ -self.motor_feedforward_bottom,
+ self.motor_feedforward_difference
+ ]])
- self.B_bottom_cont = numpy.matrix(
- [[0],
- [self.motor_feedforward_bottom]])
+ glog.debug('Cont X_ss %f',
+ self.motor_feedforward / -self.common_motor_constant)
- self.B_diff_cont = numpy.matrix(
- [[0],
- [self.motor_feedforward_difference]])
+ self.B_bottom_cont = numpy.matrix([[0],
+ [self.motor_feedforward_bottom]])
- self.C = numpy.matrix([[1, 0, 0, 0],
- [1, 1, 0, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
+ self.B_diff_cont = numpy.matrix([[0],
+ [self.motor_feedforward_difference]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0, 0, 0], [1, 1, 0, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- self.A_bottom, self.B_bottom = controls.c2d(
- self.A_bottom_cont, self.B_bottom_cont, self.dt)
- self.A_diff, self.B_diff = controls.c2d(
- self.A_diff_cont, self.B_diff_cont, self.dt)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom,
- [0.87 + 0.05j, 0.87 - 0.05j])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff,
- [0.85 + 0.05j, 0.85 - 0.05j])
+ self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont,
+ self.B_bottom_cont, self.dt)
+ self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont,
+ self.B_diff_cont, self.dt)
- glog.debug('K_diff %s', str(self.K_diff))
- glog.debug('K_bottom %s', str(self.K_bottom))
+ self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom,
+ [0.87 + 0.05j, 0.87 - 0.05j])
+ self.K_diff = controls.dplace(self.A_diff, self.B_diff,
+ [0.85 + 0.05j, 0.85 - 0.05j])
- glog.debug('A')
- glog.debug(self.A)
- glog.debug('B')
- glog.debug(self.B)
+ glog.debug('K_diff %s', str(self.K_diff))
+ glog.debug('K_bottom %s', str(self.K_bottom))
-
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.10, 0.0],
- [0.0, 0.0, 0.0, 0.1]])
+ glog.debug('A')
+ glog.debug(self.A)
+ glog.debug('B')
+ glog.debug(self.B)
- self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (5.0 ** 2.0))]])
- #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ self.Q = numpy.matrix([[(1.0 / (0.10**2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.06**2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.10, 0.0], [0.0, 0.0, 0.0, 0.1]])
- self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
- [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
+ self.R = numpy.matrix([[(1.0 / (40.0**2.0)), 0.0],
+ [0.0, (1.0 / (5.0**2.0))]])
+ #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- # Compute the feed forwards aceleration term.
- self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
+ self.K = numpy.matrix(
+ [[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
+ [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
- lstsq_A = numpy.identity(2)
- lstsq_A[0, :] = self.B[1, :]
- lstsq_A[1, :] = self.B[3, :]
- glog.debug('System of Equations coefficients:')
- glog.debug(str(lstsq_A))
- glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
+ # Compute the feed forwards aceleration term.
+ self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
- out_x = numpy.linalg.lstsq(
- lstsq_A,
- numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
- self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
+ lstsq_A = numpy.identity(2)
+ lstsq_A[0, :] = self.B[1, :]
+ lstsq_A[1, :] = self.B[3, :]
+ glog.debug('System of Equations coefficients:')
+ glog.debug(str(lstsq_A))
+ glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
- glog.debug('K unaugmented')
- glog.debug(str(self.K))
- glog.debug('B * K unaugmented')
- glog.debug(str(self.B * self.K))
- F = self.A - self.B * self.K
- glog.debug('A - B * K unaugmented')
- glog.debug(str(F))
- glog.debug('eigenvalues')
- glog.debug(str(numpy.linalg.eig(F)[0]))
+ out_x = numpy.linalg.lstsq(
+ lstsq_A, numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (
+ self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
- self.rpl = .09
- self.ipl = 0.030
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ glog.debug('K unaugmented')
+ glog.debug(str(self.K))
+ glog.debug('B * K unaugmented')
+ glog.debug(str(self.B * self.K))
+ F = self.A - self.B * self.K
+ glog.debug('A - B * K unaugmented')
+ glog.debug(str(F))
+ glog.debug('eigenvalues')
+ glog.debug(str(numpy.linalg.eig(F)[0]))
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.rpl = .09
+ self.ipl = 0.030
+ self.PlaceObserverPoles([
+ self.rpl + 1j * self.ipl, self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, self.rpl - 1j * self.ipl
+ ])
- # For the tests that check the limits, these are (upper, lower) for both
- # claws.
- self.hard_pos_limits = None
- self.pos_limits = None
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
- # Compute the steady state velocities for a given applied voltage.
- # The top and bottom of the claw should spin at the same rate if the
- # physics is right.
- X_ss = numpy.matrix([[0], [0], [0.0], [0]])
-
- U = numpy.matrix([[1.0],[1.0]])
- A = self.A
- B = self.B
- #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
- X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
- #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
- #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
- X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
+ # For the tests that check the limits, these are (upper, lower) for both
+ # claws.
+ self.hard_pos_limits = None
+ self.pos_limits = None
- glog.debug('X_ss %s', str(X_ss))
+ # Compute the steady state velocities for a given applied voltage.
+ # The top and bottom of the claw should spin at the same rate if the
+ # physics is right.
+ X_ss = numpy.matrix([[0], [0], [0.0], [0]])
- self.InitializeState()
+ U = numpy.matrix([[1.0], [1.0]])
+ A = self.A
+ B = self.B
+ #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
+ X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
+ #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[3, 0] = 1 / (1 - A[3, 3]) * (
+ X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
+ X_ss[1, 0] = (A[1, 2] * X_ss[2, 0]) + (A[1, 3] * X_ss[3, 0]) + (
+ B[1, 0] * U[0, 0]) + (B[1, 1] * U[1, 0])
+
+ glog.debug('X_ss %s', str(X_ss))
+
+ self.InitializeState()
class ClawDeltaU(Claw):
- def __init__(self, name="Claw"):
- super(ClawDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- C_unaugmented = self.C
- self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 1.0]])
- self.A[0:4, 0:4] = A_unaugmented
- self.A[0:4, 4] = B_unaugmented[0:4, 0]
+ def __init__(self, name="Claw"):
+ super(ClawDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+ C_unaugmented = self.C
- self.B = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [1.0, 0.0]])
- self.B[0:4, 1] = B_unaugmented[0:4, 1]
+ self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
+ self.A[0:4, 0:4] = A_unaugmented
+ self.A[0:4, 4] = B_unaugmented[0:4, 0]
- self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
- axis=1)
- self.D = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0]])
+ self.B = numpy.matrix([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0],
+ [1.0, 0.0]])
+ self.B[0:4, 1] = B_unaugmented[0:4, 1]
- #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
- self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.01, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.08, 0.0],
- [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
+ self.C = numpy.concatenate(
+ (C_unaugmented, numpy.matrix([[0.0], [0.0]])), axis=1)
+ self.D = numpy.matrix([[0.0, 0.0], [0.0, 0.0]])
- self.R = numpy.matrix([[0.000001, 0.0],
- [0.0, 1.0 / (10.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
+ self.Q = numpy.matrix([[(1.0 / (0.04**2.0)), 0.0, 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.01**2)), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.01, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.08, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0**2))]])
- self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
- [50.0, 0.0, 10.0, 0.0, 1.0]])
+ self.R = numpy.matrix([[0.000001, 0.0], [0.0, 1.0 / (10.0**2.0)]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- controllability = controls.ctrb(self.A, self.B)
- glog.debug('Rank of augmented controllability matrix: %d',
- numpy.linalg.matrix_rank(controllability))
+ self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
+ [50.0, 0.0, 10.0, 0.0, 1.0]])
- glog.debug('K')
- glog.debug(str(self.K))
- glog.debug('Placed controller poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- glog.debug(str([numpy.abs(x) for x in
- numpy.linalg.eig(self.A - self.B * self.K)[0]]))
+ controllability = controls.ctrb(self.A, self.B)
+ glog.debug('Rank of augmented controllability matrix: %d',
+ numpy.linalg.matrix_rank(controllability))
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
- self.rpl - 1j * self.ipl, 0.90])
- #print "A is"
- #print self.A
- #print "L is"
- #print self.L
- #print "C is"
- #print self.C
- #print "A - LC is"
- #print self.A - self.L * self.C
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug(
+ str([
+ numpy.abs(x)
+ for x in numpy.linalg.eig(self.A - self.B * self.K)[0]
+ ]))
- #print "Placed observer poles are"
- #print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([
+ self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl, 0.90
+ ])
+ #print "A is"
+ #print self.A
+ #print "L is"
+ #print self.L
+ #print "C is"
+ #print self.C
+ #print "A - LC is"
+ #print self.A - self.L * self.C
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ #print "Placed observer poles are"
+ #print numpy.linalg.eig(self.A - self.L * self.C)[0]
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
def ScaleU(claw, U, K, error):
- """Clips U as necessary.
+ """Clips U as necessary.
Args:
- claw: claw object containing moments of inertia and U limits.
- U: Input matrix to clip as necessary.
- """
+ claw: claw object containing moments of inertia and U limits.
+ U: Input matrix to clip as necessary.
+ """
- bottom_u = U[0, 0]
- top_u = U[1, 0]
- position_error = error[0:2, 0]
- velocity_error = error[2:, 0]
+ bottom_u = U[0, 0]
+ top_u = U[1, 0]
+ position_error = error[0:2, 0]
+ velocity_error = error[2:, 0]
- U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
+ U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
+ numpy.matrix([[12], [12], [12], [12]]))
- if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
- top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+ if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
+ top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
- position_K = K[:, 0:2]
- velocity_K = K[:, 2:]
+ position_K = K[:, 0:2]
+ velocity_K = K[:, 2:]
- # H * U <= k
- # U = UPos + UVel
- # H * (UPos + UVel) <= k
- # H * UPos <= k - H * UVel
- #
- # Now, we can do a coordinate transformation and say the following.
- #
- # UPos = position_K * position_error
- # (H * position_K) * position_error <= k - H * UVel
- #
- # Add in the constraint that 0 <= t <= 1
- # Now, there are 2 ways this can go. Either we have a region, or we don't
- # have a region. If we have a region, then pick the largest t and go for it.
- # If we don't have a region, we need to pick a good comprimise.
+ # H * U <= k
+ # U = UPos + UVel
+ # H * (UPos + UVel) <= k
+ # H * UPos <= k - H * UVel
+ #
+ # Now, we can do a coordinate transformation and say the following.
+ #
+ # UPos = position_K * position_error
+ # (H * position_K) * position_error <= k - H * UVel
+ #
+ # Add in the constraint that 0 <= t <= 1
+ # Now, there are 2 ways this can go. Either we have a region, or we don't
+ # have a region. If we have a region, then pick the largest t and go for it.
+ # If we don't have a region, we need to pick a good comprimise.
- pos_poly = polytope.HPolytope(
- U_poly.H * position_K,
- U_poly.k - U_poly.H * velocity_K * velocity_error)
+ pos_poly = polytope.HPolytope(
+ U_poly.H * position_K,
+ U_poly.k - U_poly.H * velocity_K * velocity_error)
- # The actual angle for the line we call 45.
- angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
- if claw.pos_limits and claw.hard_pos_limits and claw.X[0, 0] + claw.X[1, 0] > claw.pos_limits[1]:
- angle_45 = numpy.matrix([[1, 1]])
+ # The actual angle for the line we call 45.
+ angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
+ if claw.pos_limits and claw.hard_pos_limits and (
+ claw.X[0, 0] + claw.X[1, 0]) > claw.pos_limits[1]:
+ angle_45 = numpy.matrix([[1, 1]])
- P = position_error
- L45 = numpy.multiply(numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]), angle_45)
- if L45[0, 1] == 0:
- L45[0, 1] = 1
- if L45[0, 0] == 0:
- L45[0, 0] = 1
- w45 = numpy.matrix([[0]])
+ P = position_error
+ L45 = numpy.multiply(
+ numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]),
+ angle_45)
+ if L45[0, 1] == 0:
+ L45[0, 1] = 1
+ if L45[0, 0] == 0:
+ L45[0, 0] = 1
+ w45 = numpy.matrix([[0]])
- if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
- LH = numpy.matrix([[0, 1]])
- else:
- LH = numpy.matrix([[1, 0]])
- wh = LH * P
- standard = numpy.concatenate((L45, LH))
- W = numpy.concatenate((w45, wh))
- intersection = numpy.linalg.inv(standard) * W
- adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
- LH, wh, position_error)
- adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
- L45, w45, intersection)
- if pos_poly.IsInside(intersection):
- adjusted_pos_error = adjusted_pos_error_h
- else:
- if is_inside_h:
- if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
- adjusted_pos_error = adjusted_pos_error_h
+ if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
+ LH = numpy.matrix([[0, 1]])
else:
- adjusted_pos_error = adjusted_pos_error_45
- else:
- adjusted_pos_error = adjusted_pos_error_45
- #print adjusted_pos_error
+ LH = numpy.matrix([[1, 0]])
+ wh = LH * P
+ standard = numpy.concatenate((L45, LH))
+ W = numpy.concatenate((w45, wh))
+ intersection = numpy.linalg.inv(standard) * W
+ adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(
+ pos_poly, LH, wh, position_error)
+ adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(
+ pos_poly, L45, w45, intersection)
+ if pos_poly.IsInside(intersection):
+ adjusted_pos_error = adjusted_pos_error_h
+ else:
+ if is_inside_h:
+ if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(
+ adjusted_pos_error_45):
+ adjusted_pos_error = adjusted_pos_error_h
+ else:
+ adjusted_pos_error = adjusted_pos_error_45
+ else:
+ adjusted_pos_error = adjusted_pos_error_45
+ #print adjusted_pos_error
- #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
- return velocity_K * velocity_error + position_K * adjusted_pos_error
+ #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
+ return velocity_K * velocity_error + position_K * adjusted_pos_error
- #U = Kpos * poserror + Kvel * velerror
-
- #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ #U = Kpos * poserror + Kvel * velerror
- #top_u *= scalar
- #bottom_u *= scalar
+ #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
- return numpy.matrix([[bottom_u], [top_u]])
+ #top_u *= scalar
+ #bottom_u *= scalar
-def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=True, iterations=200):
- """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
+ return numpy.matrix([[bottom_u], [top_u]])
+
+
+def run_test(claw,
+ initial_X,
+ goal,
+ max_separation_error=0.01,
+ show_graph=True,
+ iterations=200):
+ """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
False if tests fail.
+
Args:
- claw: claw object to use.
- initial_X: starting state.
- goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for."""
+ claw: claw object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for."""
- claw.X = initial_X
+ claw.X = initial_X
- # Various lists for graphing things.
- t = []
- x_bottom = []
- x_top = []
- u_bottom = []
- u_top = []
- x_separation = []
+ # Various lists for graphing things.
+ t = []
+ x_bottom = []
+ x_top = []
+ u_bottom = []
+ u_top = []
+ x_separation = []
- tests_passed = True
+ tests_passed = True
- # Bounds which separation should not exceed.
- lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
- else goal[1, 0]) - max_separation_error
- upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
- else goal[1, 0]) + max_separation_error
+ # Bounds which separation should not exceed.
+ lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0] else
+ goal[1, 0]) - max_separation_error
+ upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0] else
+ goal[1, 0]) + max_separation_error
- for i in xrange(iterations):
- U = claw.K * (goal - claw.X)
- U = ScaleU(claw, U, claw.K, goal - claw.X)
- claw.Update(U)
+ for i in xrange(iterations):
+ U = claw.K * (goal - claw.X)
+ U = ScaleU(claw, U, claw.K, goal - claw.X)
+ claw.Update(U)
- if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
- tests_passed = False
- glog.info('Claw separation was %f', claw.X[1, 0])
- glog.info("Should have been between", lower_bound, "and", upper_bound)
+ if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
+ tests_passed = False
+ glog.info('Claw separation was %f', claw.X[1, 0])
+ glog.info("Should have been between", lower_bound, "and",
+ upper_bound)
- if claw.hard_pos_limits and \
- (claw.X[0, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] < claw.hard_pos_limits[0] or
- claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
- tests_passed = False
- glog.info('Claws at %f and %f', claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
- glog.info("Both should be in %s, definitely %s",
- claw.pos_limits, claw.hard_pos_limits)
+ if claw.hard_pos_limits and \
+ (claw.X[0, 0] > claw.hard_pos_limits[1] or
+ claw.X[0, 0] < claw.hard_pos_limits[0] or
+ claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
+ claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
+ tests_passed = False
+ glog.info('Claws at %f and %f', claw.X[0, 0],
+ claw.X[0, 0] + claw.X[1, 0])
+ glog.info("Both should be in %s, definitely %s", claw.pos_limits,
+ claw.hard_pos_limits)
- t.append(i * claw.dt)
- x_bottom.append(claw.X[0, 0] * 10.0)
- x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
- u_bottom.append(U[0, 0])
- u_top.append(U[1, 0])
- x_separation.append(claw.X[1, 0] * 10.0)
+ t.append(i * claw.dt)
+ x_bottom.append(claw.X[0, 0] * 10.0)
+ x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
+ u_bottom.append(U[0, 0])
+ u_top.append(U[1, 0])
+ x_separation.append(claw.X[1, 0] * 10.0)
- if show_graph:
- pylab.plot(t, x_bottom, label='x bottom * 10')
- pylab.plot(t, x_top, label='x top * 10')
- pylab.plot(t, u_bottom, label='u bottom')
- pylab.plot(t, u_top, label='u top')
- pylab.plot(t, x_separation, label='separation * 10')
- pylab.legend()
- pylab.show()
+ if show_graph:
+ pylab.plot(t, x_bottom, label='x bottom * 10')
+ pylab.plot(t, x_top, label='x top * 10')
+ pylab.plot(t, u_bottom, label='u bottom')
+ pylab.plot(t, u_top, label='u top')
+ pylab.plot(t, x_separation, label='separation * 10')
+ pylab.legend()
+ pylab.show()
- # Test to make sure that we are near the goal.
- if numpy.max(abs(claw.X - goal)) > 1e-4:
- tests_passed = False
- glog.error('X was %s Expected %s', str(claw.X), str(goal))
+ # Test to make sure that we are near the goal.
+ if numpy.max(abs(claw.X - goal)) > 1e-4:
+ tests_passed = False
+ glog.error('X was %s Expected %s', str(claw.X), str(goal))
- return tests_passed
+ return tests_passed
+
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- claw = Claw()
- if FLAGS.plot:
- # Test moving the claw with constant separation.
- initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ claw = Claw()
+ if FLAGS.plot:
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test just changing separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test just changing separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test changing both separation and position at once.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test changing both separation and position at once.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test opening with the top claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-1.6, 0.1)
- claw.pos_limits = (-1.5, 0.0)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ # Test opening with the top claw at the limit.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
+ claw.hard_pos_limits = (-1.6, 0.1)
+ claw.pos_limits = (-1.5, 0.0)
+ run_test(claw, initial_X, R)
+ claw.pos_limits = None
- # Test opening with the bottom claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-0.1, 1.6)
- claw.pos_limits = (0.0, 1.6)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ # Test opening with the bottom claw at the limit.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
+ claw.hard_pos_limits = (-0.1, 1.6)
+ claw.pos_limits = (0.0, 1.6)
+ run_test(claw, initial_X, R)
+ claw.pos_limits = None
- # Write the generated constants out to a file.
- if len(argv) != 3:
- glog.fatal('Expected .h file name and .cc file name for the claw.')
- else:
- namespaces = ['y2014', 'control_loops', 'claw']
- claw = Claw('Claw')
- loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant('kClawMomentOfInertiaRatio',
- '%f', claw.J_top / claw.J_bottom))
- loop_writer.AddConstant(control_loop.Constant('kDt', '%f',
- claw.dt))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ glog.fatal('Expected .h file name and .cc file name for the claw.')
+ else:
+ namespaces = ['y2014', 'control_loops', 'claw']
+ claw = Claw('Claw')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Claw', [claw], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kClawMomentOfInertiaRatio', '%f',
+ claw.J_top / claw.J_bottom))
+ loop_writer.AddConstant(control_loop.Constant('kDt', '%f', claw.dt))
+ loop_writer.Write(argv[1], argv[2])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 6a6bb3e..9287dae 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -13,272 +13,271 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class SprungShooter(control_loop.ControlLoop):
- def __init__(self, name="RawSprungShooter"):
- super(SprungShooter, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = .4982
- # Stall Current in Amps
- self.stall_current = 85
- # Free Speed in RPM
- self.free_speed = 19300.0
- # Free Current in Amps
- self.free_current = 1.2
- # Effective mass of the shooter in kg.
- # This rough estimate should about include the effect of the masses
- # of the gears. If this number is too low, the eigen values of self.A
- # will start to become extremely small.
- self.J = 200
- # Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 2.0
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Spring constant for the springs, N/m
- self.Ks = 2800.0
- # Maximum extension distance (Distance from the 0 force point on the
- # spring to the latch position.)
- self.max_extension = 0.32385
- # Gear ratio multiplied by radius of final sprocket.
- self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
- # Control loop time step
- self.dt = 0.005
+ def __init__(self, name="RawSprungShooter"):
+ super(SprungShooter, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = .4982
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.2
+ # Effective mass of the shooter in kg.
+ # This rough estimate should about include the effect of the masses
+ # of the gears. If this number is too low, the eigen values of self.A
+ # will start to become extremely small.
+ self.J = 200
+ # Resistance of the motor, divided by the number of motors.
+ self.R = 12.0 / self.stall_current / 2.0
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Spring constant for the springs, N/m
+ self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
+ # Gear ratio multiplied by radius of final sprocket.
+ self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (
+ 3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [-self.Ks / self.J,
- -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[0],
- [self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [
+ -self.Ks / self.J,
+ -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
+ ]])
+ self.B_continuous = numpy.matrix(
+ [[0], [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([0.45, 0.45])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl,
- self.rpl])
+ self.PlaceControllerPoles([0.45, 0.45])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl, self.rpl])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class Shooter(SprungShooter):
- def __init__(self, name="RawShooter"):
- super(Shooter, self).__init__(name)
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[0],
- [self.Kt / (self.J * self.G * self.R)]])
+ def __init__(self, name="RawShooter"):
+ super(Shooter, self).__init__(name)
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0], [self.Kt / (self.J * self.G * self.R)]])
- self.PlaceControllerPoles([0.45, 0.45])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl,
- self.rpl])
+ self.PlaceControllerPoles([0.45, 0.45])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl, self.rpl])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class SprungShooterDeltaU(SprungShooter):
- def __init__(self, name="SprungShooter"):
- super(SprungShooterDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- A_continuous_unaugmented = self.A_continuous
- B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="SprungShooter"):
+ super(SprungShooterDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
- self.A_continuous[0:2, 2] = B_continuous_unaugmented
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[2, 0] = 1.0 / self.dt
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
- self.A = numpy.matrix([[0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0],
- [0.0, 0.0, 1.0]])
- self.A[0:2, 0:2] = A_unaugmented
- self.A[0:2, 2] = B_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
- self.B = numpy.matrix([[0.0],
- [0.0],
- [1.0]])
+ self.A = numpy.matrix([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
- self.C = numpy.matrix([[1.0, 0.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B = numpy.matrix([[0.0], [0.0], [1.0]])
- self.PlaceControllerPoles([0.50, 0.35, 0.80])
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- glog.debug('K')
- glog.debug(str(self.K))
- glog.debug('Placed controller poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.PlaceControllerPoles([0.50, 0.35, 0.80])
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl, 0.90])
- glog.debug('Placed observer poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl, 0.90])
+ glog.debug('Placed observer poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class ShooterDeltaU(Shooter):
- def __init__(self, name="Shooter"):
- super(ShooterDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- A_continuous_unaugmented = self.A_continuous
- B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="Shooter"):
+ super(ShooterDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
- self.A_continuous[0:2, 2] = B_continuous_unaugmented
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[2, 0] = 1.0 / self.dt
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
- self.A = numpy.matrix([[0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0],
- [0.0, 0.0, 1.0]])
- self.A[0:2, 0:2] = A_unaugmented
- self.A[0:2, 2] = B_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
- self.B = numpy.matrix([[0.0],
- [0.0],
- [1.0]])
+ self.A = numpy.matrix([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
- self.C = numpy.matrix([[1.0, 0.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B = numpy.matrix([[0.0], [0.0], [1.0]])
- self.PlaceControllerPoles([0.55, 0.45, 0.80])
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- glog.debug('K')
- glog.debug(str(self.K))
- glog.debug('Placed controller poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.PlaceControllerPoles([0.55, 0.45, 0.80])
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl, 0.90])
- glog.debug('Placed observer poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl, 0.90])
+ glog.debug('Placed observer poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
def ClipDeltaU(shooter, old_voltage, delta_u):
- old_u = old_voltage
- new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
- return new_u - old_u
+ old_u = old_voltage
+ new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
+ return new_u - old_u
+
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- # Simulate the response of the system to a goal.
- sprung_shooter = SprungShooterDeltaU()
- raw_sprung_shooter = SprungShooter()
- close_loop_x = []
- close_loop_u = []
- goal_position = -0.3
- R = numpy.matrix([[goal_position],
- [0.0],
- [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] *
- goal_position]])
- voltage = numpy.matrix([[0.0]])
- for _ in xrange(500):
- U = sprung_shooter.K * (R - sprung_shooter.X_hat)
- U = ClipDeltaU(sprung_shooter, voltage, U)
- sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
- sprung_shooter.UpdateObserver(U)
- voltage += U
- raw_sprung_shooter.Update(voltage)
- close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
- close_loop_u.append(voltage[0, 0])
+ # Simulate the response of the system to a goal.
+ sprung_shooter = SprungShooterDeltaU()
+ raw_sprung_shooter = SprungShooter()
+ close_loop_x = []
+ close_loop_u = []
+ goal_position = -0.3
+ R = numpy.matrix(
+ [[goal_position], [0.0],
+ [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
+ for _ in xrange(500):
+ U = sprung_shooter.K * (R - sprung_shooter.X_hat)
+ U = ClipDeltaU(sprung_shooter, voltage, U)
+ sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
+ sprung_shooter.UpdateObserver(U)
+ voltage += U
+ raw_sprung_shooter.Update(voltage)
+ close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
- if FLAGS.plot:
- pylab.plot(range(500), close_loop_x)
- pylab.plot(range(500), close_loop_u)
- pylab.show()
+ if FLAGS.plot:
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
- shooter = ShooterDeltaU()
- raw_shooter = Shooter()
- close_loop_x = []
- close_loop_u = []
- goal_position = -0.3
- R = numpy.matrix([[goal_position], [0.0], [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
- voltage = numpy.matrix([[0.0]])
- for _ in xrange(500):
- U = shooter.K * (R - shooter.X_hat)
- U = ClipDeltaU(shooter, voltage, U)
- shooter.Y = raw_shooter.Y + 0.01
- shooter.UpdateObserver(U)
- voltage += U
- raw_shooter.Update(voltage)
- close_loop_x.append(raw_shooter.X[0, 0] * 10)
- close_loop_u.append(voltage[0, 0])
+ shooter = ShooterDeltaU()
+ raw_shooter = Shooter()
+ close_loop_x = []
+ close_loop_u = []
+ goal_position = -0.3
+ R = numpy.matrix([[goal_position], [0.0],
+ [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
+ for _ in xrange(500):
+ U = shooter.K * (R - shooter.X_hat)
+ U = ClipDeltaU(shooter, voltage, U)
+ shooter.Y = raw_shooter.Y + 0.01
+ shooter.UpdateObserver(U)
+ voltage += U
+ raw_shooter.Update(voltage)
+ close_loop_x.append(raw_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
- if FLAGS.plot:
- pylab.plot(range(500), close_loop_x)
- pylab.plot(range(500), close_loop_u)
- pylab.show()
+ if FLAGS.plot:
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
- # Write the generated constants out to a file.
- unaug_sprung_shooter = SprungShooter("RawSprungShooter")
- unaug_shooter = Shooter("RawShooter")
- namespaces = ['y2014', 'control_loops', 'shooter']
- unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
- [unaug_sprung_shooter,
- unaug_shooter],
- namespaces=namespaces)
- unaug_loop_writer.Write(argv[4], argv[3])
+ # Write the generated constants out to a file.
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter")
+ unaug_shooter = Shooter("RawShooter")
+ namespaces = ['y2014', 'control_loops', 'shooter']
+ unaug_loop_writer = control_loop.ControlLoopWriter(
+ "RawShooter", [unaug_sprung_shooter, unaug_shooter],
+ namespaces=namespaces)
+ unaug_loop_writer.Write(argv[4], argv[3])
- sprung_shooter = SprungShooterDeltaU()
- shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter",
- [sprung_shooter, shooter],
- namespaces=namespaces)
+ sprung_shooter = SprungShooterDeltaU()
+ shooter = ShooterDeltaU()
+ loop_writer = control_loop.ControlLoopWriter(
+ "Shooter", [sprung_shooter, shooter], namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
- sprung_shooter.max_extension))
- loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
- sprung_shooter.Ks))
- loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
- sprung_shooter.dt))
- loop_writer.Write(argv[2], argv[1])
+ loop_writer.AddConstant(
+ control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(
+ control_loop.Constant("kSpringConstant", "%f", sprung_shooter.Ks))
+ loop_writer.AddConstant(
+ control_loop.Constant("kDt", "%f", sprung_shooter.dt))
+ loop_writer.Write(argv[2], argv[1])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 5c5793b..96975bb 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -18,414 +18,439 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm", J=None):
- super(Arm, self).__init__(name=name)
- self._shoulder = Shoulder(name=name, J=J)
- self._shooter = Wrist(name=name)
- self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
- # Do a coordinate transformation.
- # X_shooter_grounded = X_shooter + X_shoulder
- # dX_shooter_grounded/dt = A_shooter * X_shooter + A_shoulder * X_shoulder +
- # B_shoulder * U_shoulder + B_shooter * U_shooter
- # dX_shooter_grounded/dt = A_shooter * (X_shooter_grounded - X_shoulder) +
- # A_shoulder * X_shoulder + B_shooter * U_shooter + B_shoulder * U_shoulder
- # X = [X_shoulder; X_shooter + X_shoulder]
- # dX/dt = [A_shoulder 0] [X_shoulder ] + [B_shoulder 0] [U_shoulder]
- # [(A_shoulder - A_shooter) A_shooter] [X_shooter_grounded] + [B_shoulder B_shooter] [ U_shooter]
- # Y_shooter_grounded = Y_shooter + Y_shoulder
+ def __init__(self, name="Arm", J=None):
+ super(Arm, self).__init__(name=name)
+ self._shoulder = Shoulder(name=name, J=J)
+ self._shooter = Wrist(name=name)
+ self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
- self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
- self.A_continuous[0:2, 0:2] = self._shoulder.A_continuous
- self.A_continuous[2:4, 0:2] = (self._shoulder.A_continuous -
- self._shooter.A_continuous)
- self.A_continuous[2:4, 2:4] = self._shooter.A_continuous
+ # Do a coordinate transformation.
+ # X_shooter_grounded = X_shooter + X_shoulder
+ # dX_shooter_grounded/dt = A_shooter * X_shooter + A_shoulder * X_shoulder +
+ # B_shoulder * U_shoulder + B_shooter * U_shooter
+ # dX_shooter_grounded/dt = A_shooter * (X_shooter_grounded - X_shoulder) +
+ # A_shoulder * X_shoulder + B_shooter * U_shooter + B_shoulder * U_shoulder
+ # X = [X_shoulder; X_shooter + X_shoulder]
+ # dX/dt = [A_shoulder 0] [X_shoulder ] + [B_shoulder 0] [U_shoulder]
+ # [(A_shoulder - A_shooter) A_shooter] [X_shooter_grounded] + [B_shoulder B_shooter] [ U_shooter]
+ # Y_shooter_grounded = Y_shooter + Y_shoulder
- self.B_continuous = numpy.matrix(numpy.zeros((4, 2)))
- self.B_continuous[0:2, 0:1] = self._shoulder.B_continuous
- self.B_continuous[2:4, 1:2] = self._shooter.B_continuous
- self.B_continuous[2:4, 0:1] = self._shoulder.B_continuous
+ self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ self.A_continuous[0:2, 0:2] = self._shoulder.A_continuous
+ self.A_continuous[2:4, 0:2] = (
+ self._shoulder.A_continuous - self._shooter.A_continuous)
+ self.A_continuous[2:4, 2:4] = self._shooter.A_continuous
- self.C = numpy.matrix(numpy.zeros((2, 4)))
- self.C[0:1, 0:2] = self._shoulder.C
- self.C[1:2, 0:2] = -self._shoulder.C
- self.C[1:2, 2:4] = self._shooter.C
+ self.B_continuous = numpy.matrix(numpy.zeros((4, 2)))
+ self.B_continuous[0:2, 0:1] = self._shoulder.B_continuous
+ self.B_continuous[2:4, 1:2] = self._shooter.B_continuous
+ self.B_continuous[2:4, 0:1] = self._shoulder.B_continuous
- # D is 0 for all our loops.
- self.D = numpy.matrix(numpy.zeros((2, 2)))
+ self.C = numpy.matrix(numpy.zeros((2, 4)))
+ self.C[0:1, 0:2] = self._shoulder.C
+ self.C[1:2, 0:2] = -self._shoulder.C
+ self.C[1:2, 2:4] = self._shooter.C
- self.dt = 0.005
+ # D is 0 for all our loops.
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.dt = 0.005
- # Cost of error
- self.Q = numpy.matrix(numpy.zeros((4, 4)))
- q_pos_shoulder = 0.014
- q_vel_shoulder = 4.00
- q_pos_shooter = 0.014
- q_vel_shooter = 4.00
- self.Q[0, 0] = 1.0 / q_pos_shoulder ** 2.0
- self.Q[1, 1] = 1.0 / q_vel_shoulder ** 2.0
- self.Q[2, 2] = 1.0 / q_pos_shooter ** 2.0
- self.Q[3, 3] = 1.0 / q_vel_shooter ** 2.0
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.Qff = numpy.matrix(numpy.zeros((4, 4)))
- qff_pos_shoulder = 0.005
- qff_vel_shoulder = 1.00
- qff_pos_shooter = 0.005
- qff_vel_shooter = 1.00
- self.Qff[0, 0] = 1.0 / qff_pos_shoulder ** 2.0
- self.Qff[1, 1] = 1.0 / qff_vel_shoulder ** 2.0
- self.Qff[2, 2] = 1.0 / qff_pos_shooter ** 2.0
- self.Qff[3, 3] = 1.0 / qff_vel_shooter ** 2.0
+ # Cost of error
+ self.Q = numpy.matrix(numpy.zeros((4, 4)))
+ q_pos_shoulder = 0.014
+ q_vel_shoulder = 4.00
+ q_pos_shooter = 0.014
+ q_vel_shooter = 4.00
+ self.Q[0, 0] = 1.0 / q_pos_shoulder**2.0
+ self.Q[1, 1] = 1.0 / q_vel_shoulder**2.0
+ self.Q[2, 2] = 1.0 / q_pos_shooter**2.0
+ self.Q[3, 3] = 1.0 / q_vel_shooter**2.0
- # Cost of control effort
- self.R = numpy.matrix(numpy.zeros((2, 2)))
- r_voltage = 1.0 / 12.0
- self.R[0, 0] = r_voltage ** 2.0
- self.R[1, 1] = r_voltage ** 2.0
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos_shoulder = 0.005
+ qff_vel_shoulder = 1.00
+ qff_pos_shooter = 0.005
+ qff_vel_shooter = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos_shoulder**2.0
+ self.Qff[1, 1] = 1.0 / qff_vel_shoulder**2.0
+ self.Qff[2, 2] = 1.0 / qff_pos_shooter**2.0
+ self.Qff[3, 3] = 1.0 / qff_vel_shooter**2.0
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ # Cost of control effort
+ self.R = numpy.matrix(numpy.zeros((2, 2)))
+ r_voltage = 1.0 / 12.0
+ self.R[0, 0] = r_voltage**2.0
+ self.R[1, 1] = r_voltage**2.0
- glog.debug('Shoulder K')
- glog.debug(repr(self._shoulder.K))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self._shoulder.A -
- self._shoulder.B * self._shoulder.K)[0]))
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- # Compute controller gains.
- # self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.K = numpy.matrix(numpy.zeros((2, 4)))
- self.K[0:1, 0:2] = self._shoulder.K
- self.K[1:2, 0:2] = (
- -self.Kff[1:2, 2:4] * self.B[2:4, 0:1] * self._shoulder.K
- + self.Kff[1:2, 2:4] * self.A[2:4, 0:2])
- self.K[1:2, 2:4] = self._shooter.K
+ glog.debug('Shoulder K')
+ glog.debug(repr(self._shoulder.K))
+ glog.debug(
+ 'Poles are %s',
+ repr(
+ numpy.linalg.eig(self._shoulder.A -
+ self._shoulder.B * self._shoulder.K)[0]))
- glog.debug('Arm controller %s', repr(self.K))
+ # Compute controller gains.
+ # self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ self.K = numpy.matrix(numpy.zeros((2, 4)))
+ self.K[0:1, 0:2] = self._shoulder.K
+ self.K[1:2, 0:2] = (
+ -self.Kff[1:2, 2:4] * self.B[2:4, 0:1] * self._shoulder.K +
+ self.Kff[1:2, 2:4] * self.A[2:4, 0:2])
+ self.K[1:2, 2:4] = self._shooter.K
- # Cost of error
- self.Q = numpy.matrix(numpy.zeros((4, 4)))
- q_pos_shoulder = 0.05
- q_vel_shoulder = 2.65
- q_pos_shooter = 0.05
- q_vel_shooter = 2.65
- self.Q[0, 0] = q_pos_shoulder ** 2.0
- self.Q[1, 1] = q_vel_shoulder ** 2.0
- self.Q[2, 2] = q_pos_shooter ** 2.0
- self.Q[3, 3] = q_vel_shooter ** 2.0
+ glog.debug('Arm controller %s', repr(self.K))
- # Cost of control effort
- self.R = numpy.matrix(numpy.zeros((2, 2)))
- r_voltage = 0.025
- self.R[0, 0] = r_voltage ** 2.0
- self.R[1, 1] = r_voltage ** 2.0
+ # Cost of error
+ self.Q = numpy.matrix(numpy.zeros((4, 4)))
+ q_pos_shoulder = 0.05
+ q_vel_shoulder = 2.65
+ q_pos_shooter = 0.05
+ q_vel_shooter = 2.65
+ self.Q[0, 0] = q_pos_shoulder**2.0
+ self.Q[1, 1] = q_vel_shoulder**2.0
+ self.Q[2, 2] = q_pos_shooter**2.0
+ self.Q[3, 3] = q_vel_shooter**2.0
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ # Cost of control effort
+ self.R = numpy.matrix(numpy.zeros((2, 2)))
+ r_voltage = 0.025
+ self.R[0, 0] = r_voltage**2.0
+ self.R[1, 1] = r_voltage**2.0
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm", J=None):
- super(IntegralArm, self).__init__(name=name, J=J)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralArm", J=None):
+ super(IntegralArm, self).__init__(name=name, J=J)
- self.A_continuous = numpy.matrix(numpy.zeros((6, 6)))
- self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
- self.A_continuous[0:4, 4:6] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((6, 2)))
- self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((6, 6)))
+ self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
+ self.A_continuous[0:4, 4:6] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((2, 6)))
- self.C[0:2, 0:4] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((6, 2)))
+ self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((2, 6)))
+ self.C[0:2, 0:4] = self.C_unaugmented
- q_pos_shoulder = 0.10
- q_vel_shoulder = 0.005
- q_voltage_shoulder = 3.5
- q_pos_shooter = 0.08
- q_vel_shooter = 2.00
- q_voltage_shooter = 1.0
- self.Q = numpy.matrix(numpy.zeros((6, 6)))
- self.Q[0, 0] = q_pos_shoulder ** 2.0
- self.Q[1, 1] = q_vel_shoulder ** 2.0
- self.Q[2, 2] = q_pos_shooter ** 2.0
- self.Q[3, 3] = q_vel_shooter ** 2.0
- self.Q[4, 4] = q_voltage_shoulder ** 2.0
- self.Q[5, 5] = q_voltage_shooter ** 2.0
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix(numpy.zeros((2, 2)))
- r_pos = 0.05
- self.R[0, 0] = r_pos ** 2.0
- self.R[1, 1] = r_pos ** 2.0
+ q_pos_shoulder = 0.10
+ q_vel_shoulder = 0.005
+ q_voltage_shoulder = 3.5
+ q_pos_shooter = 0.08
+ q_vel_shooter = 2.00
+ q_voltage_shooter = 1.0
+ self.Q = numpy.matrix(numpy.zeros((6, 6)))
+ self.Q[0, 0] = q_pos_shoulder**2.0
+ self.Q[1, 1] = q_vel_shoulder**2.0
+ self.Q[2, 2] = q_pos_shooter**2.0
+ self.Q[3, 3] = q_vel_shooter**2.0
+ self.Q[4, 4] = q_voltage_shoulder**2.0
+ self.Q[5, 5] = q_voltage_shooter**2.0
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ self.R = numpy.matrix(numpy.zeros((2, 2)))
+ r_pos = 0.05
+ self.R[0, 0] = r_pos**2.0
+ self.R[1, 1] = r_pos**2.0
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((2, 6)))
- self.K[0:2, 0:4] = self.K_unaugmented
- self.K[0, 4] = 1
- self.K[1, 5] = 1
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.Kff = numpy.concatenate((self.Kff, numpy.matrix(numpy.zeros((2, 2)))), axis=1)
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 6)))
+ self.K[0:2, 0:4] = self.K_unaugmented
+ self.K[0, 4] = 1
+ self.K[1, 5] = 1
- self.InitializeState()
+ self.Kff = numpy.concatenate(
+ (self.Kff, numpy.matrix(numpy.zeros((2, 2)))), axis=1)
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x_shoulder = []
- self.v_shoulder = []
- self.a_shoulder = []
- self.x_hat_shoulder = []
- self.u_shoulder = []
- self.offset_shoulder = []
- self.x_shooter = []
- self.v_shooter = []
- self.a_shooter = []
- self.x_hat_shooter = []
- self.u_shooter = []
- self.offset_shooter = []
- self.goal_x_shoulder = []
- self.goal_v_shoulder = []
- self.goal_x_shooter = []
- self.goal_v_shooter = []
- def run_test(self, arm, end_goal,
- iterations=200, controller=None, observer=None):
- """Runs the plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x_shoulder = []
+ self.v_shoulder = []
+ self.a_shoulder = []
+ self.x_hat_shoulder = []
+ self.u_shoulder = []
+ self.offset_shoulder = []
+ self.x_shooter = []
+ self.v_shooter = []
+ self.a_shooter = []
+ self.x_hat_shooter = []
+ self.u_shooter = []
+ self.offset_shooter = []
+ self.goal_x_shoulder = []
+ self.goal_v_shoulder = []
+ self.goal_x_shooter = []
+ self.goal_v_shooter = []
- Args:
- arm: Arm object to use.
- end_goal: numpy.Matrix[6, 1], end goal state.
- iterations: Number of timesteps to run the model for.
- controller: Arm object to get K from, or None if we should
- use arm.
- observer: Arm object to use for the observer, or None if we should
- use the actual state.
- """
+ def run_test(self,
+ arm,
+ end_goal,
+ iterations=200,
+ controller=None,
+ observer=None):
+ """Runs the plant with an initial condition and goal.
- if controller is None:
- controller = arm
+ Args:
+ arm: Arm object to use.
+ end_goal: numpy.Matrix[6, 1], end goal state.
+ iterations: Number of timesteps to run the model for.
+ controller: Arm object to get K from, or None if we should
+ use arm.
+ observer: Arm object to use for the observer, or None if we should
+ use the actual state.
+ """
- vbat = 12.0
+ if controller is None:
+ controller = arm
- if self.t:
- initial_t = self.t[-1] + arm.dt
- else:
- initial_t = 0
+ vbat = 12.0
- goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
+ if self.t:
+ initial_t = self.t[-1] + arm.dt
+ else:
+ initial_t = 0
- shoulder_profile = TrapezoidProfile(arm.dt)
- shoulder_profile.set_maximum_acceleration(12.0)
- shoulder_profile.set_maximum_velocity(10.0)
- shoulder_profile.SetGoal(goal[0, 0])
- shooter_profile = TrapezoidProfile(arm.dt)
- shooter_profile.set_maximum_acceleration(50.0)
- shooter_profile.set_maximum_velocity(10.0)
- shooter_profile.SetGoal(goal[2, 0])
+ goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))),
+ axis=0)
- U_last = numpy.matrix(numpy.zeros((2, 1)))
- for i in xrange(iterations):
- X_hat = arm.X
+ shoulder_profile = TrapezoidProfile(arm.dt)
+ shoulder_profile.set_maximum_acceleration(12.0)
+ shoulder_profile.set_maximum_velocity(10.0)
+ shoulder_profile.SetGoal(goal[0, 0])
+ shooter_profile = TrapezoidProfile(arm.dt)
+ shooter_profile.set_maximum_acceleration(50.0)
+ shooter_profile.set_maximum_velocity(10.0)
+ shooter_profile.SetGoal(goal[2, 0])
- if observer is not None:
- observer.Y = arm.Y
- observer.CorrectObserver(U_last)
- self.offset_shoulder.append(observer.X_hat[4, 0])
- self.offset_shooter.append(observer.X_hat[5, 0])
+ U_last = numpy.matrix(numpy.zeros((2, 1)))
+ for i in xrange(iterations):
+ X_hat = arm.X
- X_hat = observer.X_hat
- self.x_hat_shoulder.append(observer.X_hat[0, 0])
- self.x_hat_shooter.append(observer.X_hat[2, 0])
+ if observer is not None:
+ observer.Y = arm.Y
+ observer.CorrectObserver(U_last)
+ self.offset_shoulder.append(observer.X_hat[4, 0])
+ self.offset_shooter.append(observer.X_hat[5, 0])
- next_shoulder_goal = shoulder_profile.Update(end_goal[0, 0], end_goal[1, 0])
- next_shooter_goal = shooter_profile.Update(end_goal[2, 0], end_goal[3, 0])
+ X_hat = observer.X_hat
+ self.x_hat_shoulder.append(observer.X_hat[0, 0])
+ self.x_hat_shooter.append(observer.X_hat[2, 0])
- next_goal = numpy.concatenate(
- (next_shoulder_goal,
- next_shooter_goal,
- numpy.matrix(numpy.zeros((2, 1)))),
- axis=0)
- self.goal_x_shoulder.append(goal[0, 0])
- self.goal_v_shoulder.append(goal[1, 0])
- self.goal_x_shooter.append(goal[2, 0])
- self.goal_v_shooter.append(goal[3, 0])
+ next_shoulder_goal = shoulder_profile.Update(
+ end_goal[0, 0], end_goal[1, 0])
+ next_shooter_goal = shooter_profile.Update(end_goal[2, 0],
+ end_goal[3, 0])
- ff_U = controller.Kff * (next_goal - observer.A * goal)
+ next_goal = numpy.concatenate(
+ (next_shoulder_goal, next_shooter_goal,
+ numpy.matrix(numpy.zeros((2, 1)))),
+ axis=0)
+ self.goal_x_shoulder.append(goal[0, 0])
+ self.goal_v_shoulder.append(goal[1, 0])
+ self.goal_x_shooter.append(goal[2, 0])
+ self.goal_v_shooter.append(goal[3, 0])
- U_uncapped = controller.K * (goal - X_hat) + ff_U
- U = U_uncapped.copy()
+ ff_U = controller.Kff * (next_goal - observer.A * goal)
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
- self.x_shoulder.append(arm.X[0, 0])
- self.x_shooter.append(arm.X[2, 0])
+ U_uncapped = controller.K * (goal - X_hat) + ff_U
+ U = U_uncapped.copy()
- if self.v_shoulder:
- last_v_shoulder = self.v_shoulder[-1]
- else:
- last_v_shoulder = 0
- self.v_shoulder.append(arm.X[1, 0])
- self.a_shoulder.append(
- (self.v_shoulder[-1] - last_v_shoulder) / arm.dt)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
+ self.x_shoulder.append(arm.X[0, 0])
+ self.x_shooter.append(arm.X[2, 0])
- if self.v_shooter:
- last_v_shooter = self.v_shooter[-1]
- else:
- last_v_shooter = 0
- self.v_shooter.append(arm.X[3, 0])
- self.a_shooter.append(
- (self.v_shooter[-1] - last_v_shooter) / arm.dt)
+ if self.v_shoulder:
+ last_v_shoulder = self.v_shoulder[-1]
+ else:
+ last_v_shoulder = 0
+ self.v_shoulder.append(arm.X[1, 0])
+ self.a_shoulder.append(
+ (self.v_shoulder[-1] - last_v_shoulder) / arm.dt)
- if i % 40 == 0:
- # Test that if we move the shoulder, the shooter stays perfect.
- #observer.X_hat[0, 0] += 0.20
- #arm.X[0, 0] += 0.20
- pass
- U_error = numpy.matrix([[2.0], [2.0]])
- # Kick it and see what happens.
- #if (initial_t + i * arm.dt) % 0.4 > 0.2:
- #U_error = numpy.matrix([[4.0], [0.0]])
- #else:
- #U_error = numpy.matrix([[-4.0], [0.0]])
+ if self.v_shooter:
+ last_v_shooter = self.v_shooter[-1]
+ else:
+ last_v_shooter = 0
+ self.v_shooter.append(arm.X[3, 0])
+ self.a_shooter.append(
+ (self.v_shooter[-1] - last_v_shooter) / arm.dt)
- arm.Update(U + U_error)
+ if i % 40 == 0:
+ # Test that if we move the shoulder, the shooter stays perfect.
+ #observer.X_hat[0, 0] += 0.20
+ #arm.X[0, 0] += 0.20
+ pass
+ U_error = numpy.matrix([[2.0], [2.0]])
+ # Kick it and see what happens.
+ #if (initial_t + i * arm.dt) % 0.4 > 0.2:
+ #U_error = numpy.matrix([[4.0], [0.0]])
+ #else:
+ #U_error = numpy.matrix([[-4.0], [0.0]])
- if observer is not None:
- observer.PredictObserver(U)
+ arm.Update(U + U_error)
- self.t.append(initial_t + i * arm.dt)
- self.u_shoulder.append(U[0, 0])
- self.u_shooter.append(U[1, 0])
+ if observer is not None:
+ observer.PredictObserver(U)
- ff_U -= U_uncapped - U
- goal = controller.A * goal + controller.B * ff_U
+ self.t.append(initial_t + i * arm.dt)
+ self.u_shoulder.append(U[0, 0])
+ self.u_shooter.append(U[1, 0])
- if U[0, 0] != U_uncapped[0, 0]:
- glog.debug('Moving shoulder %s', repr(initial_t + i * arm.dt))
- glog.debug('U error %s', repr(U_uncapped - U))
- glog.debug('goal change is %s',
- repr(next_shoulder_goal -
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]])))
- shoulder_profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
- if U[1, 0] != U_uncapped[1, 0]:
- glog.debug('Moving shooter %s', repr(initial_t + i * arm.dt))
- glog.debug('U error %s', repr(U_uncapped - U))
- shooter_profile.MoveCurrentState(
- numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
- U_last = U
- glog.debug('goal_error %s', repr(end_goal - goal))
- glog.debug('error %s', repr(observer.X_hat - end_goal))
+ ff_U -= U_uncapped - U
+ goal = controller.A * goal + controller.B * ff_U
+ if U[0, 0] != U_uncapped[0, 0]:
+ glog.debug('Moving shoulder %s', repr(initial_t + i * arm.dt))
+ glog.debug('U error %s', repr(U_uncapped - U))
+ glog.debug(
+ 'goal change is %s',
+ repr(next_shoulder_goal -
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]])))
+ shoulder_profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ if U[1, 0] != U_uncapped[1, 0]:
+ glog.debug('Moving shooter %s', repr(initial_t + i * arm.dt))
+ glog.debug('U error %s', repr(U_uncapped - U))
+ shooter_profile.MoveCurrentState(
+ numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
+ U_last = U
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer.X_hat - end_goal))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x_shoulder, label='x shoulder')
- pylab.plot(self.t, self.goal_x_shoulder, label='goal x shoulder')
- pylab.plot(self.t, self.x_hat_shoulder, label='x_hat shoulder')
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x_shoulder, label='x shoulder')
+ pylab.plot(self.t, self.goal_x_shoulder, label='goal x shoulder')
+ pylab.plot(self.t, self.x_hat_shoulder, label='x_hat shoulder')
- pylab.plot(self.t, self.x_shooter, label='x shooter')
- pylab.plot(self.t, self.x_hat_shooter, label='x_hat shooter')
- pylab.plot(self.t, self.goal_x_shooter, label='goal x shooter')
- pylab.plot(self.t, map(operator.add, self.x_shooter, self.x_shoulder),
- label='x shooter ground')
- pylab.plot(self.t, map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
- label='x_hat shooter ground')
- pylab.legend()
+ pylab.plot(self.t, self.x_shooter, label='x shooter')
+ pylab.plot(self.t, self.x_hat_shooter, label='x_hat shooter')
+ pylab.plot(self.t, self.goal_x_shooter, label='goal x shooter')
+ pylab.plot(
+ self.t,
+ map(operator.add, self.x_shooter, self.x_shoulder),
+ label='x shooter ground')
+ pylab.plot(
+ self.t,
+ map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
+ label='x_hat shooter ground')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u_shoulder, label='u shoulder')
- pylab.plot(self.t, self.offset_shoulder, label='voltage_offset shoulder')
- pylab.plot(self.t, self.u_shooter, label='u shooter')
- pylab.plot(self.t, self.offset_shooter, label='voltage_offset shooter')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u_shoulder, label='u shoulder')
+ pylab.plot(
+ self.t, self.offset_shoulder, label='voltage_offset shoulder')
+ pylab.plot(self.t, self.u_shooter, label='u shooter')
+ pylab.plot(self.t, self.offset_shooter, label='voltage_offset shooter')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a_shoulder, label='a_shoulder')
- pylab.plot(self.t, self.a_shooter, label='a_shooter')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a_shoulder, label='a_shoulder')
+ pylab.plot(self.t, self.a_shooter, label='a_shooter')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- J_accelerating = 18
- J_decelerating = 7
+ J_accelerating = 18
+ J_decelerating = 7
- arm = Arm(name='AcceleratingArm', J=J_accelerating)
- arm_integral_controller = IntegralArm(
- name='AcceleratingIntegralArm', J=J_accelerating)
- arm_observer = IntegralArm()
+ arm = Arm(name='AcceleratingArm', J=J_accelerating)
+ arm_integral_controller = IntegralArm(
+ name='AcceleratingIntegralArm', J=J_accelerating)
+ arm_observer = IntegralArm()
- # Test moving the shoulder with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[numpy.pi / 2.0],
- [0.0],
- [0.0], #[numpy.pi / 2.0],
- [0.0],
- [0.0],
- [0.0]])
- arm.X = initial_X[0:4, 0]
- arm_observer.X = initial_X
+ # Test moving the shoulder with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([
+ [numpy.pi / 2.0],
+ [0.0],
+ [0.0], #[numpy.pi / 2.0],
+ [0.0],
+ [0.0],
+ [0.0]
+ ])
+ arm.X = initial_X[0:4, 0]
+ arm_observer.X = initial_X
- scenario_plotter.run_test(arm=arm,
- end_goal=R,
- iterations=300,
- controller=arm_integral_controller,
- observer=arm_observer)
+ scenario_plotter.run_test(
+ arm=arm,
+ end_goal=R,
+ iterations=300,
+ controller=arm_integral_controller,
+ observer=arm_observer)
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
- else:
- namespaces = ['y2016', 'control_loops', 'superstructure']
- decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
- loop_writer = control_loop.ControlLoopWriter(
- 'Arm', [arm, decelerating_arm], namespaces=namespaces)
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2016', 'control_loops', 'superstructure']
+ decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Arm', [arm, decelerating_arm], namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- decelerating_integral_arm_controller = IntegralArm(
- name='DeceleratingIntegralArm', J=J_decelerating)
+ decelerating_integral_arm_controller = IntegralArm(
+ name='DeceleratingIntegralArm', J=J_decelerating)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralArm',
- [arm_integral_controller, decelerating_integral_arm_controller],
- namespaces=namespaces)
- integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
- arm_integral_controller.shoulder_Kv))
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralArm',
+ [arm_integral_controller, decelerating_integral_arm_controller],
+ namespaces=namespaces)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("kV_shoulder", "%f",
+ arm_integral_controller.shoulder_Kv))
+ integral_loop_writer.Write(argv[3], argv[4])
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index 53793d0..b3c988c 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -13,262 +13,274 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class VelocityShooter(control_loop.ControlLoop):
- def __init__(self, name='VelocityShooter'):
- super(VelocityShooter, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.71
- # Stall Current in Amps
- self.stall_current = 134
- # Free Speed in RPM
- self.free_speed = 18730.0
- # Free Current in Amps
- self.free_current = 0.7
- # Moment of inertia of the shooter wheel in kg m^2
- self.J = 0.00032
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 12.0 / 18.0
- # Control loop time step
- self.dt = 0.005
- # State feedback matrices
- # [angular velocity]
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ def __init__(self, name='VelocityShooter'):
+ super(VelocityShooter, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134
+ # Free Speed in RPM
+ self.free_speed = 18730.0
+ # Free Current in Amps
+ self.free_current = 0.7
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.00032
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 12.0 / 18.0
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.87])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceObserverPoles([0.3])
+ self.PlaceControllerPoles([.87])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
class Shooter(VelocityShooter):
- def __init__(self, name='Shooter'):
- super(Shooter, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='Shooter'):
+ super(Shooter, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
- self.A_continuous[0, 1] = 1
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.rpl = .45
- self.ipl = 0.07
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.K[0, 1:2] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 2)))
- self.Kff[0, 1:2] = self.Kff_unaugmented
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl])
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class IntegralShooter(Shooter):
- def __init__(self, name="IntegralShooter"):
- super(IntegralShooter, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralShooter"):
+ super(IntegralShooter, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.08
- q_vel = 4.00
- q_voltage = 0.3
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.08
+ q_vel = 4.00
+ q_voltage = 0.3
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None):
- """Runs the shooter plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
- Args:
- shooter: Shooter object to use.
- goal: goal state.
- iterations: Number of timesteps to run the model for.
- controller_shooter: Shooter object to get K from, or None if we should
- use shooter.
- observer_shooter: Shooter object to use for the observer, or None if we
- should use the actual state.
- """
+ def run_test(self,
+ shooter,
+ goal,
+ iterations=200,
+ controller_shooter=None,
+ observer_shooter=None):
+ """Runs the shooter plant with an initial condition and goal.
- if controller_shooter is None:
- controller_shooter = shooter
+ Args:
+ shooter: Shooter object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_shooter: Shooter object to get K from, or None if we should
+ use shooter.
+ observer_shooter: Shooter object to use for the observer, or None if we
+ should use the actual state.
+ """
- vbat = 12.0
+ if controller_shooter is None:
+ controller_shooter = shooter
- if self.t:
- initial_t = self.t[-1] + shooter.dt
- else:
- initial_t = 0
+ vbat = 12.0
- for i in xrange(iterations):
- X_hat = shooter.X
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
+ else:
+ initial_t = 0
- if observer_shooter is not None:
- X_hat = observer_shooter.X_hat
- self.x_hat.append(observer_shooter.X_hat[1, 0])
+ for i in xrange(iterations):
+ X_hat = shooter.X
- ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+ if observer_shooter is not None:
+ X_hat = observer_shooter.X_hat
+ self.x_hat.append(observer_shooter.X_hat[1, 0])
- U = controller_shooter.K * (goal - X_hat) + ff_U
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(shooter.X[0, 0])
+ ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+ U = controller_shooter.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shooter.X[0, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(shooter.X[1, 0])
- self.a.append((self.v[-1] - last_v) / shooter.dt)
+ self.v.append(shooter.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / shooter.dt)
- if observer_shooter is not None:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
- self.offset.append(observer_shooter.X_hat[2, 0])
+ if observer_shooter is not None:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
+ self.offset.append(observer_shooter.X_hat[2, 0])
- applied_U = U.copy()
- if i > 30:
- applied_U += 2
- shooter.Update(applied_U)
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ shooter.Update(applied_U)
- if observer_shooter is not None:
- observer_shooter.PredictObserver(U)
+ if observer_shooter is not None:
+ observer_shooter.PredictObserver(U)
- self.t.append(initial_t + i * shooter.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * shooter.dt)
+ self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
+ glog.debug('Time: %f', self.t[-1])
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.v, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter = IntegralShooter()
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter, iterations=200)
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(
+ shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2016', 'control_loops', 'shooter']
- shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
- namespaces=namespaces)
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2016', 'control_loops', 'shooter']
+ shooter = Shooter('Shooter')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_shooter = IntegralShooter('IntegralShooter')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_shooter = IntegralShooter('IntegralShooter')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
index 1f8bd76..70cd649 100755
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -14,383 +14,413 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
class ColumnController(control_loop.ControlLoop):
- def __init__(self, name='Column'):
- super(ColumnController, self).__init__(name)
- self.turret = turret.Turret(name + 'Turret')
- self.indexer = indexer.Indexer(name + 'Indexer')
- # Control loop time step
- self.dt = 0.005
+ def __init__(self, name='Column'):
+ super(ColumnController, self).__init__(name)
+ self.turret = turret.Turret(name + 'Turret')
+ self.indexer = indexer.Indexer(name + 'Indexer')
- # State is [position_indexer,
- # velocity_indexer,
- # position_shooter,
- # velocity_shooter]
- # Input is [volts_indexer, volts_shooter]
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
+ # Control loop time step
+ self.dt = 0.005
- self.A_continuous[0, 0] = -(self.indexer.Kt / self.indexer.Kv / (self.indexer.J * self.indexer.resistance * self.indexer.G * self.indexer.G) +
- self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G))
- self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.B_continuous[0, 0] = self.indexer.Kt / (self.indexer.J * self.indexer.resistance * self.indexer.G)
- self.B_continuous[0, 1] = -self.turret.Kt / (self.indexer.J * self.turret.resistance * self.turret.G)
+ # State is [position_indexer,
+ # velocity_indexer,
+ # position_shooter,
+ # velocity_shooter]
+ # Input is [volts_indexer, volts_shooter]
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
- self.A_continuous[1, 2] = 1
+ self.A_continuous[0, 0] = -(
+ self.indexer.Kt / self.indexer.Kv /
+ (self.indexer.J * self.indexer.resistance * self.indexer.G *
+ self.indexer.G) + self.turret.Kt / self.turret.Kv /
+ (self.indexer.J * self.turret.resistance * self.turret.G *
+ self.turret.G))
+ self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (
+ self.indexer.J * self.turret.resistance * self.turret.G *
+ self.turret.G)
+ self.B_continuous[0, 0] = self.indexer.Kt / (
+ self.indexer.J * self.indexer.resistance * self.indexer.G)
+ self.B_continuous[0, 1] = -self.turret.Kt / (
+ self.indexer.J * self.turret.resistance * self.turret.G)
- self.A_continuous[2, 0] = self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.A_continuous[2, 2] = -self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.A_continuous[1, 2] = 1
- self.B_continuous[2, 1] = self.turret.Kt / (self.turret.J * self.turret.resistance * self.turret.G)
+ self.A_continuous[2, 0] = self.turret.Kt / self.turret.Kv / (
+ self.turret.J * self.turret.resistance * self.turret.G *
+ self.turret.G)
+ self.A_continuous[2, 2] = -self.turret.Kt / self.turret.Kv / (
+ self.turret.J * self.turret.resistance * self.turret.G *
+ self.turret.G)
- self.C = numpy.matrix([[1, 0, 0], [0, 1, 0]])
- self.D = numpy.matrix([[0, 0], [0, 0]])
+ self.B_continuous[2, 1] = self.turret.Kt / (
+ self.turret.J * self.turret.resistance * self.turret.G)
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0, 0], [0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- q_indexer_vel = 13.0
- q_pos = 0.05
- q_vel = 0.8
- self.Q = numpy.matrix([[(1.0 / (q_indexer_vel ** 2.0)), 0.0, 0.0],
- [0.0, (1.0 / (q_pos ** 2.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)
+ q_indexer_vel = 13.0
+ q_pos = 0.05
+ q_vel = 0.8
+ self.Q = numpy.matrix([[(1.0 / (q_indexer_vel**2.0)), 0.0, 0.0],
+ [0.0, (1.0 / (q_pos**2.0)), 0.0],
+ [0.0, 0.0, (1.0 / (q_vel**2.0))]])
- glog.debug('Controller poles are ' + repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ 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)
- q_vel_indexer_ff = 0.000005
- q_pos_ff = 0.0000005
- q_vel_ff = 0.00008
- self.Qff = numpy.matrix([[(1.0 / (q_vel_indexer_ff ** 2.0)), 0.0, 0.0],
- [0.0, (1.0 / (q_pos_ff ** 2.0)), 0.0],
- [0.0, 0.0, (1.0 / (q_vel_ff ** 2.0))]])
+ glog.debug('Controller poles are ' +
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ q_vel_indexer_ff = 0.000005
+ q_pos_ff = 0.0000005
+ q_vel_ff = 0.00008
+ self.Qff = numpy.matrix([[(1.0 / (q_vel_indexer_ff**2.0)), 0.0, 0.0],
+ [0.0, (1.0 / (q_pos_ff**2.0)), 0.0],
+ [0.0, 0.0, (1.0 / (q_vel_ff**2.0))]])
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
class Column(ColumnController):
- def __init__(self, name='Column', disable_indexer=False):
- super(Column, self).__init__(name)
- A_continuous = numpy.matrix(numpy.zeros((4, 4)))
- B_continuous = numpy.matrix(numpy.zeros((4, 2)))
- A_continuous[0, 1] = 1
- A_continuous[1:, 1:] = self.A_continuous
- B_continuous[1:, :] = self.B_continuous
+ def __init__(self, name='Column', disable_indexer=False):
+ super(Column, self).__init__(name)
+ A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ B_continuous = numpy.matrix(numpy.zeros((4, 2)))
- self.A_continuous = A_continuous
- self.B_continuous = B_continuous
+ A_continuous[0, 1] = 1
+ A_continuous[1:, 1:] = self.A_continuous
+ B_continuous[1:, :] = self.B_continuous
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.A_continuous = A_continuous
+ self.B_continuous = B_continuous
- self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
- self.D = numpy.matrix([[0, 0], [0, 0]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- orig_K = self.K
- self.K = numpy.matrix(numpy.zeros((2, 4)))
- self.K[:, 1:] = orig_K
+ self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- glog.debug('K is ' + repr(self.K))
- # TODO(austin): Do we want to damp velocity out or not when disabled?
- #if disable_indexer:
- # self.K[0, 1] = 0.0
- # self.K[1, 1] = 0.0
+ orig_K = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 4)))
+ self.K[:, 1:] = orig_K
- orig_Kff = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((2, 4)))
- self.Kff[:, 1:] = orig_Kff
+ glog.debug('K is ' + repr(self.K))
+ # TODO(austin): Do we want to damp velocity out or not when disabled?
+ #if disable_indexer:
+ # self.K[0, 1] = 0.0
+ # self.K[1, 1] = 0.0
- q_pos = 0.12
- q_vel = 2.00
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0],
- [0.0, 0.0, (q_pos ** 2.0), 0.0],
- [0.0, 0.0, 0.0, (q_vel ** 2.0)]])
+ orig_Kff = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((2, 4)))
+ self.Kff[:, 1:] = orig_Kff
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
- [0.0, (r_pos ** 2.0)]])
+ q_pos = 0.12
+ q_vel = 2.00
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0],
+ [0.0, 0.0, (q_pos**2.0), 0.0],
+ [0.0, 0.0, 0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0), 0.0], [0.0, (r_pos**2.0)]])
- self.InitializeState()
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.InitializeState()
class IntegralColumn(Column):
- def __init__(self, name='IntegralColumn', voltage_error_noise=None,
- disable_indexer=False):
- super(IntegralColumn, self).__init__(name)
- A_continuous = numpy.matrix(numpy.zeros((6, 6)))
- A_continuous[0:4, 0:4] = self.A_continuous
- A_continuous[0:4:, 4:6] = self.B_continuous
+ def __init__(self,
+ name='IntegralColumn',
+ voltage_error_noise=None,
+ disable_indexer=False):
+ super(IntegralColumn, self).__init__(name)
- B_continuous = numpy.matrix(numpy.zeros((6, 2)))
- B_continuous[0:4, :] = self.B_continuous
+ A_continuous = numpy.matrix(numpy.zeros((6, 6)))
+ A_continuous[0:4, 0:4] = self.A_continuous
+ A_continuous[0:4:, 4:6] = self.B_continuous
- self.A_continuous = A_continuous
- self.B_continuous = B_continuous
+ B_continuous = numpy.matrix(numpy.zeros((6, 2)))
+ B_continuous[0:4, :] = self.B_continuous
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.A_continuous = A_continuous
+ self.B_continuous = B_continuous
- C = numpy.matrix(numpy.zeros((2, 6)))
- C[0:2, 0:4] = self.C
- self.C = C
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.D = numpy.matrix([[0, 0], [0, 0]])
+ C = numpy.matrix(numpy.zeros((2, 6)))
+ C[0:2, 0:4] = self.C
+ self.C = C
- orig_K = self.K
- self.K = numpy.matrix(numpy.zeros((2, 6)))
- self.K[:, 0:4] = orig_K
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- # TODO(austin): I'm not certain this is ideal. If someone spins the bottom
- # at a constant rate, we'll learn a voltage offset. That should translate
- # directly to a voltage on the turret to hold it steady. I'm also not
- # convinced we care that much. If the indexer is off, it'll stop rather
- # quickly anyways, so this is mostly a moot point.
- if not disable_indexer:
- self.K[0, 4] = 1
- self.K[1, 5] = 1
+ orig_K = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 6)))
+ self.K[:, 0:4] = orig_K
- orig_Kff = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((2, 6)))
- self.Kff[:, 0:4] = orig_Kff
+ # TODO(austin): I'm not certain this is ideal. If someone spins the bottom
+ # at a constant rate, we'll learn a voltage offset. That should translate
+ # directly to a voltage on the turret to hold it steady. I'm also not
+ # convinced we care that much. If the indexer is off, it'll stop rather
+ # quickly anyways, so this is mostly a moot point.
+ if not disable_indexer:
+ self.K[0, 4] = 1
+ self.K[1, 5] = 1
- q_pos = 0.40
- q_vel = 2.00
- q_voltage = 8.0
- if voltage_error_noise is not None:
- q_voltage = voltage_error_noise
+ orig_Kff = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((2, 6)))
+ self.Kff[:, 0:4] = orig_Kff
- self.Q = numpy.matrix([[(q_pos ** 2.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, (q_pos ** 2.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, (q_voltage ** 2.0), 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+ q_pos = 0.40
+ q_vel = 2.00
+ q_voltage = 8.0
+ if voltage_error_noise is not None:
+ q_voltage = voltage_error_noise
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
- [0.0, (r_pos ** 2.0)]])
+ self.Q = numpy.matrix([[(q_pos**2.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, (q_pos**2.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, (q_voltage**2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0), 0.0], [0.0, (r_pos**2.0)]])
- self.InitializeState()
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.xi = []
- self.xt = []
- self.vi = []
- self.vt = []
- self.ai = []
- self.at = []
- self.x_hat = []
- self.ui = []
- self.ut = []
- self.ui_fb = []
- self.ut_fb = []
- self.offseti = []
- self.offsett = []
- self.turret_error = []
- def run_test(self, column, end_goal,
- controller_column,
- observer_column=None,
- iterations=200):
- """Runs the column plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.xi = []
+ self.xt = []
+ self.vi = []
+ self.vt = []
+ self.ai = []
+ self.at = []
+ self.x_hat = []
+ self.ui = []
+ self.ut = []
+ self.ui_fb = []
+ self.ut_fb = []
+ self.offseti = []
+ self.offsett = []
+ self.turret_error = []
- Args:
- column: column object to use.
- end_goal: end_goal state.
- controller_column: Intake object to get K from, or None if we should
- use column.
- observer_column: Intake object to use for the observer, or None if we should
- use the actual state.
- iterations: Number of timesteps to run the model for.
- """
+ def run_test(self,
+ column,
+ end_goal,
+ controller_column,
+ observer_column=None,
+ iterations=200):
+ """Runs the column plant with an initial condition and goal.
- if controller_column is None:
- controller_column = column
+ Args:
+ column: column object to use.
+ end_goal: end_goal state.
+ controller_column: Intake object to get K from, or None if we should
+ use column.
+ observer_column: Intake object to use for the observer, or None if we should
+ use the actual state.
+ iterations: Number of timesteps to run the model for.
+ """
- vbat = 12.0
+ if controller_column is None:
+ controller_column = column
- if self.t:
- initial_t = self.t[-1] + column.dt
- else:
- initial_t = 0
+ vbat = 12.0
- goal = numpy.concatenate((column.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
+ if self.t:
+ initial_t = self.t[-1] + column.dt
+ else:
+ initial_t = 0
- profile = TrapezoidProfile(column.dt)
- profile.set_maximum_acceleration(10.0)
- profile.set_maximum_velocity(3.0)
- profile.SetGoal(goal[2, 0])
+ goal = numpy.concatenate((column.X, numpy.matrix(numpy.zeros((2, 1)))),
+ axis=0)
- U_last = numpy.matrix(numpy.zeros((2, 1)))
- for i in xrange(iterations):
- observer_column.Y = column.Y
- observer_column.CorrectObserver(U_last)
+ profile = TrapezoidProfile(column.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(3.0)
+ profile.SetGoal(goal[2, 0])
- self.offseti.append(observer_column.X_hat[4, 0])
- self.offsett.append(observer_column.X_hat[5, 0])
- self.x_hat.append(observer_column.X_hat[0, 0])
+ U_last = numpy.matrix(numpy.zeros((2, 1)))
+ for i in xrange(iterations):
+ observer_column.Y = column.Y
+ observer_column.CorrectObserver(U_last)
- next_goal = numpy.concatenate(
- (end_goal[0:2, :],
- profile.Update(end_goal[2, 0], end_goal[3, 0]),
- end_goal[4:6, :]),
- axis=0)
+ self.offseti.append(observer_column.X_hat[4, 0])
+ self.offsett.append(observer_column.X_hat[5, 0])
+ self.x_hat.append(observer_column.X_hat[0, 0])
- ff_U = controller_column.Kff * (next_goal - observer_column.A * goal)
- fb_U = controller_column.K * (goal - observer_column.X_hat)
- self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
- self.ui_fb.append(fb_U[0, 0])
- self.ut_fb.append(fb_U[1, 0])
+ next_goal = numpy.concatenate(
+ (end_goal[0:2, :], profile.Update(
+ end_goal[2, 0], end_goal[3, 0]), end_goal[4:6, :]),
+ axis=0)
- U_uncapped = ff_U + fb_U
+ ff_U = controller_column.Kff * (
+ next_goal - observer_column.A * goal)
+ fb_U = controller_column.K * (goal - observer_column.X_hat)
+ self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
+ self.ui_fb.append(fb_U[0, 0])
+ self.ut_fb.append(fb_U[1, 0])
- U = U_uncapped.copy()
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
- self.xi.append(column.X[0, 0])
- self.xt.append(column.X[2, 0])
+ U_uncapped = ff_U + fb_U
- if self.vi:
- last_vi = self.vi[-1]
- else:
- last_vi = 0
- if self.vt:
- last_vt = self.vt[-1]
- else:
- last_vt = 0
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
+ self.xi.append(column.X[0, 0])
+ self.xt.append(column.X[2, 0])
- self.vi.append(column.X[1, 0])
- self.vt.append(column.X[3, 0])
- self.ai.append((self.vi[-1] - last_vi) / column.dt)
- self.at.append((self.vt[-1] - last_vt) / column.dt)
+ if self.vi:
+ last_vi = self.vi[-1]
+ else:
+ last_vi = 0
+ if self.vt:
+ last_vt = self.vt[-1]
+ else:
+ last_vt = 0
- offset = 0.0
- if i > 100:
- offset = 1.0
- column.Update(U + numpy.matrix([[0.0], [offset]]))
+ self.vi.append(column.X[1, 0])
+ self.vt.append(column.X[3, 0])
+ self.ai.append((self.vi[-1] - last_vi) / column.dt)
+ self.at.append((self.vt[-1] - last_vt) / column.dt)
- observer_column.PredictObserver(U)
+ offset = 0.0
+ if i > 100:
+ offset = 1.0
+ column.Update(U + numpy.matrix([[0.0], [offset]]))
- self.t.append(initial_t + i * column.dt)
- self.ui.append(U[0, 0])
- self.ut.append(U[1, 0])
+ observer_column.PredictObserver(U)
- ff_U -= U_uncapped - U
- goal = controller_column.A * goal + controller_column.B * ff_U
+ self.t.append(initial_t + i * column.dt)
+ self.ui.append(U[0, 0])
+ self.ut.append(U[1, 0])
- if U[1, 0] != U_uncapped[1, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
+ ff_U -= U_uncapped - U
+ goal = controller_column.A * goal + controller_column.B * ff_U
- glog.debug('Time: %f', self.t[-1])
- glog.debug('goal_error %s', repr((end_goal - goal).T))
- glog.debug('error %s', repr((observer_column.X_hat - end_goal).T))
+ if U[1, 0] != U_uncapped[1, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.xi, label='x_indexer')
- pylab.plot(self.t, self.xt, label='x_turret')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.plot(self.t, self.turret_error, label='turret_error * 100')
- pylab.legend()
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr((end_goal - goal).T))
+ glog.debug('error %s', repr((observer_column.X_hat - end_goal).T))
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.ui, label='u_indexer')
- pylab.plot(self.t, self.ui_fb, label='u_indexer_fb')
- pylab.plot(self.t, self.ut, label='u_turret')
- pylab.plot(self.t, self.ut_fb, label='u_turret_fb')
- pylab.plot(self.t, self.offseti, label='voltage_offset_indexer')
- pylab.plot(self.t, self.offsett, label='voltage_offset_turret')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.xi, label='x_indexer')
+ pylab.plot(self.t, self.xt, label='x_turret')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.plot(self.t, self.turret_error, label='turret_error * 100')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.ai, label='a_indexer')
- pylab.plot(self.t, self.at, label='a_turret')
- pylab.plot(self.t, self.vi, label='v_indexer')
- pylab.plot(self.t, self.vt, label='v_turret')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.ui, label='u_indexer')
+ pylab.plot(self.t, self.ui_fb, label='u_indexer_fb')
+ pylab.plot(self.t, self.ut, label='u_turret')
+ pylab.plot(self.t, self.ut_fb, label='u_turret_fb')
+ pylab.plot(self.t, self.offseti, label='voltage_offset_indexer')
+ pylab.plot(self.t, self.offsett, label='voltage_offset_turret')
+ pylab.legend()
- pylab.show()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.ai, label='a_indexer')
+ pylab.plot(self.t, self.at, label='a_turret')
+ pylab.plot(self.t, self.vi, label='v_indexer')
+ pylab.plot(self.t, self.vt, label='v_turret')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- column = Column()
- column_controller = IntegralColumn()
- observer_column = IntegralColumn()
+ column = Column()
+ column_controller = IntegralColumn()
+ observer_column = IntegralColumn()
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
- scenario_plotter.run_test(column, end_goal=R, controller_column=column_controller,
- observer_column=observer_column, iterations=400)
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
+ scenario_plotter.run_test(
+ column,
+ end_goal=R,
+ controller_column=column_controller,
+ observer_column=observer_column,
+ iterations=400)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- if len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file names')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
- column = Column('Column')
- loop_writer = control_loop.ControlLoopWriter('Column', [column],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kIndexerFreeSpeed', '%f', column.indexer.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kIndexerOutputRatio', '%f', column.indexer.G))
- loop_writer.AddConstant(control_loop.Constant(
- 'kTurretFreeSpeed', '%f', column.turret.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kTurretOutputRatio', '%f', column.turret.G))
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file names')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
+ column = Column('Column')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Column', [column], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kIndexerFreeSpeed', '%f',
+ column.indexer.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kIndexerOutputRatio', '%f',
+ column.indexer.G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kTurretFreeSpeed', '%f',
+ column.turret.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kTurretOutputRatio', '%f', column.turret.G))
+ loop_writer.Write(argv[1], argv[2])
- # IntegralColumn controller 1 will disable the indexer.
- integral_column = IntegralColumn('IntegralColumn')
- disabled_integral_column = IntegralColumn('DisabledIntegralColumn',
- disable_indexer=True)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralColumn', [integral_column, disabled_integral_column],
- namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ # IntegralColumn controller 1 will disable the indexer.
+ integral_column = IntegralColumn('IntegralColumn')
+ disabled_integral_column = IntegralColumn(
+ 'DisabledIntegralColumn', disable_indexer=True)
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralColumn', [integral_column, disabled_integral_column],
+ namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_column = IntegralColumn('StuckIntegralColumn', voltage_error_noise=8.0)
- stuck_integral_loop_writer = control_loop.ControlLoopWriter(
- 'StuckIntegralColumn', [stuck_integral_column], namespaces=namespaces)
- stuck_integral_loop_writer.Write(argv[5], argv[6])
+ stuck_integral_column = IntegralColumn(
+ 'StuckIntegralColumn', voltage_error_noise=8.0)
+ stuck_integral_loop_writer = control_loop.ControlLoopWriter(
+ 'StuckIntegralColumn', [stuck_integral_column],
+ namespaces=namespaces)
+ stuck_integral_loop_writer.Write(argv[5], argv[6])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index fb5aa4e..58bd15e 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -12,329 +12,339 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Hood(control_loop.ControlLoop):
- def __init__(self, name='Hood'):
- super(Hood, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.43
- # Stall Current in Amps
- self.stall_current = 53.0
- self.free_speed_rpm = 13180.0
- # Free Speed in rotations/second.
- self.free_speed = self.free_speed_rpm / 60
- # Free Current in Amps
- self.free_current = 1.8
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # First axle gear ratio off the motor
- self.G1 = (12.0 / 60.0)
- # Second axle gear ratio off the motor
- self.G2 = self.G1 * (14.0 / 36.0)
- # Third axle gear ratio off the motor
- self.G3 = self.G2 * (14.0 / 36.0)
- # The last gear reduction (encoder -> hood angle)
- self.last_G = (20.0 / 345.0)
- # Gear ratio
- self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
+ def __init__(self, name='Hood'):
+ super(Hood, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.43
+ # Stall Current in Amps
+ self.stall_current = 53.0
+ self.free_speed_rpm = 13180.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60
+ # Free Current in Amps
+ self.free_current = 1.8
- # 36 tooth gear inertia in kg * m^2
- self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025) ** 2)
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # First axle gear ratio off the motor
+ self.G1 = (12.0 / 60.0)
+ # Second axle gear ratio off the motor
+ self.G2 = self.G1 * (14.0 / 36.0)
+ # Third axle gear ratio off the motor
+ self.G3 = self.G2 * (14.0 / 36.0)
+ # The last gear reduction (encoder -> hood angle)
+ self.last_G = (20.0 / 345.0)
+ # Gear ratio
+ self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
- # Motor inertia in kg * m^2
- self.motor_inertia = 0.000006
- glog.debug(self.big_gear_inertia)
+ # 36 tooth gear inertia in kg * m^2
+ self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025)**2)
- # Moment of inertia, measured in CAD.
- # Extra mass to compensate for friction is added on.
- self.J = 0.08 + 2.3 + \
- self.big_gear_inertia * ((self.G1 / self.G) ** 2) + \
- self.big_gear_inertia * ((self.G2 / self.G) ** 2) + \
- self.motor_inertia * ((1.0 / self.G) ** 2.0)
- glog.debug('J effective %f', self.J)
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.000006
+ glog.debug(self.big_gear_inertia)
- # Control loop time step
- self.dt = 0.005
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.08 + 2.3 + \
+ self.big_gear_inertia * ((self.G1 / self.G) ** 2) + \
+ self.big_gear_inertia * ((self.G2 / self.G) ** 2) + \
+ self.motor_inertia * ((1.0 / self.G) ** 2.0)
+ glog.debug('J effective %f', self.J)
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
- C2 = self.Kt / (self.J * self.R * self.G)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.R * self.G)
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0],
- [C2]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix([[0], [C2]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- controllability = controls.ctrb(self.A, self.B)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- glog.debug('Free speed is %f',
- -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
- glog.debug(repr(self.A_continuous))
+ controllability = controls.ctrb(self.A, self.B)
- # Calculate the LQR controller gain
- q_pos = 0.015
- q_vel = 0.40
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel ** 2.0))]])
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+ glog.debug(repr(self.A_continuous))
- self.R = numpy.matrix([[(5.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ # Calculate the LQR controller gain
+ q_pos = 0.015
+ q_vel = 0.40
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel**2.0))]])
- # Calculate the feed forwards gain.
- q_pos_ff = 0.005
- q_vel_ff = 1.0
- self.Qff = numpy.matrix([[(1.0 / (q_pos_ff ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel_ff ** 2.0))]])
+ self.R = numpy.matrix([[(5.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ # Calculate the feed forwards gain.
+ q_pos_ff = 0.005
+ q_vel_ff = 1.0
+ self.Qff = numpy.matrix([[(1.0 / (q_pos_ff**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel_ff**2.0))]])
- glog.debug('K %s', repr(self.K))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- q_pos = 0.10
- q_vel = 1.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.10
+ q_vel = 1.65
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0], [0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- glog.debug('Kal %s', repr(self.KalmanGain))
- self.L = self.A * self.KalmanGain
- glog.debug('KalL is %s', repr(self.L))
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ glog.debug('Kal %s', repr(self.KalmanGain))
+ self.L = self.A * self.KalmanGain
+ glog.debug('KalL is %s', repr(self.L))
- self.InitializeState()
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
class IntegralHood(Hood):
- def __init__(self, name='IntegralHood'):
- super(IntegralHood, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='IntegralHood'):
+ super(IntegralHood, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.01
- q_vel = 4.0
- q_voltage = 4.0
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.01
+ q_vel = 4.0
+ q_voltage = 4.0
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.001
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.Kff = numpy.concatenate((self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
- self.InitializeState()
+ self.Kff = numpy.concatenate(
+ (self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+
+ self.InitializeState()
+
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.v_hat = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, hood, end_goal,
- controller_hood,
- observer_hood=None,
- iterations=200):
- """Runs the hood plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.v_hat = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
- Args:
- hood: hood object to use.
- end_goal: end_goal state.
- controller_hood: Hood object to get K from, or None if we should
- use hood.
- observer_hood: Hood object to use for the observer, or None if we should
- use the actual state.
- iterations: Number of timesteps to run the model for.
- """
+ def run_test(self,
+ hood,
+ end_goal,
+ controller_hood,
+ observer_hood=None,
+ iterations=200):
+ """Runs the hood plant with an initial condition and goal.
- if controller_hood is None:
- controller_hood = hood
+ Args:
+ hood: hood object to use.
+ end_goal: end_goal state.
+ controller_hood: Hood object to get K from, or None if we should
+ use hood.
+ observer_hood: Hood object to use for the observer, or None if we should
+ use the actual state.
+ iterations: Number of timesteps to run the model for.
+ """
- vbat = 12.0
+ if controller_hood is None:
+ controller_hood = hood
- if self.t:
- initial_t = self.t[-1] + hood.dt
- else:
- initial_t = 0
+ vbat = 12.0
- goal = numpy.concatenate((hood.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ if self.t:
+ initial_t = self.t[-1] + hood.dt
+ else:
+ initial_t = 0
- profile = TrapezoidProfile(hood.dt)
- profile.set_maximum_acceleration(10.0)
- profile.set_maximum_velocity(1.0)
- profile.SetGoal(goal[0, 0])
+ goal = numpy.concatenate((hood.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- U_last = numpy.matrix(numpy.zeros((1, 1)))
- for i in xrange(iterations):
- observer_hood.Y = hood.Y
- observer_hood.CorrectObserver(U_last)
+ profile = TrapezoidProfile(hood.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(1.0)
+ profile.SetGoal(goal[0, 0])
- self.offset.append(observer_hood.X_hat[2, 0])
- self.x_hat.append(observer_hood.X_hat[0, 0])
+ U_last = numpy.matrix(numpy.zeros((1, 1)))
+ for i in xrange(iterations):
+ observer_hood.Y = hood.Y
+ observer_hood.CorrectObserver(U_last)
- next_goal = numpy.concatenate(
- (profile.Update(end_goal[0, 0], end_goal[1, 0]),
- numpy.matrix(numpy.zeros((1, 1)))),
- axis=0)
+ self.offset.append(observer_hood.X_hat[2, 0])
+ self.x_hat.append(observer_hood.X_hat[0, 0])
- ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
+ next_goal = numpy.concatenate(
+ (profile.Update(end_goal[0, 0], end_goal[1, 0]),
+ numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- U_uncapped = controller_hood.K * (goal - observer_hood.X_hat) + ff_U
- U = U_uncapped.copy()
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(hood.X[0, 0])
+ ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ U_uncapped = controller_hood.K * (goal - observer_hood.X_hat) + ff_U
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(hood.X[0, 0])
- self.v.append(hood.X[1, 0])
- self.a.append((self.v[-1] - last_v) / hood.dt)
- self.v_hat.append(observer_hood.X_hat[1, 0])
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- offset = 0.0
- if i > 100:
- offset = 2.0
- hood.Update(U + offset)
+ self.v.append(hood.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / hood.dt)
+ self.v_hat.append(observer_hood.X_hat[1, 0])
- observer_hood.PredictObserver(U)
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ hood.Update(U + offset)
- self.t.append(initial_t + i * hood.dt)
- self.u.append(U[0, 0])
+ observer_hood.PredictObserver(U)
- ff_U -= U_uncapped - U
- goal = controller_hood.A * goal + controller_hood.B * ff_U
+ self.t.append(initial_t + i * hood.dt)
+ self.u.append(U[0, 0])
- if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ ff_U -= U_uncapped - U
+ goal = controller_hood.A * goal + controller_hood.B * ff_U
- glog.debug('Time: %f', self.t[-1])
- glog.debug('goal_error %s', repr(end_goal - goal))
- glog.debug('error %s', repr(observer_hood.X_hat - end_goal))
+ if U[0, 0] != U_uncapped[0, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.plot(self.t, self.v, label='v')
- pylab.plot(self.t, self.v_hat, label='v_hat')
- pylab.legend()
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_hood.X_hat - end_goal))
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.plot(self.t, self.v, label='v')
+ pylab.plot(self.t, self.v_hat, label='v_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.show()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- hood = Hood()
- hood_controller = IntegralHood()
- observer_hood = IntegralHood()
+ hood = Hood()
+ hood_controller = IntegralHood()
+ observer_hood = IntegralHood()
- # Test moving the hood with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[numpy.pi/4.0], [0.0], [0.0]])
- scenario_plotter.run_test(hood, end_goal=R,
- controller_hood=hood_controller,
- observer_hood=observer_hood, iterations=200)
+ # Test moving the hood with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 4.0], [0.0], [0.0]])
+ scenario_plotter.run_test(
+ hood,
+ end_goal=R,
+ controller_hood=hood_controller,
+ observer_hood=observer_hood,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the hood and integral hood.')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'hood']
- hood = Hood('Hood')
- loop_writer = control_loop.ControlLoopWriter('Hood', [hood],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', hood.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', hood.G))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the hood and integral hood.'
+ )
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'hood']
+ hood = Hood('Hood')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Hood', [hood], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', hood.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', hood.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_hood = IntegralHood('IntegralHood')
- integral_loop_writer = control_loop.ControlLoopWriter('IntegralHood', [integral_hood],
- namespaces=namespaces)
- integral_loop_writer.AddConstant(control_loop.Constant('kLastReduction', '%f',
- integral_hood.last_G))
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_hood = IntegralHood('IntegralHood')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralHood', [integral_hood], namespaces=namespaces)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant('kLastReduction', '%f', integral_hood.last_G))
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 1b0ff13..a825ff0 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -16,370 +16,388 @@
def PlotDiff(list1, list2, time):
- pylab.subplot(1, 1, 1)
- pylab.plot(time, numpy.subtract(list1, list2), label='diff')
- pylab.legend()
+ pylab.subplot(1, 1, 1)
+ pylab.plot(time, numpy.subtract(list1, list2), label='diff')
+ pylab.legend()
+
class VelocityShooter(control_loop.HybridControlLoop):
- def __init__(self, name='VelocityShooter'):
- super(VelocityShooter, self).__init__(name)
- # Number of motors
- self.num_motors = 2.0
- # Stall Torque in N m
- self.stall_torque = 0.71 * self.num_motors
- # Stall Current in Amps
- self.stall_current = 134.0 * self.num_motors
- # Free Speed in RPM
- self.free_speed_rpm = 18730.0
- # Free Speed in rotations/second.
- self.free_speed = self.free_speed_rpm / 60.0
- # Free Current in Amps
- self.free_current = 0.7 * self.num_motors
- # Moment of inertia of the shooter wheel in kg m^2
- # 1400.6 grams/cm^2
- # 1.407 *1e-4 kg m^2
- self.J = 0.00120
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 12.0 / 36.0
- # Control loop time step
- self.dt = 0.00505
- # State feedback matrices
- # [angular velocity]
- self.A_continuous = numpy.matrix(
- [[-self.Kt / (self.Kv * self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ def __init__(self, name='VelocityShooter'):
+ super(VelocityShooter, self).__init__(name)
+ # Number of motors
+ self.num_motors = 2.0
+ # Stall Torque in N m
+ self.stall_torque = 0.71 * self.num_motors
+ # Stall Current in Amps
+ self.stall_current = 134.0 * self.num_motors
+ # Free Speed in RPM
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
+ # Free Current in Amps
+ self.free_current = 0.7 * self.num_motors
+ # Moment of inertia of the shooter wheel in kg m^2
+ # 1400.6 grams/cm^2
+ # 1.407 *1e-4 kg m^2
+ self.J = 0.00120
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 12.0 / 36.0
+ # Control loop time step
+ self.dt = 0.00505
- # The states are [unfiltered_velocity]
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / (self.Kv * self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.75])
+ # The states are [unfiltered_velocity]
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- glog.debug('K %s', repr(self.K))
- glog.debug('System poles are %s',
- repr(numpy.linalg.eig(self.A_continuous)[0]))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.PlaceControllerPoles([.75])
- self.PlaceObserverPoles([0.3])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('System poles are %s',
+ repr(numpy.linalg.eig(self.A_continuous)[0]))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
+
class SecondOrderVelocityShooter(VelocityShooter):
- def __init__(self, name='SecondOrderVelocityShooter'):
- super(SecondOrderVelocityShooter, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='SecondOrderVelocityShooter'):
+ super(SecondOrderVelocityShooter, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[0:1, 0:1] = self.A_continuous_unaugmented
- self.A_continuous[1, 0] = 175.0
- self.A_continuous[1, 1] = -self.A_continuous[1, 0]
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[0:1, 0:1] = self.A_continuous_unaugmented
+ self.A_continuous[1, 0] = 175.0
+ self.A_continuous[1, 1] = -self.A_continuous[1, 0]
- self.C = numpy.matrix([[0, 1]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
- # The states are [unfiltered_velocity, velocity]
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[0, 1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.70, 0.60])
+ # The states are [unfiltered_velocity, velocity]
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- q_vel = 40.0
- q_filteredvel = 30.0
- self.Q = numpy.matrix([[(1.0 / (q_vel ** 2.0)), 0.0],
- [0.0, (1.0 / (q_filteredvel ** 2.0))]])
+ self.PlaceControllerPoles([.70, 0.60])
- self.R = numpy.matrix([[(1.0 / (3.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ q_vel = 40.0
+ q_filteredvel = 30.0
+ self.Q = numpy.matrix([[(1.0 / (q_vel**2.0)), 0.0],
+ [0.0, (1.0 / (q_filteredvel**2.0))]])
- glog.debug('K %s', repr(self.K))
- glog.debug('System poles are %s',
- repr(numpy.linalg.eig(self.A_continuous)[0]))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.R = numpy.matrix([[(1.0 / (3.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.PlaceObserverPoles([0.3, 0.3])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('System poles are %s',
+ repr(numpy.linalg.eig(self.A_continuous)[0]))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3, 0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0), 0.0],
- [0.0, 1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0), 0.0],
+ [0.0, 1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
class Shooter(SecondOrderVelocityShooter):
- def __init__(self, name='Shooter'):
- super(Shooter, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='Shooter'):
+ super(Shooter, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[1:3, 1:3] = self.A_continuous_unaugmented
- self.A_continuous[0, 2] = 1
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[1:3, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[1:3, 1:3] = self.A_continuous_unaugmented
+ self.A_continuous[0, 2] = 1
- # State feedback matrices
- # [position, unfiltered_velocity, angular velocity]
- self.C = numpy.matrix([[1, 0, 0]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[1:3, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
- glog.debug(repr(self.A_continuous))
- glog.debug(repr(self.B_continuous))
+ # State feedback matrices
+ # [position, unfiltered_velocity, angular velocity]
+ self.C = numpy.matrix([[1, 0, 0]])
+ self.D = numpy.matrix([[0]])
- observeability = controls.ctrb(self.A.T, self.C.T)
- glog.debug('Rank of augmented observability matrix. %d', numpy.linalg.matrix_rank(
- observeability))
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+ glog.debug(repr(self.A_continuous))
+ glog.debug(repr(self.B_continuous))
+ observeability = controls.ctrb(self.A.T, self.C.T)
+ glog.debug('Rank of augmented observability matrix. %d',
+ numpy.linalg.matrix_rank(observeability))
- self.PlaceObserverPoles([0.9, 0.8, 0.7])
+ self.PlaceObserverPoles([0.9, 0.8, 0.7])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 1:3] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 1:3] = self.Kff_unaugmented
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 1:3] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 1:3] = self.Kff_unaugmented
- self.InitializeState()
+ self.InitializeState()
class IntegralShooter(Shooter):
- def __init__(self, name='IntegralShooter'):
- super(IntegralShooter, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='IntegralShooter'):
+ super(IntegralShooter, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
- self.A_continuous[0:3, 0:3] = self.A_continuous_unaugmented
- self.A_continuous[0:3, 3] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((4, 1)))
- self.B_continuous[0:3, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ self.A_continuous[0:3, 0:3] = self.A_continuous_unaugmented
+ self.A_continuous[0:3, 3] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 4)))
- self.C[0:1, 0:3] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((4, 1)))
+ self.B_continuous[0:3, 0] = self.B_continuous_unaugmented
- # The states are [position, unfiltered_velocity, velocity, torque_error]
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 4)))
+ self.C[0:1, 0:3] = self.C_unaugmented
- glog.debug('A: \n%s', repr(self.A_continuous))
- glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
- glog.debug('schur(A): \n%s', repr(scipy.linalg.schur(self.A_continuous)))
- glog.debug('A_dt(A): \n%s', repr(self.A))
+ # The states are [position, unfiltered_velocity, velocity, torque_error]
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- q_pos = 0.01
- q_vel = 5.0
- q_velfilt = 1.5
- q_voltage = 2.0
- self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0],
- [0.0, 0.0, (q_velfilt ** 2.0), 0.0],
- [0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+ glog.debug('A: \n%s', repr(self.A_continuous))
+ glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
+ glog.debug('schur(A): \n%s', repr(
+ scipy.linalg.schur(self.A_continuous)))
+ glog.debug('A_dt(A): \n%s', repr(self.A))
- r_pos = 0.0003
- self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.01
+ q_vel = 5.0
+ q_velfilt = 1.5
+ q_voltage = 2.0
+ self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0],
+ [0.0, 0.0, (q_velfilt**2.0), 0.0],
+ [0.0, 0.0, 0.0, (q_voltage**2.0)]])
- _, _, self.Q, self.R = controls.kalmd(
- A_continuous=self.A_continuous, B_continuous=self.B_continuous,
- Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
- dt=self.dt)
+ r_pos = 0.0003
+ self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 4)))
- self.K[0, 0:3] = self.K_unaugmented
- self.K[0, 3] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 4)))
- self.Kff[0, 0:3] = self.Kff_unaugmented
+ self.KalmanGain, self.P_steady_state = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 4)))
+ self.K[0, 0:3] = self.K_unaugmented
+ self.K[0, 3] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 4)))
+ self.Kff[0, 0:3] = self.Kff_unaugmented
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- self.diff = []
- def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None, hybrid_obs = False):
- """Runs the shooter plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+ self.diff = []
- Args:
- shooter: Shooter object to use.
- goal: goal state.
- iterations: Number of timesteps to run the model for.
- controller_shooter: Shooter object to get K from, or None if we should
- use shooter.
- observer_shooter: Shooter object to use for the observer, or None if we
- should use the actual state.
- """
+ def run_test(self,
+ shooter,
+ goal,
+ iterations=200,
+ controller_shooter=None,
+ observer_shooter=None,
+ hybrid_obs=False):
+ """Runs the shooter plant with an initial condition and goal.
- if controller_shooter is None:
- controller_shooter = shooter
+ Args:
+ shooter: Shooter object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_shooter: Shooter object to get K from, or None if we should
+ use shooter.
+ observer_shooter: Shooter object to use for the observer, or None if we
+ should use the actual state.
+ """
- vbat = 12.0
+ if controller_shooter is None:
+ controller_shooter = shooter
- if self.t:
- initial_t = self.t[-1] + shooter.dt
- else:
- initial_t = 0
+ vbat = 12.0
- last_U = numpy.matrix([[0.0]])
- for i in xrange(iterations):
- X_hat = shooter.X
-
- if observer_shooter is not None:
- X_hat = observer_shooter.X_hat
- self.x_hat.append(observer_shooter.X_hat[2, 0])
-
- ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
-
- U = controller_shooter.K * (goal - X_hat) + ff_U
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(shooter.X[0, 0])
-
- self.diff.append(shooter.X[2, 0] - observer_shooter.X_hat[2, 0])
-
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
-
- self.v.append(shooter.X[2, 0])
- self.a.append((self.v[-1] - last_v) / shooter.dt)
-
- if observer_shooter is not None:
- if i != 0:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
- self.offset.append(observer_shooter.X_hat[3, 0])
-
- applied_U = last_U.copy()
- if i > 60:
- applied_U += 2
- shooter.Update(applied_U)
-
- if observer_shooter is not None:
- if hybrid_obs:
- observer_shooter.PredictHybridObserver(last_U, shooter.dt)
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
else:
- observer_shooter.PredictObserver(last_U)
- last_U = U.copy()
+ initial_t = 0
+ last_U = numpy.matrix([[0.0]])
+ for i in xrange(iterations):
+ X_hat = shooter.X
- self.t.append(initial_t + i * shooter.dt)
- self.u.append(U[0, 0])
+ if observer_shooter is not None:
+ X_hat = observer_shooter.X_hat
+ self.x_hat.append(observer_shooter.X_hat[2, 0])
- def Plot(self):
- pylab.figure()
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.v, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ U = controller_shooter.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shooter.X[0, 0])
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ self.diff.append(shooter.X[2, 0] - observer_shooter.X_hat[2, 0])
- pylab.draw()
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(shooter.X[2, 0])
+ self.a.append((self.v[-1] - last_v) / shooter.dt)
+
+ if observer_shooter is not None:
+ if i != 0:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
+ self.offset.append(observer_shooter.X_hat[3, 0])
+
+ applied_U = last_U.copy()
+ if i > 60:
+ applied_U += 2
+ shooter.Update(applied_U)
+
+ if observer_shooter is not None:
+ if hybrid_obs:
+ observer_shooter.PredictHybridObserver(last_U, shooter.dt)
+ else:
+ observer_shooter.PredictObserver(last_U)
+ last_U = U.copy()
+
+ self.t.append(initial_t + i * shooter.dt)
+ self.u.append(U[0, 0])
+
+ def Plot(self):
+ pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.draw()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- if FLAGS.plot:
- iterations = 200
+ if FLAGS.plot:
+ iterations = 200
- initial_X = numpy.matrix([[0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [100.0], [0.0]])
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [100.0], [0.0]])
- scenario_plotter_int = ScenarioPlotter()
+ scenario_plotter_int = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter_hybrid = IntegralShooter()
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter_hybrid = IntegralShooter()
- scenario_plotter_int.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter_hybrid, iterations=iterations,
- hybrid_obs = True)
+ scenario_plotter_int.run_test(
+ shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid,
+ iterations=iterations,
+ hybrid_obs=True)
- scenario_plotter_int.Plot()
+ scenario_plotter_int.Plot()
- pylab.show()
+ pylab.show()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'shooter']
- shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', shooter.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', shooter.G))
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'shooter']
+ shooter = Shooter('Shooter')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', shooter.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', shooter.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_shooter = IntegralShooter('IntegralShooter')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces,
- plant_type='StateFeedbackHybridPlant',
- observer_type='HybridKalman')
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_shooter = IntegralShooter('IntegralShooter')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralShooter', [integral_shooter],
+ namespaces=namespaces,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index 471f246..b76016a 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -17,6 +17,7 @@
class Intake(control_loop.ControlLoop):
+
def __init__(self, name="Intake"):
super(Intake, self).__init__(name)
self.motor = control_loop.BAG()
@@ -67,12 +68,11 @@
self.A_continuous = numpy.matrix(
[[0.0, 1.0, 0.0, 0.0],
- [(-self.Ks / self.Jo), (-self.b/self.Jo),
- (self.Ks / self.Jo), ( self.b/self.Jo)],
- [0.0, 0.0, 0.0, 1.0],
- [( self.Ks / self.Je), ( self.b/self.Je),
- (-self.Ks / self.Je), (-self.b/self.Je)
- -self.Kt / (self.Je * self.resistance * self.Kv * self.G * self.G)]])
+ [(-self.Ks / self.Jo), (-self.b / self.Jo), (self.Ks / self.Jo),
+ (self.b / self.Jo)], [0.0, 0.0, 0.0, 1.0],
+ [(self.Ks / self.Je), (self.b / self.Je), (-self.Ks / self.Je),
+ (-self.b / self.Je) - self.Kt /
+ (self.Je * self.resistance * self.Kv * self.G * self.G)]])
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -99,8 +99,8 @@
q_pos = 0.05
q_vel = 2.65
self.Q = numpy.matrix(
- numpy.diag([(q_pos**2.0), (q_vel**2.0), (q_pos**2.0), (q_vel
- **2.0)]))
+ numpy.diag([(q_pos**2.0), (q_vel**2.0), (q_pos**2.0),
+ (q_vel**2.0)]))
r_nm = 0.025
self.R = numpy.matrix(numpy.diag([(r_nm**2.0), (r_nm**2.0)]))
@@ -117,6 +117,7 @@
class DelayedIntake(Intake):
+
def __init__(self, name="DelayedIntake"):
super(DelayedIntake, self).__init__(name=name)
@@ -137,12 +138,11 @@
# Coordinate transform fom absolute angles to relative angles.
# [output_position, output_velocity, spring_angle, spring_velocity, voltage]
- abs_to_rel = numpy.matrix(
- [[1.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 1.0, 0.0, 0.0, 0.0],
- [1.0, 0.0, -1.0, 0.0, 0.0],
- [0.0, 1.0, 0.0, -1.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 1.0]])
+ abs_to_rel = numpy.matrix([[1.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0, 0.0, 0.0],
+ [1.0, 0.0, -1.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0, -1.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
# and back again.
rel_to_abs = numpy.matrix(numpy.linalg.inv(abs_to_rel))
@@ -185,14 +185,14 @@
self.A_transformed, self.B_transformed, self.Q_lqr, self.R)
# Write the controller back out in the absolute coordinate system.
- self.K = numpy.hstack((numpy.matrix([[0.0]]),
- self.K_transformed)) * abs_to_rel
+ self.K = numpy.hstack(
+ (numpy.matrix([[0.0]]), self.K_transformed)) * abs_to_rel
controllability = controls.ctrb(self.A_transformed, self.B_transformed)
glog.debug('ctrb: ' + repr(numpy.linalg.matrix_rank(controllability)))
- w, v = numpy.linalg.eig(
- self.A_transformed - self.B_transformed * self.K_transformed)
+ w, v = numpy.linalg.eig(self.A_transformed -
+ self.B_transformed * self.K_transformed)
glog.debug('Poles are %s, for %s', repr(w), self._name)
for i in range(len(w)):
@@ -222,6 +222,7 @@
class ScenarioPlotter(object):
+
def __init__(self):
# Various lists for graphing things.
self.t = []
@@ -295,7 +296,7 @@
[goal_velocity / (intake.G * intake.Kv)]])
U = controller_intake.K * (R - X_hat) + R[4, 0]
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat) # * 0.0
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat) # * 0.0
self.x_output.append(intake.X[0, 0])
self.x_motor.append(intake.X[2, 0])
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 16cdfd9..b53fb99 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -145,7 +145,7 @@
elevator_params->zeroing_constants.measured_absolute_position = 0.049419;
elevator->potentiometer_offset = -0.022320;
- intake->zeroing_constants.measured_absolute_position = 2.303729;
+ intake->zeroing_constants.measured_absolute_position = 1.756847;
intake->zeroing_constants.middle_position =
Values::kIntakeRange().middle();
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.cc b/y2019/control_loops/drivetrain/drivetrain_base.cc
index 26ca861..02bd805 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_base.cc
@@ -40,8 +40,8 @@
drivetrain::kMass, 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 */,
+ 0.7 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
+ 1.2 /* wheel_multiplier */,
};
return kDrivetrainConfig;
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 6646f96..5edfce3 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -50,11 +50,10 @@
output != nullptr ? &(output->wrist_voltage) : nullptr,
&(status->wrist));
- intake_.Iterate(
- unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake_joint),
+ output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+ &(status->intake));
stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
&(position->stilts),
@@ -67,6 +66,16 @@
status->estopped = status->elevator.estopped || status->wrist.estopped ||
status->intake.estopped || status->stilts.estopped;
+ if (output) {
+ if (status->intake.position > kMinIntakeAngleForRollers) {
+ output->intake_roller_voltage =
+ (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
+
+ } else {
+ output->intake_roller_voltage = 0.0;
+ }
+ }
+
// TODO(theo) move these up when Iterate() is split
// update the goals
collision_avoidance_.UpdateGoal(status, unsafe_goal);
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 22dbcd6..c3e53f2 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -28,18 +28,10 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
- const PotAndAbsoluteEncoderSubsystem &elevator() const {
- return elevator_;
- }
- const PotAndAbsoluteEncoderSubsystem &wrist() const {
- return wrist_;
- }
- const AbsoluteEncoderSubsystem &intake() const {
- return intake_;
- }
- const PotAndAbsoluteEncoderSubsystem &stilts() const {
- return stilts_;
- }
+ const PotAndAbsoluteEncoderSubsystem &elevator() const { return elevator_; }
+ const PotAndAbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+ const AbsoluteEncoderSubsystem &intake() const { return intake_; }
+ const PotAndAbsoluteEncoderSubsystem &stilts() const { return stilts_; }
protected:
virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
@@ -55,6 +47,8 @@
CollisionAvoidance collision_avoidance_;
+ static constexpr double kMinIntakeAngleForRollers = -0.7;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index a71f81a..4176057 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -53,9 +53,9 @@
};
message Position {
- // Input from pressure sensor in psi
- // 0 = current atmospheric pressure, negative = suction.
- double suction_pressure;
+ // Input from pressure sensor in bar
+ // 1 = 1 atm, 0 = full vacuum
+ float suction_pressure;
// Position of the elevator, 0 at lowest position, positive when up.
.frc971.PotAndAbsolutePosition elevator;
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index de973c0..bd18b60 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -342,9 +342,9 @@
CheckCollisions();
}
- const double loop_time = chrono::duration_cast<chrono::duration<double>>(
- monotonic_clock::now() - loop_start_time)
- .count();
+ const double loop_time =
+ chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time).count();
const double elevator_acceleration =
(superstructure_plant_.elevator_velocity() -
@@ -657,6 +657,52 @@
VerifyNearGoal();
}
+// Tests that the rollers spin when allowed
+TEST_F(SuperstructureTest, IntakeRollerTest) {
+ WaitUntilZeroed();
+
+ // Get the elevator and wrist out of the way and set the Intake to where
+ // we should be able to spin and verify that they do
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage,
+ superstructure_queue_.goal->roller_voltage);
+ VerifyNearGoal();
+
+ // Move the intake where we oughtn't to spin the rollers and verify they don't
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage, 0.0);
+ VerifyNearGoal();
+}
+
+// Tests that running disabled, ya know, works
+TEST_F(SuperstructureTest, DiasableTest) {
+ RunForTime(chrono::seconds(2), false, false);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 99e3be8..e99a264 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -1,4 +1,5 @@
-package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+load("//motors:macros.bzl", "hex_from_elf")
spi_crc_args = [
"$(location //third_party/pycrc:pycrc_main)",
@@ -62,6 +63,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos/containers:sized_array",
+ "//aos/time",
"//third_party/eigen",
],
)
@@ -135,9 +137,31 @@
cc_library(
name = "serial",
- hdrs = ["serial.h"],
srcs = ["serial.cc"],
+ hdrs = ["serial.h"],
+ visibility = ["//visibility:public"],
deps = [
- "//aos/logging:logging",
+ "//aos/logging",
],
)
+
+cc_binary(
+ name = "teensy.elf",
+ srcs = [
+ "teensy.cc",
+ ],
+ restricted_to = ["//tools:cortex-m4f"],
+ deps = [
+ "//aos/time:time_mcu",
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:configuration",
+ "//motors/peripheral:uart",
+ "//motors/print:usb",
+ ],
+)
+
+hex_from_elf(
+ name = "teensy",
+ restricted_to = ["//tools:cortex-m4f"],
+)
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index c82a089..15889c4 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -10,6 +10,7 @@
#include "Eigen/Dense"
#include "aos/containers/sized_array.h"
+#include "aos/time/time.h"
namespace frc971 {
namespace jevois {
@@ -106,6 +107,15 @@
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
+ enum class CameraCommand {
+ // Stay in normal mode.
+ kNormal,
+ // Go to camera passthrough mode.
+ kCameraPassthrough,
+ // Go to being a useful USB device.
+ kUsb,
+ };
+
bool operator==(const CameraCalibration &other) const {
if (other.calibration != calibration) {
return false;
@@ -120,6 +130,17 @@
//
// TODO(Parker): What are the details on how this is defined?
Eigen::Matrix<float, 3, 4> calibration;
+
+ // A local timestamp from the Teensy. This starts at 0 when the Teensy is
+ // powered on.
+ aos::monotonic_clock::time_point teensy_now;
+
+ // A realtime timestamp from the roboRIO. This will be min_time if the roboRIO
+ // has never sent anything.
+ aos::realtime_clock::time_point realtime_now;
+
+ // What mode the camera should transition into.
+ CameraCommand camera_command;
};
// This is all the information the Teensy sends to the RoboRIO.
@@ -160,6 +181,9 @@
// Whether the light ring for each camera should be on.
std::bitset<5> light_rings;
+
+ // The current time.
+ aos::realtime_clock::time_point realtime_now;
};
} // namespace jevois
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
new file mode 100644
index 0000000..2d3c020
--- /dev/null
+++ b/y2019/jevois/teensy.cc
@@ -0,0 +1,412 @@
+#include "aos/time/time.h"
+#include "motors/core/kinetis.h"
+#include "motors/core/time.h"
+#include "motors/peripheral/configuration.h"
+#include "motors/peripheral/uart.h"
+#include "motors/print/print.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace jevois {
+namespace {
+
+struct Uarts {
+ Uarts() {
+ DisableInterrupts disable_interrupts;
+ instance = this;
+ }
+ ~Uarts() {
+ DisableInterrupts disable_interrupts;
+ instance = nullptr;
+ }
+
+ void Initialize(int baud_rate) {
+ cam0.Initialize(baud_rate);
+ cam1.Initialize(baud_rate);
+ cam2.Initialize(baud_rate);
+ cam3.Initialize(baud_rate);
+ cam4.Initialize(baud_rate);
+ }
+
+ frc971::teensy::InterruptBufferedUart cam0{&UART1, F_CPU};
+ frc971::teensy::InterruptBufferedUart cam1{&UART0, F_CPU};
+ frc971::teensy::InterruptBufferedUart cam2{&UART2, BUS_CLOCK_FREQUENCY};
+ frc971::teensy::InterruptBufferedUart cam3{&UART3, BUS_CLOCK_FREQUENCY};
+ frc971::teensy::InterruptBufferedUart cam4{&UART4, BUS_CLOCK_FREQUENCY};
+
+ static Uarts *instance;
+};
+
+Uarts *Uarts::instance = nullptr;
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+extern char *__brkval;
+extern uint32_t __bss_ram_start__[];
+extern uint32_t __heap_start__[];
+extern uint32_t __stack_end__[];
+
+void uart0_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam1.HandleInterrupt(disable_interrupts);
+}
+
+void uart1_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam0.HandleInterrupt(disable_interrupts);
+}
+
+void uart2_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam2.HandleInterrupt(disable_interrupts);
+}
+
+void uart3_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam3.HandleInterrupt(disable_interrupts);
+}
+
+void uart4_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam4.HandleInterrupt(disable_interrupts);
+}
+
+} // extern "C"
+
+// A test program which echos characters back after adding a per-UART offset to
+// them (CAM0 adds 1, CAM1 adds 2, etc).
+__attribute__((unused)) void TestUarts() {
+ Uarts *const uarts = Uarts::instance;
+ while (true) {
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam0.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 1;
+ }
+ uarts->cam0.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam1.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 2;
+ }
+ uarts->cam1.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam2.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 3;
+ }
+ uarts->cam2.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam3.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 4;
+ }
+ uarts->cam3.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam4.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 5;
+ }
+ uarts->cam4.Write(data);
+ }
+ }
+}
+
+// Tests all the I/O pins. Cycles through each one for 1 second. While active,
+// each output is turned on, and each input has its value printed.
+__attribute__((unused)) void TestIo() {
+ // Set SPI0 pins to GPIO.
+ // SPI_OUT
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 6) = 1;
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_CS
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 0) = 0;
+ PORTD_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_IN
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 7) = 0;
+ PORTC_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_SCK
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 1) = 0;
+ PORTD_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ // Set LED pins to GPIO.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 11) = 1;
+ PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 10) = 1;
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 8) = 1;
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 9) = 1;
+ PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 18) = 1;
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 2) = 1;
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 7) = 1;
+ PORTD_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 1) = 1;
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 19) = 1;
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 5) = 1;
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ auto next = aos::monotonic_clock::now();
+ static constexpr auto kTick = std::chrono::seconds(1);
+ while (true) {
+ printf("SPI_MISO\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 0;
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_CS %d\n", (int)PERIPHERAL_BITBAND(GPIOD_PDIR, 0));
+ }
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_MOSI %d\n", (int)PERIPHERAL_BITBAND(GPIOC_PDIR, 7));
+ }
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_CLK %d\n", (int)PERIPHERAL_BITBAND(GPIOD_PDIR, 1));
+ }
+ next += kTick;
+
+ printf("CAM0\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 0;
+ next += kTick;
+
+ printf("CAM1\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 0;
+ next += kTick;
+
+ printf("CAM2\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 0;
+ next += kTick;
+
+ printf("CAM3\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 0;
+ next += kTick;
+
+ printf("CAM4\n");
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = 0;
+ next += kTick;
+
+ printf("CAM5\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 0;
+ next += kTick;
+
+ printf("CAM6\n");
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 7) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 7) = 0;
+ next += kTick;
+
+ printf("CAM7\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 0;
+ next += kTick;
+
+ printf("CAM8\n");
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 19) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 19) = 0;
+ next += kTick;
+
+ printf("CAM9\n");
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 5) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 5) = 0;
+ next += kTick;
+ }
+}
+
+int Main() {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
+ NVIC_SET_SANE_PRIORITY(IRQ_UART0_STATUS, 0xE);
+
+ // Set the LED's pin to output mode.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ frc971::motors::PrintingParameters printing_parameters;
+ printing_parameters.dedicated_usb = true;
+ const ::std::unique_ptr<frc971::motors::PrintingImplementation> printing =
+ CreatePrinting(printing_parameters);
+ printing->Initialize();
+
+ DMA.CR = M_DMA_EMLM;
+
+ SIM_SCGC4 |=
+ SIM_SCGC4_UART0 | SIM_SCGC4_UART1 | SIM_SCGC4_UART2 | SIM_SCGC4_UART3;
+ SIM_SCGC1 |= SIM_SCGC1_UART4;
+
+ // SPI0 goes to the roboRIO.
+ // SPI0_PCS0 is SPI_CS.
+ PORTD_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SOUT is SPI_MISO.
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SIN is SPI_MOSI.
+ PORTC_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SCK is SPI_CLK.
+ PORTD_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ // FTM0_CH0 is LED0 (7 in silkscreen, a beacon channel).
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH1 is LED1 (5 in silkscreen, a beacon channel).
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH7 is LED2 (6 in silkscreen, a beacon channel).
+ PORTD_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH5 is LED3 (9 in silkscreen, a vision camera).
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+
+ // FTM2_CH1 is LED4 (8 in silkscreen, a vision camera).
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM2_CH0 is LED5 (for CAM4).
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // FTM3_CH4 is LED6 (for CAM2).
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH5 is LED7 (for CAM3).
+ PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH6 is LED8 (for CAM1).
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH7 is LED9 (for CAM0).
+ PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // This hardware has been deactivated, but keep this comment for now to
+ // document which pins it is on.
+#if 0
+ // This is ODROID_EN.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 0) = 1;
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 0) = 0;
+ PORTC_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // This is CAM_EN.
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 0) = 1;
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 0) = 0;
+ PORTB_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+#endif
+ // This is 5V_PGOOD.
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 6) = 0;
+ PORTD_PCR6 = PORT_PCR_MUX(1);
+
+ // These go to CAM1.
+ // UART0_RX (peripheral) is UART1_RX (schematic).
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART0_TX (peripheral) is UART1_TX (schematic).
+ PORTA_PCR14 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM0.
+ // UART1_RX (peripheral) is UART0_RX (schematic).
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART1_TX (peripheral) is UART0_TX (schematic).
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM2.
+ // UART2_RX
+ PORTD_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART2_TX
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM3.
+ // UART3_RX
+ PORTB_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART3_TX
+ PORTB_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM4.
+ // UART4_RX
+ PORTE_PCR25 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART4_TX
+ PORTE_PCR24 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ Uarts uarts;
+
+ // Give everything a chance to get going.
+ delay(100);
+
+ printf("Ram start: %p\n", __bss_ram_start__);
+ printf("Heap start: %p\n", __heap_start__);
+ printf("Heap end: %p\n", __brkval);
+ printf("Stack start: %p\n", __stack_end__);
+
+ uarts.Initialize(115200);
+ NVIC_ENABLE_IRQ(IRQ_UART0_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART1_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART2_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART3_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART4_STATUS);
+
+ while (true) {
+ }
+}
+
+extern "C" {
+
+int main(void) {
+ return Main();
+}
+
+} // extern "C"
+
+} // namespace
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
new file mode 100644
index 0000000..6736c24
--- /dev/null
+++ b/y2019/vision/BUILD
@@ -0,0 +1,80 @@
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+
+package(default_visibility = ["//visibility:public"])
+
+VISION_TARGETS = [ "//tools:k8", "//tools:armhf-debian"]
+
+cc_library(
+ name = "target_finder",
+ srcs = ["target_finder.cc", "target_geometry.cc"],
+ hdrs = ["target_finder.h", "target_types.h"],
+ deps = [
+ "@com_google_ceres_solver//:ceres",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:region_alloc",
+ "//aos/vision/blob:contour",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:overlay",
+ "//aos/vision/math:vector",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+gtk_dependent_cc_binary(
+ name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/vision/blob:move_scale",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:debug_framework",
+ "//aos/vision/math:vector",
+ ],
+ copts = ["-Wno-unused-variable"],
+ restricted_to = VISION_TARGETS,
+)
+
+cc_binary(
+ name = "target_sender",
+ srcs = ["target_sender.cc"],
+ deps = [
+ ":target_finder",
+ "//y2019/jevois:serial",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+"""
+cc_binary(
+ name = "calibration",
+ srcs = ["calibration.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+"""
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
new file mode 100644
index 0000000..4f43c9a
--- /dev/null
+++ b/y2019/vision/debug_viewer.cc
@@ -0,0 +1,269 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/move_scale.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/math/vector.h"
+
+using aos::vision::ImageRange;
+using aos::vision::ImageFormat;
+using aos::vision::RangeImage;
+using aos::vision::AnalysisAllocator;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+using aos::vision::Segment;
+using aos::vision::PixelRef;
+
+namespace y2019 {
+namespace vision {
+
+std::vector<PixelRef> GetNColors(size_t num_colors) {
+ std::vector<PixelRef> colors;
+ for (size_t i = 0; i < num_colors; ++i) {
+ int quadrent = i * 6 / num_colors;
+ uint8_t alpha = (256 * 6 * i - quadrent * num_colors * 256) / num_colors;
+ uint8_t inv_alpha = 255 - alpha;
+ switch (quadrent) {
+ case 0:
+ colors.push_back(PixelRef{255, alpha, 0});
+ break;
+ case 1:
+ colors.push_back(PixelRef{inv_alpha, 255, 0});
+ break;
+ case 2:
+ colors.push_back(PixelRef{0, 255, alpha});
+ break;
+ case 3:
+ colors.push_back(PixelRef{0, inv_alpha, 255});
+ break;
+ case 4:
+ colors.push_back(PixelRef{alpha, 0, 255});
+ break;
+ case 5:
+ colors.push_back(PixelRef{255, 0, inv_alpha});
+ break;
+ }
+ }
+ return colors;
+}
+
+class FilterHarness : public aos::vision::FilterHarness {
+ public:
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
+ return finder_.Threshold(image);
+ }
+
+ void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
+ viewer_ = viewer;
+ viewer_->SetScale(0.75);
+ overlays_.push_back(&overlay_);
+ overlays_.push_back(finder_.GetOverlay());
+ viewer_->view()->SetOverlays(&overlays_);
+ }
+
+ void DrawBlob(const RangeImage &blob, PixelRef color) {
+ if (viewer_) {
+ BlobList list;
+ list.push_back(blob);
+ viewer_->DrawBlobList(list, color);
+ }
+ }
+
+ bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ imgs_last_ = imgs;
+ fmt_last_ = fmt;
+ // reset for next drawing cycle
+ for (auto &overlay : overlays_) {
+ overlay->Reset();
+ }
+
+ if (draw_select_blob_ || draw_raw_poly_ || draw_components_ ||
+ draw_raw_target_ || draw_raw_IR_ || draw_results_) {
+ printf("_____ New Image _____\n");
+ }
+
+ // Remove bad blobs.
+ finder_.PreFilter(&imgs);
+
+ // Find polygons from blobs.
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon =
+ finder_.FillPolygon(blob, draw_raw_poly_);
+ if (polygon.empty()) {
+ DrawBlob(blob, {255, 0, 0});
+ } else {
+ raw_polys.push_back(polygon);
+ if (draw_select_blob_) {
+ DrawBlob(blob, {0, 0, 255});
+ }
+ if (draw_raw_poly_) {
+ std::vector<PixelRef> colors = GetNColors(polygon.size());
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(polygon[i].Intersect(polygon[(i + 1) % 4]));
+ }
+
+ for (size_t i = 0; i < 4; ++i) {
+ overlay_.AddLine(corners[i], corners[(i + 1) % 4], colors[i]);
+ }
+ }
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+ if (draw_components_) {
+ for (const TargetComponent &comp : target_component_list) {
+ DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
+ {0, 0, 255});
+ }
+ }
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list = finder_.FindTargetsFromComponents(
+ target_component_list, draw_raw_target_);
+ if (draw_raw_target_) {
+ for (const Target &target : target_list) {
+ DrawTarget(target);
+ }
+ }
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, true));
+ if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
+ }
+
+ // Check that our current results match possible solutions.
+ results = finder_.FilterResults(results);
+ if (draw_results_) {
+ for (const IntermediateResult &res : results) {
+ DrawResult(res, {0, 255, 0});
+ DrawTarget(res, {0, 255, 0});
+ }
+ }
+
+ // If the target list is not empty then we found a target.
+ return !results.empty();
+ }
+
+ std::function<void(uint32_t)> RegisterKeyPress() override {
+ return [this](uint32_t key) {
+ (void)key;
+ if (key == 'z') {
+ draw_results_ = !draw_results_;
+ } else if (key == 'x') {
+ draw_raw_IR_ = !draw_raw_IR_;
+ } else if (key == 'c') {
+ draw_raw_target_ = !draw_raw_target_;
+ } else if (key == 'v') {
+ draw_components_ = !draw_components_;
+ } else if (key == 'b') {
+ draw_raw_poly_ = !draw_raw_poly_;
+ } else if (key == 'n') {
+ draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'q') {
+ printf("User requested shutdown.\n");
+ exit(0);
+ }
+ HandleBlobs(imgs_last_, fmt_last_);
+ viewer_->Redraw();
+ };
+ }
+
+ void DrawComponent(const TargetComponent &comp, PixelRef top_color,
+ PixelRef bot_color, PixelRef in_color,
+ PixelRef out_color) {
+ overlay_.AddLine(comp.top, comp.inside, top_color);
+ overlay_.AddLine(comp.bottom, comp.outside, bot_color);
+
+ overlay_.AddLine(comp.bottom, comp.inside, in_color);
+ overlay_.AddLine(comp.top, comp.outside, out_color);
+ }
+
+ void DrawTarget(const Target &target) {
+ Vector<2> leftTop = (target.left.top + target.left.inside) * 0.5;
+ Vector<2> rightTop = (target.right.top + target.right.inside) * 0.5;
+ overlay_.AddLine(leftTop, rightTop, {255, 215, 0});
+
+ Vector<2> leftBot = (target.left.bottom + target.left.outside) * 0.5;
+ Vector<2> rightBot = (target.right.bottom + target.right.outside) * 0.5;
+ overlay_.AddLine(leftBot, rightBot, {255, 215, 0});
+
+ overlay_.AddLine(leftTop, leftBot, {255, 215, 0});
+ overlay_.AddLine(rightTop, rightBot, {255, 215, 0});
+ }
+
+ void DrawResult(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ DrawComponent(target.left, color, color, color, color);
+ DrawComponent(target.right, color, color, color, color);
+ }
+
+ void DrawTarget(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
+ (target.left.bottom + target.left.outside) * 0.5);
+ leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
+ leftAx.B() * 0.9 + leftAx.A() * 0.1);
+ overlay_.AddLine(leftAx, color);
+
+ Segment<2> rightAx((target.right.top + target.right.inside) * 0.5,
+ (target.right.bottom + target.right.outside) * 0.5);
+ rightAx.Set(rightAx.A() * 0.9 + rightAx.B() * 0.1,
+ rightAx.B() * 0.9 + rightAx.A() * 0.1);
+ overlay_.AddLine(rightAx, color);
+
+ overlay_.AddLine(leftAx.A(), rightAx.A(), color);
+ overlay_.AddLine(leftAx.B(), rightAx.B(), color);
+ Vector<3> p1(0.0, 0.0, 100.0);
+
+ Vector<3> p2 =
+ Rotate(intrinsics().mount_angle, result.extrinsics.r1, 0.0, p1);
+ Vector<2> p3(p2.x(), p2.y());
+ overlay_.AddLine(leftAx.A(), p3 + leftAx.A(), {0, 255, 0});
+ overlay_.AddLine(leftAx.B(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(rightAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(rightAx.B(), p3 + rightAx.B(), {0, 255, 0});
+
+ overlay_.AddLine(p3 + leftAx.A(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(p3 + rightAx.A(), p3 + rightAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
+ }
+
+ const IntrinsicParams &intrinsics() const { return finder_.intrinsics(); }
+
+ private:
+ // implementation of the filter pipeline.
+ TargetFinder finder_;
+ aos::vision::BlobStreamViewer *viewer_ = nullptr;
+ aos::vision::PixelLinesOverlay overlay_;
+ std::vector<aos::vision::OverlayBase *> overlays_;
+ BlobList imgs_last_;
+ ImageFormat fmt_last_;
+ bool draw_select_blob_ = false;
+ bool draw_raw_poly_ = false;
+ bool draw_components_ = false;
+ bool draw_raw_target_ = false;
+ bool draw_raw_IR_ = false;
+ bool draw_results_ = true;
+};
+
+} // namespace vision
+} // namespace y2017
+
+int main(int argc, char **argv) {
+ y2019::vision::FilterHarness filter_harness;
+ aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
+ aos::vision::CameraParams());
+}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
new file mode 100644
index 0000000..f69e987
--- /dev/null
+++ b/y2019/vision/target_finder.cc
@@ -0,0 +1,379 @@
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
+using namespace aos::vision;
+
+namespace y2019 {
+namespace vision {
+
+TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+
+aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
+ const uint8_t threshold_value = GetThresholdValue();
+ return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
+ if (px.g > threshold_value && px.b > threshold_value &&
+ px.r > threshold_value) {
+ return true;
+ }
+ return false;
+ });
+}
+
+// Filter blobs on size.
+void TargetFinder::PreFilter(BlobList *imgs) {
+ imgs->erase(
+ std::remove_if(imgs->begin(), imgs->end(),
+ [](RangeImage &img) {
+ // We can drop images with a small number of
+ // pixels, but images
+ // must be over 20px or the math will have issues.
+ return (img.npixels() < 100 || img.height() < 25);
+ }),
+ imgs->end());
+}
+
+// TODO: Try hierarchical merge for this.
+// Convert blobs into polygons.
+std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
+ const RangeImage &blob, bool verbose) {
+ if (verbose) printf("Process Polygon.\n");
+ alloc_.reset();
+ auto *st = RangeImgToContour(blob, &alloc_);
+
+ struct Pt {
+ float x;
+ float y;
+ };
+ std::vector<Pt> pts;
+
+ // Collect all slopes from the contour.
+ auto opt = st->pt;
+ for (auto *node = st; node->next != st;) {
+ node = node->next;
+
+ auto npt = node->pt;
+
+ pts.push_back(
+ {static_cast<float>(npt.x - opt.x), static_cast<float>(npt.y - opt.y)});
+
+ opt = npt;
+ }
+
+ const int n = pts.size();
+ auto get_pt = [&](int i) { return pts[(i + n * 2) % n]; };
+
+ std::vector<Pt> pts_new = pts;
+ auto run_box_filter = [&](int window_size) {
+ for (size_t i = 0; i < pts.size(); ++i) {
+ Pt a{0.0, 0.0};
+ for (int j = -window_size; j <= window_size; ++j) {
+ Pt p = get_pt(j + i);
+ a.x += p.x;
+ a.y += p.y;
+ }
+ a.x /= (window_size * 2 + 1);
+ a.y /= (window_size * 2 + 1);
+
+ float scale = 1.0 + (i / float(pts.size() * 10));
+ a.x *= scale;
+ a.y *= scale;
+ pts_new[i] = a;
+ }
+ pts = pts_new;
+ };
+ // Three box filter makith a guassian?
+ // Run gaussian filter over the slopes.
+ run_box_filter(2);
+ run_box_filter(2);
+ run_box_filter(2);
+
+ // Heuristic which says if a particular slope is part of a corner.
+ auto is_corner = [&](size_t i) {
+ Pt a = get_pt(i - 3);
+ Pt b = get_pt(i + 3);
+ double dx = (a.x - b.x);
+ double dy = (a.y - b.y);
+ return dx * dx + dy * dy > 0.25;
+ };
+
+ bool prev_v = is_corner(-1);
+
+ // Find all centers of corners.
+ // Because they round, multiple points may be a corner.
+ std::vector<size_t> edges;
+ size_t kBad = pts.size() + 10;
+ size_t prev_up = kBad;
+ size_t wrapped_n = prev_up;
+
+ for (size_t i = 0; i < pts.size(); ++i) {
+ bool v = is_corner(i);
+ if (prev_v && !v) {
+ if (prev_up == kBad) {
+ wrapped_n = i;
+ } else {
+ edges.push_back((prev_up + i - 1) / 2);
+ }
+ }
+ if (v && !prev_v) {
+ prev_up = i;
+ }
+ prev_v = v;
+ }
+
+ if (wrapped_n != kBad) {
+ edges.push_back(((prev_up + pts.size() + wrapped_n - 1) / 2) % pts.size());
+ }
+
+ if (verbose) printf("Edge Count (%zu).\n", edges.size());
+
+ // Get all CountourNodes from the contour.
+ using aos::vision::PixelRef;
+ std::vector<ContourNode *> segments;
+ {
+ std::vector<ContourNode *> segments_all;
+
+ for (ContourNode *node = st; node->next != st;) {
+ node = node->next;
+ segments_all.push_back(node);
+ }
+ for (size_t i : edges) {
+ segments.push_back(segments_all[i]);
+ }
+ }
+ if (verbose) printf("Segment Count (%zu).\n", segments.size());
+
+ // Run best-fits over each line segment.
+ std::vector<Segment<2>> seg_list;
+ if (segments.size() == 4) {
+ for (size_t i = 0; i < segments.size(); ++i) {
+ auto *ed = segments[(i + 1) % segments.size()];
+ auto *st = segments[i];
+ float mx = 0.0;
+ float my = 0.0;
+ int n = 0;
+ for (auto *node = st; node != ed; node = node->next) {
+ mx += node->pt.x;
+ my += node->pt.y;
+ ++n;
+ // (x - [x] / N) ** 2 = [x * x] - 2 * [x] * [x] / N + [x] * [x] / N / N;
+ }
+ mx /= n;
+ my /= n;
+
+ float xx = 0.0;
+ float xy = 0.0;
+ float yy = 0.0;
+ for (auto *node = st; node != ed; node = node->next) {
+ float x = node->pt.x - mx;
+ float y = node->pt.y - my;
+ xx += x * x;
+ xy += x * y;
+ yy += y * y;
+ }
+
+ // TODO: Extract common to hierarchical merge.
+ float neg_b_over_2 = (xx + yy) / 2.0;
+ float c = (xx * yy - xy * xy);
+
+ float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
+
+ {
+ float lam = neg_b_over_2 + sqr;
+ float x = xy;
+ float y = lam - xx;
+
+ float norm = sqrt(x * x + y * y);
+ x /= norm;
+ y /= norm;
+
+ seg_list.push_back(
+ Segment<2>(Vector<2>(mx, my), Vector<2>(mx + x, my + y)));
+ }
+
+ /* Characteristic polynomial
+ 1 lam^2 - (xx + yy) lam + (xx * yy - xy * xy) = 0
+
+ [a b]
+ [c d]
+
+ // covariance matrix.
+ [xx xy] [nx]
+ [xy yy] [ny]
+ */
+ }
+ }
+ if (verbose) printf("Poly Count (%zu).\n", seg_list.size());
+ return seg_list;
+}
+
+// Convert segments into target components (left or right)
+std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+ const std::vector<std::vector<Segment<2>>> &seg_list) {
+ std::vector<TargetComponent> list;
+ TargetComponent new_target;
+ for (const auto &poly : seg_list) {
+ // Reject missized pollygons for now. Maybe rectify them here in the future;
+ if (poly.size() != 4) continue;
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(poly[i].Intersect(poly[(i + 1) % 4]));
+ }
+
+ // Select the closest two points. Short side of the rectangle.
+ double min_dist = -1;
+ std::pair<size_t, size_t> closest;
+ for (size_t i = 0; i < 4; ++i) {
+ size_t next = (i + 1) % 4;
+ double nd = corners[i].SquaredDistanceTo(corners[next]);
+ if (min_dist == -1 || nd < min_dist) {
+ min_dist = nd;
+ closest.first = i;
+ closest.second = next;
+ }
+ }
+
+ // Verify our top is above the bottom.
+ size_t bot_index = closest.first;
+ size_t top_index = (closest.first + 2) % 4;
+ if (corners[top_index].y() < corners[bot_index].y()) {
+ closest.first = top_index;
+ closest.second = (top_index + 1) % 4;
+ }
+
+ // Find the major axis.
+ size_t far_first = (closest.first + 2) % 4;
+ size_t far_second = (closest.second + 2) % 4;
+ Segment<2> major_axis(
+ (corners[closest.first] + corners[closest.second]) * 0.5,
+ (corners[far_first] + corners[far_second]) * 0.5);
+ if (major_axis.AsVector().AngleToZero() > M_PI / 180.0 * 120.0 ||
+ major_axis.AsVector().AngleToZero() < M_PI / 180.0 * 60.0) {
+ // Target is angled way too much, drop it.
+ continue;
+ }
+
+ // organize the top points.
+ Vector<2> topA = corners[closest.first] - major_axis.B();
+ new_target.major_axis = major_axis;
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a left target since we are leaning positive.
+ new_target.is_right = false;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ }
+ } else {
+ // We have a right target since we are leaning negative.
+ new_target.is_right = true;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ }
+ }
+
+ // organize the top points.
+ Vector<2> botA = corners[far_first] - major_axis.A();
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a right target since we are leaning positive.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ }
+ } else {
+ // We have a left target since we are leaning negative.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ }
+ }
+
+ // This piece of the target should be ready now.
+ list.emplace_back(new_target);
+ }
+
+ return list;
+}
+
+// Match components into targets.
+std::vector<Target> TargetFinder::FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose) {
+ std::vector<Target> target_list;
+ using namespace aos::vision;
+ if (component_list.size() < 2) {
+ // We don't enough parts for a target.
+ return target_list;
+ }
+
+ for (size_t i = 0; i < component_list.size(); i++) {
+ const TargetComponent &a = component_list[i];
+ for (size_t j = 0; j < i; j++) {
+ bool target_valid = false;
+ Target new_target;
+ const TargetComponent &b = component_list[j];
+
+ // Reject targets that are too far off vertically.
+ Vector<2> a_center = a.major_axis.Center();
+ if (a_center.y() > b.bottom.y() || a_center.y() < b.top.y()) {
+ continue;
+ }
+ Vector<2> b_center = b.major_axis.Center();
+ if (b_center.y() > a.bottom.y() || b_center.y() < a.top.y()) {
+ continue;
+ }
+
+ if (a.is_right && !b.is_right) {
+ if (a.top.x() > b.top.x()) {
+ new_target.right = a;
+ new_target.left = b;
+ target_valid = true;
+ }
+ } else if (!a.is_right && b.is_right) {
+ if (b.top.x() > a.top.x()) {
+ new_target.right = b;
+ new_target.left = a;
+ target_valid = true;
+ }
+ }
+ if (target_valid) {
+ target_list.emplace_back(new_target);
+ }
+ }
+ }
+ if (verbose) printf("Possible Target: %zu.\n", target_list.size());
+ return target_list;
+}
+
+std::vector<IntermediateResult> TargetFinder::FilterResults(
+ const std::vector<IntermediateResult> &results) {
+ std::vector<IntermediateResult> filtered;
+ for (const IntermediateResult &res : results) {
+ if (res.solver_error < 75.0) {
+ filtered.emplace_back(res);
+ }
+ }
+ return filtered;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
new file mode 100644
index 0000000..633733a
--- /dev/null
+++ b/y2019/vision/target_finder.h
@@ -0,0 +1,80 @@
+#ifndef _Y2019_VISION_TARGET_FINDER_H_
+#define _Y2019_VISION_TARGET_FINDER_H_
+
+#include "aos/vision/blob/region_alloc.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/vector.h"
+#include "y2019/vision/target_types.h"
+
+namespace y2019 {
+namespace vision {
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+class TargetFinder {
+ public:
+ TargetFinder();
+ // Turn a raw image into blob range image.
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+ // Value against which we threshold.
+ uint8_t GetThresholdValue() { return 120; }
+
+ // filter out obvious or durranged blobs.
+ void PreFilter(BlobList *imgs);
+
+ // Turn a blob into a polgygon.
+ std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ bool verbose);
+
+ // Turn a bloblist into components of a target.
+ std::vector<TargetComponent> FillTargetComponentList(
+ const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list);
+
+ // Piece the compenents together into a target.
+ std::vector<Target> FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose);
+
+ // Given a target solve for the transformation of the template target.
+ IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
+
+ std::vector<IntermediateResult> FilterResults(
+ const std::vector<IntermediateResult> &results);
+
+ // Get the local overlay for debug if we are doing that.
+ aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+ // Convert target location into meters and radians.
+ void GetAngleDist(const aos::vision::Vector<2> &target, double down_angle,
+ double *dist, double *angle);
+
+ // Return the template target in a normalized space.
+ const Target &GetTemplateTarget() { return target_template_; }
+
+ const IntrinsicParams &intrinsics() const { return intrinsics_; }
+
+ private:
+ // Find a loosly connected target.
+ double DetectConnectedTarget(const RangeImage &img);
+
+ aos::vision::PixelLinesOverlay overlay_;
+
+ aos::vision::AnalysisAllocator alloc_;
+
+ // The template for the default target in the standard space.
+ Target target_template_;
+
+ IntrinsicParams intrinsics_;
+
+ ExtrinsicParams default_extrinsics_;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
new file mode 100644
index 0000000..77adc83
--- /dev/null
+++ b/y2019/vision/target_geometry.cc
@@ -0,0 +1,186 @@
+#include "y2019/vision/target_finder.h"
+
+#include "ceres/ceres.h"
+
+#include <math.h>
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+static constexpr double kInchesToMeters = 0.0254;
+
+using namespace aos::vision;
+using aos::vision::Vector;
+
+Target Target::MakeTemplate() {
+ Target out;
+ // This is how off-vertical the tape is.
+ const double theta = 14.5 * M_PI / 180.0;
+
+ const double tape_offset = 4 * kInchesToMeters;
+ const double tape_width = 2 * kInchesToMeters;
+ const double tape_length = 5.5 * kInchesToMeters;
+
+ const double s = sin(theta);
+ const double c = cos(theta);
+ out.right.top = Vector<2>(tape_offset, 0.0);
+ out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
+ out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
+ tape_width * s - tape_length * c);
+ out.right.outside =
+ Vector<2>(tape_offset + tape_length * s, -tape_length * c);
+
+ out.right.is_right = true;
+ out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
+ out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
+ out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
+ out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
+ return out;
+}
+
+std::array<Vector<2>, 8> Target::toPointList() const {
+ return std::array<Vector<2>, 8>{{right.top, right.inside, right.bottom,
+ right.outside, left.top, left.inside,
+ left.bottom, left.outside}};
+}
+
+Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ double y = extrinsics.y;
+ double z = extrinsics.z;
+ double r1 = extrinsics.r1;
+ double r2 = extrinsics.r2;
+ double rup = intrinsics.mount_angle;
+ double fl = intrinsics.focal_length;
+
+ ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
+
+ {
+ double theta = r1;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ pts(2) += z;
+
+ {
+ double theta = r2;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ double theta = rup;
+ double s = sin(theta);
+ double c = cos(theta);
+
+ pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Final image projection.
+ ::Eigen::Matrix<double, 1, 3> res = pts;
+
+ float scale = fl / res.z();
+ return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
+}
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ auto project = [&](Vector<2> pt) {
+ return Project(pt, intrinsics, extrinsics);
+ };
+ Target new_targ;
+ new_targ.right.is_right = true;
+ new_targ.right.top = project(target.right.top);
+ new_targ.right.inside = project(target.right.inside);
+ new_targ.right.bottom = project(target.right.bottom);
+ new_targ.right.outside = project(target.right.outside);
+
+ new_targ.left.top = project(target.left.top);
+ new_targ.left.inside = project(target.left.inside);
+ new_targ.left.bottom = project(target.left.bottom);
+ new_targ.left.outside = project(target.left.outside);
+
+ return new_targ;
+}
+
+// Used at runtime on a single image given camera parameters.
+struct RuntimeCostFunctor {
+ RuntimeCostFunctor(Vector<2> result, Vector<2> template_pt,
+ IntrinsicParams intrinsics)
+ : result(result), template_pt(template_pt), intrinsics(intrinsics) {}
+
+ bool operator()(const double *const x, double *residual) const {
+ auto extrinsics = ExtrinsicParams::get(x);
+ auto pt = result - Project(template_pt, intrinsics, extrinsics);
+ residual[0] = pt.x();
+ residual[1] = pt.y();
+ return true;
+ }
+
+ Vector<2> result;
+ Vector<2> template_pt;
+ IntrinsicParams intrinsics;
+};
+
+IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
+ bool verbose) {
+ // Memory for the ceres solver.
+ double params[ExtrinsicParams::kNumParams];
+ default_extrinsics_.set(¶ms[0]);
+
+ Problem problem;
+
+ auto target_value = target.toPointList();
+ auto template_value = target_template_.toPointList();
+
+ for (size_t i = 0; i < 8; ++i) {
+ auto a = template_value[i];
+ auto b = target_value[i];
+
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
+ new RuntimeCostFunctor(b, a, intrinsics_)),
+ NULL, ¶ms[0]);
+ }
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = false;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+ IntermediateResult IR;
+ IR.extrinsics = ExtrinsicParams::get(¶ms[0]);
+ IR.solver_error = summary.final_cost;
+
+ if (verbose) {
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
+ std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
+ std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
+ std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "error = " << summary.final_cost << ";\n";
+ }
+ return IR;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
new file mode 100644
index 0000000..0d53e2c
--- /dev/null
+++ b/y2019/vision/target_sender.cc
@@ -0,0 +1,136 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/reader.h"
+
+#include "y2019/jevois/serial.h"
+#include "y2019/vision/target_finder.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2019::jevois::open_via_terminos;
+using aos::vision::Segment;
+
+class CameraStream : public ::aos::vision::ImageStreamEvent {
+ public:
+ CameraStream(::aos::vision::CameraParams params, const ::std::string &fname)
+ : ImageStreamEvent(fname, params) {}
+
+ void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
+ LOG(INFO, "got frame: %d\n", (int)data.size());
+
+ static unsigned i = 0;
+
+ /*
+ std::ofstream ofs(std::string("/jevois/data/debug_viewer_jpeg_") +
+ std::to_string(i) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ */
+ if (on_frame) on_frame(data, monotonic_now);
+ ++i;
+
+ if (i == 200) exit(-1);
+ }
+
+ std::function<void(DataRef, monotonic_clock::time_point)> on_frame;
+};
+
+int open_terminos(const char *tty_name) { return open_via_terminos(tty_name); }
+
+std::string GetFileContents(const std::string &filename) {
+ std::ifstream in(filename, std::ios::in | std::ios::binary);
+ if (in) {
+ std::string contents;
+ in.seekg(0, std::ios::end);
+ contents.resize(in.tellg());
+ in.seekg(0, std::ios::beg);
+ in.read(&contents[0], contents.size());
+ in.close();
+ return (contents);
+ }
+ fprintf(stderr, "Could not read file: %s\n", filename.c_str());
+ exit(-1);
+}
+
+int main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ int itsDev = open_terminos("/dev/ttyS0");
+ dup2(itsDev, 1);
+ dup2(itsDev, 2);
+
+ TargetFinder finder_;
+
+ // Check that our current results match possible solutions.
+ aos::vision::CameraParams params0;
+ params0.set_exposure(0);
+ params0.set_brightness(40);
+ params0.set_width(640);
+ // params0.set_fps(10);
+ params0.set_height(480);
+
+ ::std::unique_ptr<CameraStream> camera0(
+ new CameraStream(params0, "/dev/video0"));
+ camera0->on_frame = [&](DataRef data,
+ monotonic_clock::time_point /*monotonic_now*/) {
+ aos::vision::ImageFormat fmt{640, 480};
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+ finder_.PreFilter(&imgs);
+
+ bool verbose = false;
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ if (polygon.empty()) {
+ } else {
+ raw_polys.push_back(polygon);
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list =
+ finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
+ }
+
+ results = finder_.FilterResults(results);
+ };
+
+ aos::events::EpollLoop loop;
+
+ for (int i = 0; i < 100; ++i) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ camera0->ReadEvent();
+ }
+
+ // TODO: Fix event loop on jevois:
+ // loop.Add(camera0.get());
+ // loop.Run();
+}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
new file mode 100644
index 0000000..557b04a
--- /dev/null
+++ b/y2019/vision/target_types.h
@@ -0,0 +1,128 @@
+#ifndef _Y2019_VISION_TARGET_TYPES_H_
+#define _Y2019_VISION_TARGET_TYPES_H_
+
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace y2019 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+ aos::vision::Vector<2> GetByIndex(size_t i) const {
+ switch (i) {
+ case 0:
+ return top;
+ case 1:
+ return inside;
+ case 2:
+ return bottom;
+ case 3:
+ return outside;
+ default:
+ return aos::vision::Vector<2>();
+ }
+ }
+ bool is_right;
+ aos::vision::Vector<2> top;
+ aos::vision::Vector<2> inside;
+ aos::vision::Vector<2> outside;
+ aos::vision::Vector<2> bottom;
+
+ aos::vision::Segment<2> major_axis;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+ TargetComponent left;
+ TargetComponent right;
+
+ static Target MakeTemplate();
+ // Get the points in some order (will match against the template).
+ std::array<aos::vision::Vector<2>, 8> toPointList() const;
+};
+
+struct IntrinsicParams {
+ static constexpr size_t kNumParams = 2;
+
+ double mount_angle = 10.0481 / 180.0 * M_PI;
+ double focal_length = 729.445;
+
+ void set(double *data) {
+ data[0] = mount_angle;
+ data[1] = focal_length;
+ }
+ static IntrinsicParams get(const double *data) {
+ IntrinsicParams out;
+ out.mount_angle = data[0];
+ out.focal_length = data[1];
+ return out;
+ }
+};
+
+struct ExtrinsicParams {
+ static constexpr size_t kNumParams = 4;
+
+ double y = 18.0 * 0.0254;
+ double z = 23.0 * 0.0254;
+ double r1 = 1.0 / 180 * M_PI;
+ double r2 = -1.0 / 180 * M_PI;
+
+ void set(double *data) {
+ data[0] = y;
+ data[1] = z;
+ data[2] = r1;
+ data[3] = r2;
+ }
+ static ExtrinsicParams get(const double *data) {
+ ExtrinsicParams out;
+ out.y = data[0];
+ out.z = data[1];
+ out.r1 = data[2];
+ out.r2 = data[3];
+ return out;
+ }
+};
+// Projects a point from idealized template space to camera space.
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+// An intermediate for of the results.
+// These are actual awnsers, but in a form from the solver that is not ready for
+// the wire.
+struct IntermediateResult {
+ ExtrinsicParams extrinsics;
+
+ // Error from solver calulations.
+ double solver_error;
+};
+
+// Final foramtting ready for output on the wire.
+struct TargetResult {
+ // Distance to the target in meters. Specifically, the distance from the
+ // center of the camera's image plane to the center of the target.
+ float distance;
+ // Height of the target in meters. Specifically, the distance from the floor
+ // to the center of the target.
+ float height;
+
+ // Heading of the center of the target in radians. Zero is straight out
+ // perpendicular to the camera's image plane. Images to the left (looking at a
+ // camera image) are at a positive angle.
+ float heading;
+
+ // The angle between the target and the camera's image plane. This is
+ // projected so both are assumed to be perpendicular to the floor. Parallel
+ // targets have a skew of zero. Targets rotated such that their left edge
+ // (looking at a camera image) is closer are at a positive angle.
+ float skew;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 339a388..b3969fb 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -194,6 +194,11 @@
stilts_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ // Vacuum pressure sensor
+ void set_vacuum_sensor(int port) {
+ vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -236,6 +241,13 @@
Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
values.stilts.potentiometer_offset);
+ // Suction
+ constexpr float kMinVoltage = 0.5;
+ constexpr float kMaxVoltage = 2.1;
+ superstructure_message->suction_pressure =
+ (vacuum_sensor_->GetVoltage() - kMinVoltage) /
+ (kMaxVoltage - kMinVoltage);
+
superstructure_message.Send();
}
}
@@ -244,6 +256,8 @@
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;
+ ::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
// TODO(sabina): Add wrist and elevator hall effects.
};
@@ -254,6 +268,10 @@
elevator_victor_ = ::std::move(t);
}
+ void set_suction_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ suction_victor_ = ::std::move(t);
+ }
+
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
intake_victor_ = ::std::move(t);
}
@@ -267,12 +285,12 @@
}
private:
- virtual void Read() override {
+ void Read() override {
::y2019::control_loops::superstructure::superstructure_queue.output
.FetchAnother();
}
- virtual void Write() override {
+ void Write() override {
auto &queue =
::y2019::control_loops::superstructure::superstructure_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
@@ -292,19 +310,24 @@
stilts_victor_->SetSpeed(::aos::Clip(queue->stilts_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
+
+ suction_victor_->SetSpeed(
+ ::aos::Clip(queue->pump_voltage, -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
}
- virtual void Stop() override {
+ void Stop() override {
LOG(WARNING, "Superstructure output too old.\n");
elevator_victor_->SetDisabled();
intake_victor_->SetDisabled();
wrist_victor_->SetDisabled();
stilts_victor_->SetDisabled();
+ suction_victor_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
- wrist_victor_, stilts_victor_;
+ wrist_victor_, stilts_victor_, suction_victor_;
};
class SolenoidWriter {
@@ -314,11 +337,13 @@
".y2019.control_loops.superstructure.superstructure_queue.output") {
}
- void set_big_suction_cup(int index) {
- big_suction_cup_ = pcm_.MakeSolenoid(index);
+ void set_big_suction_cup(int index0, int index1) {
+ big_suction_cup0_ = pcm_.MakeSolenoid(index0);
+ big_suction_cup1_ = pcm_.MakeSolenoid(index1);
}
- void set_small_suction_cup(int index) {
- small_suction_cup_ = pcm_.MakeSolenoid(index);
+ void set_small_suction_cup(int index0, int index1) {
+ small_suction_cup0_ = pcm_.MakeSolenoid(index0);
+ small_suction_cup1_ = pcm_.MakeSolenoid(index1);
}
void set_intake_roller_talon(
@@ -348,8 +373,10 @@
if (superstructure_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- big_suction_cup_->Set(!superstructure_->intake_suction_top);
- small_suction_cup_->Set(!superstructure_->intake_suction_bottom);
+ big_suction_cup0_->Set(!superstructure_->intake_suction_top);
+ big_suction_cup1_->Set(!superstructure_->intake_suction_top);
+ small_suction_cup0_->Set(!superstructure_->intake_suction_bottom);
+ small_suction_cup1_->Set(!superstructure_->intake_suction_bottom);
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
@@ -374,8 +401,8 @@
private:
::frc971::wpilib::BufferedPcm pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup_,
- small_suction_cup_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup0_,
+ big_suction_cup1_, small_suction_cup0_, small_suction_cup1_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
intake_rollers_talon_;
@@ -424,6 +451,7 @@
reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
+ reader.set_vacuum_sensor(7);
::std::thread reader_thread(::std::ref(reader));
@@ -450,8 +478,8 @@
superstructure_writer.set_elevator_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
// TODO(austin): Do the vacuum
- //superstructure_writer.set_vacuum(
- //::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+ superstructure_writer.set_suction_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
superstructure_writer.set_intake_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
superstructure_writer.set_wrist_victor(
@@ -465,8 +493,8 @@
SolenoidWriter solenoid_writer;
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
- solenoid_writer.set_big_suction_cup(0);
- solenoid_writer.set_small_suction_cup(1);
+ solenoid_writer.set_big_suction_cup(0, 1);
+ solenoid_writer.set_small_suction_cup(2, 3);
::std::thread solenoid_writer_thread(::std::ref(solenoid_writer));