Merge changes Id7617d6c,I4f3ae9b6
* changes:
Make dual shooter.
Import shooter from 2013
diff --git a/.clang-format b/.clang-format
index 11973e3..510c427 100644
--- a/.clang-format
+++ b/.clang-format
@@ -1 +1,8 @@
-BasedOnStyle: 'google'
+---
+BasedOnStyle: Google
+---
+Language: Cpp
+# Force pointers to the type for C++.
+DerivePointerAlignment: false
+PointerAlignment: Right
+Standard: Cpp11
diff --git a/aos/common/logging/matrix_logging.h b/aos/common/logging/matrix_logging.h
index e763ed5..a915643 100644
--- a/aos/common/logging/matrix_logging.h
+++ b/aos/common/logging/matrix_logging.h
@@ -15,23 +15,30 @@
// Logs the contents of a matrix and a constant string.
// matrix must be an instance of an Eigen matrix (or something similar).
-#define LOG_MATRIX(level, message, matrix) \
- do { \
- static const ::std::string kAosLoggingMessage( \
- LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " message); \
- ::aos::logging::DoLogMatrixTemplated(level, kAosLoggingMessage, matrix); \
- /* so that GCC knows that it won't return */ \
- if (level == FATAL) { \
- ::aos::Die("DoLogStruct(FATAL) fell through!!!!!\n"); \
- } \
+#define LOG_MATRIX(level, message, matrix) \
+ do { \
+ static const ::std::string kAosLoggingMessage( \
+ LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " message); \
+ ::aos::logging::DoLogMatrixTemplated(level, kAosLoggingMessage, \
+ (matrix).eval()); \
+ /* so that GCC knows that it won't return */ \
+ if (level == FATAL) { \
+ ::aos::Die("DoLogStruct(FATAL) fell through!!!!!\n"); \
+ } \
} while (false)
template <class T>
void DoLogMatrixTemplated(log_level level, const ::std::string &message,
const T &matrix) {
- static_assert(!T::IsRowMajor, "we only handle column-major storage");
- internal::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
- matrix.rows(), matrix.cols(), matrix.data(), 1);
+ if (T::IsRowMajor) {
+ typename T::Scalar data[matrix.rows() * matrix.cols()];
+ ::Eigen::Map<T>(data, matrix.rows(), matrix.cols()) = matrix;
+ internal::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
+ matrix.rows(), matrix.cols(), data, 1);
+ } else {
+ internal::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
+ matrix.rows(), matrix.cols(), matrix.data(), 1);
+ }
}
} // namespace logging
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index fa94d01..d172983 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -16,3 +16,13 @@
'//third_party/cddlib:_cddlib.so',
],
)
+
+py_test(
+ name = 'polytope_test',
+ srcs = [
+ 'polytope_test.py',
+ ],
+ deps = [
+ ':controls',
+ ],
+)
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
index 9a35ebe..51bf6fd 100755
--- a/frc971/control_loops/python/polytope_test.py
+++ b/frc971/control_loops/python/polytope_test.py
@@ -2,9 +2,10 @@
import numpy
from numpy.testing import *
-import polytope
import unittest
+import frc971.control_loops.python.polytope as polytope
+
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
def MakePoint(*args):
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..720319b
--- /dev/null
+++ b/y2016/control_loops/superstructure/BUILD
@@ -0,0 +1,55 @@
+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:queues',
+ ],
+)
+
+cc_library(
+ name = 'superstructure_lib',
+ srcs = [
+ 'superstructure.cc',
+ ],
+ hdrs = [
+ 'superstructure.h',
+ ],
+ deps = [
+ ':superstructure_queue',
+ '//aos/common/controls:control_loop',
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_test(
+ name = 'superstructure_lib_test',
+ srcs = [
+ 'superstructure_lib_test.cc',
+ ],
+ deps = [
+ ':superstructure_queue',
+ ':superstructure_lib',
+ '//aos/testing:googletest',
+ '//aos/common:queues',
+ '//aos/common/controls:control_loop_test',
+ ],
+)
+
+cc_binary(
+ name = 'superstructure',
+ srcs = [
+ 'superstructure_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':superstructure_lib',
+ ':superstructure_queue',
+ ],
+)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..db31ffc
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,26 @@
+#include "y2016/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+namespace y2016 {
+namespace control_loops {
+
+Superstructure::Superstructure(
+ control_loops::SuperstructureQueue *my_superstructure)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+ my_superstructure) {}
+
+void Superstructure::RunIteration(
+ const control_loops::SuperstructureQueue::Goal *goal,
+ const control_loops::SuperstructureQueue::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::SuperstructureQueue::Status *status) {
+ (void)goal;
+ (void)position;
+ (void)output;
+ (void)status;
+}
+
+} // namespace control_loops
+} // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..f53629c
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -0,0 +1,35 @@
+#ifndef Y2016_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2016_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2016/control_loops/superstructure/superstructure.q.h"
+
+namespace y2016 {
+namespace control_loops {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ control_loops::SuperstructureQueue *my_superstructure =
+ &control_loops::superstructure_queue);
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::SuperstructureQueue::Goal *goal,
+ const control_loops::SuperstructureQueue::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::SuperstructureQueue::Status *status) override;
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace control_loops
+} // namespace y2016
+
+#endif // Y2016_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..f7a93f8
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -0,0 +1,27 @@
+package y2016.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ double value;
+ };
+
+ message Status {
+ double value;
+ };
+
+ message Position {
+ double value;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue Status status;
+};
+
+queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..3a843cd
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,86 @@
+#include "y2016/control_loops/superstructure/superstructure.h"
+
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
+
+using ::aos::time::Time;
+
+namespace y2016 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation()
+ : superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
+ ".y2016.control_loops.superstructure.goal",
+ ".y2016.control_loops.superstructure.status",
+ ".y2016.control_loops.superstructure.output",
+ ".y2016.control_loops.superstructure.status") {
+ }
+
+ // Simulates superstructure for a single timestep.
+ void Simulate() { EXPECT_TRUE(superstructure_queue_.output.FetchLatest()); }
+
+ private:
+ SuperstructureQueue superstructure_queue_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
+ ".y2016.control_loops.superstructure.goal",
+ ".y2016.control_loops.superstructure.status",
+ ".y2016.control_loops.superstructure.output",
+ ".y2016.control_loops.superstructure.status"),
+ superstructure_(&superstructure_queue_),
+ superstructure_plant_() {}
+
+ void VerifyNearGoal() {
+ // TODO(phil): Implement a check here.
+ }
+
+ // Runs one iteration of the whole simulation and checks that separation
+ // remains reasonable.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ SuperstructureQueue superstructure_queue_;
+
+ // Create a control loop and simulation.
+ Superstructure superstructure_;
+ SuperstructureSimulation superstructure_plant_;
+};
+
+// Tests that the superstructure does nothing when the goal is zero.
+TEST_F(SuperstructureTest, DoesNothing) {
+ // TODO(phil): Send a goal of some sort.
+ RunIteration();
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..d825eeb
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2016/control_loops/superstructure/superstructure.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2016::control_loops::Superstructure superstructure;
+ superstructure.Run();
+ ::aos::Cleanup();
+ return 0;
+}