Merge "Chop out the MotorDanger junk"
diff --git a/NO_BUILD_ROBORIO b/NO_BUILD_ROBORIO
index acd53e3..48ddd44 100644
--- a/NO_BUILD_ROBORIO
+++ b/NO_BUILD_ROBORIO
@@ -11,3 +11,4 @@
-//y2016_bot4/control_loops/python/...
-//third_party/protobuf/...
-//y2016_bot3/...
+-//y2017/control_loops/python/...
diff --git a/WORKSPACE b/WORKSPACE
index 01187a5..4db4ba8 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -21,8 +21,8 @@
new_http_archive(
name = 'arm_frc_linux_gnueabi_repo',
build_file = 'tools/cpp/arm-frc-linux-gnueabi/arm-frc-linux-gnueabi.BUILD',
- sha256 = '9e93b0712e90d11895444f720f0c90c649fb9becb4ca28bb50749d9074eb1306',
- url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2016.tar.gz',
+ sha256 = '9e2e58f0a668c22e46486a76df8b9da08f526cd8bf4e579f19b461f70a358bf8',
+ url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2017.tar.xz',
)
# Recompressed version of the one downloaded from Linaro at
diff --git a/aos/build/queues/output/q_file.rb b/aos/build/queues/output/q_file.rb
index 471bdcb..a2f438d 100644
--- a/aos/build/queues/output/q_file.rb
+++ b/aos/build/queues/output/q_file.rb
@@ -49,7 +49,11 @@
def initialize(path)
@path = path
end
+ def extern=(value)
+ @extern=value
+ end
def create(cpp_tree)
+ return if (@extern)
# inc = cpp_tree.header.add_include("\"#{@path}\"")
inc = cpp_tree.add_header_include("\"#{@path}\"")
cpp_tree.set(self,inc)
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 474c5ce..d6813fb 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -102,10 +102,23 @@
template <class T>
void ControlLoop<T>::Run() {
- while (true) {
+ struct sigaction action;
+ action.sa_handler = &ControlLoop<T>::Quit;
+ sigemptyset(&action.sa_mask);
+ action.sa_flags = SA_RESETHAND;
+
+ PCHECK(sigaction(SIGTERM, &action, nullptr));
+ PCHECK(sigaction(SIGQUIT, &action, nullptr));
+ PCHECK(sigaction(SIGINT, &action, nullptr));
+
+ while (run_) {
Iterate();
}
+ LOG(INFO, "Shutting down\n");
}
+template <class T>
+::std::atomic<bool> ControlLoop<T>::run_{true};
+
} // namespace controls
} // namespace aos
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index 02aa067..70e87f8 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -2,10 +2,11 @@
#define AOS_CONTROL_LOOP_CONTROL_LOOP_H_
#include <string.h>
+#include <atomic>
-#include "aos/common/type_traits.h"
#include "aos/common/queue.h"
#include "aos/common/time.h"
+#include "aos/common/type_traits.h"
#include "aos/common/util/log_interval.h"
namespace aos {
@@ -115,7 +116,12 @@
uint32_t UniqueID() override { return control_loop_->hash(); }
+
protected:
+ static void Quit(int /*signum*/) {
+ run_ = false;
+ }
+
// Runs an iteration of the control loop.
// goal is the last goal that was sent. It might be any number of cycles old
// or nullptr if we haven't ever received a goal.
@@ -159,6 +165,8 @@
SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
SimpleLogInterval no_goal_ =
SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
+
+ static ::std::atomic<bool> run_;
};
} // namespace controls
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index 3ecb653..1e9b936 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -4,8 +4,8 @@
// A bitmask of the button state.
uint16_t buttons;
- // The 4 joystick axes.
- double[4] axis;
+ // The 6 joystick axes.
+ double[6] axis;
// The POV axis.
int32_t pov;
diff --git a/aos/common/scoped_fd.h b/aos/common/scoped_fd.h
index 57ecdd8..29ccf6e 100644
--- a/aos/common/scoped_fd.h
+++ b/aos/common/scoped_fd.h
@@ -1,7 +1,10 @@
+#ifndef _AOS_COMMON_SCOPED_FD_
+#define _AOS_COMMON_SCOPED_FD_
+
#include <unistd.h>
-#include "aos/common/macros.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/macros.h"
namespace aos {
@@ -9,9 +12,7 @@
class ScopedFD {
public:
explicit ScopedFD(int fd = -1) : fd_(fd) {}
- ~ScopedFD() {
- Close();
- }
+ ~ScopedFD() { Close(); }
int get() const { return fd_; }
int release() {
const int r = fd_;
@@ -25,6 +26,7 @@
}
}
operator bool() const { return fd_ != -1; }
+
private:
int fd_;
void Close() {
@@ -38,3 +40,5 @@
};
} // namespace aos
+
+#endif // _AOS_COMMON_SCOPED_FD_
diff --git a/aos/input/joystick_input.cc b/aos/input/joystick_input.cc
index a6ada96..7508c3c 100644
--- a/aos/input/joystick_input.cc
+++ b/aos/input/joystick_input.cc
@@ -1,6 +1,7 @@
#include "aos/input/joystick_input.h"
#include <string.h>
+#include <atomic>
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/logging.h"
@@ -9,9 +10,23 @@
namespace aos {
namespace input {
+::std::atomic<bool> JoystickInput::run_;
+
+void JoystickInput::Quit(int /*signum*/) { run_ = false; }
+
void JoystickInput::Run() {
+ run_ = true;
+ struct sigaction action;
+ action.sa_handler = &JoystickInput::Quit;
+ sigemptyset(&action.sa_mask);
+ action.sa_flags = SA_RESETHAND;
+
+ PCHECK(sigaction(SIGTERM, &action, nullptr));
+ PCHECK(sigaction(SIGQUIT, &action, nullptr));
+ PCHECK(sigaction(SIGINT, &action, nullptr));
+
driver_station::Data data;
- while (true) {
+ while (run_) {
joystick_state.FetchAnother();
data.Update(*joystick_state);
@@ -60,6 +75,7 @@
RunIteration(data);
}
+ LOG(INFO, "Shutting down\n");
}
} // namespace input
diff --git a/aos/input/joystick_input.h b/aos/input/joystick_input.h
index ec8b52a..b10b1ba 100644
--- a/aos/input/joystick_input.h
+++ b/aos/input/joystick_input.h
@@ -1,6 +1,8 @@
#ifndef AOS_INPUT_JOYSTICK_INPUT_H_
#define AOS_INPUT_JOYSTICK_INPUT_H_
+#include <atomic>
+
#include "aos/common/input/driver_station_data.h"
namespace aos {
@@ -18,6 +20,10 @@
private:
// Subclasses should do whatever they want with data here.
virtual void RunIteration(const driver_station::Data &data) = 0;
+
+ static void Quit(int /*signum*/);
+
+ static ::std::atomic<bool> run_;
};
// Class which will proxy joystick information from UDP packets to the queues.
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index a024aba..f823cc0 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -1,3 +1,4 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
package(default_visibility = ['//visibility:public'])
cc_library(
@@ -93,8 +94,7 @@
],
)
-"""
-cc_library(
+gtk_dependent_cc_library(
name = 'stream_view',
hdrs = ['stream_view.h'],
deps = [
@@ -103,4 +103,3 @@
'//aos/vision/image:image_types',
],
)
-"""
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
new file mode 100644
index 0000000..3b4fa7c
--- /dev/null
+++ b/aos/vision/debug/BUILD
@@ -0,0 +1,22 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+ name = "overlay",
+ hdrs = ["overlay.h"],
+ deps = [
+ '//aos/vision/math:vector',
+ '//aos/vision/math:segment',
+ '//aos/vision/image:image_types',
+ ],
+)
+
+gtk_dependent_cc_library(name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ hdrs = ["debug_viewer.h"],
+ deps = [
+ '@usr_repo//:gtk+-3.0',
+ "//aos/vision/image:image_types",
+ ":overlay",
+ ]
+)
diff --git a/aos/vision/debug/debug_viewer.cc b/aos/vision/debug/debug_viewer.cc
new file mode 100644
index 0000000..331f733
--- /dev/null
+++ b/aos/vision/debug/debug_viewer.cc
@@ -0,0 +1,166 @@
+#include "aos/vision/debug/debug_viewer.h"
+
+#include <gdk-pixbuf/gdk-pixbuf.h>
+#include <gtk/gtk.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <functional>
+#include <memory>
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+template <typename T, gboolean (T::*DrawMethod)(cairo_t *cr)>
+gboolean DrawCallback(GtkWidget *, cairo_t *cr, gpointer data) {
+ return ((*((T *)data)).*DrawMethod)(cr);
+}
+
+template <typename T, gboolean (T::*DrawMethod)(cairo_t *cr)>
+void g_draw_signal_connect(GtkWidget *widget, T *obj) {
+ gboolean (*fnptr)(GtkWidget *, cairo_t *, gpointer) =
+ DrawCallback<T, DrawMethod>;
+ g_signal_connect(widget, "draw", G_CALLBACK(fnptr), obj);
+}
+
+struct DebugViewer::Internals {
+ Internals(bool flip) : flip_(flip) {}
+
+ gboolean Draw(cairo_t *cr) {
+ needs_draw = false;
+ cairo_scale(cr, scale_factor, scale_factor);
+ if (pixbuf != nullptr) {
+ cairo_save(cr);
+ if (flip_) {
+ cairo_translate(cr, ptr.fmt().w, ptr.fmt().h);
+ cairo_scale(cr, -1, -1);
+ }
+ gdk_cairo_set_source_pixbuf(cr, pixbuf, 0.0, 0.0);
+ cairo_paint(cr);
+ cairo_restore(cr);
+ }
+
+ int w = ptr.fmt().w;
+ int h = ptr.fmt().h;
+ if (overlays) {
+ for (const auto &ov : *overlays) {
+ cairo_save(cr);
+ CairoRender render(cr);
+ // move the drawing to match the window size
+ ov->Draw(&render, w, h);
+ cairo_restore(cr);
+ }
+ }
+
+ return FALSE;
+ }
+
+ GdkPixbuf *pixbuf = nullptr;
+ GtkWidget *drawing_area = nullptr;
+ ImagePtr ptr;
+ bool needs_draw = true;
+ GtkWidget *window;
+ std::vector<OverlayBase *> *overlays = nullptr;
+ double scale_factor;
+
+ // flip the image rows on drawing
+ bool flip_ = false;
+
+ // clear per frame
+ bool clear_per_frame_ = true;
+};
+
+void DebugViewer::SetOverlays(std::vector<OverlayBase *> *overlays) {
+ self->overlays = overlays;
+}
+
+void DebugViewer::Redraw() {
+ if (!self->needs_draw) {
+ gtk_widget_queue_draw(self->drawing_area);
+ self->needs_draw = true;
+ }
+}
+
+void DebugViewer::UpdateImage(ImagePtr ptr) {
+ if (ptr.data() != self->ptr.data()) {
+ int w = ptr.fmt().w;
+ int h = ptr.fmt().h;
+ self->pixbuf = gdk_pixbuf_new_from_data(
+ (const unsigned char *)ptr.data(), GDK_COLORSPACE_RGB, FALSE, 8,
+ ptr.fmt().w, ptr.fmt().h, 3 * ptr.fmt().w, NULL, NULL);
+ self->ptr = ptr;
+
+ gtk_window_set_default_size(GTK_WINDOW(self->window), w * scale_factor,
+ h * scale_factor);
+
+ gtk_widget_set_size_request(self->drawing_area, w * scale_factor,
+ h * scale_factor);
+ window_height_ = h;
+ window_width_ = w;
+ }
+}
+
+void DebugViewer::MoveTo(int x, int y) {
+ gtk_window_move(GTK_WINDOW(self->window), x, y);
+}
+
+void DebugViewer::SetScale(double scale_factor_inp) {
+ int w = window_width_;
+ int h = window_height_;
+
+ scale_factor = scale_factor_inp;
+ self->scale_factor = scale_factor;
+
+ gtk_window_resize(GTK_WINDOW(self->window), w * scale_factor,
+ h * scale_factor);
+
+ gtk_widget_set_size_request(self->drawing_area, w * scale_factor,
+ h * scale_factor);
+}
+
+gboolean debug_viewer_key_press_event(GtkWidget * /*widget*/,
+ GdkEventKey *event, gpointer user_data) {
+ auto &key_press_cb =
+ reinterpret_cast<DebugViewer *>(user_data)->key_press_event;
+ if (key_press_cb) key_press_cb(event->keyval);
+ return FALSE;
+}
+
+DebugViewer::DebugViewer(bool flip) : self(new Internals(flip)) {
+ self->scale_factor = scale_factor;
+ GtkWidget *window;
+ auto drawing_area = self->drawing_area = gtk_drawing_area_new();
+ gtk_widget_set_size_request(drawing_area, window_width_ * scale_factor,
+ window_height_ * scale_factor);
+ gtk_widget_add_events(drawing_area, GDK_KEY_PRESS_MASK);
+
+ g_draw_signal_connect<DebugViewer::Internals, &DebugViewer::Internals::Draw>(
+ drawing_area, self.get());
+
+ window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+ self->window = window;
+ g_signal_connect(window, "key-press-event",
+ G_CALLBACK(debug_viewer_key_press_event), this);
+ gtk_window_set_title(GTK_WINDOW(window), "Window");
+ gtk_window_set_default_size(GTK_WINDOW(window), window_width_ * scale_factor,
+ window_height_ * scale_factor);
+
+ gtk_container_add(GTK_CONTAINER(window), drawing_area);
+ gtk_widget_show_all(window);
+}
+DebugViewer::~DebugViewer() {}
+
+void CairoRender::Text(int x, int y, int /*text_x*/, int /*text_y*/,
+ const std::string &text) {
+ auto *pango_lay = pango_cairo_create_layout(cr_);
+ cairo_move_to(cr_, x, y);
+ pango_layout_set_text(pango_lay, text.data(), text.size());
+ pango_cairo_show_layout(cr_, pango_lay);
+ g_object_unref(pango_lay);
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/debug_viewer.h b/aos/vision/debug/debug_viewer.h
new file mode 100644
index 0000000..3bada28
--- /dev/null
+++ b/aos/vision/debug/debug_viewer.h
@@ -0,0 +1,78 @@
+#ifndef AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#define AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+
+#include <cairo.h>
+#include <functional>
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+// Implement Cairo version of RenderInterface.
+class CairoRender : public RenderInterface {
+ public:
+ explicit CairoRender(cairo_t *cr) : cr_(cr) {}
+ virtual ~CairoRender() {}
+
+ void Translate(double x, double y) override { cairo_translate(cr_, x, y); }
+
+ void SetSourceRGB(double r, double g, double b) override {
+ cairo_set_source_rgb(cr_, r, g, b);
+ }
+
+ void MoveTo(double x, double y) override { cairo_move_to(cr_, x, y); }
+
+ void LineTo(double x, double y) override { cairo_line_to(cr_, x, y); }
+
+ void Circle(double x, double y, double r) override {
+ cairo_arc(cr_, x, y, r, 0.0, 2 * M_PI);
+ }
+
+ void Stroke() override { cairo_stroke(cr_); }
+
+ void Text(int x, int y, int text_x, int text_y,
+ const std::string &text) override;
+
+ private:
+ cairo_t *cr_;
+};
+
+// Simple debug view window.
+class DebugViewer {
+ public:
+ struct Internals;
+ explicit DebugViewer(bool flip);
+ ~DebugViewer();
+ // Explicit redraw queuing (Will not double-queue).
+ void Redraw();
+
+ // This will resize the window as well as updating to draw from the
+ // (not owned) ptr. When you change ptr, you should call Redraw();
+ void UpdateImage(ImagePtr ptr);
+
+ // Sets up the window to draw a list of overlays.
+ // See overlay.h for more info.
+ void SetOverlays(std::vector<OverlayBase *> *overlay);
+
+ // Resizes the window.
+ void SetScale(double scale_factor);
+
+ // Move window.
+ void MoveTo(int x, int y);
+
+ // Set to change the key_press behaviour.
+ // The argument type is a constant that looks like: GDK_KEY_#{key_val_name}
+ std::function<void(uint32_t)> key_press_event;
+
+ private:
+ double scale_factor = 1.0;
+ int window_width_ = 100;
+ int window_height_ = 100;
+ std::unique_ptr<Internals> self;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // AOS_VISION_DEBUG_DEBUG_VIEWER_H_
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
new file mode 100644
index 0000000..fbd6838
--- /dev/null
+++ b/aos/vision/debug/overlay.h
@@ -0,0 +1,198 @@
+#ifndef _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
+#define _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
+
+#include <string>
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+// Abstract away rendering to avoid compiling gtk for arm.
+// This should match a reduced cairo rendering api.
+class RenderInterface {
+ public:
+ RenderInterface() {}
+ RenderInterface(RenderInterface &&other) = delete;
+ RenderInterface(const RenderInterface &other) = delete;
+ ~RenderInterface() {}
+
+ virtual void Translate(double x, double y) = 0;
+ virtual void SetSourceRGB(double r, double g, double b) = 0;
+ virtual void MoveTo(double x, double y) = 0;
+ virtual void LineTo(double x, double y) = 0;
+ virtual void Circle(double x, double y, double r) = 0;
+ // negative in x, y, text_x, text_y measures from max in those value
+ virtual void Text(int x, int y, int text_x, int text_y,
+ const std::string &text) = 0;
+ virtual void Stroke() = 0;
+};
+
+// Interface for a list of overlays to be drawn onto a debug image.
+// These will be passed into the running vision algorithms to output debug info,
+// so they must not have costly side-effects.
+class OverlayBase {
+ public:
+ OverlayBase() {}
+ virtual ~OverlayBase() {}
+
+ // Draws this overlay to the given canvas.
+ virtual void Draw(RenderInterface *render, double /* width */,
+ double /* height */) = 0;
+
+ // Clears the entire overlay.
+ virtual void Reset() = 0;
+
+ PixelRef color = {255, 0, 0};
+ double scale = 1.0;
+};
+
+// A lambda that renders directly to the render interface.
+class LambdaOverlay : public OverlayBase {
+ public:
+ std::function<void(RenderInterface *, double, double)> draw_fn;
+ void Draw(RenderInterface *render, double width, double height) override {
+ if (draw_fn) draw_fn(render, width, height);
+ }
+ void Reset() override {}
+};
+
+// Lines rendered in a coordinate system where the origin is the center
+// of the screen
+class LinesOverlay : public OverlayBase {
+ public:
+ LinesOverlay() : OverlayBase() {}
+ ~LinesOverlay() {}
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
+ }
+
+ void add_point(Vector<2> pt) { add_point(pt, color); }
+
+ // add a new point connected to the last point in the line
+ void add_point(Vector<2> pt, PixelRef newColor) {
+ if (lines_.empty()) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
+ } else {
+ Vector<2> st = lines_.back().first.B();
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, pt), newColor));
+ }
+ }
+
+ void Draw(RenderInterface *render, double w, double h) override {
+ render->Translate(w / 2.0, h / 2.0);
+ for (const auto &ln : lines_) {
+ PixelRef localColor = ln.second;
+ render->SetSourceRGB(localColor.r / 255.0, localColor.g / 255.0,
+ localColor.b / 255.0);
+ render->MoveTo(scale * ln.first.A().x(), -scale * ln.first.A().y());
+ render->LineTo(scale * ln.first.B().x(), -scale * ln.first.B().y());
+ render->Stroke();
+ }
+ }
+
+ // Empting the list will blank the whole overlay
+ void Reset() override { lines_.clear(); }
+
+ private:
+ // lines in this over lay
+ std::vector<std::pair<Segment<2>, PixelRef>> lines_;
+};
+
+// Lines rendered in pixel coordinates (Should match up with the screen.)
+class PixelLinesOverlay : public OverlayBase {
+ public:
+ PixelLinesOverlay() : OverlayBase() {}
+ ~PixelLinesOverlay() {}
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
+ }
+
+ void start_new_profile() { start_profile = true; }
+
+ // add a new point connected to the last point in the line
+ void add_point(Vector<2> pt, PixelRef newColor) {
+ if (lines_.empty() || start_profile) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
+ start_profile = false;
+ } else {
+ Vector<2> st = lines_.back().first.B();
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, pt), newColor));
+ }
+ }
+
+ void Draw(RenderInterface *render, double /*width*/,
+ double /*hieght*/) override {
+ for (const auto &ln : lines_) {
+ PixelRef localColor = ln.second;
+ render->SetSourceRGB(localColor.r / 255.0, localColor.g / 255.0,
+ localColor.b / 255.0);
+ render->MoveTo(ln.first.A().x(), ln.first.A().y());
+ render->LineTo(ln.first.B().x(), ln.first.B().y());
+ render->Stroke();
+ }
+ }
+
+ // Empting the list will blank the whole overlay.
+ void Reset() override { lines_.clear(); }
+
+ private:
+ // Lines in this overlay.
+ std::vector<std::pair<Segment<2>, PixelRef>> lines_;
+ bool start_profile = false;
+};
+
+// Circles rendered in a coordinate system where the origin is the center
+// of the screen.
+class CircleOverlay : public OverlayBase {
+ public:
+ CircleOverlay() : OverlayBase() {}
+ ~CircleOverlay() {}
+
+ // build a circle as a point and radius
+ std::pair<Vector<2>, double> *add_circle(Vector<2> center, double radius) {
+ circles_.emplace_back(std::pair<Vector<2>, double>(center, radius));
+ return &(circles_.back());
+ }
+
+ void Draw(RenderInterface *render, double w, double h) {
+ render->Translate(w / 2.0, h / 2.0);
+ render->SetSourceRGB(color.r / 255.0, color.g / 255.0, color.b / 255.0);
+ for (const auto &circle : circles_) {
+ render->Circle(scale * circle.first.x(), -scale * circle.first.y(),
+ scale * circle.second);
+ render->Stroke();
+ }
+ }
+
+ // empting the list will blank the whole overlay
+ void Reset() { circles_.clear(); }
+
+ private:
+ // circles in this overlay
+ std::vector<std::pair<Vector<2>, double>> circles_;
+};
+
+} // vision
+} // aos
+
+#endif // _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index c3400fe..e003dc9 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -11,6 +11,14 @@
],
)
+cc_library(name = "socket_types",
+ hdrs = ["socket_types.h"],
+ deps = [
+ "//aos/vision/events:tcp_server",
+ "//aos/vision/image:image_types",
+ ],
+)
+
cc_library(
name = 'intrusive_free_list',
hdrs = ['intrusive_free_list.h'],
diff --git a/aos/vision/events/socket_types.h b/aos/vision/events/socket_types.h
new file mode 100644
index 0000000..2432cb9
--- /dev/null
+++ b/aos/vision/events/socket_types.h
@@ -0,0 +1,78 @@
+#ifndef _AOS_VISION_EVENTS_SOCKET_TYPES_H_
+#define _AOS_VISION_EVENTS_SOCKET_TYPES_H_
+
+#include <poll.h>
+#include <stdint.h>
+
+#include "aos/vision/events/tcp_server.h"
+#include "aos/vision/image/image_types.h"
+#include "google/protobuf/message.h"
+
+namespace aos {
+namespace events {
+
+// Simple TCP client connection that sends messages prefixed by length.
+// Useful to broadcast to a message all connected clients.
+class DataSocket : public events::SocketConnection {
+ public:
+ // Aliasing cast.
+ union data_len {
+ uint32_t len;
+ char buf[4];
+ };
+
+ DataSocket(events::TCPServerBase *serv, int fd)
+ : events::SocketConnection(serv, fd) {}
+
+ ~DataSocket() { printf("Closed connection on descriptor %d\n", fd()); }
+
+ void ReadEvent() override {
+ // Ignore reads, but don't leave them pending.
+ ssize_t count;
+ char buf[512];
+ while (true) {
+ count = read(fd(), &buf, sizeof buf);
+ if (count <= 0) return;
+ }
+ }
+
+ void Emit(const google::protobuf::Message &data) {
+ std::string d;
+ if (data.SerializeToString(&d)) {
+ Emit(d);
+ }
+ }
+
+ void Emit(vision::DataRef data) {
+ data_len len;
+ len.len = data.size();
+ int res = write(fd(), len.buf, sizeof len.buf);
+ if (res == -1) {
+ printf("Emit Error on write\n");
+ }
+ size_t write_count = 0;
+ while (write_count < data.size()) {
+ int len =
+ write(fd(), &data.data()[write_count], data.size() - write_count);
+ if (len == -1) {
+ if (errno == EAGAIN) {
+ struct pollfd waiting;
+ waiting.fd = fd();
+ waiting.events = POLLOUT;
+ poll(&waiting, 1, -1);
+ } else {
+ close(fd());
+ return;
+ }
+ } else {
+ write_count += len;
+ }
+ if (write_count != data.size()) printf("wrote: %d\n", len);
+ }
+ }
+};
+
+} // namespace events
+} // namespace aos
+
+#endif // _AOS_VISION_EVENTS_SOCKET_TYPES_H_
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index 2d3d1d1..b5367f6 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -5,24 +5,24 @@
#include "aos/common/logging/logging.h"
namespace aos {
-namespace vision {
+namespace events {
-TXUdpSocket::TXUdpSocket(const char *ip_addr, int port)
+TXUdpSocket::TXUdpSocket(const std::string &ip_addr, int port)
: fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
sockaddr_in destination_in;
memset(&destination_in, 0, sizeof(destination_in));
destination_in.sin_family = AF_INET;
destination_in.sin_port = htons(port);
- if (inet_aton(ip_addr, &destination_in.sin_addr) == 0) {
- LOG(FATAL, "invalid IP address %s\n", ip_addr);
+ if (inet_aton(ip_addr.c_str(), &destination_in.sin_addr) == 0) {
+ LOG(FATAL, "invalid IP address %s\n", ip_addr.c_str());
}
PCHECK(connect(fd_.get(), reinterpret_cast<sockaddr *>(&destination_in),
sizeof(destination_in)));
}
-int TXUdpSocket::Send(const void *data, int size) {
- return PCHECK(send(fd_.get(), static_cast<const char *>(data), size, 0));
+int TXUdpSocket::Send(const char *data, int size) {
+ return PCHECK(send(fd_.get(), data, size, 0));
}
RXUdpSocket::RXUdpSocket(int port)
@@ -42,5 +42,5 @@
return PCHECK(recv(fd_.get(), static_cast<char *>(data), size, 0));
}
-} // namespace vision
+} // namespace events
} // namespace aos
diff --git a/aos/vision/events/udp.h b/aos/vision/events/udp.h
index a12a8bf..ed4a8e7 100644
--- a/aos/vision/events/udp.h
+++ b/aos/vision/events/udp.h
@@ -1,27 +1,28 @@
-#ifndef AOS_VISION_IMAGE_UDP_H_
-#define AOS_VISION_IMAGE_UDP_H_
+#ifndef AOS_VISION_EVENTS_UDP_H_
+#define AOS_VISION_EVENTS_UDP_H_
#include <arpa/inet.h>
-#include <sys/socket.h>
#include <math.h>
+#include <sys/socket.h>
#include <unistd.h>
+#include <string>
#include <vector>
#include "aos/common/macros.h"
#include "aos/common/scoped_fd.h"
namespace aos {
-namespace vision {
+namespace events {
// Simple wrapper around a transmitting UDP socket.
//
// LOG(FATAL)s for all errors, including from Send.
class TXUdpSocket {
public:
- TXUdpSocket(const char *ip_addr, int port);
+ TXUdpSocket(const std::string &ip_addr, int port);
// Returns the number of bytes actually sent.
- int Send(const void *data, int size);
+ int Send(const char *data, int size);
private:
ScopedFD fd_;
@@ -45,7 +46,7 @@
DISALLOW_COPY_AND_ASSIGN(RXUdpSocket);
};
-} // namespace vision
+} // namespace events
} // namespace aos
-#endif // AOS_VISION_IMAGE_UDP_H_
+#endif // AOS_VISION_EVENTS_UDP_H_
diff --git a/aos/vision/events/udp_test.cc b/aos/vision/events/udp_test.cc
index 87046ba..33fb4d6 100644
--- a/aos/vision/events/udp_test.cc
+++ b/aos/vision/events/udp_test.cc
@@ -3,7 +3,7 @@
#include "gtest/gtest.h"
namespace aos {
-namespace vision {
+namespace events {
TEST(UDPTest, SendRecv) {
RXUdpSocket rx(1109);
@@ -11,13 +11,13 @@
int txdata[] = {1, 2, 3, 4};
int rxdata[4];
- tx.Send(static_cast<const void *>(&txdata), sizeof(txdata));
- rx.Recv(static_cast<void *>(&rxdata), sizeof(rxdata));
+ tx.Send(reinterpret_cast<const char *>(&txdata), sizeof(txdata));
+ rx.Recv(reinterpret_cast<char *>(&rxdata), sizeof(rxdata));
EXPECT_EQ(txdata[0], rxdata[0]);
EXPECT_EQ(txdata[1], rxdata[1]);
EXPECT_EQ(txdata[2], rxdata[2]);
EXPECT_EQ(txdata[3], rxdata[3]);
}
-} // namespace vision
+} // namespace events
} // namespace aos
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index ca2d8be..bdfbc31 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -9,6 +9,8 @@
namespace aos {
namespace vision {
+// Converts a camera reader into a virtual base class that calls ProcessImage
+// on each new image.
class ImageStreamEvent : public ::aos::events::EpollEvent {
public:
static std::unique_ptr<::camera::Reader> GetCamera(
@@ -17,28 +19,28 @@
using namespace std::placeholders;
std::unique_ptr<::camera::Reader> camread(new ::camera::Reader(
fname,
- std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params);
+ std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params));
camread->StartAsync();
return camread;
}
explicit ImageStreamEvent(std::unique_ptr<::camera::Reader> reader)
- : ::aos::events::EpollEvent(reader->fd()), reader_(reader) {}
+ : ::aos::events::EpollEvent(reader->fd()), reader_(std::move(reader)) {}
explicit ImageStreamEvent(const std::string &fname,
camera::CameraParams params)
: ImageStreamEvent(GetCamera(fname, this, params)) {}
- void ProcessHelper(DataRef data, uint64_t timestamp) {
+ void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp) {
if (data.size() < 300) {
- LOG(INFO, "got bad img: %d of size(%lu)\n", (int)timestamp, data.size());
+ LOG(INFO, "got bad img of size(%lu)\n", data.size());
return;
}
ProcessImage(data, timestamp);
}
- virtual void ProcessImage(DataRef data, uint64_t timestamp) = 0;
+ virtual void ProcessImage(DataRef data, aos::monotonic_clock::time_point timestamp) = 0;
- void ReadEvent(Context /*ctx*/) override { reader_->HandleFrame(); }
+ void ReadEvent() override { reader_->HandleFrame(); }
private:
std::unique_ptr<::camera::Reader> reader_;
diff --git a/debian/usr.BUILD b/debian/usr.BUILD
index 7ee1bf3..e34e933 100644
--- a/debian/usr.BUILD
+++ b/debian/usr.BUILD
@@ -1,3 +1,5 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+
package(default_visibility = ['@//debian:__pkg__'])
cc_library(
@@ -86,3 +88,61 @@
],
visibility = ['//visibility:public'],
)
+
+gtk_dependent_cc_library(
+ name = 'gtk+-3.0',
+ hdrs = glob([
+ 'include/gtk-3.0/**/*.h',
+ 'include/at-spi2-atk/2.0/**/*.h',
+ 'include/at-spi-2.0/**/*.h',
+ 'include/dbus-1.0/**/*.h',
+ 'lib/x86_64-linux-gnu/dbus-1.0/include/**/*.h',
+ 'include/gtk-3.0/**/*.h',
+ 'include/gio-unix-2.0/**/*.h',
+ 'include/cairo/**/*.h',
+ 'include/pango-1.0/**/*.h',
+ 'include/harfbuzz/**/*.h',
+ 'include/pango-1.0/**/*.h',
+ 'include/atk-1.0/**/*.h',
+ 'include/pixman-1/**/*.h',
+ 'include/freetype2/**/*.h',
+ 'include/libpng12/**/*.h',
+ 'include/gdk-pixbuf-2.0/**/*.h',
+ 'include/glib-2.0/**/*.h',
+ 'lib/x86_64-linux-gnu/glib-2.0/include/**/*.h',
+ ]),
+ includes = [
+ 'include/gtk-3.0',
+ 'include/at-spi2-atk/2.0',
+ 'include/at-spi-2.0',
+ 'include/dbus-1.0',
+ 'lib/x86_64-linux-gnu/dbus-1.0/include',
+ 'include/gtk-3.0',
+ 'include/gio-unix-2.0/',
+ 'include/cairo',
+ 'include/pango-1.0',
+ 'include/harfbuzz',
+ 'include/pango-1.0',
+ 'include/atk-1.0',
+ 'include/pixman-1',
+ 'include/freetype2',
+ 'include/libpng12',
+ 'include/gdk-pixbuf-2.0',
+ 'include/glib-2.0',
+ 'lib/x86_64-linux-gnu/glib-2.0/include',
+ ],
+ linkopts = [
+ '-lgtk-3',
+ '-lgdk-3',
+ '-lpangocairo-1.0',
+ '-lpango-1.0',
+ '-latk-1.0',
+ '-lcairo-gobject',
+ '-lcairo',
+ '-lgdk_pixbuf-2.0',
+ '-lgio-2.0',
+ '-lgobject-2.0',
+ '-lglib-2.0'
+ ],
+ visibility = ['//visibility:public'],
+)
diff --git a/frc971/constants.h b/frc971/constants.h
index ad3ce71..fbf7b1c 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -4,7 +4,7 @@
namespace frc971 {
namespace constants {
-struct ZeroingConstants {
+struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
int average_filter_size;
// The difference in scaled units between two index pulses.
@@ -16,6 +16,19 @@
double allowable_encoder_error;
};
+struct EncoderPlusIndexZeroingConstants {
+ // The amount of index pulses in the limb's range of motion.
+ int num_index_pulses;
+};
+
+struct PotAndAbsoluteEncoderZeroingConstants {
+ // The distance that the absolute encoder needs to complete a full rotation.
+ double abs_duration;
+ // Sample mechanism angle and absolute encoder value.
+ double sample_abs_value;
+ double sample_degrees;
+};
+
// Defines a range of motion for a subsystem.
// These are all absolute positions in scaled units.
struct Range {
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 2075ca7..8f8b381 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -147,3 +147,31 @@
'//third_party/eigen',
],
)
+
+queue_library(
+ name = 'profiled_subsystem_queue',
+ srcs = [
+ 'profiled_subsystem.q',
+ ],
+ deps = [
+ ':queues',
+ ],
+)
+
+cc_library(
+ name = 'profiled_subsystem',
+ srcs = [
+ 'profiled_subsystem.cc',
+ ],
+ hdrs = [
+ 'profiled_subsystem.h',
+ ],
+ deps = [
+ ':profiled_subsystem_queue',
+ ':simple_capped_state_feedback_loop',
+ ':state_feedback_loop',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:trapezoid_profile',
+ '//frc971/zeroing:zeroing',
+ ],
+)
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 05a660b..f13d82b 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -1,5 +1,20 @@
package frc971;
+// Represents all of the data for a single indexed encoder. In other words,
+// just a relative encoder with an index pulse.
+// The units on all of the positions are the same.
+// All encoder values are relative to where the encoder was at some arbitrary
+// point in time. All potentiometer values are relative to some arbitrary 0
+// position which varies with each robot.
+struct IndexPosition {
+ // Current position read from the encoder.
+ double encoder;
+ // Position from the encoder latched at the last index pulse.
+ double latched_encoder;
+ // How many index pulses we've seen since startup. Starts at 0.
+ uint32_t index_pulses;
+};
+
// Represents all of the data for a single potentiometer and indexed encoder
// pair.
// The units on all of the positions are the same.
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 6075155..1d06974 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -27,6 +27,33 @@
* A
* |
* index pulse
+ *
+ *
+ *
+ * Absolute encoder explanation:
+ *
+ * If we were to graph the output of an absolute encoder that resets every 0.1
+ * meters for example, it would looks something like the following. The y-axis
+ * represents the output of the absolute encoder. The x-axis represents the
+ * actual position of the robot's mechanism.
+ *
+ * 1 encoder segment
+ * +------+
+ *
+ * |
+ * 0.1 + /| /| /| /| /| /| /| /|
+ * | / | / | / | / | / | / | / | / |
+ * | / | / | / | / | / | / | / | / |
+ * | / | / | / | / | / | / | / | / |
+ * | / | / | / | / | / | / | / | / |
+ * | / |/ |/ |/ |/ |/ |/ |/ |
+ * 0.0 ++------+------+------+------+------+------+------+------+----
+ * 0.05 0.15 0.25 0.35 0.45 0.55 0.65 0.75 0.85
+ *
+ * An absolute encoder can be used to determine exactly where the mechanism in
+ * question is within a certain segment. As long as you know a single combo of
+ * absolute encoder reading and mechanism location you can extrapolate the
+ * remainder of the graph.
*/
PositionSensorSimulator::PositionSensorSimulator(double index_diff,
@@ -35,12 +62,14 @@
Initialize(0.0, 0.0);
}
-void PositionSensorSimulator::Initialize(double start_position,
- double pot_noise_stddev,
- double known_index_pos /* = 0*/) {
+void PositionSensorSimulator::Initialize(
+ double start_position, double pot_noise_stddev,
+ double known_index_pos /* = 0*/,
+ double known_absolute_encoder_pos /* = 0*/) {
// We're going to make the index pulse we know "segment zero".
cur_index_segment_ = floor((start_position - known_index_pos) / index_diff_);
known_index_pos_ = known_index_pos;
+ known_absolute_encoder_ = known_absolute_encoder_pos;
cur_index_ = 0;
index_count_ = 0;
cur_pos_ = start_position;
@@ -79,6 +108,22 @@
cur_pos_ = new_pos;
}
+void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
+ values->encoder = cur_pos_ - start_position_;
+
+ if (index_count_ == 0) {
+ values->latched_encoder = 0.0;
+ } else {
+ // Determine the position of the index pulse relative to absolute zero.
+ double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
+
+ // Populate the latched encoder samples.
+ values->latched_encoder = index_pulse_position - start_position_;
+ }
+
+ values->index_pulses = index_count_;
+}
+
void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
values->encoder = cur_pos_ - start_position_;
@@ -98,5 +143,15 @@
values->index_pulses = index_count_;
}
+void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
+ values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
+ values->relative_encoder = cur_pos_ - start_position_;
+ // TODO(phil): Create some lag here since this is a PWM signal it won't be
+ // instantaneous like the other signals. Better yet, its lag varies
+ // randomly with the distribution varying depending on the reading.
+ values->absolute_encoder =
+ fmod(cur_pos_ - known_index_pos_ + known_absolute_encoder_, index_diff_);
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 3ce3056..3de96c1 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -17,6 +17,9 @@
// index_diff: The interval between index pulses. This is measured in SI
// units. For example, if an index pulse hits every 5cm on the
// elevator, set this to 0.05.
+ // NOTE: When retrieving the sensor values for a
+ // PotAndAbsolutePosition message this field represents the
+ // interval between when the absolute encoder reads 0.
// noise_seed: The seed to feed into the random number generator for the
// potentiometer values.
// TODO(danielp): Allow for starting with a non-zero encoder value.
@@ -34,7 +37,8 @@
// known_index_pos: The absolute position of an index pulse.
void Initialize(double start_position,
double pot_noise_stddev,
- double known_index_pos = 0.0);
+ double known_index_pos = 0.0,
+ double known_absolute_encoder_pos = 0.0);
// Simulate the structure moving to a new position. The new value is measured
// relative to absolute zero. This will update the simulated sensors with new
@@ -46,8 +50,20 @@
// values: The target structure will be populated with simulated sensor
// readings. The readings will be in SI units. For example the units
// can be given in radians, meters, etc.
+ void GetSensorValues(IndexPosition* values);
+
+ // Get the current values of the simulated sensors.
+ // values: The target structure will be populated with simulated sensor
+ // readings. The readings will be in SI units. For example the units
+ // can be given in radians, meters, etc.
void GetSensorValues(PotAndIndexPosition* values);
+ // Get the current values of the simulated sensors.
+ // values: The target structure will be populated with simulated sensor
+ // readings. The readings will be in SI units. For example the units
+ // can be given in radians, meters, etc.
+ void GetSensorValues(PotAndAbsolutePosition* values);
+
private:
// The absolute segment between two index pulses the simulation is on. For
// example, when the current position is betwen index pulse zero and one,
@@ -65,7 +81,13 @@
// Distance between index pulses on the mechanism.
double index_diff_;
// Absolute position of a known index pulse.
+ // OR
+ // Absolute position of the absolute encoder's reading stored in
+ // known_absolute_encoder_.
double known_index_pos_;
+ // The readout of the absolute encoder when the robot's mechanism is at
+ // known_index_pos_.
+ double known_absolute_encoder_;
// Current position of the mechanism relative to absolute zero.
double cur_pos_;
// Starting position of the mechanism relative to absolute zero. See the
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 1ca5033..0c7fa0e 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -23,37 +23,46 @@
// this test is to verify that no false index pulses are generated while the
// mechanism stays between two index pulses.
const double index_diff = 0.5;
- PotAndIndexPosition position;
+ IndexPosition index_position;
+ PotAndIndexPosition pot_and_index_position;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, 0);
// Make sure that we don't accidentally hit an index pulse.
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(3.6 * index_diff, position.pot);
- ASSERT_EQ(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_position.index_pulses);
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(3.0 * index_diff, position.pot);
- ASSERT_EQ(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_position.index_pulses);
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.99 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(3.99 * index_diff, position.pot);
- ASSERT_EQ(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_position.index_pulses);
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(3.0 * index_diff, position.pot);
- ASSERT_EQ(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_position.index_pulses);
}
}
@@ -63,43 +72,58 @@
// again simulate zero noise on the potentiometer to accurately verify the
// mechanism's position during the index pulses.
const double index_diff = 0.8;
- PotAndIndexPosition position;
+ IndexPosition index_position;
+ PotAndIndexPosition pot_and_index_position;
PositionSensorSimulator sim(index_diff);
sim.Initialize(4.6 * index_diff, 0);
// Make sure that we get an index pulse on every transition.
- sim.GetSensorValues(&position);
- ASSERT_EQ(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_EQ(0u, index_position.index_pulses);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
- ASSERT_EQ(1u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(1u, index_position.index_pulses);
+ ASSERT_EQ(1u, pot_and_index_position.index_pulses);
sim.MoveTo(4.5 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
- ASSERT_EQ(2u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(2u, index_position.index_pulses);
+ ASSERT_EQ(2u, pot_and_index_position.index_pulses);
sim.MoveTo(5.9 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(5.0 * index_diff, position.latched_pot);
- ASSERT_EQ(3u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(3u, index_position.index_pulses);
+ ASSERT_EQ(3u, pot_and_index_position.index_pulses);
sim.MoveTo(6.1 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(6.0 * index_diff, position.latched_pot);
- ASSERT_EQ(4u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(4u, index_position.index_pulses);
+ ASSERT_EQ(4u, pot_and_index_position.index_pulses);
sim.MoveTo(8.7 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
- ASSERT_EQ(5u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(5u, index_position.index_pulses);
+ ASSERT_EQ(5u, pot_and_index_position.index_pulses);
sim.MoveTo(7.3 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
- ASSERT_EQ(6u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(6u, index_position.index_pulses);
+ ASSERT_EQ(6u, pot_and_index_position.index_pulses);
}
// Tests that the simulator handles non-zero specified index pulse locations
@@ -108,46 +132,65 @@
const double index_diff = 0.5;
PositionSensorSimulator sim(index_diff);
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 0.5);
- PotAndIndexPosition position;
+ IndexPosition index_position;
+ PotAndIndexPosition pot_and_index_position;
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(2u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(2u, index_position.index_pulses);
+ EXPECT_EQ(2u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
// Try it with our known index pulse not being our first one.
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(2u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(2u, index_position.index_pulses);
+ EXPECT_EQ(2u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
}
// Tests that the latched values update correctly.
@@ -195,5 +238,70 @@
EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
}
+// This test makes sure that our simulation for an absolute encoder + relative
+// encoder + pot combo works OK. Let's pretend that we know that a reading of
+// 0.07m on the absolute encoder corresponds to 0.2m on the robot. We also know
+// that every 0.1m the absolute encoder resets. Then we can construct a table
+// quickly from there:
+//
+// abs_encoder | robot
+// 0.07m | 0.20m
+// 0.07m | 0.30m
+// 0.07m | 0.40m
+// 0.01m | 0.34m
+// 0.01m | 0.24m
+// 0.00m | 0.23m
+// 0.00m | 0.13m
+//
+// Since the absolute encoder wraps around, we'll notice that the same reading
+// can correspond to multiple positions on the robot.
+//
+// NOTE: We use EXPECT_NEAR rather than EXPECT_DOUBLE_EQ for the absolute
+// encoder because the modulo operation inside the simulator introduces just
+// enough imprecision to fail the EXPECT_DOUBLE_EQ macro.
+TEST_F(PositionSensorSimTest, PotAndEncodersNoIndexPulse) {
+ const double index_diff = 0.1;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(0.20, 0.05, 0.2, 0.07);
+ PotAndAbsolutePosition position;
+
+ sim.MoveTo(0.20);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(0.00, position.relative_encoder);
+ EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+
+ sim.MoveTo(0.30);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(0.10, position.relative_encoder);
+ EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+
+ sim.MoveTo(0.40);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(0.20, position.relative_encoder);
+ EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+
+ sim.MoveTo(0.34);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(0.14, position.relative_encoder);
+ EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+
+ sim.MoveTo(0.24);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(0.04, position.relative_encoder);
+ EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+
+ sim.MoveTo(0.23);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(0.03, position.relative_encoder);
+ EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+
+ sim.MoveTo(0.13);
+ sim.GetSensorValues(&position);
+ EXPECT_DOUBLE_EQ(-0.07, position.relative_encoder);
+ EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+
+ // TODO(philipp): Test negative values.
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/profiled_subsystem.cc b/frc971/control_loops/profiled_subsystem.cc
new file mode 100644
index 0000000..4a90bf5
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.cc
@@ -0,0 +1,17 @@
+#include "frc971/control_loops/profiled_subsystem.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace internal {
+
+double UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+
+} // namespace internal
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
new file mode 100644
index 0000000..7dbd1a8
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -0,0 +1,379 @@
+#ifndef FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
+#define FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
+
+#include <array>
+#include <chrono>
+#include <memory>
+#include <utility>
+
+#include "Eigen/Dense"
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/profiled_subsystem.q.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/zeroing.h"
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// TODO(Brian): Use a tuple instead of an array to support heterogeneous zeroing
+// styles.
+template <int number_of_states, int number_of_axes,
+ class ZeroingEstimator =
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+class ProfiledSubsystem {
+ public:
+ ProfiledSubsystem(
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>> loop,
+ ::std::array<ZeroingEstimator, number_of_axes> &&estimators)
+ : loop_(::std::move(loop)), estimators_(::std::move(estimators)) {
+ zeroed_.fill(false);
+ unprofiled_goal_.setZero();
+ }
+
+ // Returns whether an error has occured
+ bool error() const {
+ for (const auto &estimator : estimators_) {
+ if (estimator.error()) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ void Reset() {
+ zeroed_.fill(false);
+ for (auto &estimator : estimators_) {
+ estimator.Reset();
+ }
+ }
+
+ // Returns the controller.
+ const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes> &
+ controller() const {
+ return *loop_;
+ }
+
+ int controller_index() const { return loop_->controller_index(); }
+
+ // Returns whether the estimators have been initialized and zeroed.
+ bool initialized() const { return initialized_; }
+
+ bool zeroed() const {
+ for (int i = 0; i < number_of_axes; ++i) {
+ if (!zeroed_[i]) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool zeroed(int index) const { return zeroed_[index]; };
+
+ // Returns the filtered goal.
+ const Eigen::Matrix<double, number_of_states, 1> &goal() const {
+ return loop_->R();
+ }
+ double goal(int row, int col) const { return loop_->R(row, col); }
+
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
+ // Returns the current state estimate.
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return loop_->X_hat();
+ }
+ double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+
+ // Returns the current internal estimator state for logging.
+ ::frc971::EstimatorState EstimatorState(int index) {
+ ::frc971::EstimatorState estimator_state;
+ ::frc971::zeroing::PopulateEstimatorState(estimators_[index],
+ &estimator_state);
+
+ return estimator_state;
+ }
+
+ // Sets the maximum voltage that will be commanded by the loop.
+ void set_max_voltage(::std::array<double, number_of_axes> voltages) {
+ for (int i = 0; i < number_of_axes; ++i) {
+ loop_->set_max_voltage(i, voltages[i]);
+ }
+ }
+
+ protected:
+ void set_zeroed(int index, bool val) { zeroed_[index] = val; }
+
+ // TODO(austin): It's a bold assumption to assume that we will have the same
+ // number of sensors as axes. So far, that's been fine.
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>> loop_;
+
+ // The goal that the profile tries to reach.
+ Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
+
+ bool initialized_ = false;
+
+ ::std::array<ZeroingEstimator, number_of_axes> estimators_;
+
+ private:
+ ::std::array<bool, number_of_axes> zeroed_;
+};
+
+template <class ZeroingEstimator =
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+class SingleDOFProfiledSubsystem
+ : public ::frc971::control_loops::ProfiledSubsystem<3, 1> {
+ public:
+ SingleDOFProfiledSubsystem(
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
+ const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_angular_velocity,
+ double default_angular_acceleration);
+
+ // Updates our estimator with the latest position.
+ void Correct(typename ZeroingEstimator::Position position);
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+
+ // Fills out the ProfiledJointStatus structure with the current state.
+ void PopulateStatus(ProfiledJointStatus *status);
+
+ // Forces the current goal to the provided goal, bypassing the profiler.
+ void ForceGoal(double goal);
+ // Sets the unprofiled goal. The profiler will generate a profile to go to
+ // this goal.
+ void set_unprofiled_goal(double unprofiled_goal);
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+ void AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration);
+
+ // Returns true if we have exceeded any hard limits.
+ bool CheckHardLimits();
+
+ // Returns the requested voltage.
+ double voltage() const { return loop_->U(0, 0); }
+
+ // Returns the current position.
+ double position() const { return Y_(0, 0); }
+
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimators_[0].TriggerError(); }
+
+ const ::frc971::constants::Range &range() const { return range_; }
+
+ private:
+ // Limits the provided goal to the soft limits. Prints "name" when it fails
+ // to aid debugging.
+ void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
+
+ void UpdateOffset(double offset);
+
+ aos::util::TrapezoidProfile profile_;
+
+ // Current measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // Current offset. Y_ = offset_ + raw_sensor;
+ Eigen::Matrix<double, 1, 1> offset_;
+
+ const ::frc971::constants::Range range_;
+
+ const double default_velocity_;
+ const double default_acceleration_;
+
+ double last_position_ = 0;
+};
+
+namespace internal {
+
+double UseUnlessZero(double target_value, double default_value);
+
+} // namespace internal
+
+template <class ZeroingEstimator>
+SingleDOFProfiledSubsystem<ZeroingEstimator>::SingleDOFProfiledSubsystem(
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
+ const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_velocity,
+ double default_acceleration)
+ : ProfiledSubsystem(::std::move(loop), {{zeroing_constants}}),
+ profile_(::aos::controls::kLoopFrequency),
+ range_(range),
+ default_velocity_(default_velocity),
+ default_acceleration_(default_acceleration) {
+ Y_.setZero();
+ offset_.setZero();
+ AdjustProfile(0.0, 0.0);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateOffset(double offset) {
+ const double doffset = offset - offset_(0, 0);
+ LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
+
+ loop_->mutable_X_hat()(0, 0) += doffset;
+ Y_(0, 0) += doffset;
+ last_position_ += doffset;
+ loop_->mutable_R(0, 0) += doffset;
+
+ profile_.MoveGoal(doffset);
+ offset_(0, 0) = offset;
+
+ CapGoal("R", &loop_->mutable_R());
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
+ ProfiledJointStatus *status) {
+ status->zeroed = zeroed();
+ status->state = -1;
+ // We don't know, so default to the bad case.
+ status->estopped = true;
+
+ status->position = X_hat(0, 0);
+ status->velocity = X_hat(1, 0);
+ status->goal_position = goal(0, 0);
+ status->goal_velocity = goal(1, 0);
+ status->unprofiled_goal_position = unprofiled_goal(0, 0);
+ status->unprofiled_goal_velocity = unprofiled_goal(1, 0);
+ status->voltage_error = X_hat(2, 0);
+ status->calculated_velocity =
+ (position() - last_position_) /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+
+ status->estimator_state = EstimatorState(0);
+
+ Eigen::Matrix<double, 3, 1> error = controller().error();
+ status->position_power = controller().K(0, 0) * error(0, 0);
+ status->velocity_power = controller().K(0, 1) * error(1, 0);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
+ typename ZeroingEstimator::Position new_position) {
+ estimators_[0].UpdateEstimate(new_position);
+
+ if (estimators_[0].error()) {
+ LOG(ERROR, "zeroing error\n");
+ return;
+ }
+
+ if (!initialized_) {
+ if (estimators_[0].offset_ready()) {
+ UpdateOffset(estimators_[0].offset());
+ initialized_ = true;
+ }
+ }
+
+ if (!zeroed(0) && estimators_[0].zeroed()) {
+ UpdateOffset(estimators_[0].offset());
+ set_zeroed(0, true);
+ }
+
+ last_position_ = position();
+ Y_ << new_position.encoder;
+ Y_ += offset_;
+ loop_->Correct(Y_);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
+ const char *name, Eigen::Matrix<double, 3, 1> *goal) {
+ // Limit the goal to min/max allowable positions.
+ if ((*goal)(0, 0) > range_.upper) {
+ LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ range_.upper);
+ (*goal)(0, 0) = range_.upper;
+ }
+ if ((*goal)(0, 0) < range_.lower) {
+ LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ range_.lower);
+ (*goal)(0, 0) = range_.lower;
+ }
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(double goal) {
+ set_unprofiled_goal(goal);
+ loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::set_unprofiled_goal(
+ double unprofiled_goal) {
+ unprofiled_goal_(0, 0) = unprofiled_goal;
+ unprofiled_goal_(1, 0) = 0.0;
+ unprofiled_goal_(2, 0) = 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+ if (!disable) {
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
+
+ loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+ loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+ loop_->mutable_next_R(2, 0) = 0.0;
+ CapGoal("next R", &loop_->mutable_next_R());
+ }
+
+ loop_->Update(disable);
+
+ if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ }
+}
+
+template <class ZeroingEstimator>
+bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
+ // Returns whether hard limits have been exceeded.
+
+ if (position() > range_.upper_hard || position() < range_.lower_hard) {
+ LOG(ERROR,
+ "SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
+ position(), range_.lower_hard, range_.upper_hard);
+ return true;
+ }
+
+ return false;
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+ const ::frc971::ProfileParameters &profile_parameters) {
+ AdjustProfile(profile_parameters.max_velocity,
+ profile_parameters.max_acceleration);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+ double max_angular_velocity, double max_angular_acceleration) {
+ profile_.set_maximum_velocity(
+ internal::UseUnlessZero(max_angular_velocity, default_velocity_));
+ profile_.set_maximum_acceleration(
+ internal::UseUnlessZero(max_angular_acceleration, default_acceleration_));
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
new file mode 100644
index 0000000..11d641d
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -0,0 +1,41 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/control_loops.q";
+
+struct ProfiledJointStatus {
+ // Is the turret zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ .frc971.EstimatorState estimator_state;
+};
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index ed6a809..211b478 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -89,7 +89,8 @@
"""Converts from continuous time state space representation to discrete time.
Returns (A, B). C and D are unchanged."""
- ans_a, ans_b, _, _, _ = scipy.signal.cont2discrete((A, B, None, None), dt)
+ ans_a, ans_b, _, _, _ = scipy.signal.cont2discrete(
+ (numpy.array(A), numpy.array(B), None, None), dt)
return numpy.matrix(ans_a), numpy.matrix(ans_b)
def ctrb(A, B):
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index b4866e9..a490e67 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -47,7 +47,7 @@
for (int i = 0; i < 4; ++i) {
new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 4; ++j) {
+ for (int j = 0; j < 6; ++j) {
new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
}
new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 4ff6391..69c4de6 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -6,16 +6,17 @@
namespace frc971 {
namespace zeroing {
-void PopulateEstimatorState(const zeroing::ZeroingEstimator& estimator,
- EstimatorState* state) {
+void PopulateEstimatorState(
+ const zeroing::PotAndIndexPulseZeroingEstimator &estimator,
+ EstimatorState *state) {
state->error = estimator.error();
state->zeroed = estimator.zeroed();
state->position = estimator.position();
state->pot_position = estimator.filtered_position();
}
-ZeroingEstimator::ZeroingEstimator(
- const constants::ZeroingConstants& constants) {
+PotAndIndexPulseZeroingEstimator::PotAndIndexPulseZeroingEstimator(
+ const constants::PotAndIndexPulseZeroingConstants &constants) {
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
known_index_pos_ = constants.measured_index_position;
@@ -24,7 +25,7 @@
Reset();
}
-void ZeroingEstimator::Reset() {
+void PotAndIndexPulseZeroingEstimator::Reset() {
samples_idx_ = 0;
start_pos_ = 0;
start_pos_samples_.clear();
@@ -35,15 +36,15 @@
error_ = false;
}
-void ZeroingEstimator::TriggerError() {
+void PotAndIndexPulseZeroingEstimator::TriggerError() {
if (!error_) {
LOG(ERROR, "Manually triggered zeroing error.\n");
error_ = true;
}
}
-double ZeroingEstimator::CalculateStartPosition(double start_average,
- double latched_encoder) const {
+double PotAndIndexPulseZeroingEstimator::CalculateStartPosition(
+ double start_average, double latched_encoder) const {
// We calculate an aproximation of the value of the last index position.
// Also account for index pulses not lining up with integer multiples of the
// index_diff.
@@ -54,7 +55,8 @@
return accurate_index_pos - latched_encoder + known_index_pos_;
}
-void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
+void PotAndIndexPulseZeroingEstimator::UpdateEstimate(
+ const PotAndIndexPosition &info) {
// We want to make sure that we encounter at least one index pulse while
// zeroing. So we take the index pulse count from the first sample after
// reset and wait for that count to change before we consider ourselves
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index b5e2c14..1872ad0 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -19,11 +19,15 @@
namespace frc971 {
namespace zeroing {
-// Estimates the position with encoder,
-// the pot and the indices.
-class ZeroingEstimator {
+// Estimates the position with an incremental encoder with an index pulse and a
+// potentiometer.
+class PotAndIndexPulseZeroingEstimator {
public:
- ZeroingEstimator(const constants::ZeroingConstants &constants);
+ using Position = PotAndIndexPosition;
+ using ZeroingConstants = constants::PotAndIndexPulseZeroingConstants;
+
+ PotAndIndexPulseZeroingEstimator(
+ const constants::PotAndIndexPulseZeroingConstants &constants);
// Update the internal logic with the next sensor values.
void UpdateEstimate(const PotAndIndexPosition &info);
@@ -123,7 +127,7 @@
// Populates an EstimatorState struct with information from the zeroing
// estimator.
-void PopulateEstimatorState(const ZeroingEstimator &estimator,
+void PopulateEstimatorState(const PotAndIndexPulseZeroingEstimator &estimator,
EstimatorState *state);
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 1134259..4cfb5e0 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -16,7 +16,7 @@
namespace zeroing {
using control_loops::PositionSensorSimulator;
-using constants::ZeroingConstants;
+using constants::PotAndIndexPulseZeroingConstants;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -26,12 +26,13 @@
protected:
void SetUp() override { aos::SetDieTestMode(true); }
- void MoveTo(PositionSensorSimulator* simulator, ZeroingEstimator* estimator,
+ void MoveTo(PositionSensorSimulator *simulator,
+ PotAndIndexPulseZeroingEstimator *estimator,
double new_position) {
- PotAndIndexPosition sensor_values_;
+ PotAndIndexPosition sensor_values;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ simulator->GetSensorValues(&sensor_values);
+ estimator->UpdateEstimate(sensor_values);
}
::aos::testing::TestSharedMemory my_shm_;
@@ -41,7 +42,7 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
@@ -65,7 +66,7 @@
double position = 3.6 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(position, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure that the zeroing code does not consider itself zeroed until we
@@ -84,7 +85,7 @@
double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
@@ -117,7 +118,7 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
@@ -151,7 +152,7 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
for (unsigned int i = 0; i < kSampleSize / 2; i++) {
@@ -171,7 +172,7 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
MoveTo(&sim, &estimator, 3.1 * index_diff);
@@ -187,7 +188,7 @@
double index_diff = 0.6;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
@@ -222,7 +223,7 @@
const double known_index_pos = 3.5 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.3 * index_diff, index_diff / 3.0, known_index_pos);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
@@ -247,7 +248,7 @@
TEST_F(ZeroingTest, BasicErrorAPITest) {
const double index_diff = 1.0;
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
PositionSensorSimulator sim(index_diff);
sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
@@ -283,7 +284,7 @@
int sample_size = 30;
PositionSensorSimulator sim(index_diff);
sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
sample_size, index_diff, known_index_pos, kIndexErrorFraction});
for (int i = 0; i < sample_size; i++) {
diff --git a/tools/build_rules/gtk_dependent.bzl b/tools/build_rules/gtk_dependent.bzl
new file mode 100644
index 0000000..0a21a9a
--- /dev/null
+++ b/tools/build_rules/gtk_dependent.bzl
@@ -0,0 +1,9 @@
+disable_gtk_binaries = True
+
+def gtk_dependent_cc_library(**kwargs):
+ if not disable_gtk_binaries:
+ native.cc_library(**kwargs)
+
+def gtk_dependent_cc_binary(**kwargs):
+ if not disable_gtk_binaries:
+ native.cc_binary(**kwargs)
diff --git a/y2012/control_loops/python/polydrivetrain.py b/y2012/control_loops/python/polydrivetrain.py
index c19c888..944c09b 100755
--- a/y2012/control_loops/python/polydrivetrain.py
+++ b/y2012/control_loops/python/polydrivetrain.py
@@ -134,7 +134,7 @@
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
- self.R = self._drivetrain.R
+ self.resistance = self._drivetrain.R
self.r = self._drivetrain.r
self.Kv = self._drivetrain.Kv
self.Kt = self._drivetrain.Kt
@@ -184,6 +184,10 @@
[[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.
@@ -239,9 +243,9 @@
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().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
high_power = high_torque * high_omega
low_power = low_torque * low_omega
#if should_print:
@@ -407,27 +411,26 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2012', '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)
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2012', '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)
- dog_loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
- cim_writer.Write(argv[3], argv[4])
- return
+ cim_writer.Write(argv[3], argv[4])
+ return
vl_plot = []
vr_plot = []
@@ -475,22 +478,6 @@
else:
radius_plot.append(turn_velocity / fwd_velocity)
- cim_velocity_plot = []
- cim_voltage_plot = []
- cim_time = []
- cim = CIM()
- R = numpy.matrix([[300]])
- for t in numpy.arange(0, 0.5, cim.dt):
- U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
- cim.Update(U)
- cim_velocity_plot.append(cim.X[0, 0])
- cim_voltage_plot.append(U[0, 0] * 10)
- cim_time.append(t)
- pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- pylab.legend()
- pylab.show()
-
# TODO(austin):
# Shifting compensation.
@@ -509,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014/constants.h b/y2014/constants.h
index 4ad06bc..7fb2507 100644
--- a/y2014/constants.h
+++ b/y2014/constants.h
@@ -43,7 +43,7 @@
double drivetrain_max_speed;
- struct ZeroingConstants {
+ struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
int average_filter_size;
// The difference in scaled units between two index pulses.
@@ -126,7 +126,7 @@
// should be larger than claw_shooting_separation so that we can shoot
// promptly.
double claw_separation_goal;
- };
+ };
ShooterAction shooter_action;
};
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index 387fd78..83fff45 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -134,7 +134,7 @@
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
- self.R = self._drivetrain.R
+ self.resistance = self._drivetrain.R
self.r = self._drivetrain.r
self.Kv = self._drivetrain.Kv
self.Kt = self._drivetrain.Kt
@@ -184,6 +184,10 @@
[[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.
@@ -239,9 +243,9 @@
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().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
high_power = high_torque * high_omega
low_power = low_torque * low_omega
#if should_print:
@@ -407,27 +411,26 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2014', '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)
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2014', '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)
- dog_loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
- cim_writer.Write(argv[3], argv[4])
- return
+ cim_writer.Write(argv[3], argv[4])
+ return
vl_plot = []
vr_plot = []
@@ -474,7 +477,6 @@
radius_plot.append(turn_velocity)
else:
radius_plot.append(turn_velocity / fwd_velocity)
-
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
@@ -509,4 +511,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 1d426d9..9bffa2f 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/constants.h b/y2015/constants.h
index 8cefe7e..d60efac 100644
--- a/y2015/constants.h
+++ b/y2015/constants.h
@@ -69,7 +69,7 @@
struct Claw {
Range wrist;
- ::frc971::constants::ZeroingConstants zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants zeroing;
// The value to add to potentiometer readings after they have been converted
// to radians so that the resulting value is 0 when the claw is at absolute
// 0 (horizontal straight out the front).
@@ -87,10 +87,10 @@
Range elevator;
Range arm;
- ::frc971::constants::ZeroingConstants left_elev_zeroing;
- ::frc971::constants::ZeroingConstants right_elev_zeroing;
- ::frc971::constants::ZeroingConstants left_arm_zeroing;
- ::frc971::constants::ZeroingConstants right_arm_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants left_elev_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants right_elev_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants left_arm_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants right_arm_zeroing;
// Values to add to scaled potentiometer readings so 0 lines up with the
// physical absolute 0.
diff --git a/y2015/control_loops/claw/claw.h b/y2015/control_loops/claw/claw.h
index 324f9f3..e7523eb 100644
--- a/y2015/control_loops/claw/claw.h
+++ b/y2015/control_loops/claw/claw.h
@@ -95,7 +95,7 @@
// Latest position from queue.
control_loops::ClawQueue::Position current_position_;
// Zeroing estimator for claw.
- ::frc971::zeroing::ZeroingEstimator claw_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator claw_estimator_;
// The goal for the claw.
double claw_goal_ = 0.0;
diff --git a/y2015/control_loops/fridge/fridge.h b/y2015/control_loops/fridge/fridge.h
index ee64554..6152caf 100644
--- a/y2015/control_loops/fridge/fridge.h
+++ b/y2015/control_loops/fridge/fridge.h
@@ -22,7 +22,7 @@
class FridgeTest_SafeArmZeroing_Test;
} // namespace testing
-template<int S>
+template <int S>
class CappedStateFeedbackLoop : public StateFeedbackLoop<S, 2, 2> {
public:
CappedStateFeedbackLoop(StateFeedbackLoop<S, 2, 2> &&loop)
@@ -42,8 +42,7 @@
double max_voltage_;
};
-class Fridge
- : public aos::controls::ControlLoop<FridgeQueue> {
+class Fridge : public aos::controls::ControlLoop<FridgeQueue> {
public:
explicit Fridge(FridgeQueue *fridge_queue =
&::y2015::control_loops::fridge::fridge_queue);
@@ -128,10 +127,10 @@
::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
- ::frc971::zeroing::ZeroingEstimator left_arm_estimator_;
- ::frc971::zeroing::ZeroingEstimator right_arm_estimator_;
- ::frc971::zeroing::ZeroingEstimator left_elevator_estimator_;
- ::frc971::zeroing::ZeroingEstimator right_elevator_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator left_arm_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator right_arm_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator left_elevator_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator right_elevator_estimator_;
// Offsets from the encoder position to the absolute position. Add these to
// the encoder position to get the absolute position.
@@ -170,5 +169,4 @@
} // namespace control_loops
} // namespace y2015
-#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
-
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 9a5b48b..29a55a6 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -412,8 +412,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -515,4 +513,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index e85d131..1d2a74e 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -412,8 +412,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -482,22 +480,6 @@
else:
radius_plot.append(turn_velocity / fwd_velocity)
- cim_velocity_plot = []
- cim_voltage_plot = []
- cim_time = []
- cim = CIM()
- R = numpy.matrix([[300]])
- for t in numpy.arange(0, 0.5, cim.dt):
- U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
- cim.Update(U)
- cim_velocity_plot.append(cim.X[0, 0])
- cim_voltage_plot.append(U[0, 0] * 10)
- cim_time.append(t)
- pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- pylab.legend()
- pylab.show()
-
# TODO(austin):
# Shifting compensation.
@@ -516,4 +498,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2016/constants.h b/y2016/constants.h
index a83456c..3cbb895 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -11,7 +11,7 @@
namespace constants {
using ::frc971::constants::ShifterHallEffect;
-using ::frc971::constants::ZeroingConstants;
+using ::frc971::constants::PotAndIndexPulseZeroingConstants;
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
@@ -83,19 +83,19 @@
struct Intake {
double pot_offset;
- ZeroingConstants zeroing;
+ PotAndIndexPulseZeroingConstants zeroing;
};
Intake intake;
struct Shoulder {
double pot_offset;
- ZeroingConstants zeroing;
+ PotAndIndexPulseZeroingConstants zeroing;
};
Shoulder shoulder;
struct Wrist {
double pot_offset;
- ZeroingConstants zeroing;
+ PotAndIndexPulseZeroingConstants zeroing;
};
Wrist wrist;
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 62a4895..c131326 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a10b61..af02d14 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,10 +78,11 @@
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
'//aos/common:math',
- '//y2016/queues:ball_detector',
- '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:profiled_subsystem',
'//frc971/control_loops:simple_capped_state_feedback_loop',
+ '//frc971/control_loops:state_feedback_loop',
'//frc971/zeroing',
+ '//y2016/queues:ball_detector',
'//y2016:constants',
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 0c1ac6a..3bf0966 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -41,7 +41,7 @@
double shoulder_angle = arm_->shoulder_angle();
double wrist_angle = arm_->wrist_angle();
- double intake_angle = intake_->angle();
+ double intake_angle = intake_->position();
// TODO(phil): This may need tuning to account for bounciness in the limbs or
// some other thing that I haven't thought of. At the very least,
@@ -163,7 +163,7 @@
bool CollisionAvoidance::collided() const {
return collided_with_given_angles(arm_->shoulder_angle(), arm_->wrist_angle(),
- intake_->angle());
+ intake_->position());
}
bool CollisionAvoidance::collided_with_given_angles(double shoulder_angle,
@@ -345,7 +345,7 @@
// Set the goals to where we are now so when we start back up, we don't
// jump.
- intake_.ForceGoal(intake_.angle());
+ intake_.ForceGoal(intake_.position());
arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
// Set up the profile to be the zeroing profile.
intake_.AdjustProfile(0.5, 10);
@@ -390,7 +390,7 @@
// enough.
if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) {
intake_.set_unprofiled_goal(
- MoveButKeepBelow(kIntakeUpperClear, intake_.angle(),
+ MoveButKeepBelow(kIntakeUpperClear, intake_.position(),
kIntakeEncoderIndexDifference * 2.5));
}
@@ -428,7 +428,7 @@
// far enough to zero.
if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) {
intake_.set_unprofiled_goal(
- MoveButKeepBelow(kIntakeLowerClear, intake_.angle(),
+ MoveButKeepBelow(kIntakeLowerClear, intake_.position(),
kIntakeEncoderIndexDifference * 2.5));
}
if (IsIntakeNear(kLooseTolerance)) {
@@ -539,7 +539,7 @@
}
// Reset the profile to the current position so it moves well from here.
- intake_.ForceGoal(intake_.angle());
+ intake_.ForceGoal(intake_.position());
arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
} else {
// If we are in slow_running and are no longer collided, let 'er rip.
@@ -648,11 +648,11 @@
}
}
arm_.set_max_voltage(
- kill_shoulder_ ? 0.0 : max_voltage,
- kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
- : kShooterHangingVoltage)
- : max_voltage);
- intake_.set_max_voltage(max_voltage);
+ {{kill_shoulder_ ? 0.0 : max_voltage,
+ kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
+ : kShooterHangingVoltage)
+ : max_voltage}});
+ intake_.set_max_voltage({{max_voltage}});
if (IsRunning() && !kill_shoulder_) {
// We don't want lots of negative voltage when we are near the bellypan on
@@ -687,7 +687,7 @@
// Write out all the voltages.
if (output) {
- output->voltage_intake = intake_.intake_voltage();
+ output->voltage_intake = intake_.voltage();
output->voltage_shoulder = arm_.shoulder_voltage();
output->voltage_wrist = arm_.wrist_voltage();
@@ -743,7 +743,7 @@
status->shoulder.voltage_error = arm_.X_hat(4, 0);
status->shoulder.calculated_velocity =
(arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
+ status->shoulder.estimator_state = arm_.EstimatorState(0);
status->wrist.angle = arm_.X_hat(2, 0);
status->wrist.angular_velocity = arm_.X_hat(3, 0);
@@ -754,7 +754,7 @@
status->wrist.voltage_error = arm_.X_hat(5, 0);
status->wrist.calculated_velocity =
(arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.WristEstimatorState();
+ status->wrist.estimator_state = arm_.EstimatorState(1);
status->intake.angle = intake_.X_hat(0, 0);
status->intake.angular_velocity = intake_.X_hat(1, 0);
@@ -764,16 +764,16 @@
status->intake.unprofiled_goal_angular_velocity =
intake_.unprofiled_goal(1, 0);
status->intake.calculated_velocity =
- (intake_.angle() - last_intake_angle_) / 0.005;
+ (intake_.position() - last_intake_angle_) / 0.005;
status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.IntakeEstimatorState();
+ status->intake.estimator_state = intake_.EstimatorState(0);
status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
status->shoulder_controller_index = arm_.controller_index();
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
- last_intake_angle_ = intake_.angle();
+ last_intake_angle_ = intake_.position();
status->estopped = (state_ == ESTOP);
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7d4dea5..ce334bf 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -13,7 +13,6 @@
namespace superstructure {
using ::frc971::PotAndIndexPosition;
-using ::frc971::EstimatorState;
namespace {
double UseUnlessZero(double target_value, double default_value) {
@@ -30,148 +29,23 @@
// Intake
Intake::Intake()
- : ProfiledSubsystem(
+ : ::frc971::control_loops::SingleDOFProfiledSubsystem<>(
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
3, 1, 1>(::y2016::control_loops::superstructure::
- MakeIntegralIntakeLoop()))),
- estimator_(constants::GetValues().intake.zeroing),
- profile_(::aos::controls::kLoopFrequency) {
- Y_.setZero();
- offset_.setZero();
- AdjustProfile(0.0, 0.0);
-}
-
-void Intake::UpdateIntakeOffset(double offset) {
- const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting Intake offset from %f to %f\n", offset_(0, 0), offset);
-
- loop_->mutable_X_hat()(0, 0) += doffset;
- Y_(0, 0) += doffset;
- loop_->mutable_R(0, 0) += doffset;
-
- profile_.MoveGoal(doffset);
- offset_(0, 0) = offset;
-
- CapGoal("R", &loop_->mutable_R());
-}
-
-void Intake::Correct(PotAndIndexPosition position) {
- estimator_.UpdateEstimate(position);
-
- if (estimator_.error()) {
- LOG(ERROR, "zeroing error with intake_estimator\n");
- return;
- }
-
- if (!initialized_) {
- if (estimator_.offset_ready()) {
- UpdateIntakeOffset(estimator_.offset());
- initialized_ = true;
- }
- }
-
- if (!zeroed(0) && estimator_.zeroed()) {
- UpdateIntakeOffset(estimator_.offset());
- set_zeroed(0, true);
- }
-
- Y_ << position.encoder;
- Y_ += offset_;
- loop_->Correct(Y_);
-}
-
-void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- // Limit the goal to min/max allowable angles.
- if ((*goal)(0, 0) > constants::Values::kIntakeRange.upper) {
- LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- constants::Values::kIntakeRange.upper);
- (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
- }
- if ((*goal)(0, 0) < constants::Values::kIntakeRange.lower) {
- LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- constants::Values::kIntakeRange.lower);
- (*goal)(0, 0) = constants::Values::kIntakeRange.lower;
- }
-}
-
-void Intake::ForceGoal(double goal) {
- set_unprofiled_goal(goal);
- loop_->mutable_R() = unprofiled_goal_;
- loop_->mutable_next_R() = loop_->R();
-
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
-}
-
-void Intake::set_unprofiled_goal(double unprofiled_goal) {
- unprofiled_goal_(0, 0) = unprofiled_goal;
- unprofiled_goal_(1, 0) = 0.0;
- unprofiled_goal_(2, 0) = 0.0;
- CapGoal("unprofiled R", &unprofiled_goal_);
-}
-
-void Intake::Update(bool disable) {
- if (!disable) {
- ::Eigen::Matrix<double, 2, 1> goal_state =
- profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
-
- loop_->mutable_next_R(0, 0) = goal_state(0, 0);
- loop_->mutable_next_R(1, 0) = goal_state(1, 0);
- loop_->mutable_next_R(2, 0) = 0.0;
- CapGoal("next R", &loop_->mutable_next_R());
- }
-
- loop_->Update(disable);
-
- if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
- }
-}
-
-bool Intake::CheckHardLimits() {
- // Returns whether hard limits have been exceeded.
-
- if (angle() > constants::Values::kIntakeRange.upper_hard ||
- angle() < constants::Values::kIntakeRange.lower_hard) {
- LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
- constants::Values::kIntakeRange.lower_hard,
- constants::Values::kIntakeRange.upper_hard);
- return true;
- }
-
- return false;
-}
-
-void Intake::set_max_voltage(double voltage) {
- loop_->set_max_voltage(0, voltage);
-}
-
-void Intake::AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration) {
- profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
- profile_.set_maximum_acceleration(
- UseUnlessZero(max_angular_acceleration, 10.0));
-}
-
-void Intake::DoReset() {
- estimator_.Reset();
-}
-
-EstimatorState Intake::IntakeEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
-
- return estimator_state;
-}
+ MakeIntegralIntakeLoop())),
+ constants::GetValues().intake.zeroing,
+ constants::Values::kIntakeRange, 10.0, 10.0) {}
Arm::Arm()
- : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
- ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
+ : ProfiledSubsystem(
+ ::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+ ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+ {{constants::GetValues().shoulder.zeroing,
+ constants::GetValues().wrist.zeroing}}),
shoulder_profile_(::aos::controls::kLoopFrequency),
- wrist_profile_(::aos::controls::kLoopFrequency),
- shoulder_estimator_(constants::GetValues().shoulder.zeroing),
- wrist_estimator_(constants::GetValues().wrist.zeroing) {
+ wrist_profile_(::aos::controls::kLoopFrequency) {
Y_.setZero();
offset_.setZero();
AdjustProfile(0.0, 0.0, 0.0, 0.0);
@@ -220,33 +94,34 @@
void Arm::Correct(PotAndIndexPosition position_shoulder,
PotAndIndexPosition position_wrist) {
- shoulder_estimator_.UpdateEstimate(position_shoulder);
- wrist_estimator_.UpdateEstimate(position_wrist);
+ estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(position_wrist);
// Handle zeroing errors
- if (shoulder_estimator_.error()) {
+ if (estimators_[kShoulderIndex].error()) {
LOG(ERROR, "zeroing error with shoulder_estimator\n");
return;
}
- if (wrist_estimator_.error()) {
+ if (estimators_[kWristIndex].error()) {
LOG(ERROR, "zeroing error with wrist_estimator\n");
return;
}
if (!initialized_) {
- if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
- UpdateWristOffset(wrist_estimator_.offset());
+ if (estimators_[kShoulderIndex].offset_ready() &&
+ estimators_[kWristIndex].offset_ready()) {
+ UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
+ UpdateWristOffset(estimators_[kWristIndex].offset());
initialized_ = true;
}
}
- if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
+ if (!zeroed(kShoulderIndex) && estimators_[kShoulderIndex].zeroed()) {
+ UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
set_zeroed(kShoulderIndex, true);
}
- if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
- UpdateWristOffset(wrist_estimator_.offset());
+ if (!zeroed(kWristIndex) && estimators_[kWristIndex].zeroed()) {
+ UpdateWristOffset(estimators_[kWristIndex].offset());
set_zeroed(kWristIndex, true);
}
@@ -370,32 +245,6 @@
}
}
-void Arm::set_max_voltage(double shoulder_max_voltage,
- double wrist_max_voltage) {
- loop_->set_max_voltage(0, shoulder_max_voltage);
- loop_->set_max_voltage(1, wrist_max_voltage);
-}
-
-void Arm::DoReset() {
- shoulder_estimator_.Reset();
- wrist_estimator_.Reset();
-}
-
-EstimatorState Arm::ShoulderEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(shoulder_estimator_,
- &estimator_state);
-
- return estimator_state;
-}
-
-EstimatorState Arm::WristEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(wrist_estimator_, &estimator_state);
-
- return estimator_state;
-}
-
} // namespace superstructure
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index adb40e0..8f3a59e 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -4,13 +4,14 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
namespace y2016 {
namespace control_loops {
@@ -66,7 +67,8 @@
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) * overage_amount;
+ (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
@@ -96,149 +98,15 @@
}
};
-template <int number_of_states, int number_of_axes>
-class ProfiledSubsystem {
- public:
- ProfiledSubsystem(
- ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>>
- loop)
- : loop_(::std::move(loop)) {
- zeroed_.fill(false);
- unprofiled_goal_.setZero();
- }
-
- void Reset() {
- zeroed_.fill(false);
- DoReset();
- }
-
- // Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes>
- &controller() const {
- return *loop_;
- }
-
- int controller_index() const { return loop_->controller_index(); }
-
- // Returns whether the estimators have been initialized and zeroed.
- bool initialized() const { return initialized_; }
-
- bool zeroed() const {
- for (int i = 0; i < number_of_axes; ++i) {
- if (!zeroed_[i]) {
- return false;
- }
- }
- return true;
- }
-
- bool zeroed(int index) const { return zeroed_[index]; };
-
- // Returns the filtered goal.
- const Eigen::Matrix<double, number_of_states, 1> &goal() const {
- return loop_->R();
- }
- double goal(int row, int col) const { return loop_->R(row, col); }
-
- // Returns the unprofiled goal.
- const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
- return unprofiled_goal_;
- }
- double unprofiled_goal(int row, int col) const {
- return unprofiled_goal_(row, col);
- }
-
- // Returns the current state estimate.
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
- return loop_->X_hat();
- }
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
- protected:
- void set_zeroed(int index, bool val) { zeroed_[index] = val; }
-
- // TODO(austin): It's a bold assumption to assume that we will have the same
- // number of sensors as axes. So far, that's been fine.
- ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>>
- loop_;
-
- // The goal that the profile tries to reach.
- Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
-
- bool initialized_ = false;
-
- private:
- ::std::array<bool, number_of_axes> zeroed_;
-
- virtual void DoReset() = 0;
-};
-
-class Intake : public ProfiledSubsystem<3, 1> {
+class Intake : public ::frc971::control_loops::SingleDOFProfiledSubsystem<> {
public:
Intake();
- // Returns whether an error has occured
- bool error() const { return estimator_.error(); }
-
- // Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position);
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
- // Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(double voltage);
-
- // Forces the current goal to the provided goal, bypassing the profiler.
- void ForceGoal(double goal);
- // Sets the unprofiled goal. The profiler will generate a profile to go to
- // this goal.
- void set_unprofiled_goal(double unprofiled_goal);
- // Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration);
-
- // Returns true if we have exceeded any hard limits.
- bool CheckHardLimits();
-
- // Returns the current internal estimator state for logging.
- ::frc971::EstimatorState IntakeEstimatorState();
-
- // Returns the requested intake voltage.
- double intake_voltage() const { return loop_->U(0, 0); }
-
- // Returns the current position.
- double angle() const { return Y_(0, 0); }
-
- // For testing:
- // Triggers an estimator error.
- void TriggerEstimatorError() { estimator_.TriggerError(); }
-
- private:
- // Limits the provided goal to the soft limits. Prints "name" when it fails
- // to aid debugging.
- void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
-
- // Resets the internal state.
- void DoReset() override;
-
- void UpdateIntakeOffset(double offset);
-
- ::frc971::zeroing::ZeroingEstimator estimator_;
- aos::util::TrapezoidProfile profile_;
-
- // Current measurement.
- Eigen::Matrix<double, 1, 1> Y_;
- // Current offset. Y_ = offset_ + raw_sensor;
- Eigen::Matrix<double, 1, 1> offset_;
};
-class Arm : public ProfiledSubsystem<6, 2> {
+
+class Arm : public ::frc971::control_loops::ProfiledSubsystem<6, 2> {
public:
Arm();
- // Returns whether an error has occured
- bool error() const {
- return shoulder_estimator_.error() || wrist_estimator_.error();
- }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position_shoulder,
@@ -259,7 +127,6 @@
double max_angular_acceleration_shoulder,
double max_angular_velocity_wrist,
double max_angular_acceleration_wrist);
- void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
void set_shoulder_asymetric_limits(double shoulder_min_voltage,
double shoulder_max_voltage) {
@@ -269,10 +136,6 @@
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // Returns the current internal estimator state for logging.
- ::frc971::EstimatorState ShoulderEstimatorState();
- ::frc971::EstimatorState WristEstimatorState();
-
// Returns the requested shoulder and wrist voltages.
double shoulder_voltage() const { return loop_->U(0, 0); }
double wrist_voltage() const { return loop_->U(1, 0); }
@@ -283,12 +146,9 @@
// For testing:
// Triggers an estimator error.
- void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
+ void TriggerEstimatorError() { estimators_[0].TriggerError(); }
private:
- // Resets the internal state.
- void DoReset() override;
-
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
@@ -300,7 +160,6 @@
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
- ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
// Current measurement.
Eigen::Matrix<double, 2, 1> Y_;
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index f41ea84..59139e1 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -76,6 +76,39 @@
],
)
+cc_binary(name = 'target_sender',
+ srcs = ['target_sender.cc'],
+ deps = [
+ '//aos/common:time',
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/vision/image:reader',
+ '//aos/vision/image:jpeg_routines',
+ '//aos/vision/image:image_stream',
+ '//y2016/vision:blob_filters',
+ '//aos/vision/events:udp',
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/events:socket_types',
+ ':stereo_geometry',
+ ':vision_data',
+ ':calibration',
+ ]
+)
+
+cc_library(name = "blob_filters",
+ srcs = ["blob_filters.cc"],
+ hdrs = ["blob_filters.h"],
+ deps = [
+ "//aos/vision/math:vector",
+ "//aos/vision/math:segment",
+ "//aos/vision/blob:range_image",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:codec",
+ ],
+)
+
cc_binary(
name = 'target_receiver',
srcs = [
diff --git a/y2016/vision/blob_filters.cc b/y2016/vision/blob_filters.cc
new file mode 100644
index 0000000..e9f881d
--- /dev/null
+++ b/y2016/vision/blob_filters.cc
@@ -0,0 +1,393 @@
+#include "y2016/vision/blob_filters.h"
+#include <unistd.h>
+
+namespace aos {
+namespace vision {
+
+double CornerFinder::LineScore(Vector<2> A, Vector<2> B, FittedLine line) {
+ Vector<2> st(line.st.x, line.st.y);
+ Vector<2> ed(line.ed.x, line.ed.y);
+
+ double dist_st_a = A.SquaredDistanceTo(st);
+ double dist_st_b = B.SquaredDistanceTo(st);
+
+ if (dist_st_b < dist_st_a) {
+ Vector<2> tmp = B;
+ B = A;
+ A = tmp;
+ }
+
+ return A.SquaredDistanceTo(st) + B.SquaredDistanceTo(ed);
+}
+
+void CornerFinder::EnqueueLine(std::vector<FittedLine> *list, FittedLine line) {
+ if (list->size() < 2) {
+ list->emplace_back(line);
+ return;
+ }
+
+ Vector<2> st(line.st.x, line.st.y);
+ Vector<2> ed(line.ed.x, line.ed.y);
+
+ double llen = st.SquaredDistanceTo(ed);
+ FittedLine ins = line;
+ for (int i = 0; i < (int)list->size(); i++) {
+ Vector<2> ist((*list)[i].st.x, (*list)[i].st.y);
+ Vector<2> ied((*list)[i].ed.x, (*list)[i].ed.y);
+ double ilen = ist.SquaredDistanceTo(ied);
+ if (ilen < llen) {
+ // our new line is longer, so lets use that.
+ // but we still need to check the one we are replacing against the
+ // others.
+ FittedLine tmp = (*list)[i];
+ (*list)[i] = ins;
+ ins = tmp;
+ llen = ilen;
+ }
+ }
+}
+
+Segment<2> CornerFinder::MakeLine(const std::vector<FittedLine> &list) {
+ assert(list.size() == 2);
+ Vector<2> st0(list[0].st.x, list[0].st.y);
+ Vector<2> ed0(list[0].ed.x, list[0].ed.y);
+ Vector<2> st1(list[1].st.x, list[1].st.y);
+ Vector<2> ed1(list[1].ed.x, list[1].ed.y);
+
+ if (ed1.SquaredDistanceTo(st0) < st1.SquaredDistanceTo(st0)) {
+ Vector<2> sttmp = st1;
+ st1 = ed1;
+ ed1 = sttmp;
+ }
+
+ return Segment<2>((st0 + st1), (ed0 + ed1)).Scale(0.5);
+}
+
+std::vector<std::pair<Vector<2>, Vector<2>>> CornerFinder::Find(
+ const std::vector<SelectedBlob> &blobl) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> res;
+ alloc_.reset();
+ for (const SelectedBlob &blob : blobl) {
+ ContourNode *n = RangeImgToContour(blob.blob, &alloc_);
+ std::vector<FittedLine> lines;
+ HierarchicalMerge(n, &lines, merge_rate_, min_len_);
+
+ if (do_overlay_) {
+ for (FittedLine &line : lines) {
+ overlay_->add_line(Vector<2>(line.st.x, line.st.y),
+ Vector<2>(line.ed.x, line.ed.y), {255, 0, 0});
+ }
+ }
+
+ std::vector<FittedLine> leftLine;
+ std::vector<FittedLine> rightLine;
+ std::vector<FittedLine> bottomLine;
+
+ for (auto &line : lines) {
+ double left_score = LineScore(blob.upper_left, blob.lower_left, line);
+ double right_score = LineScore(blob.upper_right, blob.lower_right, line);
+ double bottom_score = LineScore(blob.lower_left, blob.lower_right, line);
+ if (left_score < right_score && left_score < bottom_score) {
+ if (line.st.x > 0 && line.st.y > 0 && line.ed.x > 0 && line.ed.y > 0) {
+ EnqueueLine(&leftLine, line);
+ }
+ } else if (right_score < left_score && right_score < bottom_score) {
+ EnqueueLine(&rightLine, line);
+ } else {
+ EnqueueLine(&bottomLine, line);
+ }
+ }
+
+ if (leftLine.size() == 2 && rightLine.size() == 2 &&
+ bottomLine.size() == 2) {
+ Segment<2> left(MakeLine(leftLine));
+ Segment<2> right(MakeLine(rightLine));
+ Segment<2> bottom(MakeLine(bottomLine));
+
+ if (do_overlay_) {
+ overlay_->add_line(left.A(), left.B(), {155, 0, 255});
+ overlay_->add_line(right.A(), right.B(), {255, 155, 0});
+ overlay_->add_line(bottom.A(), bottom.B(), {255, 0, 155});
+ }
+
+ res.emplace_back(left.Intersect(bottom), right.Intersect(bottom));
+ }
+ }
+ return res;
+}
+
+//*****************************************************************************
+
+std::vector<SelectedBlob> BlobFilterBase::PreFilter(const BlobList &blobl) {
+ std::vector<SelectedBlob> filt;
+ for (const RangeImage &blob : blobl) {
+ int area = blob.calc_area();
+ if (area > min_area_ && area < max_area_) {
+ filt.emplace_back(SelectedBlob(blob));
+ }
+ }
+ return filt;
+}
+
+//*****************************************************************************
+
+bool HistogramBlobFilter::PickClosest(const Vector<2> &goal, const Vector<2> &A,
+ const Vector<2> &B) {
+ double sq_dist_a = goal.SquaredDistanceTo(A);
+ double sq_dist_b = goal.SquaredDistanceTo(B);
+ if (sq_dist_a < sq_dist_b) {
+ // A is closest
+ return true;
+ } else {
+ // B is closest
+ return false;
+ }
+}
+
+namespace {
+void CalcBoundingBox(const RangeImage &rimage, Vector<2> &ul, Vector<2> &ur,
+ Vector<2> &lr, Vector<2> &ll) {
+ const auto &ranges = rimage.ranges();
+ int mini = rimage.mini();
+ double x_min = ranges[0][0].st;
+ double x_max = ranges[0][0].ed;
+ double y_min = mini;
+ double y_max = mini + ranges.size();
+ for (auto &range : ranges) {
+ for (auto &interval : range) {
+ if (interval.st < x_min) {
+ x_min = interval.st;
+ }
+ if (interval.ed > x_max) {
+ x_max = interval.ed;
+ }
+ }
+ }
+ ul = Vector<2>(x_min, y_min);
+ ur = Vector<2>(x_max, y_min);
+ lr = Vector<2>(x_max, y_max);
+ ll = Vector<2>(x_min, y_max);
+}
+} // namespace
+
+std::vector<SelectedBlob> HistogramBlobFilter::PostFilter(
+ std::vector<SelectedBlob> blobl) {
+ std::vector<SelectedBlob> ret;
+ for (int ti = 0; ti < (int)blobl.size(); ti++) {
+ Vector<2> upper_left(1280, 960);
+ Vector<2> upper_right(0.0, 960);
+ Vector<2> lower_right(0.0, 0.0);
+ Vector<2> lower_left(1280, 0.0);
+
+ Vector<2> tul;
+ Vector<2> tur;
+ Vector<2> tlr;
+ Vector<2> tll;
+ CalcBoundingBox(blobl[ti].blob, tul, tur, tlr, tll);
+ for (int j = 0; j < (int)blobl[ti].blob.ranges().size(); j++) {
+ auto &range = blobl[ti].blob.ranges()[j];
+ Vector<2> first(range[0].st, j + blobl[ti].blob.mini());
+ Vector<2> last(range.back().ed, j + blobl[ti].blob.mini());
+ if (!PickClosest(tul, upper_left, first)) {
+ upper_left = first;
+ }
+ if (!PickClosest(tll, lower_left, first)) {
+ lower_left = first;
+ }
+ if (!PickClosest(tur, upper_right, last)) {
+ upper_right = last;
+ }
+ if (!PickClosest(tlr, lower_right, last)) {
+ lower_right = last;
+ }
+ }
+ blobl[ti].upper_left = upper_left;
+ blobl[ti].upper_right = upper_right;
+ blobl[ti].lower_right = lower_right;
+ blobl[ti].lower_left = lower_left;
+
+ double error = CheckHistogram(&blobl[ti], upper_left, upper_right,
+ lower_right, lower_left);
+ const double k_hist_threshold = 0.05;
+ if (error < k_hist_threshold) {
+ ret.emplace_back(blobl[ti]);
+ }
+ }
+
+ return ret;
+}
+
+double HistogramBlobFilter::CheckHistogram(SelectedBlob *blob,
+ const Vector<2> &ul,
+ const Vector<2> &ur,
+ const Vector<2> &lr,
+ const Vector<2> &ll) {
+ // found horiz histogram
+ std::vector<double> hist_lr(hist_size_);
+ // step size along left edge
+ Vector<2> delta_left = (ul - ll) * (hist_step_);
+ // step size along right edge
+ Vector<2> delta_right = (ur - lr) * (hist_step_);
+ // sum each left to right line for the histogram
+ Vector<2> s;
+ Vector<2> e;
+ for (int i = 0; i < hist_size_; i++) {
+ s = ll + (i * delta_left);
+ e = lr + (i * delta_right);
+ hist_lr[i] = calcHistComponent(&blob->blob, s, e);
+ if (do_overlay_) {
+ double a = hist_lr[i];
+ Vector<2> mid = a * s + (1.0 - a) * e;
+ overlay_->add_line(s, mid, {0, 0, 255});
+ overlay_->add_line(mid, e, {255, 255, 0});
+ }
+ }
+ double check_vert_up = L22_dist(hist_size_, vert_hist_, hist_lr);
+ double check_vert_fliped = L22_dist(hist_size_, vert_hist_fliped_, hist_lr);
+
+ // found vert histogram
+ std::vector<double> hist_ub(hist_size_);
+ // step size along bottom edge
+ Vector<2> delta_bottom = (ll - lr) * (hist_step_);
+ // step size along top edge
+ Vector<2> delta_top = (ul - ur) * (hist_step_);
+ // sum each top to bottom line for the histogram
+ for (int i = 0; i < hist_size_; i++) {
+ s = ur + (i * delta_top);
+ e = lr + (i * delta_bottom);
+ hist_ub[i] = calcHistComponent(&blob->blob, s, e);
+ if (do_overlay_) {
+ double a = hist_ub[i];
+ Vector<2> mid = a * s + (1.0 - a) * e;
+ overlay_->add_line(s, mid, {0, 0, 255});
+ overlay_->add_line(mid, e, {255, 255, 0});
+ }
+ }
+ double check_horiz = L22_dist(hist_size_, horiz_hist_, hist_ub);
+
+ if (do_overlay_) {
+ Vector<2> A = blob->upper_left + Vector<2>(-10, 10);
+ Vector<2> B = blob->upper_left - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ A = blob->upper_right + Vector<2>(-10, 10);
+ B = blob->upper_right - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ A = blob->lower_right + Vector<2>(-10, 10);
+ B = blob->lower_right - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ A = blob->lower_left + Vector<2>(-10, 10);
+ B = blob->lower_left - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ }
+ // If this target is better upside down, if it is flip the blob.
+ // The horizontal will not be effected, so we will not change that.
+ double check_vert;
+ if (check_vert_up < check_vert_fliped) {
+ // normal one is better, leave it alone
+ check_vert = check_vert_up;
+ } else {
+ check_vert = check_vert_fliped;
+ blob->Flip(fmt_);
+ }
+ if (do_overlay_) {
+ Vector<2> A = blob->upper_left + Vector<2>(10, 10);
+ Vector<2> B = blob->upper_left - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ A = blob->upper_right + Vector<2>(10, 10);
+ B = blob->upper_right - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ A = blob->lower_right + Vector<2>(10, 10);
+ B = blob->lower_right - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ A = blob->lower_left + Vector<2>(10, 10);
+ B = blob->lower_left - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ }
+
+ // average the two distances
+ double check = (check_vert + check_horiz) / (2.0 * hist_size_);
+ return check;
+}
+
+double HistogramBlobFilter::calcHistComponent(const RangeImage *blob,
+ const Vector<2> &start,
+ const Vector<2> &end) {
+ int startx = (int)std::floor(start.x());
+ int endx = (int)std::floor(end.x());
+ int starty = (int)std::floor(start.y()) - blob->mini();
+ int endy = (int)std::floor(end.y()) - blob->mini();
+ int dx = std::abs(endx - startx);
+ int dy = std::abs(endy - starty);
+ int sx = (startx < endx) ? 1 : -1;
+ int sy = (starty < endy) ? 1 : -1;
+ int error = dx - dy;
+
+ int total = 0;
+ int value = 0;
+ int total_error;
+ while (1) {
+ total++;
+ if (starty < 0 || starty >= (int)blob->ranges().size()) {
+ printf("starty (%d) size(%d)\n", starty, (int)blob->ranges().size());
+ fflush(stdout);
+ return 0;
+ }
+ const std::vector<ImageRange> &rangel = blob->ranges()[starty];
+ for (const ImageRange &range : rangel) {
+ if (startx >= range.st && startx <= range.ed) {
+ value++;
+ if (do_imgdbg_) {
+ image_->get_px(startx, starty + blob->mini()) = {255, 255, 255};
+ }
+ }
+ }
+
+ // bresenham logic to move along a line
+ if (startx == endx && starty == endy) break;
+ total_error = 2 * error;
+ if (total_error > -dy) {
+ error -= dy;
+ startx += sx;
+ }
+ if (total_error < dx) {
+ error += dx;
+ starty += sy;
+ }
+ }
+ return (double)value / (double)total;
+}
+
+void HistogramBlobFilter::MakeGoalHist(bool is_90) {
+ // calculate a desired histogram before we start
+ double targ_height = 14.0;
+ double targ_width = 20.0;
+ double tape_width = 2.0;
+ horiz_hist_.resize(hist_size_);
+ vert_hist_fliped_.resize(hist_size_);
+ vert_hist_.resize(hist_size_);
+ int j = 0;
+ for (double i = 0; j < hist_size_; i += hist_step_) {
+ if (is_90) {
+ assert(false);
+ } else {
+ if (i < (tape_width / targ_height)) {
+ vert_hist_[j] = 1.0;
+ } else {
+ vert_hist_[j] = 2 * tape_width / targ_width;
+ }
+
+ if (i < tape_width / targ_width || i > 1.0 - (tape_width / targ_width)) {
+ horiz_hist_[j] = 1.0;
+ } else {
+ horiz_hist_[j] = tape_width / targ_height;
+ }
+ }
+ j++;
+ }
+ for (int i = 0; i < hist_size_; i++) {
+ vert_hist_fliped_[hist_size_ - i - 1] = vert_hist_[i];
+ }
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/y2016/vision/blob_filters.h b/y2016/vision/blob_filters.h
new file mode 100644
index 0000000..85d2c94
--- /dev/null
+++ b/y2016/vision/blob_filters.h
@@ -0,0 +1,202 @@
+#ifndef Y2016_VISION_BLOB_FILTERS_H_
+#define Y2016_VISION_BLOB_FILTERS_H_
+
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+struct SelectedBlob {
+ SelectedBlob(const RangeImage &blob_inp) : blob(blob_inp) {}
+ RangeImage blob;
+ Vector<2> upper_left;
+ Vector<2> upper_right;
+ Vector<2> lower_right;
+ Vector<2> lower_left;
+
+ void Flip(ImageFormat fmt) {
+ auto image_width = fmt.w;
+ auto image_height = fmt.h;
+ blob.Flip(fmt);
+
+ Vector<2> tmp = lower_right;
+ lower_right = upper_left;
+ upper_left = tmp;
+ tmp = lower_left;
+ lower_left = upper_right;
+ upper_right = tmp;
+
+ // now flip the box
+ lower_right = Vector<2>(image_width - lower_right.x(),
+ image_height - lower_right.y());
+ upper_right = Vector<2>(image_width - upper_right.x(),
+ image_height - upper_right.y());
+ lower_left =
+ Vector<2>(image_width - lower_left.x(), image_height - lower_left.y());
+ upper_left =
+ Vector<2>(image_width - upper_left.x(), image_height - upper_left.y());
+ }
+};
+
+class CornerFinder {
+ public:
+ CornerFinder(float merge_rate, int min_line_length)
+ : merge_rate_(merge_rate), min_len_(min_line_length) {}
+
+ CornerFinder() : CornerFinder(1.0, 25) {}
+
+ // score how well the line matches the points. Lower score is better.
+ double LineScore(Vector<2> A, Vector<2> B, FittedLine line);
+
+ // We want to save the "best" two lines. Here we will difine that as the
+ // longest
+ // two we see.
+ void EnqueueLine(std::vector<FittedLine> *list, FittedLine line);
+ // Lines come in as two sides of the tape, we will flip them around so they
+ // have the same orientation then avrage the two end points to get a line in
+ // the center.
+ Segment<2> MakeLine(const std::vector<FittedLine> &list);
+
+ // take the given blob and find lines to represent it, then return the
+ // target
+ // corners from those lines. Left first, then bottom.
+ std::vector<std::pair<Vector<2>, Vector<2>>> Find(
+ const std::vector<SelectedBlob> &blobl);
+
+ // Enable overlay debugging.
+ void EnableOverlay(PixelLinesOverlay *overlay) {
+ do_overlay_ = true;
+ overlay_ = overlay;
+ }
+
+ private:
+ // Parker did some sort of optimization with the memory.
+ AnalysisAllocator alloc_;
+
+ // check if we shuld draw the overlay
+ bool do_overlay_ = false;
+ PixelLinesOverlay *overlay_ = NULL;
+
+ // how smooth do we want the lines
+ float merge_rate_;
+ // how short do we want the lines
+ int min_len_;
+};
+
+class BlobFilterBase {
+ public:
+ BlobFilterBase(int min_blob_area, int max_blob_area)
+ : min_area_(min_blob_area), max_area_(max_blob_area) {}
+
+ std::vector<SelectedBlob> PreFilter(const BlobList &blobl);
+
+ std::vector<SelectedBlob> FilterBlobs(const BlobList &blobl) {
+ return PostFilter(PreFilter(blobl));
+ }
+
+ // Enable overlay debugging.
+ void EnableOverlay(PixelLinesOverlay *overlay) {
+ do_overlay_ = true;
+ overlay_ = overlay;
+ }
+
+ private:
+ virtual std::vector<SelectedBlob> PostFilter(
+ std::vector<SelectedBlob> blobl) = 0;
+
+ protected:
+ // absolute minimum for even looking at a blob.
+ int min_area_;
+ // absolute maximum for even looking at a blob.
+ int max_area_;
+
+ // check if we shuld draw the overlay
+ bool do_overlay_ = false;
+ PixelLinesOverlay *overlay_ = NULL;
+};
+
+class HistogramBlobFilter : public BlobFilterBase {
+ public:
+ HistogramBlobFilter(ImageFormat fmt, int hist_size, int min_blob_area,
+ int max_blob_area)
+ : BlobFilterBase(min_blob_area, max_blob_area),
+ fmt_(fmt),
+ hist_size_(hist_size),
+ hist_step_(1.0 / (double)hist_size) {
+ MakeGoalHist(false);
+ }
+
+ // Enable image debugging.
+ void EnableImageHist(ImagePtr *img) {
+ do_imgdbg_ = true;
+ image_ = img;
+ }
+
+ private:
+ // Returns the point closest to the goal point.
+ bool PickClosest(const Vector<2> &goal, const Vector<2> &A,
+ const Vector<2> &B);
+ // Main filter function.
+ std::vector<SelectedBlob> PostFilter(std::vector<SelectedBlob> blobl);
+
+ // calc and compare the histograms to the desired
+ double CheckHistogram(SelectedBlob *blob, const Vector<2> &ul,
+ const Vector<2> &ur, const Vector<2> &lr,
+ const Vector<2> &ll);
+
+ // sum over values between these two points and normalize
+ // see Bresenham's Line Algorithm for the logic of moving
+ // over all the pixels between these two points.
+ double calcHistComponent(const RangeImage *blob, const Vector<2> &start,
+ const Vector<2> &end);
+
+ void MakeGoalHist(bool is_90);
+
+ // just a distance function
+ double chiSquared(int length, double *histA, double *histB) {
+ double sum = 0;
+ for (int i = 0; i < length; i++) {
+ double diff = *(histB + i) - *(histA + i);
+ sum += (diff * diff) / *(histA + i);
+ }
+ return sum;
+ }
+
+ // squared euclidiean dist function
+ double L22_dist(int length, std::vector<double> &histA,
+ std::vector<double> histB) {
+ double sum = 0;
+ for (int i = 0; i < length; i++) {
+ double diff = histB[i] - histA[i];
+ sum += (diff * diff);
+ }
+ return sum;
+ }
+
+ // size of image
+ ImageFormat fmt_;
+ // Number of elements in eaach histogram.
+ int hist_size_;
+ // Percent step of the above size.
+ double hist_step_;
+ // histogram summing in y.
+ std::vector<double> horiz_hist_;
+ // histogram summing in x direction.
+ std::vector<double> vert_hist_;
+ // histogram summing in x from greatest to least.
+ std::vector<double> vert_hist_fliped_;
+ bool do_imgdbg_ = false;
+ ImagePtr *image_ = NULL;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // Y2016_VISION_BLOB_FILTERS_H_
diff --git a/y2016/vision/stereo_geometry.h b/y2016/vision/stereo_geometry.h
index 93f3892..a758f1c 100644
--- a/y2016/vision/stereo_geometry.h
+++ b/y2016/vision/stereo_geometry.h
@@ -3,8 +3,8 @@
#include <string>
-#include "aos/vision/math/vector.h"
#include "aos/common/logging/logging.h"
+#include "aos/vision/math/vector.h"
#include "y2016/vision/calibration.pb.h"
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 7ade8e7..3e4465d 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -1,5 +1,5 @@
-#include <stdlib.h>
#include <netdb.h>
+#include <stdlib.h>
#include <unistd.h>
#include <array>
@@ -10,19 +10,19 @@
#include <thread>
#include <vector>
-#include "aos/linux_code/init.h"
-#include "aos/common/time.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/vision/events/udp.h"
#include "aos/common/mutex.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/constants.h"
+#include "y2016/vision/stereo_geometry.h"
#include "y2016/vision/vision.q.h"
#include "y2016/vision/vision_data.pb.h"
-#include "y2016/vision/stereo_geometry.h"
-#include "y2016/constants.h"
namespace y2016 {
namespace vision {
@@ -179,11 +179,12 @@
double angle, double last_angle,
::aos::vision::Vector<2> *interpolated_result,
double *interpolated_angle) {
- const double age_ratio =
- chrono::duration_cast<chrono::duration<double>>(
- older.capture_time() - newer.last_capture_time()).count() /
- chrono::duration_cast<chrono::duration<double>>(
- newer.capture_time() - newer.last_capture_time()).count();
+ const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
+ older.capture_time() - newer.last_capture_time())
+ .count() /
+ chrono::duration_cast<chrono::duration<double>>(
+ newer.capture_time() - newer.last_capture_time())
+ .count();
interpolated_result->Set(
newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
@@ -226,9 +227,11 @@
status->drivetrain_right_position = before.right;
} else {
const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- capture_time - before.time).count() /
+ capture_time - before.time)
+ .count() /
chrono::duration_cast<chrono::duration<double>>(
- after.time - before.time).count();
+ after.time - before.time)
+ .count();
status->drivetrain_left_position =
before.left * (1 - age_ratio) + after.left * age_ratio;
status->drivetrain_right_position =
@@ -313,7 +316,7 @@
CameraHandler left;
CameraHandler right;
- ::aos::vision::RXUdpSocket recv(8080);
+ ::aos::events::RXUdpSocket recv(8080);
char rawData[65507];
while (true) {
@@ -358,8 +361,8 @@
double last_angle_left;
double last_angle_right;
SelectTargets(left.last_target(), right.last_target(),
- &last_center_left, &last_center_right,
- &last_angle_left, &last_angle_right);
+ &last_center_left, &last_center_right, &last_angle_left,
+ &last_angle_right);
::aos::vision::Vector<2> filtered_center_left(0.0, 0.0);
::aos::vision::Vector<2> filtered_center_right(0.0, 0.0);
@@ -370,7 +373,8 @@
filtered_angle_left = angle_left;
new_vision_status->target_time =
chrono::duration_cast<chrono::nanoseconds>(
- left.capture_time().time_since_epoch()).count();
+ left.capture_time().time_since_epoch())
+ .count();
CalculateFiltered(left, right, center_right, last_center_right,
angle_right, last_angle_right,
&filtered_center_right, &filtered_angle_right);
@@ -379,7 +383,8 @@
filtered_angle_right = angle_right;
new_vision_status->target_time =
chrono::duration_cast<chrono::nanoseconds>(
- right.capture_time().time_since_epoch()).count();
+ right.capture_time().time_since_epoch())
+ .count();
CalculateFiltered(right, left, center_left, last_center_left,
angle_left, last_angle_left, &filtered_center_left,
&filtered_angle_left);
@@ -393,7 +398,8 @@
new_vision_status->right_image_timestamp =
right.target().image_timestamp();
new_vision_status->left_send_timestamp = left.target().send_timestamp();
- new_vision_status->right_send_timestamp = right.target().send_timestamp();
+ new_vision_status->right_send_timestamp =
+ right.target().send_timestamp();
new_vision_status->horizontal_angle = horizontal_angle;
new_vision_status->vertical_angle = vertical_angle;
new_vision_status->distance = distance;
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
new file mode 100644
index 0000000..706c348
--- /dev/null
+++ b/y2016/vision/target_sender.cc
@@ -0,0 +1,242 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.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/jpeg_routines.h"
+#include "aos/vision/image/reader.h"
+#include "y2016/vision/blob_filters.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/vision/vision_data.pb.h"
+
+namespace y2016 {
+namespace vision {
+using aos::vision::ImageStreamEvent;
+using aos::vision::DataRef;
+using aos::events::TCPServer;
+using aos::vision::BlobLRef;
+using aos::vision::Vector;
+using aos::vision::Int32Codec;
+using aos::vision::BlobList;
+using aos::vision::RangeImage;
+using aos::vision::PixelRef;
+using aos::vision::ImageValue;
+using aos::vision::HistogramBlobFilter;
+using aos::vision::CornerFinder;
+using aos::vision::Int64Codec;
+using aos::events::TXUdpSocket;
+using aos::events::DataSocket;
+using aos::vision::ImageFormat;
+
+::camera::CameraParams GetCameraParams(const Calibration &calibration) {
+ return ::camera::CameraParams{.width = calibration.camera_image_width(),
+ .height = calibration.camera_image_height(),
+ .exposure = calibration.camera_exposure(),
+ .brightness = calibration.camera_brightness(),
+ .gain = calibration.camera_gain(),
+ .fps = calibration.camera_fps()};
+}
+
+int64_t Nanos(aos::monotonic_clock::duration time_diff) {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(time_diff)
+ .count();
+}
+
+int64_t NowNanos() {
+ return Nanos(aos::monotonic_clock::now().time_since_epoch());
+}
+
+inline bool FileExist(const std::string &name) {
+ struct stat buffer;
+ return (stat(name.c_str(), &buffer) == 0);
+}
+
+class ImageSender : public ImageStreamEvent {
+ public:
+ ImageSender(int camera_index, camera::CameraParams params,
+ const std::string &fname, const std::string &ipadder, int port)
+ : ImageStreamEvent(fname, params),
+ camera_index_(camera_index),
+ udp_serv_(ipadder, 8080),
+ tcp_serv_(port),
+ blob_filt_(ImageFormat(params.width, params.height), 40, 750, 250000),
+ finder_(0.25, 35) {
+ int index = 0;
+ while (true) {
+ std::string file = "./logging/blob_record_" + std::to_string(index) +
+ "_" + std::to_string(camera_index_) + ".dat";
+ if (FileExist(file)) {
+ index++;
+ } else {
+ printf("Logging to file (%s)\n", file.c_str());
+ ofst_.open(file);
+ assert(ofst_.is_open());
+ break;
+ }
+ }
+ }
+
+ ~ImageSender() { ofst_.close(); }
+
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
+ int64_t timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
+ tp.time_since_epoch())
+ .count();
+ DecodeJpeg(data, &image_);
+ auto fmt = image_.fmt();
+
+ RangeImage rimg = DoThreshold(image_.get(), [](PixelRef &px) {
+ return (px.g > 88);
+ });
+
+ // flip the right image as this camera is mount backward
+ if (camera_index_ == 0) {
+ rimg.Flip(fmt.w, fmt.h);
+ }
+
+ BlobList blobl = aos::vision::FindBlobs(rimg);
+ auto whatever = blob_filt_.FilterBlobs(blobl);
+
+ VisionData target;
+ target.set_camera_index(camera_index_);
+ target.set_image_timestamp(timestamp);
+
+ if (!whatever.empty()) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> corners =
+ finder_.Find(whatever);
+
+ if (!corners.empty()) {
+ for (int i = 0; i < (int)corners.size(); i++) {
+ Target *pos = target.add_target();
+ pos->set_left_corner_x(corners[i].first.x());
+ pos->set_left_corner_y(corners[i].first.y());
+ pos->set_right_corner_x(corners[i].second.x());
+ pos->set_right_corner_y(corners[i].second.y());
+ }
+ }
+ }
+ target.set_send_timestamp(NowNanos());
+
+ // always send data
+ std::string dat;
+ if (target.SerializeToString(&dat)) {
+ if (print_once_) {
+ printf("Beginning data streaming camera %d...\n", camera_index_);
+ print_once_ = false;
+ }
+
+ // Send only target over udp.
+ udp_serv_.Send(dat.data(), dat.size());
+ }
+
+ // Write blob to file for logging.
+ int blob_size = CalculateSize(blobl);
+ int tmp_size = blob_size + sizeof(int32_t) + sizeof(uint64_t);
+ char *buf;
+ blob_data_.resize(tmp_size, 0);
+ {
+ buf = Int32Codec::Write(&blob_data_[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ SerializeBlob(blobl, buf);
+ }
+ ofst_.write(&blob_data_[0], tmp_size);
+
+ // Add blob debug
+ bool debug = true;
+ if (debug) {
+ target.set_raw(buf, blob_size);
+ if (target.SerializeToString(&dat)) {
+ tcp_serv_.Broadcast([&](DataSocket *client) { client->Emit(dat); });
+ }
+ }
+
+ bool timing = false;
+ if (timing) {
+ if (n_time > 0) {
+ auto now = aos::monotonic_clock::now();
+ printf("%g, %g\n",
+ (((double)Nanos(now - tstart)) / (double)(n_time)) / 1e6,
+ (double)Nanos(now - tp) / 1e6);
+ } else {
+ tstart = aos::monotonic_clock::now();
+ }
+ ++n_time;
+ }
+ }
+
+ TCPServer<DataSocket> *GetTCPServ() { return &tcp_serv_; }
+
+ // print when we start
+ bool print_once_ = true;
+
+ // left or right camera
+ int camera_index_;
+
+ // udp socket on which to send to robot
+ TXUdpSocket udp_serv_;
+
+ // tcp socket on which to debug to laptop
+ TCPServer<DataSocket> tcp_serv_;
+
+ // our blob processing object
+ HistogramBlobFilter blob_filt_;
+
+ // corner finder to align aiming
+ CornerFinder finder_;
+
+ ImageValue image_;
+ std::string blob_data_;
+ std::ofstream ofst_;
+ aos::monotonic_clock::time_point tstart;
+ int n_time = 0;
+
+ private:
+};
+
+void RunCamera(int camera_index, camera::CameraParams params,
+ const std::string &device, const std::string &ip_addr,
+ int port) {
+ printf("Creating camera %d (%d, %d).\n", camera_index, params.width,
+ params.height);
+ ImageSender strm(camera_index, params, device, ip_addr, port);
+
+ aos::events::EpollLoop loop;
+ loop.Add(strm.GetTCPServ());
+ loop.Add(&strm);
+ printf("Running Camera (%d)\n", camera_index);
+ loop.Run();
+}
+
+} // namespace vision
+} // namespace y2016
+
+int main(int, char **) {
+ using namespace y2016::vision;
+ StereoGeometry stereo("./stereo_rig.calib");
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ std::thread cam0([stereo]() {
+ RunCamera(0, GetCameraParams(stereo.calibration()),
+ stereo.calibration().right_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ std::thread cam1([stereo]() {
+ RunCamera(1, GetCameraParams(stereo.calibration()),
+ stereo.calibration().left_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ cam0.join();
+ cam1.join();
+
+ return EXIT_SUCCESS;
+}
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index b50ccca..6fee1d2 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -11,7 +11,8 @@
namespace y2016_bot3 {
namespace constants {
constexpr double IntakeZero::pot_offset;
-constexpr ::frc971::constants::ZeroingConstants IntakeZero::zeroing;
+constexpr ::frc971::constants::PotAndIndexPulseZeroingConstants
+ IntakeZero::zeroing;
} // namespace constants
namespace control_loops {
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index fdde74b..c654591 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -39,9 +39,9 @@
struct IntakeZero {
static constexpr double pot_offset = 5.462409 + 0.333162;
- static constexpr ::frc971::constants::ZeroingConstants zeroing{
- kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.148604 - 0.291240,
- 0.3};
+ static constexpr ::frc971::constants::PotAndIndexPulseZeroingConstants
+ zeroing{kZeroingSampleSize, kIntakeEncoderIndexDifference,
+ 0.148604 - 0.291240, 0.3};
};
} // namespace constants
namespace control_loops {
diff --git a/y2016_bot3/control_loops/intake/intake_controls.h b/y2016_bot3/control_loops/intake/intake_controls.h
index 0b2daa0..320d803 100644
--- a/y2016_bot3/control_loops/intake/intake_controls.h
+++ b/y2016_bot3/control_loops/intake/intake_controls.h
@@ -90,7 +90,7 @@
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>> loop_;
- ::frc971::zeroing::ZeroingEstimator estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator estimator_;
aos::util::TrapezoidProfile profile_;
// Current measurement.
diff --git a/y2016_bot3/control_loops/python/polydrivetrain.py b/y2016_bot3/control_loops/python/polydrivetrain.py
index aed8ebe..ed799f4 100755
--- a/y2016_bot3/control_loops/python/polydrivetrain.py
+++ b/y2016_bot3/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2016_bot4/control_loops/python/polydrivetrain.py b/y2016_bot4/control_loops/python/polydrivetrain.py
index 6c70946..e0f94d0 100755
--- a/y2016_bot4/control_loops/python/polydrivetrain.py
+++ b/y2016_bot4/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 33deee3..36cf348 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -38,18 +38,21 @@
case 1: // for tests
return new Values{
5.0, // drivetrain max speed
+ 0.0, // down error
};
break;
case kCompTeamNumber:
return new Values{
5.0, // drivetrain max speed
+ 0.0, // down error
};
break;
case kPracticeTeamNumber:
return new Values{
5.0, // drivetrain max speed
+ 0.0, // down error
};
break;
diff --git a/y2017/constants.h b/y2017/constants.h
index be29a2a..3ca12c9 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -25,6 +25,8 @@
// ///// Dynamic constants. /////
double drivetrain_max_speed;
+
+ double down_error;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2017/control_loops/drivetrain/BUILD b/y2017/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..c44da31
--- /dev/null
+++ b/y2017/control_loops/drivetrain/BUILD
@@ -0,0 +1,78 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:drivetrain',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+)
+
+genrule(
+ name = 'genrule_polydrivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:polydrivetrain) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:polydrivetrain',
+ ],
+ outs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.h',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain_plants',
+ srcs = [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_base',
+ srcs = [
+ 'drivetrain_base.cc',
+ ],
+ hdrs = [
+ 'drivetrain_base.h',
+ ],
+ deps = [
+ ':polydrivetrain_plants',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
+ '//frc971:shifter_hall_effect',
+ '//y2017:constants',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain_main.cc',
+ ],
+ deps = [
+ ':drivetrain_base',
+ '//aos/linux_code:init',
+ '//frc971/control_loops/drivetrain:drivetrain_lib',
+ ],
+)
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..0eb506e
--- /dev/null
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,49 @@
+#include "y2017/control_loops/drivetrain/drivetrain_base.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2017/constants.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace y2017 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig &GetDrivetrainConfig() {
+ static DrivetrainConfig kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+
+ ::y2017::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2017::control_loops::drivetrain::MakeKFDrivetrainLoop,
+
+ drivetrain::kDt,
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
+
+ drivetrain::kHighGearRatio,
+ drivetrain::kHighGearRatio,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true,
+ constants::GetValues().down_error,
+ 0.4,
+ 1.0};
+
+ return kDrivetrainConfig;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.h b/y2017/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..4dea38b
--- /dev/null
+++ b/y2017/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,21 @@
+#ifndef Y2017_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2017_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2017 {
+namespace constants {
+// The ratio from the encoder shaft to the drivetrain wheels.
+static constexpr double kDrivetrainEncoderRatio = 1.0;
+
+} // namespace constants
+namespace control_loops {
+namespace drivetrain {
+const ::frc971::control_loops::drivetrain::DrivetrainConfig &
+GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..aab4ea0
--- /dev/null
+++ b/y2017/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,15 @@
+#include "aos/linux_code/init.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2017/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main() {
+ ::aos::Init();
+ DrivetrainLoop drivetrain(
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2017/control_loops/python/BUILD b/y2017/control_loops/python/BUILD
new file mode 100644
index 0000000..cb599a3
--- /dev/null
+++ b/y2017/control_loops/python/BUILD
@@ -0,0 +1,102 @@
+package(default_visibility = ['//y2017:__subpackages__'])
+
+py_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_binary(
+ name = 'polydrivetrain',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_library(
+ name = 'polydrivetrain_lib',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_binary(
+ name = 'shooter',
+ srcs = [
+ 'shooter.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
+
+py_binary(
+ name = 'indexer',
+ srcs = [
+ 'indexer.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
+
+py_binary(
+ name = 'intake',
+ srcs = [
+ 'intake.py',
+ ],
+ deps = [
+ '//aos/common/util:py_trapezoid_profile',
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
+
+py_binary(
+ name = 'turret',
+ srcs = [
+ 'turret.py',
+ ],
+ deps = [
+ '//aos/common/util:py_trapezoid_profile',
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
+
+py_binary(
+ name = 'hood',
+ srcs = [
+ 'hood.py',
+ ],
+ deps = [
+ '//aos/common/util:py_trapezoid_profile',
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2017/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..17e0079
--- /dev/null
+++ b/y2017/control_loops/python/drivetrain.py
@@ -0,0 +1,356 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+ super(Drivetrain, self).__init__(name)
+ # Number of motors per side
+ self.num_motors = 2
+ # Stall Torque in N m
+ self.stall_torque = 2.42 * self.num_motors * 0.60
+ # Stall Current in Amps
+ self.stall_current = 133.0 * self.num_motors
+ # Free Speed in RPM. Used number from last year.
+ self.free_speed = 5500.0
+ # Free Current in Amps
+ self.free_current = 4.7 * self.num_motors
+ # Moment of inertia of the drivetrain in kg m^2
+ self.J = 2.0
+ # Mass of the robot, in kg.
+ self.m = 24
+ # Radius of the robot, in meters (requires tuning by hand)
+ self.rb = 0.59055 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = 0.08255 / 2.0
+ # Resistance of the motor, divided by the number of 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
+ # Gear ratios
+ self.G_low = 12.0 / 54.0
+ self.G_high = 12.0 / 54.0
+ if left_low:
+ self.Gl = self.G_low
+ else:
+ self.Gl = self.G_high
+ if right_low:
+ self.Gr = self.G_low
+ else:
+ self.Gr = self.G_high
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
+ self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+ self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[positionl], [velocityl], [positionr], velocityr]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tcl, 0, self.msn * self.tcr],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mpl, self.msn * self.mpr],
+ [0, 0],
+ [self.msn * self.mpl, self.msp * self.mpr]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ if left_low or right_low:
+ q_pos = 0.12
+ q_vel = 1.0
+ else:
+ q_pos = 0.14
+ q_vel = 0.95
+
+ # 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.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)
+
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
+
+ self.hlp = 0.3
+ self.llp = 0.4
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+class KFDrivetrain(Drivetrain):
+ def __init__(self, name="KFDrivetrain", left_low=True, right_low=True):
+ super(KFDrivetrain, self).__init__(name, left_low, right_low)
+
+ self.unaugmented_A_continuous = self.A_continuous
+ self.unaugmented_B_continuous = self.B_continuous
+
+ # The practical voltage applied to the wheels is
+ # V_left = U_left + left_voltage_error
+ #
+ # The states are
+ # [left position, left velocity, right position, right velocity,
+ # left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
+ self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+ self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+ self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
+ self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
+ self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+ self.A_continuous[0,6] = 1
+ self.A_continuous[2,6] = -1
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [0, -0.5 / self.rb, 0, 0.5 / self.rb, 0, 0, 0]])
+
+ self.D = numpy.matrix([[0, 0],
+ [0, 0],
+ [0, 0]])
+
+ q_pos = 0.05
+ q_vel = 1.00
+ q_voltage = 10.0
+ 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],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
+
+ r_pos = 0.0001
+ r_gyro = 0.000001
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
+ [0.0, (r_pos ** 2.0), 0.0],
+ [0.0, 0.0, (r_gyro ** 2.0)]])
+
+ # Solving for kf gains.
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.L = self.A * self.KalmanGain
+
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
+ self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+ self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+ self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+ self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+ self.InitializeState()
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), simulated_left)
+ pylab.plot(range(100), simulated_right)
+ pylab.suptitle('Acceleration Test')
+ pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ close_loop_left = []
+ close_loop_right = []
+ left_power = []
+ right_power = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, label='right power')
+ pylab.suptitle('Linear Move')
+ pylab.legend()
+ pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Angular Move')
+ pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Pivot')
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ drivetrain_low_low = Drivetrain(
+ name="DrivetrainLowLow", left_low=True, right_low=True)
+ drivetrain_low_high = Drivetrain(
+ name="DrivetrainLowHigh", left_low=True, right_low=False)
+ drivetrain_high_low = Drivetrain(
+ name="DrivetrainHighLow", left_low=False, right_low=True)
+ drivetrain_high_high = Drivetrain(
+ name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+ kf_drivetrain_low_low = KFDrivetrain(
+ name="KFDrivetrainLowLow", left_low=True, right_low=True)
+ kf_drivetrain_low_high = KFDrivetrain(
+ name="KFDrivetrainLowHigh", left_low=True, right_low=False)
+ kf_drivetrain_high_low = KFDrivetrain(
+ name="KFDrivetrainHighLow", left_low=False, right_low=True)
+ kf_drivetrain_high_high = KFDrivetrain(
+ name="KFDrivetrainHighHigh", left_low=False, right_low=False)
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2017', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces = namespaces)
+ dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ drivetrain_low_low.dt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
+ drivetrain_low_low.stall_torque))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
+ drivetrain_low_low.stall_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
+ drivetrain_low_low.free_speed))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
+ drivetrain_low_low.free_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
+ drivetrain_low_low.J))
+ dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
+ drivetrain_low_low.m))
+ dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
+ drivetrain_low_low.rb))
+ dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
+ drivetrain_low_low.r))
+ dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
+ drivetrain_low_low.resistance))
+ dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
+ drivetrain_low_low.Kv))
+ dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
+ drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+ drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
+ kf_drivetrain_high_low, kf_drivetrain_high_high],
+ namespaces = namespaces)
+ kf_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
new file mode 100755
index 0000000..376a22a
--- /dev/null
+++ b/y2017/control_loops/python/hood.py
@@ -0,0 +1,332 @@
+#!/usr/bin/python
+
+from aos.common.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ 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
+ # Free Speed in RPM
+ self.free_speed = 13180.0
+ # 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 / 60.0 * 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 = (18.0 / 345.0)
+ # Gear ratio
+ self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
+
+ # 36 tooth gear inertia in kg * m^2
+ self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025) ** 2)
+
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.000006
+ glog.debug(self.big_gear_inertia)
+
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.08 + \
+ 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)
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position, velocity]
+ # Input is [Voltage]
+
+ C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.R * self.G)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -C1]])
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [C2]])
+
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ controllability = controls.ctrb(self.A, self.B)
+
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+ glog.debug(repr(self.A_continuous))
+
+ # Calculate the LQR controller gain
+ q_pos = 2.0
+ q_vel = 500.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(5.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ # 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.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+ glog.debug('K %s', repr(self.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[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)]])
+
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ glog.debug('Kal %s', repr(self.KalmanGain))
+ self.L = self.A * self.KalmanGain
+ glog.debug('KalL is %s', repr(self.L))
+
+ # 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
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.12
+ q_vel = 2.00
+ q_voltage = 3.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)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 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.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 = 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.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.
+
+ 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.
+ """
+
+ if controller_hood is None:
+ controller_hood = hood
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + hood.dt
+ else:
+ initial_t = 0
+
+ goal = numpy.concatenate((hood.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+
+ profile = TrapezoidProfile(hood.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(1.0)
+ profile.SetGoal(goal[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)
+
+ self.offset.append(observer_hood.X_hat[2, 0])
+ self.x_hat.append(observer_hood.X_hat[0, 0])
+
+ next_goal = numpy.concatenate(
+ (profile.Update(end_goal[0, 0], end_goal[1, 0]),
+ numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
+
+ ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
+
+ 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])
+
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(hood.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / hood.dt)
+
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ hood.Update(U + offset)
+
+ observer_hood.PredictObserver(U)
+
+ self.t.append(initial_t + i * hood.dt)
+ self.u.append(U[0, 0])
+
+ ff_U -= U_uncapped - U
+ goal = controller_hood.A * goal + controller_hood.B * ff_U
+
+ if U[0, 0] != U_uncapped[0, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+
+ 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))
+
+ 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.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.show()
+
+
+def main(argv):
+
+ scenario_plotter = ScenarioPlotter()
+
+ 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/2.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()
+
+ # 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.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])
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
new file mode 100755
index 0000000..a5088cb
--- /dev/null
+++ b/y2017/control_loops/python/indexer.py
@@ -0,0 +1,298 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+class VelocityIndexer(control_loop.ControlLoop):
+ def __init__(self, name='VelocityIndexer'):
+ super(VelocityIndexer, 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 indexer halves in kg m^2
+ # This is measured as Iyy in CAD (the moment of inertia around the Y axis).
+ # Inner part of indexer -> Iyy = 59500 lb * mm * mm
+ # Inner spins with 12 / 48 * 18 / 48 * 24 / 36 * 16 / 72
+ # Outer part of indexer -> Iyy = 210000 lb * mm * mm
+ # 1 775 pro -> 12 / 48 * 18 / 48 * 30 / 422
+
+ self.J_inner = 0.0269
+ self.J_outer = 0.0952
+ # Gear ratios for the inner and outer parts.
+ self.G_inner = (12.0 / 48.0) * (18.0 / 48.0) * (24.0 / 36.0) * (16.0 / 72.0)
+ self.G_outer = (12.0 / 48.0) * (18.0 / 48.0) * (30.0 / 422.0)
+
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.000006
+
+ # The output coordinate system is in radians for the inner part of the
+ # indexer.
+ # Compute the effective moment of inertia assuming all the mass is in that
+ # coordinate system.
+ self.J = (
+ self.J_inner * self.G_inner * self.G_inner +
+ self.J_outer * self.G_outer * self.G_outer) / (self.G_inner * self.G_inner) + \
+ self.motor_inertia * ((1.0 / self.G_inner) ** 2.0)
+ glog.debug('J is %f', self.J)
+ self.G = self.G_inner
+
+ # 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
+ # 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]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.82])
+ glog.debug(repr(self.K))
+
+ self.PlaceObserverPoles([0.3])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ 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 Indexer(VelocityIndexer):
+ def __init__(self, name='Indexer'):
+ super(Indexer, self).__init__(name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ 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 IntegralIndexer(Indexer):
+ def __init__(self, name="IntegralIndexer"):
+ super(IntegralIndexer, self).__init__(name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 2.0
+ q_vel = 0.001
+ q_voltage = 10.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)]])
+
+ r_pos = 0.001
+ self.R = numpy.matrix([[(r_pos ** 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.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, indexer, goal, iterations=200, controller_indexer=None,
+ observer_indexer=None):
+ """Runs the indexer plant with an initial condition and goal.
+
+ Args:
+ indexer: Indexer object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_indexer: Indexer object to get K from, or None if we should
+ use indexer.
+ observer_indexer: Indexer object to use for the observer, or None if we
+ should use the actual state.
+ """
+
+ if controller_indexer is None:
+ controller_indexer = indexer
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + indexer.dt
+ else:
+ initial_t = 0
+
+ for i in xrange(iterations):
+ X_hat = indexer.X
+
+ if observer_indexer is not None:
+ X_hat = observer_indexer.X_hat
+ self.x_hat.append(observer_indexer.X_hat[1, 0])
+
+ ff_U = controller_indexer.Kff * (goal - observer_indexer.A * goal)
+
+ U = controller_indexer.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(indexer.X[0, 0])
+
+
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(indexer.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / indexer.dt)
+
+ if observer_indexer is not None:
+ observer_indexer.Y = indexer.Y
+ observer_indexer.CorrectObserver(U)
+ self.offset.append(observer_indexer.X_hat[2, 0])
+
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ indexer.Update(applied_U)
+
+ if observer_indexer is not None:
+ observer_indexer.PredictObserver(U)
+
+ self.t.append(initial_t + i * indexer.dt)
+ self.u.append(U[0, 0])
+
+ 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, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
+
+
+def main(argv):
+ scenario_plotter = ScenarioPlotter()
+
+ indexer = Indexer()
+ indexer_controller = IntegralIndexer()
+ observer_indexer = IntegralIndexer()
+
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [20.0], [0.0]])
+ scenario_plotter.run_test(indexer, goal=R, controller_indexer=indexer_controller,
+ observer_indexer=observer_indexer, iterations=200)
+
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'indexer']
+ indexer = Indexer('Indexer')
+ loop_writer = control_loop.ControlLoopWriter('Indexer', [indexer],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
+
+ integral_indexer = IntegralIndexer('IntegralIndexer')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralIndexer', [integral_indexer], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/intake.py b/y2017/control_loops/python/intake.py
new file mode 100755
index 0000000..41cfc0d
--- /dev/null
+++ b/y2017/control_loops/python/intake.py
@@ -0,0 +1,321 @@
+#!/usr/bin/python
+
+from aos.common.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+class Intake(control_loop.ControlLoop):
+ def __init__(self, name='Intake'):
+ super(Intake, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134.0
+ # Free Speed in RPM
+ self.free_speed = 18730.0
+ # Free Current in Amps
+ self.free_current = 0.7
+
+ # 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) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+
+ # (1 / 35.0) * (20.0 / 40.0) -> 16 tooth sprocket on #25 chain
+ # Gear ratio
+ self.G = (1.0 / 35.0) * (20.0 / 40.0)
+ self.r = 16.0 * 0.25 / (2.0 * numpy.pi) * 0.0254
+
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.00001187
+
+ # 5.4 kg of moving mass for the intake
+ self.mass = 5.4 + self.motor_inertia / ((self.G * self.r) ** 2.0)
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position, velocity]
+ # Input is [Voltage]
+
+ C1 = self.Kt / (self.G * self.G * self.r * self.r * self.R * self.mass * self.Kv)
+ C2 = self.Kt / (self.G * self.r * self.R * self.mass)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -C1]])
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [C2]])
+ glog.debug(repr(self.A_continuous))
+ glog.debug(repr(self.B_continuous))
+
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ controllability = controls.ctrb(self.A, self.B)
+
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+
+ q_pos = 0.015
+ q_vel = 0.3
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ 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.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+ glog.debug('K %s', repr(self.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+ self.rpl = 0.30
+ self.ipl = 0.10
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ glog.debug('L is %s', repr(self.L))
+
+ q_pos = 0.10
+ q_vel = 1.65
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+ [0.0, (q_vel ** 2.0)]])
+
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ glog.debug('Kal %s', repr(self.KalmanGain))
+ self.L = self.A * self.KalmanGain
+ glog.debug('KalL is %s', repr(self.L))
+
+ # 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 IntegralIntake(Intake):
+ def __init__(self, name='IntegralIntake'):
+ super(IntegralIntake, self).__init__(name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.12
+ q_vel = 2.00
+ q_voltage = 40.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)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 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.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 = 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.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self, intake, end_goal,
+ controller_intake,
+ observer_intake=None,
+ iterations=200):
+ """Runs the intake plant with an initial condition and goal.
+
+ Args:
+ intake: intake object to use.
+ end_goal: end_goal state.
+ controller_intake: Intake object to get K from, or None if we should
+ use intake.
+ observer_intake: 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.
+ """
+
+ if controller_intake is None:
+ controller_intake = intake
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + intake.dt
+ else:
+ initial_t = 0
+
+ goal = numpy.concatenate((intake.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+
+ profile = TrapezoidProfile(intake.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(0.3)
+ profile.SetGoal(goal[0, 0])
+
+ U_last = numpy.matrix(numpy.zeros((1, 1)))
+ for i in xrange(iterations):
+ observer_intake.Y = intake.Y
+ observer_intake.CorrectObserver(U_last)
+
+ self.offset.append(observer_intake.X_hat[2, 0])
+ self.x_hat.append(observer_intake.X_hat[0, 0])
+
+ next_goal = numpy.concatenate(
+ (profile.Update(end_goal[0, 0], end_goal[1, 0]),
+ numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
+
+ ff_U = controller_intake.Kff * (next_goal - observer_intake.A * goal)
+
+ U_uncapped = controller_intake.K * (goal - observer_intake.X_hat) + ff_U
+ U_uncapped = controller_intake.K * (end_goal - observer_intake.X_hat)
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(intake.X[0, 0])
+
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(intake.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / intake.dt)
+
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ intake.Update(U + offset)
+
+ observer_intake.PredictObserver(U)
+
+ self.t.append(initial_t + i * intake.dt)
+ self.u.append(U[0, 0])
+
+ ff_U -= U_uncapped - U
+ goal = controller_intake.A * goal + controller_intake.B * ff_U
+
+ if U[0, 0] != U_uncapped[0, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_intake.X_hat - end_goal))
+
+ 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.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.show()
+
+
+def main(argv):
+ scenario_plotter = ScenarioPlotter()
+
+ intake = Intake()
+ intake_controller = IntegralIntake()
+ observer_intake = IntegralIntake()
+
+ # Test moving the intake with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.1], [0.0], [0.0]])
+ scenario_plotter.run_test(intake, end_goal=R,
+ controller_intake=intake_controller,
+ observer_intake=observer_intake, iterations=400)
+
+ 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 intake and integral intake.')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'intake']
+ intake = Intake('Intake')
+ loop_writer = control_loop.ControlLoopWriter('Intake', [intake],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
+
+ integral_intake = IntegralIntake('IntegralIntake')
+ integral_loop_writer = control_loop.ControlLoopWriter('IntegralIntake', [integral_intake],
+ namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..df1f18f
--- /dev/null
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -0,0 +1,501 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from frc971.control_loops.python import polytope
+from y2017.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python.cim import CIM
+from matplotlib import pylab
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+def CoerceGoal(region, K, w, 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
+
+ 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]
+
+def DoCoerceGoal(region, K, w, R):
+ 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]]])
+
+ # 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)
+
+ 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
+
+ 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)
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+ right_low=right_low)
+ self.dt = 0.005
+ 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)))
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+ self.PlaceControllerPoles([0.85, 0.85])
+ self.PlaceObserverPoles([0.02, 0.02])
+
+ 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.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
+
+ def __init__(self):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+ # 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_max = numpy.matrix(
+ [[12.0],
+ [12.0]])
+ self.U_min = numpy.matrix(
+ [[-12.0000000000],
+ [-12.0000000000]])
+
+ self.dt = 0.005
+
+ self.R = 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
+
+ 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 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
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
+
+ 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.
+
+ # 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()
+
+ # 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
+
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
+
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
+
+ # 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))
+
+ 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.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])
+
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
+
+ # 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
+
+ # 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
+
+ # (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
+
+ equality_k = numpy.matrix(
+ [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+ equality_w = 0.0
+
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
+
+ # 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)
+
+ # 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 main(argv):
+ vdrivetrain = VelocityDrivetrain()
+
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2017', '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)
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+
+ cim_writer.Write(argv[3], argv[4])
+ return
+
+ 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))
+
+ 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)
+ 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)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
+
+ # 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()
+ return 0
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/polydrivetrain_test.py b/y2017/control_loops/python/polydrivetrain_test.py
new file mode 100755
index 0000000..434cdca
--- /dev/null
+++ b/y2017/control_loops/python/polydrivetrain_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/python
+
+import polydrivetrain
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+class TestVelocityDrivetrain(unittest.TestCase):
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]])
+ K = numpy.matrix([[x1_max],
+ [-x1_min],
+ [x2_max],
+ [-x2_min]])
+ return polytope.HPolytope(H, K)
+
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
+ numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
+
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
+
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
new file mode 100755
index 0000000..d7c505b
--- /dev/null
+++ b/y2017/control_loops/python/shooter.py
@@ -0,0 +1,280 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+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)
+ # 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 = 18730.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.00080
+ # 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 / 36.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]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.90])
+
+ self.PlaceObserverPoles([0.3])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ 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 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
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ 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
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 2.0
+ q_vel = 0.001
+ q_voltage = 10.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)]])
+
+ r_pos = 0.001
+ self.R = numpy.matrix([[(r_pos ** 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.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.
+
+ 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.
+ """
+
+ if controller_shooter is None:
+ controller_shooter = shooter
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
+ else:
+ initial_t = 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[1, 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
+
+ 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])
+
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ shooter.Update(applied_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])
+
+ 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()
+
+ 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.show()
+
+
+def main(argv):
+ scenario_plotter = ScenarioPlotter()
+
+ 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)
+
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
+ 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.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])
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
new file mode 100755
index 0000000..454be9e
--- /dev/null
+++ b/y2017/control_loops/python/turret.py
@@ -0,0 +1,314 @@
+#!/usr/bin/python
+
+from aos.common.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+class Turret(control_loop.ControlLoop):
+ def __init__(self, name='Turret'):
+ super(Turret, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.43
+ # Stall Current in Amps
+ self.stall_current = 53
+ # Free Speed in RPM
+ self.free_speed = 13180
+ # 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 / 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 = (1.0 / 7.0) * (1.0 / 5.0) * (16.0 / 92.0)
+
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.000006
+
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.05 + self.motor_inertia * ((1.0 / self.G) ** 2.0)
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position, velocity]
+ # Input is [Voltage]
+
+ C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.R * self.G)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -C1]])
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [C2]])
+
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ controllability = controls.ctrb(self.A, self.B)
+
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+
+ # Calculate the LQR controller gain
+ q_pos = 0.20
+ q_vel = 5.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ # 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.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+ glog.debug('K %s', repr(self.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[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)]])
+
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ glog.debug('Kal %s', repr(self.KalmanGain))
+ self.L = self.A * self.KalmanGain
+ glog.debug('KalL is %s', repr(self.L))
+
+ # 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 IntegralTurret(Turret):
+ def __init__(self, name='IntegralTurret'):
+ super(IntegralTurret, self).__init__(name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ 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.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.12
+ q_vel = 2.00
+ q_voltage = 3.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)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 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.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 = 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.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self, turret, end_goal,
+ controller_turret,
+ observer_turret=None,
+ iterations=200):
+ """Runs the turret plant with an initial condition and goal.
+
+ Args:
+ turret: turret object to use.
+ end_goal: end_goal state.
+ controller_turret: Turret object to get K from, or None if we should
+ use turret.
+ observer_turret: Turret object to use for the observer, or None if we should
+ use the actual state.
+ iterations: Number of timesteps to run the model for.
+ """
+
+ if controller_turret is None:
+ controller_turret = turret
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + turret.dt
+ else:
+ initial_t = 0
+
+ goal = numpy.concatenate((turret.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+
+ profile = TrapezoidProfile(turret.dt)
+ profile.set_maximum_acceleration(100.0)
+ profile.set_maximum_velocity(7.0)
+ profile.SetGoal(goal[0, 0])
+
+ U_last = numpy.matrix(numpy.zeros((1, 1)))
+ for i in xrange(iterations):
+ observer_turret.Y = turret.Y
+ observer_turret.CorrectObserver(U_last)
+
+ self.offset.append(observer_turret.X_hat[2, 0])
+ self.x_hat.append(observer_turret.X_hat[0, 0])
+
+ next_goal = numpy.concatenate(
+ (profile.Update(end_goal[0, 0], end_goal[1, 0]),
+ numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
+
+ ff_U = controller_turret.Kff * (next_goal - observer_turret.A * goal)
+
+ U_uncapped = controller_turret.K * (goal - observer_turret.X_hat) + ff_U
+ U_uncapped = controller_turret.K * (end_goal - observer_turret.X_hat)
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(turret.X[0, 0])
+
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(turret.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / turret.dt)
+
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ turret.Update(U + offset)
+
+ observer_turret.PredictObserver(U)
+
+ self.t.append(initial_t + i * turret.dt)
+ self.u.append(U[0, 0])
+
+ ff_U -= U_uncapped - U
+ goal = controller_turret.A * goal + controller_turret.B * ff_U
+
+ if U[0, 0] != U_uncapped[0, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_turret.X_hat - end_goal))
+
+ 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.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.show()
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ scenario_plotter = ScenarioPlotter()
+
+ turret = Turret()
+ turret_controller = IntegralTurret()
+ observer_turret = IntegralTurret()
+
+ # Test moving the turret with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi/2.0], [0.0], [0.0]])
+ scenario_plotter.run_test(turret, end_goal=R,
+ controller_turret=turret_controller,
+ observer_turret=observer_turret, iterations=200)
+
+ 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 turret and integral turret.')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'turret']
+ turret = Turret('Turret')
+ loop_writer = control_loop.ControlLoopWriter('Turret', [turret],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
+
+ integral_turret = IntegralTurret('IntegralTurret')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralTurret', [integral_turret],
+ namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..d0603ba
--- /dev/null
+++ b/y2017/control_loops/superstructure/BUILD
@@ -0,0 +1,15 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'superstructure_queue',
+ srcs = [
+ 'superstructure.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:profiled_subsystem_queue',
+ '//frc971/control_loops:queues',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
new file mode 100644
index 0000000..7b03141
--- /dev/null
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -0,0 +1,31 @@
+package(default_visibility = ['//visibility:public'])
+
+genrule(
+ name = 'genrule_hood',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:hood) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:hood',
+ ],
+ outs = [
+ 'hood_plant.h',
+ 'hood_plant.cc',
+ 'hood_integral_plant.h',
+ 'hood_integral_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'hood_plants',
+ srcs = [
+ 'hood_plant.cc',
+ 'hood_integral_plant.cc',
+ ],
+ hdrs = [
+ 'hood_plant.h',
+ 'hood_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/indexer/BUILD b/y2017/control_loops/superstructure/indexer/BUILD
new file mode 100644
index 0000000..01a52c4
--- /dev/null
+++ b/y2017/control_loops/superstructure/indexer/BUILD
@@ -0,0 +1,31 @@
+package(default_visibility = ['//visibility:public'])
+
+genrule(
+ name = 'genrule_indexer',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:indexer) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:indexer',
+ ],
+ outs = [
+ 'indexer_plant.h',
+ 'indexer_plant.cc',
+ 'indexer_integral_plant.h',
+ 'indexer_integral_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'indexer_plants',
+ srcs = [
+ 'indexer_plant.cc',
+ 'indexer_integral_plant.cc',
+ ],
+ hdrs = [
+ 'indexer_plant.h',
+ 'indexer_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
new file mode 100644
index 0000000..41e6869
--- /dev/null
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -0,0 +1,31 @@
+package(default_visibility = ['//visibility:public'])
+
+genrule(
+ name = 'genrule_intake',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:intake) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:intake',
+ ],
+ outs = [
+ 'intake_plant.h',
+ 'intake_plant.cc',
+ 'intake_integral_plant.h',
+ 'intake_integral_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'intake_plants',
+ srcs = [
+ 'intake_plant.cc',
+ 'intake_integral_plant.cc',
+ ],
+ hdrs = [
+ 'intake_plant.h',
+ 'intake_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/shooter/BUILD b/y2017/control_loops/superstructure/shooter/BUILD
new file mode 100644
index 0000000..588e12c
--- /dev/null
+++ b/y2017/control_loops/superstructure/shooter/BUILD
@@ -0,0 +1,31 @@
+package(default_visibility = ['//visibility:public'])
+
+genrule(
+ name = 'genrule_shooter',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:shooter) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:shooter',
+ ],
+ outs = [
+ 'shooter_plant.h',
+ 'shooter_plant.cc',
+ 'shooter_integral_plant.h',
+ 'shooter_integral_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'shooter_plants',
+ srcs = [
+ 'shooter_plant.cc',
+ 'shooter_integral_plant.cc',
+ ],
+ hdrs = [
+ 'shooter_plant.h',
+ 'shooter_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..4f854b7
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -0,0 +1,153 @@
+package y2017.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/profiled_subsystem.q";
+// TODO(austin): Add this back in when the queue compiler supports diamond
+// inheritance.
+//import "frc971/control_loops/control_loops.q";
+
+struct IntakeGoal {
+ // Zero on the intake is when the intake is retracted inside the robot,
+ // unable to intake. Positive is out.
+
+ // Goal distance of the intake.
+ double distance;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ .frc971.ProfileParameters profile_params;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ double voltage_rollers;
+};
+
+struct SerializerGoal {
+ // Serializer angular velocity goals in radians/second.
+ double angular_velocity;
+};
+
+struct TurretGoal {
+ // An angle of zero means the turrent faces toward the front of the
+ // robot where the intake is located. The angle increases when the turret
+ // turns clockwise (towards right from the front), and decreases when
+ // the turrent turns counter-clockwise (towards left from the front).
+ // These are from a top view above the robot.
+ double angle;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ .frc971.ProfileParameters profile_params;
+};
+
+struct HoodGoal {
+ // Angle the hood is currently at
+ double angle;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ .frc971.ProfileParameters profile_params;
+};
+
+struct ShooterGoal {
+ // Angular velocity goals in radians/second.
+ double angular_velocity;
+};
+
+struct SerializerStatus {
+ // The current average velocity in radians/second.
+ double avg_angular_velocity;
+
+ // The current instantaneous filtered velocity in radians/second.
+ double angular_velocity;
+
+ // True if the serializer is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ bool ready;
+
+ // If true, we have aborted.
+ bool estopped;
+};
+
+struct ShooterStatus {
+ // The current average velocity in radians/second.
+ double avg_angular_velocity;
+
+ // The current instantaneous filtered velocity in radians/second.
+ double angular_velocity;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ bool ready;
+
+ // If true, we have aborted.
+ bool estopped;
+};
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ IntakeGoal intake;
+ SerializerGoal serializer;
+ TurretGoal turret;
+ HoodGoal hood;
+ ShooterGoal shooter;
+ };
+
+ message Status {
+ // Are all the subsystems zeroed?
+ bool zeroed;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ bool estopped;
+
+ // Each subsystems status.
+ .frc971.control_loops.ProfiledJointStatus intake;
+ .frc971.control_loops.ProfiledJointStatus turret;
+ .frc971.control_loops.ProfiledJointStatus hood;
+ SerializerStatus serializer;
+ ShooterStatus shooter;
+ };
+
+ message Position {
+ // TODO(austin): The turret and intake really should be absolute. Switch
+ // them over when that class is ready.
+
+ // Position of the intake, zero when the intake is in, positive when it is
+ // out.
+ .frc971.PotAndIndexPosition intake;
+
+ // Serializer angle in radians.
+ double theta_serializer;
+
+ // The sensor readings for the turret. The units and sign are defined the
+ // same as what's in the Goal message.
+ .frc971.PotAndIndexPosition turret;
+
+ // The sensor readings for the hood. The units and sign are defined the
+ // same as what's in the Goal message.
+ .frc971.PotAndIndexPosition hood;
+
+ // Shooter wheel angle in radians.
+ double theta_shooter;
+ };
+
+ message Output {
+ // Voltages for some of the subsystems.
+ double voltage_intake;
+ double voltage_serializer;
+ double voltage_shooter;
+
+ // Rollers on the intake.
+ double voltage_intake_rollers;
+ // Roller on the serializer
+ double voltage_serializer_rollers;
+
+ double voltage_turret;
+ double voltage_hood;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2017/control_loops/superstructure/turret/BUILD b/y2017/control_loops/superstructure/turret/BUILD
new file mode 100644
index 0000000..2370fd7
--- /dev/null
+++ b/y2017/control_loops/superstructure/turret/BUILD
@@ -0,0 +1,31 @@
+package(default_visibility = ['//visibility:public'])
+
+genrule(
+ name = 'genrule_turret',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:turret) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:turret',
+ ],
+ outs = [
+ 'turret_plant.h',
+ 'turret_plant.cc',
+ 'turret_integral_plant.h',
+ 'turret_integral_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'turret_plants',
+ srcs = [
+ 'turret_plant.cc',
+ 'turret_integral_plant.cc',
+ ],
+ hdrs = [
+ 'turret_plant.h',
+ 'turret_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)