Merge "Fix naming of constant"
diff --git a/NO_BUILD_AMD64 b/NO_BUILD_AMD64
index e80b47c..1f39e78 100644
--- a/NO_BUILD_AMD64
+++ b/NO_BUILD_AMD64
@@ -6,7 +6,7 @@
-//y2014/wpilib/...
-//y2014:download
-//y2014_bot3/wpilib/...
--//y2014_bot3:download
+-//y2014_bot3:download_stripped
-//y2015/wpilib/...
-//y2015:download
-//y2015_bot3/wpilib/...
diff --git a/README.md b/README.md
index 530baa8..99d6678 100644
--- a/README.md
+++ b/README.md
@@ -32,7 +32,8 @@
```console
apt-get install python libpython-dev bazel ruby clang-format-3.5 clang-3.6 gfortran libblas-dev liblapack-dev python-scipy python-matplotlib
```
- 2. Allow Bazel's sandboxing to work
+ 2. Allow Bazel's sandboxing to work:
+ Follow the direction in `doc/frc971.conf`.
Some useful Bazel commands:
* Build and test everything (on the host system):
diff --git a/aos/common/actions/actor.h b/aos/common/actions/actor.h
index b3a0220..e591084 100644
--- a/aos/common/actions/actor.h
+++ b/aos/common/actions/actor.h
@@ -65,11 +65,12 @@
// succeeded.
bool WaitOrCancel(const ::aos::time::Time& duration) {
return !WaitUntil([]() {
- ::aos::time::PhasedLoopXMS(
- ::aos::controls::kLoopFrequency.ToMSec(), 2500);
- return false;
- },
- ::aos::time::Time::Now() + duration);
+ ::aos::time::PhasedLoopXMS(
+ ::std::chrono::duration_cast<::std::chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
+ return false;
+ }, ::aos::time::Time::Now() + duration);
}
// Returns true if the action should be canceled.
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index d56e5d8..f589ed7 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -37,7 +37,8 @@
};
// Control loops run this often, "starting" at time 0.
-constexpr time::Time kLoopFrequency = time::Time::InSeconds(0.005);
+constexpr ::std::chrono::nanoseconds kLoopFrequency =
+ ::std::chrono::milliseconds(5);
// Provides helper methods to assist in writing control loops.
// This template expects to be constructed with a queue group as an argument
diff --git a/aos/common/controls/polytope.h b/aos/common/controls/polytope.h
index ccd257b..6973867 100644
--- a/aos/common/controls/polytope.h
+++ b/aos/common/controls/polytope.h
@@ -19,129 +19,140 @@
// avoid issues with that, using the "shifting" constructor is recommended
// whenever possible.
template <int number_of_dimensions>
-class HPolytope {
+class Polytope {
public:
- // Constructs a polytope given the H and k matrices.
- // Do NOT use this to calculate a polytope derived from another one in a way
- // another constructor can be used instead.
- HPolytope(
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &k)
- : H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
-
- // Constructs a polytope with H = other.H() * H_multiplier and
- // k = other.k() + other.H() * k_shifter.
- // This avoids calculating the vertices for the new H and k directly because
- // that sometimes confuses libcdd depending on what exactly the new H and k
- // are.
- //
- // This currently has the following restrictions (CHECKed internally) because
- // we don't have uses for anything else:
- // If number_of_dimensions is not 1, it must be 2, other.H() *
- // H_multiplier.inverse() * k_shifter must have its first 2 columns and last
- // 2 columns as opposites, and the 1st+2nd and 3rd+4th vertices must be
- // normal to each other.
- HPolytope(const HPolytope<number_of_dimensions> &other,
- const Eigen::Matrix<double, number_of_dimensions,
- number_of_dimensions> &H_multiplier,
- const Eigen::Matrix<double, number_of_dimensions, 1> &k_shifter)
- : H_(other.H() * H_multiplier),
- k_(other.k() + other.H() * k_shifter),
- vertices_(
- ShiftVertices(CalculateVertices(H_, other.k()),
- other.H() * H_multiplier.inverse() * k_shifter)) {}
-
- // This is an initialization function shared across all instantiations of this
- // template.
- // This must be called at least once before creating any instances. It is
- // not thread-safe.
- static void Init() {
- dd_set_global_constants();
- }
+ virtual ~Polytope() {}
// Returns a reference to H.
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H() const {
- return H_;
- }
+ virtual Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const = 0;
// Returns a reference to k.
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &k() const { return k_; }
+ virtual Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const = 0;
// Returns the number of dimensions in the polytope.
- int ndim() const { return number_of_dimensions; }
+ constexpr int ndim() const { return number_of_dimensions; }
// Returns the number of constraints currently in the polytope.
- int num_constraints() const { return k_.rows(); }
+ int num_constraints() const { return k().rows(); }
// Returns true if the point is inside the polytope.
bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point) const;
// Returns the list of vertices inside the polytope.
- const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> &Vertices()
+ virtual Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const = 0;
+};
+
+template <int number_of_dimensions, int num_vertices>
+Eigen::Matrix<double, number_of_dimensions, num_vertices> ShiftPoints(
+ const Eigen::Matrix<double, number_of_dimensions, num_vertices> &vertices,
+ const Eigen::Matrix<double, number_of_dimensions, 1> &offset) {
+ Eigen::Matrix<double, number_of_dimensions, num_vertices> ans = vertices;
+ for (int i = 0; i < num_vertices; ++i) {
+ ans.col(i) += offset;
+ }
+ return ans;
+}
+
+template <int number_of_dimensions, int num_constraints, int num_vertices>
+class HVPolytope : public Polytope<number_of_dimensions> {
+ public:
+ // Constructs a polytope given the H and k matrices.
+ HVPolytope(Eigen::Ref<const Eigen::Matrix<double, num_constraints,
+ number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<double, num_constraints, 1>> k,
+ Eigen::Ref<const Eigen::Matrix<double, number_of_dimensions,
+ num_vertices>> vertices)
+ : H_(H), k_(k), vertices_(vertices) {}
+
+ Eigen::Ref<const Eigen::Matrix<double, num_constraints, number_of_dimensions>>
+ static_H() const {
+ return H_;
+ }
+
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const override {
+ return H_;
+ }
+
+ Eigen::Ref<const Eigen::Matrix<double, num_constraints, 1>> static_k()
const {
+ return k_;
+ }
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const override {
+ return k_;
+ }
+
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const override {
+ return vertices_;
+ }
+
+ Eigen::Matrix<double, number_of_dimensions, num_vertices>
+ StaticVertices() const {
return vertices_;
}
private:
- static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
- CalculateVertices(
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &k);
+ const Eigen::Matrix<double, num_constraints, number_of_dimensions> H_;
+ const Eigen::Matrix<double, num_constraints, 1> k_;
+ const Eigen::Matrix<double, number_of_dimensions, num_vertices> vertices_;
+};
- static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
- ShiftVertices(const Eigen::Matrix<double, number_of_dimensions,
- Eigen::Dynamic> &vertices,
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &shift) {
- static_assert(number_of_dimensions <= 2, "not implemented yet");
- if (vertices.cols() != number_of_dimensions * 2) {
- LOG(FATAL,
- "less vertices not supported yet: %zd vertices vs %d dimensions\n",
- vertices.cols(), number_of_dimensions);
- }
- if (number_of_dimensions == 2) {
- if ((shift.row(0) + shift.row(1)).norm() > 0.0001) {
- LOG_MATRIX(FATAL, "bad shift amount", shift.row(0) + shift.row(1));
- }
- if ((shift.row(2) + shift.row(3)).norm() > 0.0001) {
- LOG_MATRIX(FATAL, "bad shift amount", shift.row(2) + shift.row(3));
- }
- if (vertices.col(0).dot(vertices.col(1)) > 0.0001) {
- ::Eigen::Matrix<double, number_of_dimensions, 2> bad;
- bad.col(0) = vertices.col(0);
- bad.col(1) = vertices.col(1);
- LOG_MATRIX(FATAL, "bad vertices", bad);
- }
- if (vertices.col(2).dot(vertices.col(3)) > 0.0001) {
- ::Eigen::Matrix<double, number_of_dimensions, 2> bad;
- bad.col(0) = vertices.col(2);
- bad.col(1) = vertices.col(3);
- LOG_MATRIX(FATAL, "bad vertices", bad);
- }
- }
- Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> r(
- number_of_dimensions, vertices.cols());
- Eigen::Matrix<double, number_of_dimensions, 1> real_shift;
- real_shift(0, 0) = shift(0, 0);
- if (number_of_dimensions == 2) real_shift(1, 0) = shift(2, 0);
- for (int i = 0; i < vertices.cols(); ++i) {
- r.col(i) = vertices.col(i) + real_shift;
- }
- return r;
+
+template <int number_of_dimensions>
+class HPolytope : public Polytope<number_of_dimensions> {
+ public:
+ // Constructs a polytope given the H and k matrices.
+ HPolytope(Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic,
+ number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
+ : H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
+
+ // This is an initialization function shared across all instantiations of this
+ // template.
+ // This must be called at least once before creating any instances. It is
+ // not thread-safe.
+ static void Init() { dd_set_global_constants(); }
+
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const override {
+ return H_;
+ }
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const override {
+ return k_;
}
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const override {
+ return vertices_;
+ }
+
+ private:
const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
const Eigen::Matrix<double, Eigen::Dynamic, 1> k_;
-
const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices_;
+
+ static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+ CalculateVertices(
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k);
};
template <int number_of_dimensions>
-bool HPolytope<number_of_dimensions>::IsInside(
+bool Polytope<number_of_dimensions>::IsInside(
Eigen::Matrix<double, number_of_dimensions, 1> point) const {
- auto ev = H_ * point;
- for (int i = 0; i < num_constraints(); ++i) {
- if (ev(i, 0) > k_(i, 0)) {
+ auto k_ptr = k();
+ auto ev = H() * point;
+ for (int i = 0; i < k_ptr.rows(); ++i) {
+ if (ev(i, 0) > k_ptr(i, 0)) {
return false;
}
}
@@ -151,8 +162,9 @@
template <int number_of_dimensions>
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
HPolytope<number_of_dimensions>::CalculateVertices(
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &k) {
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k) {
dd_MatrixPtr matrix = dd_CreateMatrix(k.rows(), number_of_dimensions + 1);
// Copy the data over. TODO(aschuh): Is there a better way? I hate copying...
diff --git a/aos/common/controls/polytope_test.cc b/aos/common/controls/polytope_test.cc
index b458b74..a43b056 100644
--- a/aos/common/controls/polytope_test.cc
+++ b/aos/common/controls/polytope_test.cc
@@ -72,23 +72,6 @@
}
return r;
}
-
- template <int number_of_dimensions>
- void CheckShiftedVertices(
- const HPolytope<number_of_dimensions> &base,
- const Eigen::Matrix<double, number_of_dimensions, number_of_dimensions> &
- H_multiplier,
- const Eigen::Matrix<double, number_of_dimensions, 1> &k_shifter) {
- LOG_MATRIX(DEBUG, "base vertices", base.Vertices());
- const auto shifted = HPolytope<number_of_dimensions>(
- base.H() * H_multiplier, base.k() + base.H() * k_shifter);
- LOG_MATRIX(DEBUG, "shifted vertices", shifted.Vertices());
- LOG_MATRIX(DEBUG, "shifted - base", shifted.Vertices() - base.Vertices());
- EXPECT_THAT(MatrixToVectors(HPolytope<number_of_dimensions>(
- base, H_multiplier, k_shifter).Vertices()),
- ::testing::UnorderedElementsAreArray(
- MatrixToMatchers(shifted.Vertices())));
- }
};
// Tests that the vertices for various polytopes calculated from H and k are
@@ -115,40 +98,5 @@
1, 0, 0.5, 0, -0.5).finished())));
}
-TEST_F(HPolytopeTest, ShiftedVertices) {
- CheckShiftedVertices(
- Polytope1(), (Eigen::Matrix<double, 2, 2>() << 1, 1, 1, -1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
- CheckShiftedVertices(
- Polytope1(), (Eigen::Matrix<double, 2, 2>() << 1, -1, 1, 1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
- CheckShiftedVertices(
- Polytope1(), (Eigen::Matrix<double, 2, 2>() << 1, 1, 1.5, -1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
- CheckShiftedVertices(
- Polytope1(),
- (Eigen::Matrix<double, 2, 2>() << 1, 1.5, -1.5, -1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
- CheckShiftedVertices(
- Polytope1(),
- (Eigen::Matrix<double, 2, 2>() << 1, -1.5, -1.5, -1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
- CheckShiftedVertices(
- Polytope1(),
- (Eigen::Matrix<double, 2, 2>() << 2, -1.5, -1.5, -1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
- CheckShiftedVertices(
- Polytope1(),
- (Eigen::Matrix<double, 2, 2>() << 2, -1.5, -1.25, -1).finished(),
- (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
-
- CheckShiftedVertices(Polytope3(),
- (Eigen::Matrix<double, 1, 1>() << 1).finished(),
- (Eigen::Matrix<double, 1, 1>() << 9.71).finished());
- CheckShiftedVertices(Polytope3(),
- (Eigen::Matrix<double, 1, 1>() << -2.54).finished(),
- (Eigen::Matrix<double, 1, 1>() << 16.78).finished());
-}
-
} // namespace controls
} // namespace aos
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 125b016..b2d3675 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -41,16 +41,6 @@
namespace aos {
-monotonic_clock::time_point monotonic_clock::now() noexcept {
- struct timespec current_time;
- if (clock_gettime(CLOCK_MONOTONIC, ¤t_time) != 0) {
- PLOG(FATAL, "clock_gettime(%jd, %p) failed",
- static_cast<uintmax_t>(CLOCK_MONOTONIC), ¤t_time);
- }
- return time_point(::std::chrono::seconds(current_time.tv_sec) +
- ::std::chrono::nanoseconds(current_time.tv_nsec));
-}
-
namespace time {
// State required to enable and use mock time.
@@ -281,4 +271,26 @@
}
} // namespace time
+
+constexpr monotonic_clock::time_point monotonic_clock::min_time;
+
+monotonic_clock::time_point monotonic_clock::now() noexcept {
+ {
+ if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
+ MutexLocker time_mutex_locker(&time::time_mutex);
+ return monotonic_clock::time_point(
+ ::std::chrono::nanoseconds(time::current_mock_time.ToNSec()));
+ }
+ }
+
+ struct timespec current_time;
+ if (clock_gettime(CLOCK_MONOTONIC, ¤t_time) != 0) {
+ PLOG(FATAL, "clock_gettime(%jd, %p) failed",
+ static_cast<uintmax_t>(CLOCK_MONOTONIC), ¤t_time);
+ }
+ return time_point(::std::chrono::seconds(current_time.tv_sec) +
+ ::std::chrono::nanoseconds(current_time.tv_nsec));
+}
+
+
} // namespace aos
diff --git a/aos/common/time.h b/aos/common/time.h
index ba795a9..8f57916 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -32,6 +32,9 @@
}
static constexpr monotonic_clock::duration zero() { return duration(0); }
+
+ static constexpr time_point min_time{
+ time_point(duration(::std::numeric_limits<duration::rep>::min()))};
};
namespace time {
diff --git a/aos/common/util/trapezoid_profile.cc b/aos/common/util/trapezoid_profile.cc
index f441fbe..4e4546e 100644
--- a/aos/common/util/trapezoid_profile.cc
+++ b/aos/common/util/trapezoid_profile.cc
@@ -7,10 +7,8 @@
namespace aos {
namespace util {
-TrapezoidProfile::TrapezoidProfile(const time::Time &delta_time)
- : maximum_acceleration_(0),
- maximum_velocity_(0),
- timestep_(delta_time) {
+TrapezoidProfile::TrapezoidProfile(::std::chrono::nanoseconds delta_time)
+ : maximum_acceleration_(0), maximum_velocity_(0), timestep_(delta_time) {
output_.setZero();
}
@@ -26,7 +24,9 @@
double goal_velocity) {
CalculateTimes(goal_position - output_(0), goal_velocity);
- double next_timestep = timestep_.ToSeconds();
+ double next_timestep =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(timestep_)
+ .count();
if (acceleration_time_ > next_timestep) {
UpdateVals(acceleration_, next_timestep);
diff --git a/aos/common/util/trapezoid_profile.h b/aos/common/util/trapezoid_profile.h
index fe63352..cc91db1 100644
--- a/aos/common/util/trapezoid_profile.h
+++ b/aos/common/util/trapezoid_profile.h
@@ -17,7 +17,7 @@
class TrapezoidProfile {
public:
// delta_time is how long between each call to Update.
- TrapezoidProfile(const time::Time &delta_time);
+ TrapezoidProfile(::std::chrono::nanoseconds delta_time);
// Updates the state.
const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
@@ -58,7 +58,7 @@
double maximum_velocity_;
// How long between calls to Update.
- const time::Time timestep_;
+ ::std::chrono::nanoseconds timestep_;
DISALLOW_COPY_AND_ASSIGN(TrapezoidProfile);
};
diff --git a/aos/common/util/trapezoid_profile_test.cc b/aos/common/util/trapezoid_profile_test.cc
index 1dfeff3..c6e811c 100644
--- a/aos/common/util/trapezoid_profile_test.cc
+++ b/aos/common/util/trapezoid_profile_test.cc
@@ -45,11 +45,13 @@
}
private:
- static const time::Time delta_time;
+ static constexpr ::std::chrono::nanoseconds delta_time =
+ ::std::chrono::milliseconds(10);
Eigen::Matrix<double, 2, 1> position_;
};
-const time::Time TrapezoidProfileTest::delta_time = time::Time::InSeconds(0.01);
+
+constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::delta_time;
TEST_F(TrapezoidProfileTest, ReachesGoal) {
for (int i = 0; i < 450; ++i) {
diff --git a/aos/linux_code/ipc_lib/ipc_comparison.cc b/aos/linux_code/ipc_lib/ipc_comparison.cc
index f375a17..73c0f9b 100644
--- a/aos/linux_code/ipc_lib/ipc_comparison.cc
+++ b/aos/linux_code/ipc_lib/ipc_comparison.cc
@@ -5,6 +5,7 @@
#include <sys/un.h>
#include <pthread.h>
#include <netinet/in.h>
+#include <netinet/tcp.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
@@ -224,8 +225,12 @@
class TCPPingPonger : public FDPingPonger {
public:
- TCPPingPonger() {
+ TCPPingPonger(bool nodelay) {
server_ = PCHECK(socket(AF_INET, SOCK_STREAM, 0));
+ if (nodelay) {
+ const int yes = 1;
+ PCHECK(setsockopt(server_, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes)));
+ }
{
sockaddr_in server_address;
memset(&server_address, 0, sizeof(server_address));
@@ -237,6 +242,10 @@
PCHECK(listen(server_, 1));
client_ = PCHECK(socket(AF_INET, SOCK_STREAM, 0));
+ if (nodelay) {
+ const int yes = 1;
+ PCHECK(setsockopt(client_, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes)));
+ }
{
sockaddr_in client_address;
unsigned int length = sizeof(client_address);
@@ -465,19 +474,42 @@
class PthreadMutexPingPonger : public ConditionVariablePingPonger {
public:
- PthreadMutexPingPonger()
+ PthreadMutexPingPonger(int pshared, bool pi)
: ConditionVariablePingPonger(
::std::unique_ptr<ConditionVariableInterface>(
- new PthreadConditionVariable()),
+ new PthreadConditionVariable(pshared, pi)),
::std::unique_ptr<ConditionVariableInterface>(
- new PthreadConditionVariable())) {}
+ new PthreadConditionVariable(pshared, pi))) {}
private:
class PthreadConditionVariable : public ConditionVariableInterface {
public:
- PthreadConditionVariable() {
- PRCHECK(pthread_cond_init(&condition_, nullptr));
- PRCHECK(pthread_mutex_init(&mutex_, nullptr));
+ PthreadConditionVariable(bool pshared, bool pi) {
+ {
+ pthread_condattr_t cond_attr;
+ PRCHECK(pthread_condattr_init(&cond_attr));
+ if (pshared) {
+ PRCHECK(
+ pthread_condattr_setpshared(&cond_attr, PTHREAD_PROCESS_SHARED));
+ }
+ PRCHECK(pthread_cond_init(&condition_, &cond_attr));
+ PRCHECK(pthread_condattr_destroy(&cond_attr));
+ }
+
+ {
+ pthread_mutexattr_t mutex_attr;
+ PRCHECK(pthread_mutexattr_init(&mutex_attr));
+ if (pshared) {
+ PRCHECK(pthread_mutexattr_setpshared(&mutex_attr,
+ PTHREAD_PROCESS_SHARED));
+ }
+ if (pi) {
+ PRCHECK(
+ pthread_mutexattr_setprotocol(&mutex_attr, PTHREAD_PRIO_INHERIT));
+ }
+ PRCHECK(pthread_mutex_init(&mutex_, nullptr));
+ PRCHECK(pthread_mutexattr_destroy(&mutex_attr));
+ }
}
~PthreadConditionVariable() {
PRCHECK(pthread_mutex_destroy(&mutex_));
@@ -779,7 +811,13 @@
} else if (FLAGS_method == "aos_event") {
ping_ponger.reset(new AOSEventPingPonger());
} else if (FLAGS_method == "pthread_mutex") {
- ping_ponger.reset(new PthreadMutexPingPonger());
+ ping_ponger.reset(new PthreadMutexPingPonger(false, false));
+ } else if (FLAGS_method == "pthread_mutex_pshared") {
+ ping_ponger.reset(new PthreadMutexPingPonger(true, false));
+ } else if (FLAGS_method == "pthread_mutex_pshared_pi") {
+ ping_ponger.reset(new PthreadMutexPingPonger(true, true));
+ } else if (FLAGS_method == "pthread_mutex_pi") {
+ ping_ponger.reset(new PthreadMutexPingPonger(false, true));
} else if (FLAGS_method == "aos_queue") {
ping_ponger.reset(new AOSQueuePingPonger());
} else if (FLAGS_method == "eventfd") {
@@ -803,7 +841,9 @@
} else if (FLAGS_method == "unix_seqpacket") {
ping_ponger.reset(new UnixPingPonger(SOCK_SEQPACKET));
} else if (FLAGS_method == "tcp") {
- ping_ponger.reset(new TCPPingPonger());
+ ping_ponger.reset(new TCPPingPonger(false));
+ } else if (FLAGS_method == "tcp_nodelay") {
+ ping_ponger.reset(new TCPPingPonger(true));
} else if (FLAGS_method == "udp") {
ping_ponger.reset(new UDPPingPonger());
} else {
@@ -890,6 +930,9 @@
"\taos_mutex\n"
"\taos_event\n"
"\tpthread_mutex\n"
+ "\tpthread_mutex_pshared\n"
+ "\tpthread_mutex_pshared_pi\n"
+ "\tpthread_mutex_pi\n"
"\taos_queue\n"
"\teventfd\n"
"\tsysv_semaphore\n"
@@ -902,6 +945,7 @@
"\tunix_datagram\n"
"\tunix_seqpacket\n"
"\ttcp\n"
+ "\ttcp_nodelay\n"
"\tudp\n");
::gflags::ParseCommandLineFlags(&argc, &argv, true);
diff --git a/aos/linux_code/ipc_lib/queue.cc b/aos/linux_code/ipc_lib/queue.cc
index 2738e85..b874b08 100644
--- a/aos/linux_code/ipc_lib/queue.cc
+++ b/aos/linux_code/ipc_lib/queue.cc
@@ -195,7 +195,7 @@
if (queue_length < 1) {
LOG(FATAL, "queue length %d of %s needs to be at least 1\n", queue_length,
- name_);
+ name);
}
const size_t name_size = strlen(name) + 1;
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 0868c2c..0c98560 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -8,7 +8,7 @@
namespace control_loops {
Eigen::Matrix<double, 2, 1> DoCoerceGoal(
- const aos::controls::HPolytope<2> ®ion,
+ const aos::controls::HVPolytope<2, 4, 4> ®ion,
const Eigen::Matrix<double, 1, 2> &K, double w,
const Eigen::Matrix<double, 2, 1> &R, bool *is_inside) {
if (region.IsInside(R)) {
@@ -20,12 +20,26 @@
perpendicular_vector = K.transpose().normalized();
parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
- aos::controls::HPolytope<1> t_poly(
- region.H() * parallel_vector,
- region.k() - region.H() * perpendicular_vector * w);
+ Eigen::Matrix<double, 4, 1> projectedh = region.static_H() * parallel_vector;
+ Eigen::Matrix<double, 4, 1> projectedk =
+ region.static_k() - region.static_H() * perpendicular_vector * w;
- Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
- if (vertices.innerSize() > 0) {
+ double min_boundary = ::std::numeric_limits<double>::lowest();
+ double max_boundary = ::std::numeric_limits<double>::max();
+ for (int i = 0; i < 4; ++i) {
+ if (projectedh(i, 0) > 0) {
+ max_boundary =
+ ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
+ } else {
+ min_boundary =
+ ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
+ }
+ }
+
+ Eigen::Matrix<double, 1, 2> vertices;
+ vertices << max_boundary, min_boundary;
+
+ if (max_boundary > min_boundary) {
double min_distance_sqr = 0;
Eigen::Matrix<double, 2, 1> closest_point;
for (int i = 0; i < vertices.innerSize(); i++) {
@@ -40,8 +54,8 @@
if (is_inside) *is_inside = true;
return closest_point;
} else {
- Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
- region.Vertices();
+ Eigen::Matrix<double, 2, 4> region_vertices =
+ region.StaticVertices();
CHECK_GT(region_vertices.outerSize(), 0);
double min_distance = INFINITY;
int closest_i = 0;
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 3933050..83c401d 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,21 +8,19 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R,
- bool *is_inside);
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K, double w,
+ const Eigen::Matrix<double, 2, 1> &R, bool *is_inside);
// Intersects a line with a region, and finds the closest point to R.
// Finds a point that is closest to R inside the region, and on the line
// defined by K X = w. If it is not possible to find a point on the line,
// finds a point that is inside the region and closest to the line.
-static inline Eigen::Matrix<double, 2, 1>
- CoerceGoal(const aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R) {
+static inline Eigen::Matrix<double, 2, 1> CoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K, double w,
+ const Eigen::Matrix<double, 2, 1> &R) {
return DoCoerceGoal(region, K, w, R, nullptr);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 5757b58..7bbba74 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -140,7 +140,7 @@
// z accel is down
// x accel is the front of the robot pointed down.
Eigen::Matrix<double, 1, 1> Y;
- Y << angle;
+ Y(0, 0) = angle;
down_estimator_.Correct(Y);
}
@@ -148,7 +148,7 @@
"New IMU value from ADIS16448, rate is %f, angle %f, fused %f, bias "
"%f\n",
rate, angle, down_estimator_.X_hat(0, 0), down_estimator_.X_hat(1, 0));
- down_U_ << rate;
+ down_U_(0, 0) = rate;
}
down_estimator_.UpdateObserver(down_U_);
@@ -205,14 +205,14 @@
status->robot_speed = (kf_.X_hat(1, 0) + kf_.X_hat(3, 0)) / 2.0;
Eigen::Matrix<double, 2, 1> linear =
- dt_closedloop_.LeftRightToLinear(kf_.X_hat());
+ dt_config_.LeftRightToLinear(kf_.X_hat());
Eigen::Matrix<double, 2, 1> angular =
- dt_closedloop_.LeftRightToAngular(kf_.X_hat());
+ dt_config_.LeftRightToAngular(kf_.X_hat());
angular(0, 0) = integrated_kf_heading_;
Eigen::Matrix<double, 4, 1> gyro_left_right =
- dt_closedloop_.AngularLinearToLeftRight(linear, angular);
+ dt_config_.AngularLinearToLeftRight(linear, angular);
status->estimated_left_position = gyro_left_right(0, 0);
status->estimated_right_position = gyro_left_right(2, 0);
@@ -256,7 +256,8 @@
// Voltage error.
Eigen::Matrix<double, 2, 1> U;
- U << last_left_voltage_, last_right_voltage_;
+ U(0, 0) = last_left_voltage_;
+ U(1, 0) = last_right_voltage_;
last_left_voltage_ = left_voltage;
last_right_voltage_ = right_voltage;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 24cc628..1fd633a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -51,6 +51,44 @@
bool default_high_gear;
double down_offset;
+
+ double wheel_non_linearity;
+
+ double quickturn_wheel_multiplier;
+
+ // Converts the robot state to a linear distance position, velocity.
+ static Eigen::Matrix<double, 2, 1> LeftRightToLinear(
+ const Eigen::Matrix<double, 7, 1> &left_right) {
+ Eigen::Matrix<double, 2, 1> linear;
+ linear(0, 0) = (left_right(0, 0) + left_right(2, 0)) / 2.0;
+ linear(1, 0) = (left_right(1, 0) + left_right(3, 0)) / 2.0;
+ return linear;
+ }
+ // Converts the robot state to an anglular distance, velocity.
+ Eigen::Matrix<double, 2, 1> LeftRightToAngular(
+ const Eigen::Matrix<double, 7, 1> &left_right) const {
+ Eigen::Matrix<double, 2, 1> angular;
+ angular(0, 0) =
+ (left_right(2, 0) - left_right(0, 0)) / (this->robot_radius * 2.0);
+ angular(1, 0) =
+ (left_right(3, 0) - left_right(1, 0)) / (this->robot_radius * 2.0);
+ return angular;
+ }
+
+ // Converts the linear and angular position, velocity to the top 4 states of
+ // the robot state.
+ Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
+ const Eigen::Matrix<double, 2, 1> &linear,
+ const Eigen::Matrix<double, 2, 1> &angular) const {
+ Eigen::Matrix<double, 2, 1> scaled_angle =
+ angular * this->robot_radius;
+ Eigen::Matrix<double, 4, 1> state;
+ state(0, 0) = linear(0, 0) - scaled_angle(0, 0);
+ state(1, 0) = linear(1, 0) - scaled_angle(1, 0);
+ state(2, 0) = linear(0, 0) + scaled_angle(0, 0);
+ state(3, 0) = linear(1, 0) + scaled_angle(1, 0);
+ return state;
+ }
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index aa18aec..d3ea2f0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -49,7 +49,13 @@
::y2016::control_loops::drivetrain::kHighGearRatio,
::y2016::control_loops::drivetrain::kLowGearRatio,
- kThreeStateDriveShifter, kThreeStateDriveShifter, false, 0};
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ false,
+ 0,
+
+ 0.25,
+ 1.00};
return kDrivetrainConfig;
};
@@ -88,8 +94,7 @@
// Constructs a motor simulation.
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
- : drivetrain_plant_(
- new DrivetrainPlant(MakeDrivetrainPlant())),
+ : drivetrain_plant_(new DrivetrainPlant(MakeDrivetrainPlant())),
my_drivetrain_queue_(".frc971.control_loops.drivetrain", 0x8a8dde77,
".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
@@ -348,27 +353,23 @@
// Tests that converting from a left, right position to a distance, angle
// coordinate system and back returns the same answer.
TEST_F(DrivetrainTest, LinearToAngularAndBack) {
- StateFeedbackLoop<7, 2, 3> kf(
- GetDrivetrainConfig().make_kf_drivetrain_loop());
- double kf_heading = 0;
- DrivetrainMotorsSS drivetrain_ss(GetDrivetrainConfig(), &kf, &kf_heading);
-
- const double width = GetDrivetrainConfig().robot_radius * 2.0;
+ const DrivetrainConfig dt_config = GetDrivetrainConfig();
+ const double width = dt_config.robot_radius * 2.0;
Eigen::Matrix<double, 7, 1> state;
state << 2, 3, 4, 5, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> linear = drivetrain_ss.LeftRightToLinear(state);
+ Eigen::Matrix<double, 2, 1> linear = dt_config.LeftRightToLinear(state);
EXPECT_NEAR(3.0, linear(0, 0), 1e-6);
EXPECT_NEAR(4.0, linear(1, 0), 1e-6);
- Eigen::Matrix<double, 2, 1> angular = drivetrain_ss.LeftRightToAngular(state);
+ Eigen::Matrix<double, 2, 1> angular = dt_config.LeftRightToAngular(state);
EXPECT_NEAR(2.0 / width, angular(0, 0), 1e-6);
EXPECT_NEAR(2.0 / width, angular(1, 0), 1e-6);
Eigen::Matrix<double, 4, 1> back_state =
- drivetrain_ss.AngularLinearToLeftRight(linear, angular);
+ dt_config.AngularLinearToLeftRight(linear, angular);
for (int i = 0; i < 4; ++i) {
EXPECT_NEAR(state(i, 0), back_state(i, 0), 1e-8);
@@ -522,8 +523,8 @@
VerifyNearGoal();
}
-::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
- double x2_min, double x2_max) {
+::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
+ double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
box_H << /*[[*/ 1.0, 0.0 /*]*/,
/*[*/ -1.0, 0.0 /*]*/,
@@ -535,7 +536,8 @@
/*[*/ x2_max /*]*/,
/*[*/ -x2_min /*]]*/;
::aos::controls::HPolytope<2> t_poly(box_H, box_k);
- return t_poly;
+ return ::aos::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
+ t_poly.Vertices());
}
class CoerceGoalTest : public ::testing::Test {
@@ -545,7 +547,7 @@
// WHOOOHH!
TEST_F(CoerceGoalTest, Inside) {
- ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << /*[[*/ 1, -1 /*]]*/;
@@ -561,7 +563,7 @@
}
TEST_F(CoerceGoalTest, Outside_Inside_Intersect) {
- ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << 1, -1;
@@ -577,7 +579,7 @@
}
TEST_F(CoerceGoalTest, Outside_Inside_no_Intersect) {
- ::aos::controls::HPolytope<2> box = MakeBox(3, 4, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << 1, -1;
@@ -593,7 +595,7 @@
}
TEST_F(CoerceGoalTest, Middle_Of_Edge) {
- ::aos::controls::HPolytope<2> box = MakeBox(0, 4, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << -1, 1;
@@ -609,7 +611,7 @@
}
TEST_F(CoerceGoalTest, PerpendicularLine) {
- ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << 1, 1;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 121a231..654ca2c 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -16,10 +16,6 @@
namespace control_loops {
namespace drivetrain {
-using ::frc971::control_loops::GearLogging;
-using ::frc971::control_loops::CIMLogging;
-using ::frc971::control_loops::CoerceGoal;
-
PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
StateFeedbackLoop<7, 2, 3> *kf)
: kf_(kf),
@@ -30,7 +26,9 @@
(Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/).finished()),
+ /*[*/ 12 /*]]*/).finished(),
+ (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+ /*[*/ -12, 12, 12, -12 /*]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
ttrust_(1.1),
wheel_(0.0),
@@ -112,15 +110,18 @@
const bool quickturn = goal.quickturn;
const bool highgear = goal.highgear;
- const double kWheelNonLinearity = 0.25;
// Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI_2 * kWheelNonLinearity;
+ const double angular_range = M_PI_2 * dt_config_.wheel_non_linearity;
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
wheel_ = 2.0 * wheel - wheel_;
quickturn_ = quickturn;
+ if (!quickturn_) {
+ wheel_ *= dt_config_.quickturn_wheel_multiplier;
+ }
+
static const double kThrottleDeadband = 0.05;
if (::std::abs(throttle) < kThrottleDeadband) {
throttle_ = 0;
@@ -239,13 +240,16 @@
const double equality_w = 0.0;
// Construct a constraint on R by manipulating the constraint on U
- ::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
- U_Poly_.H() * (loop_->K() + FF),
- U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
+ ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
+ U_Poly_.static_H() * (loop_->K() + FF),
+ U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
+ (loop_->K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
+ loop_->K() * loop_->X_hat()));
// Limit R back inside the box.
loop_->mutable_R() =
- CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+ CoerceGoal(R_poly_hv, equality_k, equality_w, loop_->R());
}
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 1ea38a7..20f1a49 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -47,7 +47,7 @@
private:
StateFeedbackLoop<7, 2, 3> *kf_;
- const ::aos::controls::HPolytope<2> U_Poly_;
+ const ::aos::controls::HVPolytope<2, 4, 4> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 235738d..b7a68a9 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -13,8 +13,6 @@
namespace control_loops {
namespace drivetrain {
-using ::frc971::control_loops::DoCoerceGoal;
-
void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
output_was_capped_ =
::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
@@ -53,9 +51,15 @@
Eigen::Matrix<double, 2, 1> U_integral;
U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
- const ::aos::controls::HPolytope<2> pos_poly(
- U_poly_, position_K * T_,
- -velocity_K * velocity_error + U_integral - kf_->ff_U());
+ const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
+ U_poly_.static_H() * position_K * T_,
+ U_poly_.static_H() *
+ (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
+ U_poly_.static_k(),
+ (position_K * T_).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(
+ U_poly_.StaticVertices(),
+ -velocity_K * velocity_error + U_integral - kf_->ff_U()));
Eigen::Matrix<double, 2, 1> adjusted_pos_error;
{
@@ -81,10 +85,10 @@
bool is_inside_h, is_inside_45;
const auto adjusted_pos_error_h =
- DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ DoCoerceGoal(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
const auto adjusted_pos_error_45 =
- DoCoerceGoal(pos_poly, L45, w45, intersection, &is_inside_45);
- if (pos_poly.IsInside(intersection)) {
+ DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+ if (pos_poly_hv.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
if (is_inside_h) {
@@ -115,10 +119,16 @@
double *integrated_kf_heading)
: dt_config_(dt_config),
kf_(kf),
- U_poly_(
- (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
- .finished(),
- (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()),
+ U_poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+ /*[*/ -1, 0 /*]*/,
+ /*[*/ 0, 1 /*]*/,
+ /*[*/ 0, -1 /*]]*/).finished(),
+ (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]]*/).finished(),
+ (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+ /*[*/ -12, 12, 12, -12 /*]*/).finished()),
linear_profile_(::aos::controls::kLoopFrequency),
angular_profile_(::aos::controls::kLoopFrequency),
integrated_kf_heading_(integrated_kf_heading) {
@@ -144,38 +154,9 @@
angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
}
-// (left + right) / 2 = linear
-// (right - left) / width = angular
-
-Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToLinear(
- const Eigen::Matrix<double, 7, 1> &left_right) const {
- Eigen::Matrix<double, 2, 1> linear;
- linear << (left_right(0, 0) + left_right(2, 0)) / 2.0,
- (left_right(1, 0) + left_right(3, 0)) / 2.0;
- return linear;
-}
-
-Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToAngular(
- const Eigen::Matrix<double, 7, 1> &left_right) const {
- Eigen::Matrix<double, 2, 1> angular;
- angular << (left_right(2, 0) - left_right(0, 0)) /
- (dt_config_.robot_radius * 2.0),
- (left_right(3, 0) - left_right(1, 0)) / (dt_config_.robot_radius * 2.0);
- return angular;
-}
-
-Eigen::Matrix<double, 4, 1> DrivetrainMotorsSS::AngularLinearToLeftRight(
- const Eigen::Matrix<double, 2, 1> &linear,
- const Eigen::Matrix<double, 2, 1> &angular) const {
- Eigen::Matrix<double, 2, 1> scaled_angle = angular * dt_config_.robot_radius;
- Eigen::Matrix<double, 4, 1> state;
- state << linear(0, 0) - scaled_angle(0, 0), linear(1, 0) - scaled_angle(1, 0),
- linear(0, 0) + scaled_angle(0, 0), linear(1, 0) + scaled_angle(1, 0);
- return state;
-}
-
void DrivetrainMotorsSS::Update(bool enable_control_loop) {
- Eigen::Matrix<double, 2, 1> wheel_heading = LeftRightToAngular(kf_->X_hat());
+ Eigen::Matrix<double, 2, 1> wheel_heading =
+ dt_config_.LeftRightToAngular(kf_->X_hat());
const double gyro_to_wheel_offset =
wheel_heading(0, 0) - *integrated_kf_heading_;
@@ -183,9 +164,9 @@
if (enable_control_loop) {
// Update profiles.
Eigen::Matrix<double, 2, 1> unprofiled_linear =
- LeftRightToLinear(unprofiled_goal_);
+ dt_config_.LeftRightToLinear(unprofiled_goal_);
Eigen::Matrix<double, 2, 1> unprofiled_angular =
- LeftRightToAngular(unprofiled_goal_);
+ dt_config_.LeftRightToAngular(unprofiled_goal_);
Eigen::Matrix<double, 2, 1> next_linear;
Eigen::Matrix<double, 2, 1> next_angular;
@@ -207,7 +188,7 @@
dt_config_.robot_radius;
kf_->mutable_next_R().block<4, 1>(0, 0) =
- AngularLinearToLeftRight(next_linear, next_angular);
+ dt_config_.AngularLinearToLeftRight(next_linear, next_angular);
kf_->mutable_next_R().block<3, 1>(4, 0) =
unprofiled_goal_.block<3, 1>(4, 0);
@@ -236,25 +217,26 @@
(kf_->U() - kf_->U_uncapped()).lpNorm<Eigen::Infinity>() > 1e-4) {
// kf_->R() is in wheel coordinates, while the profile is in absolute
// coordinates. Convert back...
- linear_profile_.MoveCurrentState(LeftRightToLinear(kf_->R()));
+ linear_profile_.MoveCurrentState(dt_config_.LeftRightToLinear(kf_->R()));
LOG(DEBUG, "Saturated while moving\n");
Eigen::Matrix<double, 2, 1> absolute_angular =
- LeftRightToAngular(kf_->R());
+ dt_config_.LeftRightToAngular(kf_->R());
absolute_angular(0, 0) -= gyro_to_wheel_offset;
angular_profile_.MoveCurrentState(absolute_angular);
}
} else {
- Eigen::Matrix<double, 2, 1> wheel_linear = LeftRightToLinear(kf_->X_hat());
+ Eigen::Matrix<double, 2, 1> wheel_linear =
+ dt_config_.LeftRightToLinear(kf_->X_hat());
Eigen::Matrix<double, 2, 1> next_angular = wheel_heading;
next_angular(0, 0) = *integrated_kf_heading_;
unprofiled_goal_.block<4, 1>(0, 0) =
- AngularLinearToLeftRight(wheel_linear, next_angular);
+ dt_config_.AngularLinearToLeftRight(wheel_linear, next_angular);
- auto current_linear = LeftRightToLinear(unprofiled_goal_);
- auto current_angular = LeftRightToAngular(unprofiled_goal_);
+ auto current_linear = dt_config_.LeftRightToLinear(unprofiled_goal_);
+ auto current_angular = dt_config_.LeftRightToAngular(unprofiled_goal_);
linear_profile_.MoveCurrentState(current_linear);
angular_profile_.MoveCurrentState(current_angular);
@@ -277,14 +259,14 @@
void DrivetrainMotorsSS::PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const {
Eigen::Matrix<double, 2, 1> profiled_linear =
- LeftRightToLinear(kf_->next_R());
+ dt_config_.LeftRightToLinear(kf_->next_R());
Eigen::Matrix<double, 2, 1> profiled_angular =
- LeftRightToAngular(kf_->next_R());
+ dt_config_.LeftRightToAngular(kf_->next_R());
profiled_angular(0, 0) -= last_gyro_to_wheel_offset_;
Eigen::Matrix<double, 4, 1> profiled_gyro_left_right =
- AngularLinearToLeftRight(profiled_linear, profiled_angular);
+ dt_config_.AngularLinearToLeftRight(profiled_linear, profiled_angular);
status->profiled_left_position_goal = profiled_gyro_left_right(0, 0);
status->profiled_left_velocity_goal = profiled_gyro_left_right(1, 0);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 15a4823..ce12b19 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -38,19 +38,6 @@
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
- // Converts the robot state to a linear distance position, velocity.
- Eigen::Matrix<double, 2, 1> LeftRightToLinear(
- const Eigen::Matrix<double, 7, 1> &left_right) const;
- // Converts the robot state to an anglular distance, velocity.
- Eigen::Matrix<double, 2, 1> LeftRightToAngular(
- const Eigen::Matrix<double, 7, 1> &left_right) const;
-
- // Converts the linear and angular position, velocity to the top 4 states of
- // the robot state.
- Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
- const Eigen::Matrix<double, 2, 1> &linear,
- const Eigen::Matrix<double, 2, 1> &angular) const;
-
private:
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
@@ -63,7 +50,7 @@
// Reprsents +/- full power on each motor in U-space, aka the square from
// (-12, -12) to (12, 12).
- const ::aos::controls::HPolytope<2> U_poly_;
+ const ::aos::controls::HVPolytope<2, 4, 4> U_poly_;
// multiplying by T converts [left_error, right_error] to
// [left_right_error_difference, total_distance_error].
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 951a114..d6bd85f 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -244,18 +244,10 @@
"""
ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
matrix.shape[0], matrix.shape[1], matrix_name)]
- first = True
for x in xrange(matrix.shape[0]):
for y in xrange(matrix.shape[1]):
- element = matrix[x, y]
- if first:
- ans.append(' %s << ' % matrix_name)
- first = False
- else:
- ans.append(', ')
- ans.append(str(element))
+ ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, repr(matrix[x, y])))
- ans.append(';\n')
return ''.join(ans)
def DumpPlantHeader(self):
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 037fab9..c0ee47b 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -19,6 +19,8 @@
namespace y2014 {
namespace actors {
+namespace chrono = ::std::chrono;
+
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
loop_(constants::GetValues().make_drivetrain_loop()) {
@@ -34,8 +36,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index 96b27a6..4df1a0f 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -32,7 +32,9 @@
constants::GetValues().left_drive,
constants::GetValues().right_drive,
true,
- 0};
+ 0,
+ 0.25,
+ 1.0};
return kDrivetrainConfig;
};
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index faeeaf0..af310f0 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -4,6 +4,7 @@
#include <algorithm>
#include <limits>
+#include <chrono>
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -15,11 +16,13 @@
namespace y2014 {
namespace control_loops {
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
using ::y2014::control_loops::shooter::kSpringConstant;
using ::y2014::control_loops::shooter::kMaxExtension;
using ::y2014::control_loops::shooter::kDt;
using ::y2014::control_loops::shooter::MakeShooterLoop;
-using ::aos::time::Time;
void ZeroedStateFeedbackLoop::CapU() {
const double old_voltage = voltage_;
@@ -115,11 +118,6 @@
my_shooter),
shooter_(MakeShooterLoop()),
state_(STATE_INITIALIZE),
- loading_problem_end_time_(0, 0),
- load_timeout_(0, 0),
- shooter_brake_set_time_(0, 0),
- unload_timeout_(0, 0),
- shot_end_time_(0, 0),
cycles_not_moved_(0),
shot_count_(0),
zeroed_(false),
@@ -404,16 +402,18 @@
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ = Time::Now() + kLoadProblemEndTimeout;
+ loading_problem_end_time_ =
+ monotonic_clock::now() + kLoadProblemEndTimeout;
}
}
- if (load_timeout_ < Time::Now()) {
+ if (load_timeout_ < monotonic_clock::now()) {
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
if (!(position->pusher_distal.current ||
position->pusher_proximal.current) ||
- (load_timeout_ + Time::InSeconds(0.5) < Time::Now())) {
+ (load_timeout_ + chrono::milliseconds(500) <
+ monotonic_clock::now())) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -428,7 +428,7 @@
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
- if (Time::Now() > loading_problem_end_time_) {
+ if (monotonic_clock::now() > loading_problem_end_time_) {
// Timeout by unloading.
Unload();
} else if (position && position->plunger && position->latch) {
@@ -468,7 +468,7 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = Time::Now() + kShooterBrakeSetTime;
+ shooter_brake_set_time_ = monotonic_clock::now() + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
@@ -482,7 +482,7 @@
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (Time::Now() > shooter_brake_set_time_) {
+ if (monotonic_clock::now() > shooter_brake_set_time_) {
status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
@@ -491,7 +491,7 @@
if (goal && goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- shot_end_time_ = Time::Now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
firing_starting_position_ = shooter_.absolute_position();
state_ = STATE_FIRE;
}
@@ -526,7 +526,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = Time::Now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
}
}
}
@@ -545,7 +545,7 @@
if (((::std::abs(firing_starting_position_ -
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
- Time::Now() > shot_end_time_) &&
+ monotonic_clock::now() > shot_end_time_) &&
::aos::robot_state->voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
@@ -585,10 +585,10 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = Time::Now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
}
- if (Time::Now() > unload_timeout_) {
+ if (monotonic_clock::now() > unload_timeout_) {
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
@@ -599,7 +599,7 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = Time::Now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index cccb953..fb991e3 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -19,8 +19,6 @@
class ShooterTest_RezeroWhileUnloading_Test;
};
-using ::aos::time::Time;
-
// Note: Everything in this file assumes that there is a 1 cycle delay between
// power being requested and it showing up at the motor. It assumes that
// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
@@ -113,13 +111,19 @@
bool capped_goal_;
};
-const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(2.0);
-const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
-const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
+static constexpr ::std::chrono::nanoseconds kUnloadTimeout =
+ ::std::chrono::seconds(10);
+static constexpr ::std::chrono::nanoseconds kLoadTimeout =
+ ::std::chrono::seconds(2);
+static constexpr ::std::chrono::nanoseconds kLoadProblemEndTimeout =
+ ::std::chrono::seconds(1);
+static constexpr ::std::chrono::nanoseconds kShooterBrakeSetTime =
+ ::std::chrono::milliseconds(50);
// Time to wait after releasing the latch piston before winching back again.
-const Time kShotEndTimeout = Time::InSeconds(0.2);
-const Time kPrepareFireEndTime = Time::InMS(40);
+static constexpr ::std::chrono::nanoseconds kShotEndTimeout =
+ ::std::chrono::milliseconds(200);
+static constexpr ::std::chrono::nanoseconds kPrepareFireEndTime =
+ ::std::chrono::milliseconds(40);
class ShooterMotor
: public aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue> {
@@ -171,12 +175,12 @@
// Enter state STATE_UNLOAD
void Unload() {
state_ = STATE_UNLOAD;
- unload_timeout_ = Time::Now() + kUnloadTimeout;
+ unload_timeout_ = ::aos::monotonic_clock::now() + kUnloadTimeout;
}
// Enter state STATE_LOAD
void Load() {
state_ = STATE_LOAD;
- load_timeout_ = Time::Now() + kLoadTimeout;
+ load_timeout_ = ::aos::monotonic_clock::now() + kLoadTimeout;
}
::y2014::control_loops::ShooterQueue::Position last_position_;
@@ -187,19 +191,24 @@
State state_;
// time to giving up on loading problem
- Time loading_problem_end_time_;
+ ::aos::monotonic_clock::time_point loading_problem_end_time_ =
+ ::aos::monotonic_clock::min_time;
// The end time when loading for it to timeout.
- Time load_timeout_;
+ ::aos::monotonic_clock::time_point load_timeout_ =
+ ::aos::monotonic_clock::min_time;
// wait for brake to set
- Time shooter_brake_set_time_;
+ ::aos::monotonic_clock::time_point shooter_brake_set_time_ =
+ ::aos::monotonic_clock::min_time;
// The timeout for unloading.
- Time unload_timeout_;
+ ::aos::monotonic_clock::time_point unload_timeout_ =
+ ::aos::monotonic_clock::min_time;
// time that shot must have completed
- Time shot_end_time_;
+ ::aos::monotonic_clock::time_point shot_end_time_ =
+ ::aos::monotonic_clock::min_time;
// track cycles that we are stuck to detect errors
int cycles_not_moved_;
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index d2d1375..fcfe33c 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -191,10 +191,6 @@
const double kThrottleGain = 1.0 / 2.5;
if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
data.IsPressed(kDriveControlLoopEnable2))) {
- // TODO(austin): Static sucks!
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
if (data.PosEdge(kDriveControlLoopEnable1) ||
data.PosEdge(kDriveControlLoopEnable2)) {
if (drivetrain_queue.position.FetchLatest() &&
@@ -504,6 +500,10 @@
double goal_angle_;
double separation_angle_, shot_separation_angle_;
double velocity_compensation_;
+ // Distance, angle, and filtered goal for closed loop driving.
+ double distance;
+ double angle;
+ double filtered_goal_distance;
double intake_power_;
bool was_running_;
bool moving_for_shot_ = false;
diff --git a/y2014_bot3/actions/drivetrain_action.cc b/y2014_bot3/actions/drivetrain_action.cc
index bef5bd6..d7660b3 100644
--- a/y2014_bot3/actions/drivetrain_action.cc
+++ b/y2014_bot3/actions/drivetrain_action.cc
@@ -16,6 +16,8 @@
namespace bot3 {
namespace actions {
+namespace chrono = ::std::chrono;
+
DrivetrainAction::DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s)
: ::frc971::actions::ActionBase
<::frc971::actions::DrivetrainActionQueueGroup>(s) {}
@@ -29,8 +31,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(10));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(10));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index bca3f24..1ebc4e9 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -37,7 +37,10 @@
// No shifter sensors, so we could put anything for the things below.
kThreeStateDriveShifter,
kThreeStateDriveShifter,
- false};
+ false,
+ 0.0,
+ 0.60,
+ 0.60};
return kDrivetrainConfig;
};
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index b0555a0..4c3577b 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -75,9 +75,9 @@
# Mass of the robot, in kg.
self.m = 37
# Radius of the robot, in meters (requires tuning by hand)
- self.rb = 0.601 / 2.0
+ self.rb = 0.45 / 2.0
# Radius of the wheels, in meters.
- self.r = 0.0508
+ self.r = 0.04445
# Resistance of the motor, divided by the number of motors.
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 86f78dc..22610ae 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -66,8 +66,6 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
- static double left_goal = 0.0;
- static double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
@@ -139,6 +137,9 @@
bool auto_running_ = false;
bool is_high_gear_;
+ // Turning goals.
+ double left_goal;
+ double right_goal;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 5ace60d..f998fff 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -18,6 +18,8 @@
namespace frc971 {
namespace actors {
+namespace chrono = ::std::chrono;
+
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
@@ -30,8 +32,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2015/actors/held_to_lift_actor.cc b/y2015/actors/held_to_lift_actor.cc
index 2336364..ddb2e29 100644
--- a/y2015/actors/held_to_lift_actor.cc
+++ b/y2015/actors/held_to_lift_actor.cc
@@ -17,6 +17,8 @@
constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
} // namespace
+namespace chrono = ::std::chrono;
+
HeldToLiftActor::HeldToLiftActor(HeldToLiftActionQueueGroup *queues)
: FridgeActorBase<HeldToLiftActionQueueGroup>(queues) {}
@@ -90,7 +92,8 @@
MakeLiftAction(params.lift_params);
lift_action->Start();
while (lift_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
diff --git a/y2015/actors/pickup_actor.cc b/y2015/actors/pickup_actor.cc
index 0863ba6..4ae988e 100644
--- a/y2015/actors/pickup_actor.cc
+++ b/y2015/actors/pickup_actor.cc
@@ -92,7 +92,10 @@
::aos::time::Time end_time =
::aos::time::Time::Now() + aos::time::Time::InSeconds(params.intake_time);
while ( ::aos::time::Time::Now() <= end_time) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(
+ ::std::chrono::duration_cast<::std::chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) return true;
}
diff --git a/y2015/actors/score_actor.cc b/y2015/actors/score_actor.cc
index c0a398f..50b5f35 100644
--- a/y2015/actors/score_actor.cc
+++ b/y2015/actors/score_actor.cc
@@ -12,6 +12,8 @@
using ::frc971::control_loops::fridge_queue;
+namespace chrono = ::std::chrono;
+
namespace frc971 {
namespace actors {
namespace {
@@ -67,7 +69,9 @@
}
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
@@ -91,7 +95,9 @@
bool started_lowering = false;
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
@@ -129,7 +135,9 @@
}
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
@@ -158,7 +166,9 @@
bool has_lifted = false;
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
diff --git a/y2015/actors/stack_and_hold_actor.cc b/y2015/actors/stack_and_hold_actor.cc
index ae1eeda..1e87ff3 100644
--- a/y2015/actors/stack_and_hold_actor.cc
+++ b/y2015/actors/stack_and_hold_actor.cc
@@ -19,6 +19,8 @@
constexpr ProfileParams kFastElevatorMove{1.2, 4.0};
} // namespace
+namespace chrono = ::std::chrono;
+
StackAndHoldActor::StackAndHoldActor(StackAndHoldActionQueueGroup *queues)
: FridgeActorBase<StackAndHoldActionQueueGroup>(queues) {}
@@ -76,7 +78,8 @@
MakeStackAction(stack_params);
stack_action->Start();
while (stack_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
diff --git a/y2015/actors/stack_and_lift_actor.cc b/y2015/actors/stack_and_lift_actor.cc
index 32f7417..bff3c85 100644
--- a/y2015/actors/stack_and_lift_actor.cc
+++ b/y2015/actors/stack_and_lift_actor.cc
@@ -14,6 +14,8 @@
namespace frc971 {
namespace actors {
+namespace chrono = ::std::chrono;
+
StackAndLiftActor::StackAndLiftActor(StackAndLiftActionQueueGroup *queues)
: FridgeActorBase<StackAndLiftActionQueueGroup>(queues) {}
@@ -36,7 +38,8 @@
MakeStackAction(stack_params);
stack_action->Start();
while (stack_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
@@ -75,7 +78,8 @@
::std::unique_ptr<LiftAction> lift_action = MakeLiftAction(lift_params);
lift_action->Start();
while (lift_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index e6ab9e7..ad8aaee 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -13,6 +13,7 @@
namespace control_loops {
using ::aos::time::Time;
+namespace chrono = ::std::chrono;
constexpr double kZeroingVoltage = 4.0;
@@ -169,8 +170,9 @@
SetClawOffset(claw_estimator_.offset());
} else if (!disable) {
claw_goal_velocity = claw_zeroing_velocity();
- claw_goal_ +=
- claw_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
+ claw_goal_ += claw_goal_velocity *
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
// Clear the current profile state if we are zeroing.
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index 083baf2..9f2241a 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -15,6 +15,8 @@
namespace frc971 {
namespace control_loops {
+namespace chrono = ::std::chrono;
+
namespace {
constexpr double kZeroingVoltage = 4.0;
constexpr double kElevatorZeroingVelocity = 0.10;
@@ -333,7 +335,8 @@
LOG(DEBUG, "Moving elevator to safe height.\n");
if (elevator_goal_ < values.fridge.arm_zeroing_height) {
elevator_goal_ += kElevatorSafeHeightVelocity *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
elevator_goal_velocity_ = kElevatorSafeHeightVelocity;
state_ = ZEROING_ELEVATOR;
} else {
@@ -344,7 +347,8 @@
} else if (!disable) {
elevator_goal_velocity_ = elevator_zeroing_velocity();
elevator_goal_ += elevator_goal_velocity_ *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
// Bypass motion profiles while we are zeroing.
@@ -380,8 +384,9 @@
LOG(DEBUG, "Zeroed the arm!\n");
} else if (!disable) {
arm_goal_velocity_ = arm_zeroing_velocity();
- arm_goal_ +=
- arm_goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ arm_goal_ += arm_goal_velocity_ *
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
// Bypass motion profiles while we are zeroing.
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 5a83626..c48e762 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -19,6 +19,8 @@
namespace frc971 {
namespace actors {
+namespace chrono = ::std::chrono;
+
using ::y2015_bot3::control_loops::drivetrain_queue;
using ::y2015_bot3::control_loops::kDrivetrainTurnWidth;
@@ -35,8 +37,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2015_bot3/control_loops/elevator/elevator.cc b/y2015_bot3/control_loops/elevator/elevator.cc
index 16ba064..d8b1368 100644
--- a/y2015_bot3/control_loops/elevator/elevator.cc
+++ b/y2015_bot3/control_loops/elevator/elevator.cc
@@ -1,6 +1,7 @@
#include "y2015_bot3/control_loops/elevator/elevator.h"
#include <cmath>
+#include <chrono>
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -10,6 +11,8 @@
namespace y2015_bot3 {
namespace control_loops {
+namespace chrono = ::std::chrono;
+
void SimpleCappedStateFeedbackLoop::CapU() {
mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
@@ -131,8 +134,10 @@
// Move the elevator either up or down based on where the zeroing hall
// effect is located.
- goal_velocity_ = zeroing_velocity_temp;
- goal_ += goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ goal_velocity_ = zeroing_velocity_temp;
+ goal_ += goal_velocity_ *
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
}
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 453d49e..cfbeb2b 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -5,60 +5,12 @@
filegroup(
name = 'binaries',
srcs = [
- ':drivetrain_action',
':superstructure_action',
':autonomous_action',
],
)
queue_library(
- name = 'drivetrain_action_queue',
- srcs = [
- 'drivetrain_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
-cc_library(
- name = 'drivetrain_action_lib',
- srcs = [
- 'drivetrain_actor.cc',
- ],
- hdrs = [
- 'drivetrain_actor.h',
- ],
- deps = [
- ':drivetrain_action_queue',
- '//aos/common:time',
- '//aos/common:math',
- '//aos/common/util:phased_loop',
- '//aos/common/logging',
- '//aos/common/actions:action_lib',
- '//aos/common/logging:queue_logging',
- '//aos/common/util:trapezoid_profile',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//frc971/control_loops:state_feedback_loop',
- '//third_party/eigen',
- '//y2016:constants',
- '//y2016/control_loops/drivetrain:polydrivetrain_plants',
- ],
-)
-
-cc_binary(
- name = 'drivetrain_action',
- srcs = [
- 'drivetrain_actor_main.cc',
- ],
- deps = [
- ':drivetrain_action_lib',
- ':drivetrain_action_queue',
- '//aos/linux_code:init',
- ],
-)
-
-queue_library(
name = 'superstructure_action_queue',
srcs = [
'superstructure_action.q',
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 7b3fc60..f298420 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -1092,6 +1092,7 @@
break;
case 5:
+ case 15:
TwoBallAuto();
return true;
break;
diff --git a/y2016/actors/drivetrain_action.q b/y2016/actors/drivetrain_action.q
deleted file mode 100644
index 9bbebb2..0000000
--- a/y2016/actors/drivetrain_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2016.actors;
-
-import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct DrivetrainActionParams {
- double left_initial_position;
- double right_initial_position;
- double y_offset;
- double theta_offset;
- double maximum_velocity;
- double maximum_acceleration;
- double maximum_turn_velocity;
- double maximum_turn_acceleration;
-};
-
-queue_group DrivetrainActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- DrivetrainActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
deleted file mode 100644
index 2c7c0e5..0000000
--- a/y2016/actors/drivetrain_actor.cc
+++ /dev/null
@@ -1,182 +0,0 @@
-#include "y2016/actors/drivetrain_actor.h"
-
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/util/trapezoid_profile.h"
-#include "aos/common/commonmath.h"
-#include "aos/common/time.h"
-
-#include "y2016/actors/drivetrain_actor.h"
-#include "y2016/constants.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-
-namespace y2016 {
-namespace actors {
-
-DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
- loop_(::y2016::control_loops::drivetrain::MakeDrivetrainLoop()) {
- loop_.set_controller_index(3);
-}
-
-bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
-
- const double yoffset = params.y_offset;
- const double turn_offset =
- params.theta_offset * control_loops::drivetrain::kRobotRadius;
- LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
- ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
-
- profile.set_maximum_acceleration(params.maximum_acceleration);
- profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(
- params.maximum_turn_acceleration *
- control_loops::drivetrain::kRobotRadius);
- turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- control_loops::drivetrain::kRobotRadius);
-
- while (true) {
- ::aos::time::PhasedLoopXMS(5, 2500);
-
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto &status = *::frc971::control_loops::drivetrain_queue.status;
- if (::std::abs(status.uncapped_left_voltage -
- status.uncapped_right_voltage) > 24) {
- LOG(DEBUG, "spinning in place\n");
- // They're more than 24V apart, so stop moving forwards and let it deal
- // with spinning first.
- profile.SetGoal(
- (status.estimated_left_position + status.estimated_right_position -
- params.left_initial_position - params.right_initial_position) /
- 2.0);
- } else {
- static const double divisor = K(0, 0) + K(0, 2);
- double dx_left, dx_right;
-
- if (status.uncapped_left_voltage > 12.0) {
- dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
-
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
-
- double dx;
-
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
-
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(ERROR, "no drivetrain status!\n");
- return false;
- }
-
- const auto drive_profile_goal_state =
- profile.Update(yoffset, goal_velocity);
- const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
- left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
- right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
-
- if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
- ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
- break;
- }
-
- if (ShouldCancel()) return true;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + params.left_initial_position,
- right_goal_state(0, 0) + params.right_initial_position);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(true)
- .left_goal(left_goal_state(0, 0) + params.left_initial_position)
- .right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
- .Send();
- }
- if (ShouldCancel()) return true;
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
-
- while (!::frc971::control_loops::drivetrain_queue.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- if (ShouldCancel()) return true;
- }
-
- while (true) {
- if (ShouldCancel()) return true;
- const double kPositionThreshold = 0.05;
-
- const double left_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- }
-
- LOG(INFO, "Done moving\n");
- return true;
-}
-
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016::actors::DrivetrainActionParams ¶ms) {
- return ::std::unique_ptr<DrivetrainAction>(
- new DrivetrainAction(&::y2016::actors::drivetrain_action, params));
-}
-
-} // namespace actors
-} // namespace y2016
diff --git a/y2016/actors/drivetrain_actor.h b/y2016/actors/drivetrain_actor.h
deleted file mode 100644
index 0ab3bf2..0000000
--- a/y2016/actors/drivetrain_actor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
-#define Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
-
-#include <memory>
-
-#include "aos/common/actions/actor.h"
-#include "aos/common/actions/actions.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2016/actors/drivetrain_action.q.h"
-
-namespace y2016 {
-namespace actors {
-
-class DrivetrainActor
- : public ::aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainActor(DrivetrainActionQueueGroup *s);
-
- bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
-
- private:
- StateFeedbackLoop<4, 2, 2> loop_;
-};
-
-typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
- DrivetrainAction;
-
-// Makes a new DrivetrainActor action.
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016::actors::DrivetrainActionParams ¶ms);
-
-} // namespace actors
-} // namespace y2016
-
-#endif
diff --git a/y2016/actors/drivetrain_actor_main.cc b/y2016/actors/drivetrain_actor_main.cc
deleted file mode 100644
index 0fe9e71..0000000
--- a/y2016/actors/drivetrain_actor_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "y2016/actors/drivetrain_action.q.h"
-#include "y2016/actors/drivetrain_actor.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init(-1);
-
- ::y2016::actors::DrivetrainActor drivetrain(
- &::y2016::actors::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 9aeb4c8..1674540 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -37,7 +37,9 @@
kThreeStateDriveShifter,
kThreeStateDriveShifter,
true,
- constants::GetValues().down_error};
+ constants::GetValues().down_error,
+ 0.25,
+ 1.0};
return kDrivetrainConfig;
};
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 629b980..c9ee712 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -1,5 +1,7 @@
#include "y2016/control_loops/shooter/shooter.h"
+#include <chrono>
+
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
@@ -12,6 +14,7 @@
namespace shooter {
using ::aos::time::Time;
+namespace chrono = ::std::chrono;
// TODO(austin): Pseudo current limit?
@@ -57,7 +60,8 @@
// Compute the distance moved over that time period.
status->avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
- (::aos::controls::kLoopFrequency.ToSeconds() *
+ (chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count() *
static_cast<double>(kHistoryLength - 1));
status->angular_velocity = loop_->X_hat(1, 0);
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 22a77fa..16ad077 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -124,7 +124,9 @@
AddPoint("big indicator", big_indicator);
AddPoint("superstructure state indicator", superstructure_state_indicator);
- AddPoint("auto mode indicator", auto_mode_indicator);
+ if(auto_mode_indicator != 15) {
+ AddPoint("auto mode indicator", auto_mode_indicator);
+ }
#endif
// Get ready for next iteration. /////
diff --git a/y2016/dashboard/www/index.html b/y2016/dashboard/www/index.html
index 3821b48..b7f943f 100644
--- a/y2016/dashboard/www/index.html
+++ b/y2016/dashboard/www/index.html
@@ -90,7 +90,7 @@
default:
$("#bigIndicator").css("background", "#000000");
$("#bigIndicator").css("color", "#444444");
- $("#bigIndicator").text("No ball, no target");
+ $("#bigIndicator").text("No ball");
break;
}
@@ -218,7 +218,8 @@
}
#autoModeIndicator {
- background: #FF0000;
+ background: #444444;
+ color: #AAAAAA;
}
</style>
</head>
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 45733ff..6a0edf0 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -120,8 +120,6 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
- static double left_goal = 0.0;
- static double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
@@ -238,18 +236,15 @@
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kBackFender)) {
- // Fender shot back
- shoulder_goal_ = M_PI / 2.0 - 0.2;
- wrist_goal_ = -0.55;
- shooter_velocity_ = 600.0;
+ // Backwards shot no IMU
+ shoulder_goal_ = M_PI / 2.0 - 0.4;
+ wrist_goal_ = -0.62 - 0.02;
+ shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kFrontFender)) {
- // Forwards shot, higher
+ // Forwards shot no IMU
shoulder_goal_ = M_PI / 2.0 + 0.1;
- wrist_goal_ = M_PI + 0.41 + 0.02 + 0.020;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
- }
+ wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kExpand) || data.IsPressed(kWinch)) {
@@ -458,6 +453,10 @@
double wrist_goal_;
double shooter_velocity_ = 0.0;
+ // Turning goals
+ double left_goal;
+ double right_goal;
+
bool was_running_ = false;
bool auto_running_ = false;
diff --git a/y2016_bot3/actors/BUILD b/y2016_bot3/actors/BUILD
index 6453f13..aa774a3 100644
--- a/y2016_bot3/actors/BUILD
+++ b/y2016_bot3/actors/BUILD
@@ -5,60 +5,11 @@
filegroup(
name = 'binaries',
srcs = [
- ':drivetrain_action',
':autonomous_action',
],
)
queue_library(
- name = 'drivetrain_action_queue',
- srcs = [
- 'drivetrain_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
-cc_library(
- name = 'drivetrain_action_lib',
- srcs = [
- 'drivetrain_actor.cc',
- ],
- hdrs = [
- 'drivetrain_actor.h',
- ],
- deps = [
- ':drivetrain_action_queue',
- '//aos/common:time',
- '//aos/common:math',
- '//aos/common/util:phased_loop',
- '//aos/common/logging',
- '//aos/common/actions:action_lib',
- '//aos/common/logging:queue_logging',
- '//aos/common/util:trapezoid_profile',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//frc971/control_loops:state_feedback_loop',
- '//third_party/eigen',
- '//y2016_bot3/control_loops/intake:intake_lib',
- '//y2016_bot3/control_loops/drivetrain:drivetrain_base',
- '//y2016_bot3/control_loops/drivetrain:polydrivetrain_plants',
- ],
-)
-
-cc_binary(
- name = 'drivetrain_action',
- srcs = [
- 'drivetrain_actor_main.cc',
- ],
- deps = [
- ':drivetrain_action_lib',
- ':drivetrain_action_queue',
- '//aos/linux_code:init',
- ],
-)
-
-queue_library(
name = 'autonomous_action_queue',
srcs = [
'autonomous_action.q',
diff --git a/y2016_bot3/actors/autonomous_actor.cc b/y2016_bot3/actors/autonomous_actor.cc
index d6263a7..27dae0e 100644
--- a/y2016_bot3/actors/autonomous_actor.cc
+++ b/y2016_bot3/actors/autonomous_actor.cc
@@ -78,8 +78,8 @@
return;
}
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
while (true) {
// Poll the running bit and see if we should cancel.
phased_loop.SleepUntilNext();
@@ -90,8 +90,8 @@
}
bool AutonomousActor::WaitForDriveNear(double distance, double angle) {
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
constexpr double kPositionTolerance = 0.02;
constexpr double kProfileTolerance = 0.001;
@@ -138,8 +138,8 @@
}
bool AutonomousActor::WaitForDriveProfileDone() {
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
constexpr double kProfileTolerance = 0.001;
while (true) {
@@ -186,8 +186,8 @@
}
bool AutonomousActor::WaitForDriveDone() {
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
while (true) {
if (ShouldCancel()) {
@@ -204,14 +204,12 @@
void AutonomousActor::MoveIntake(double intake_goal,
const ProfileParameters intake_params,
bool traverse_up, double roller_power) {
-
auto new_intake_goal =
::y2016_bot3::control_loops::intake_queue.goal.MakeMessage();
new_intake_goal->angle_intake = intake_goal;
- new_intake_goal->max_angular_velocity_intake =
- intake_params.max_velocity;
+ new_intake_goal->max_angular_velocity_intake = intake_params.max_velocity;
new_intake_goal->max_angular_acceleration_intake =
intake_params.max_acceleration;
@@ -219,7 +217,6 @@
new_intake_goal->voltage_top_rollers = roller_power;
new_intake_goal->voltage_bottom_rollers = roller_power;
- new_intake_goal->traverse_unlatched = true;
new_intake_goal->traverse_down = !traverse_up;
if (!new_intake_goal.Send()) {
@@ -241,13 +238,15 @@
if (::std::abs(control_loops::intake_queue.status->intake.goal_angle -
intake_goal_.intake) < kProfileError &&
- ::std::abs(control_loops::intake_queue.status->intake
- .goal_angular_velocity) < kProfileError) {
+ ::std::abs(
+ control_loops::intake_queue.status->intake.goal_angular_velocity) <
+ kProfileError) {
LOG(DEBUG, "Profile done.\n");
if (::std::abs(control_loops::intake_queue.status->intake.angle -
intake_goal_.intake) < kEpsilon &&
- ::std::abs(control_loops::intake_queue.status->intake
- .angular_velocity) < kEpsilon) {
+ ::std::abs(
+ control_loops::intake_queue.status->intake.angular_velocity) <
+ kEpsilon) {
LOG(INFO, "Near goal, done.\n");
return true;
}
@@ -281,8 +280,8 @@
}
void AutonomousActor::WaitForBallOrDriveDone() {
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
while (true) {
if (ShouldCancel()) {
return;
@@ -337,8 +336,8 @@
LOG(INFO, "Done %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
while (!ShouldCancel()) {
phased_loop.SleepUntilNext();
diff --git a/y2016_bot3/actors/drivetrain_action.q b/y2016_bot3/actors/drivetrain_action.q
deleted file mode 100644
index 7775cad..0000000
--- a/y2016_bot3/actors/drivetrain_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2016_bot3.actors;
-
-import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct DrivetrainActionParams {
- double left_initial_position;
- double right_initial_position;
- double y_offset;
- double theta_offset;
- double maximum_velocity;
- double maximum_acceleration;
- double maximum_turn_velocity;
- double maximum_turn_acceleration;
-};
-
-queue_group DrivetrainActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- DrivetrainActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/y2016_bot3/actors/drivetrain_actor.cc b/y2016_bot3/actors/drivetrain_actor.cc
deleted file mode 100644
index 686c50f..0000000
--- a/y2016_bot3/actors/drivetrain_actor.cc
+++ /dev/null
@@ -1,181 +0,0 @@
-#include "y2016_bot3/actors/drivetrain_actor.h"
-
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/util/trapezoid_profile.h"
-#include "aos/common/commonmath.h"
-#include "aos/common/time.h"
-
-#include "y2016_bot3/actors/drivetrain_actor.h"
-#include "y2016_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-
-namespace y2016_bot3 {
-namespace actors {
-
-DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
- loop_(::y2016_bot3::control_loops::drivetrain::MakeDrivetrainLoop()) {
- loop_.set_controller_index(3);
-}
-
-bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
-
- const double yoffset = params.y_offset;
- const double turn_offset =
- params.theta_offset * control_loops::drivetrain::kRobotRadius;
- LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
- ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
-
- profile.set_maximum_acceleration(params.maximum_acceleration);
- profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(
- params.maximum_turn_acceleration *
- control_loops::drivetrain::kRobotRadius);
- turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- control_loops::drivetrain::kRobotRadius);
-
- while (true) {
- ::aos::time::PhasedLoopXMS(5, 2500);
-
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto &status = *::frc971::control_loops::drivetrain_queue.status;
- if (::std::abs(status.uncapped_left_voltage -
- status.uncapped_right_voltage) > 24) {
- LOG(DEBUG, "spinning in place\n");
- // They're more than 24V apart, so stop moving forwards and let it deal
- // with spinning first.
- profile.SetGoal(
- (status.estimated_left_position + status.estimated_right_position -
- params.left_initial_position - params.right_initial_position) /
- 2.0);
- } else {
- static const double divisor = K(0, 0) + K(0, 2);
- double dx_left, dx_right;
-
- if (status.uncapped_left_voltage > 12.0) {
- dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
-
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
-
- double dx;
-
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
-
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(ERROR, "no drivetrain status!\n");
- return false;
- }
-
- const auto drive_profile_goal_state =
- profile.Update(yoffset, goal_velocity);
- const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
- left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
- right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
-
- if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
- ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
- break;
- }
-
- if (ShouldCancel()) return true;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + params.left_initial_position,
- right_goal_state(0, 0) + params.right_initial_position);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(true)
- .left_goal(left_goal_state(0, 0) + params.left_initial_position)
- .right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
- .Send();
- }
- if (ShouldCancel()) return true;
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
-
- while (!::frc971::control_loops::drivetrain_queue.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- if (ShouldCancel()) return true;
- }
-
- while (true) {
- if (ShouldCancel()) return true;
- const double kPositionThreshold = 0.05;
-
- const double left_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- }
-
- LOG(INFO, "Done moving\n");
- return true;
-}
-
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016_bot3::actors::DrivetrainActionParams ¶ms) {
- return ::std::unique_ptr<DrivetrainAction>(
- new DrivetrainAction(&::y2016_bot3::actors::drivetrain_action, params));
-}
-
-} // namespace actors
-} // namespace y2016_bot3
diff --git a/y2016_bot3/actors/drivetrain_actor.h b/y2016_bot3/actors/drivetrain_actor.h
deleted file mode 100644
index ebf3ed5..0000000
--- a/y2016_bot3/actors/drivetrain_actor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef Y2016_BOT3_ACTORS_DRIVETRAIN_ACTOR_H_
-#define Y2016_BOT3_ACTORS_DRIVETRAIN_ACTOR_H_
-
-#include <memory>
-
-#include "aos/common/actions/actor.h"
-#include "aos/common/actions/actions.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2016_bot3/actors/drivetrain_action.q.h"
-
-namespace y2016_bot3 {
-namespace actors {
-
-class DrivetrainActor
- : public ::aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainActor(DrivetrainActionQueueGroup *s);
-
- bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
-
- private:
- StateFeedbackLoop<4, 2, 2> loop_;
-};
-
-typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
- DrivetrainAction;
-
-// Makes a new DrivetrainActor action.
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016_bot3::actors::DrivetrainActionParams ¶ms);
-
-} // namespace actors
-} // namespace y2016_bot3
-
-#endif
diff --git a/y2016_bot3/actors/drivetrain_actor_main.cc b/y2016_bot3/actors/drivetrain_actor_main.cc
deleted file mode 100644
index 41a4d28..0000000
--- a/y2016_bot3/actors/drivetrain_actor_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "y2016_bot3/actors/drivetrain_action.q.h"
-#include "y2016_bot3/actors/drivetrain_actor.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init(-1);
-
- ::y2016_bot3::actors::DrivetrainActor drivetrain(
- &::y2016_bot3::actors::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
index cefb1cf..5997342 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -36,7 +36,9 @@
kThreeStateDriveShifter,
kThreeStateDriveShifter,
true,
- 0.0};
+ 0.0,
+ 0.4,
+ 1.0};
return kDrivetrainConfig;
};
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.h b/y2016_bot3/control_loops/drivetrain/drivetrain_base.h
index d6a041e..280d1c5 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -8,7 +8,6 @@
static constexpr double drivetrain_max_speed = 5.0;
// The ratio from the encoder shaft to the drivetrain wheels.
-// TODO(constants): Update these.
static constexpr double kDrivetrainEncoderRatio = 1.0;
} // namespace constants
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index 5320cd5..b3d86af 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -16,7 +16,7 @@
// The maximum voltage the intake roller will be allowed to use.
constexpr float kMaxIntakeTopVoltage = 12.0;
constexpr float kMaxIntakeBottomVoltage = 12.0;
-
+constexpr float kMaxIntakeRollersVoltage = 12.0;
}
// namespace
@@ -33,28 +33,6 @@
.lpNorm<Eigen::Infinity>() < tolerance);
}
-double Intake::MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance) {
- return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance);
-}
-
-double Intake::MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance) {
- // There are 3 interesting places to move to.
- const double small_negative_move = current_angle - move_distance;
- const double small_positive_move = current_angle + move_distance;
- // And the reference angle.
-
- // Move the the highest one that is below reference_angle.
- if (small_negative_move > reference_angle) {
- return reference_angle;
- } else if (small_positive_move > reference_angle) {
- return small_negative_move;
- } else {
- return small_positive_move;
- }
-}
-
void Intake::RunIteration(const control_loops::IntakeQueue::Goal *unsafe_goal,
const control_loops::IntakeQueue::Position *position,
control_loops::IntakeQueue::Output *output,
@@ -162,10 +140,10 @@
requested_intake = unsafe_goal->angle_intake;
}
- //Push the request out to the hardware.
+ // Push the request out to the hardware.
limit_checker_.UpdateGoal(requested_intake);
- // ESTOP if we hit the hard limits.
+ // ESTOP if we hit the hard limits.
if (intake_.CheckHardLimits() && output) {
state_ = ESTOP;
}
@@ -198,6 +176,7 @@
output->voltage_top_rollers = 0.0;
output->voltage_bottom_rollers = 0.0;
+ output->voltage_intake_rollers = 0.0;
if (unsafe_goal) {
// Ball detector lights.
@@ -212,6 +191,10 @@
output->voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
+ output->voltage_intake_rollers =
+ ::std::max(-kMaxIntakeRollersVoltage,
+ ::std::min(unsafe_goal->voltage_intake_rollers,
+ kMaxIntakeRollersVoltage));
output->voltage_bottom_rollers =
::std::max(-kMaxIntakeBottomVoltage,
::std::min(unsafe_goal->voltage_bottom_rollers,
@@ -222,7 +205,6 @@
}
// Traverse.
- output->traverse_unlatched = unsafe_goal->traverse_unlatched;
output->traverse_down = unsafe_goal->traverse_down;
}
}
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index fed17ab..cfc204f 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -29,18 +29,19 @@
// the use.
// TODO(constants): Update these.
static constexpr ::frc971::constants::Range kIntakeRange{// Lower hard stop
- -0.5,
+ -0.4,
// Upper hard stop
2.90,
// Lower soft stop
- -0.300,
+ -0.28,
// Uppper soft stop
- 2.725};
+ 2.77};
struct IntakeZero {
- double pot_offset = 0.0;
- ::frc971::constants::ZeroingConstants zeroing{
- kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.0, 0.3};
+ double pot_offset = 5.462409 + 0.333162;
+ ::frc971::constants::ZeroingConstants zeroing{kZeroingSampleSize,
+ kIntakeEncoderIndexDifference,
+ +(-0.291240 + 0.148604), 0.3};
};
} // namespace constants
namespace control_loops {
@@ -54,12 +55,14 @@
class IntakeTest_DisabledWhileZeroingLow_Test;
}
+// TODO(Adam): Implement this class and delete it from here.
class LimitChecker {
- public:
- LimitChecker(IntakeArm *intake) : intake_(intake) {}
- void UpdateGoal(double intake_angle_goal);
- private:
- IntakeArm *intake_;
+ public:
+ LimitChecker(IntakeArm *intake) : intake_(intake) {}
+ void UpdateGoal(double intake_angle_goal);
+
+ private:
+ IntakeArm *intake_;
};
class Intake : public ::aos::controls::ControlLoop<control_loops::IntakeQueue> {
@@ -109,15 +112,6 @@
State state() const { return state_; }
- // Returns the value to move the joint to such that it will stay below
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance);
- // Returns the value to move the joint to such that it will stay above
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance);
-
protected:
void RunIteration(const control_loops::IntakeQueue::Goal *unsafe_goal,
const control_loops::IntakeQueue::Position *position,
@@ -145,7 +139,6 @@
DISALLOW_COPY_AND_ASSIGN(Intake);
};
-
} // namespace intake
} // namespace control_loops
} // namespace y2016_bot3
diff --git a/y2016_bot3/control_loops/intake/intake.q b/y2016_bot3/control_loops/intake/intake.q
index da3edbb..55d102d 100644
--- a/y2016_bot3/control_loops/intake/intake.q
+++ b/y2016_bot3/control_loops/intake/intake.q
@@ -57,12 +57,10 @@
// Voltage to send to the rollers. Positive is sucking in.
float voltage_top_rollers;
float voltage_bottom_rollers;
+ float voltage_intake_rollers;
bool force_intake;
- // If true, release the latch which holds the traverse mechanism in the
- // middle.
- bool traverse_unlatched;
// If true, fire the traverse mechanism down.
bool traverse_down;
};
@@ -77,12 +75,8 @@
// The internal state of the state machine.
int32_t state;
-
// Estimated angle and angular velocitie of the intake.
JointState intake;
-
- // Is the intake collided?
- bool is_collided;
};
message Position {
@@ -96,9 +90,8 @@
float voltage_top_rollers;
float voltage_bottom_rollers;
+ float voltage_intake_rollers;
- // If true, release the latch to hold the traverse mechanism in the middle.
- bool traverse_unlatched;
// If true, fire the traverse mechanism down.
bool traverse_down;
};
diff --git a/y2016_bot3/control_loops/intake/intake_lib_test.cc b/y2016_bot3/control_loops/intake/intake_lib_test.cc
index a82da71..fa82802 100644
--- a/y2016_bot3/control_loops/intake/intake_lib_test.cc
+++ b/y2016_bot3/control_loops/intake/intake_lib_test.cc
@@ -443,14 +443,6 @@
EXPECT_EQ(Intake::RUNNING, intake_.state());
}
-// Tests that MoveButKeepBelow returns sane values.
-TEST_F(IntakeTest, MoveButKeepBelowTest) {
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 10.0, 1.0));
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 2.0, 1.0));
- EXPECT_EQ(0.0, Intake::MoveButKeepBelow(1.0, 1.0, 1.0));
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 0.0, 1.0));
-}
-
// Tests that the integrators works.
TEST_F(IntakeTest, IntegratorTest) {
intake_plant_.InitializeIntakePosition(
diff --git a/y2016_bot3/control_loops/python/drivetrain.py b/y2016_bot3/control_loops/python/drivetrain.py
index 5a67416..5de7dbc 100755
--- a/y2016_bot3/control_loops/python/drivetrain.py
+++ b/y2016_bot3/control_loops/python/drivetrain.py
@@ -71,9 +71,9 @@
# 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
+ self.J = 8.0
# Mass of the robot, in kg.
- self.m = 68
+ self.m = 45
# Radius of the robot, in meters (requires tuning by hand)
self.rb = 0.601 / 2.0
# Radius of the wheels, in meters.
diff --git a/y2016_bot3/control_loops/python/polydrivetrain.py b/y2016_bot3/control_loops/python/polydrivetrain.py
index ac85bb6..07f9ff9 100755
--- a/y2016_bot3/control_loops/python/polydrivetrain.py
+++ b/y2016_bot3/control_loops/python/polydrivetrain.py
@@ -128,7 +128,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.67, 0.67])
+ self.PlaceControllerPoles([0.85, 0.85])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
diff --git a/y2016_bot3/joystick_reader.cc b/y2016_bot3/joystick_reader.cc
index a02142d..02e6817 100644
--- a/y2016_bot3/joystick_reader.cc
+++ b/y2016_bot3/joystick_reader.cc
@@ -40,11 +40,11 @@
const ButtonLocation kTurn2(1, 11);
// Buttons on the lexan driver station to get things running on bring-up day.
-const ButtonLocation kIntakeDown(3, 11);
-const ButtonLocation kIntakeIn(3, 12);
+const ButtonLocation kIntakeDown(3, 5);
+const ButtonLocation kIntakeIn(3, 4);
const ButtonLocation kFire(3, 3);
-const ButtonLocation kIntakeOut(3, 9);
-const ButtonLocation kChevalDeFrise(3, 8);
+const ButtonLocation kIntakeOut(3, 3);
+const ButtonLocation kChevalDeFrise(3, 11);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -143,11 +143,8 @@
saw_ball_when_started_intaking_ = ball_detected;
}
- if (data.IsPressed(kIntakeIn)) {
- is_intaking_ = (!ball_detected || saw_ball_when_started_intaking_);
- } else {
- is_intaking_ = false;
- }
+ is_intaking_ = data.IsPressed(kIntakeIn) &&
+ (!ball_detected || saw_ball_when_started_intaking_);
is_outtaking_ = data.IsPressed(kIntakeOut);
@@ -168,10 +165,8 @@
}
if (data.IsPressed(kChevalDeFrise)) {
- traverse_unlatched_ = false;
traverse_down_ = true;
} else {
- traverse_unlatched_ = true;
traverse_down_ = false;
}
@@ -182,23 +177,26 @@
new_intake_goal->max_angular_velocity_intake = 7.0;
new_intake_goal->max_angular_acceleration_intake = 40.0;
- // Granny mode
- /*
+#define GrannyMode false
+#if GrannyMode
new_intake_goal->max_angular_velocity_intake = 0.2;
new_intake_goal->max_angular_acceleration_intake = 1.0;
- */
+#endif
+
if (is_intaking_) {
- new_intake_goal->voltage_top_rollers = 12.0;
+ new_intake_goal->voltage_intake_rollers = -12.0;
+ new_intake_goal->voltage_top_rollers = -12.0;
new_intake_goal->voltage_bottom_rollers = 12.0;
} else if (is_outtaking_) {
- new_intake_goal->voltage_top_rollers = -12.0;
+ new_intake_goal->voltage_intake_rollers = 12.0;
+ new_intake_goal->voltage_top_rollers = 12.0;
new_intake_goal->voltage_bottom_rollers = -7.0;
} else {
+ new_intake_goal->voltage_intake_rollers = 0.0;
new_intake_goal->voltage_top_rollers = 0.0;
new_intake_goal->voltage_bottom_rollers = 0.0;
}
- new_intake_goal->traverse_unlatched = traverse_unlatched_;
new_intake_goal->traverse_down = traverse_down_;
new_intake_goal->force_intake = true;
@@ -236,7 +234,6 @@
bool was_running_ = false;
bool auto_running_ = false;
- bool traverse_unlatched_ = false;
bool traverse_down_ = false;
// If we're waiting for the subsystems to zero.
diff --git a/y2016_bot3/wpilib/wpilib_interface.cc b/y2016_bot3/wpilib/wpilib_interface.cc
index 59dce44..3d7d8ce 100644
--- a/y2016_bot3/wpilib/wpilib_interface.cc
+++ b/y2016_bot3/wpilib/wpilib_interface.cc
@@ -84,13 +84,13 @@
// with proper units.
double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
::y2016_bot3::constants::kDrivetrainEncoderRatio *
control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
}
double drivetrain_velocity_translate(double in) {
- return (1.0 / in) / 256.0 /*cpr*/ *
+ return (1.0 / in) / 512.0 /*cpr*/ *
::y2016_bot3::constants::kDrivetrainEncoderRatio *
control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
}
@@ -102,7 +102,7 @@
double intake_pot_translate(double voltage) {
return voltage * ::y2016_bot3::constants::kIntakePotRatio *
- (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
@@ -170,8 +170,8 @@
dma_synchronizer_->Start();
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(4));
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(4));
::aos::SetCurrentThreadRealtimePriority(40);
while (run_) {
@@ -194,7 +194,7 @@
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
- drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
-drivetrain_translate(drivetrain_left_encoder_->GetRaw());
drivetrain_message->left_speed =
@@ -210,7 +210,7 @@
{
auto intake_message = intake_queue.position.MakeMessage();
CopyPotAndIndexPosition(intake_encoder_, &intake_message->intake,
- intake_translate, intake_pot_translate, false,
+ intake_translate, intake_pot_translate, true,
intake_pot_offset);
intake_message.Send();
@@ -286,18 +286,13 @@
traverse_ = ::std::move(s);
}
- void set_traverse_latch(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- traverse_latch_ = ::std::move(s);
- }
-
void operator()() {
compressor_->Start();
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(20),
- ::aos::time::Time::InMS(1));
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
while (run_) {
{
@@ -311,9 +306,7 @@
intake_.FetchLatest();
if (intake_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *intake_);
-
traverse_->Set(intake_->traverse_down);
- traverse_latch_->Set(intake_->traverse_unlatched);
}
}
@@ -333,8 +326,7 @@
private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> traverse_,
- traverse_latch_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> traverse_;
::std::unique_ptr<Compressor> compressor_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
@@ -345,12 +337,16 @@
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
- void set_drivetrain_left_talon(::std::unique_ptr<Talon> t) {
- drivetrain_left_talon_ = ::std::move(t);
+ void set_drivetrain_left_talon(::std::unique_ptr<Talon> t0,
+ ::std::unique_ptr<Talon> t1) {
+ drivetrain_left_talon_0_ = ::std::move(t0);
+ drivetrain_left_talon_1_ = ::std::move(t1);
}
- void set_drivetrain_right_talon(::std::unique_ptr<Talon> t) {
- drivetrain_right_talon_ = ::std::move(t);
+ void set_drivetrain_right_talon(::std::unique_ptr<Talon> t0,
+ ::std::unique_ptr<Talon> t1) {
+ drivetrain_right_talon_0_ = ::std::move(t0);
+ drivetrain_right_talon_1_ = ::std::move(t1);
}
private:
@@ -361,17 +357,22 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_talon_->Set(queue->left_voltage / 12.0);
- drivetrain_right_talon_->Set(-queue->right_voltage / 12.0);
+ drivetrain_left_talon_0_->Set(queue->left_voltage / 12.0);
+ drivetrain_left_talon_1_->Set(queue->left_voltage / 12.0);
+ drivetrain_right_talon_0_->Set(-queue->right_voltage / 12.0);
+ drivetrain_right_talon_1_->Set(-queue->right_voltage / 12.0);
}
virtual void Stop() override {
LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_talon_->Disable();
- drivetrain_right_talon_->Disable();
+ drivetrain_left_talon_0_->Disable();
+ drivetrain_right_talon_0_->Disable();
+ drivetrain_left_talon_1_->Disable();
+ drivetrain_right_talon_1_->Disable();
}
- ::std::unique_ptr<Talon> drivetrain_left_talon_, drivetrain_right_talon_;
+ ::std::unique_ptr<Talon> drivetrain_left_talon_0_, drivetrain_right_talon_0_,
+ drivetrain_right_talon_1_, drivetrain_left_talon_1_;
};
class IntakeWriter : public ::frc971::wpilib::LoopOutputHandler {
@@ -380,6 +381,10 @@
intake_talon_ = ::std::move(t);
}
+ void set_intake_rollers_talon(::std::unique_ptr<Talon> t) {
+ intake_rollers_talon_ = ::std::move(t);
+ }
+
void set_top_rollers_talon(::std::unique_ptr<Talon> t) {
top_rollers_talon_ = ::std::move(t);
}
@@ -400,6 +405,7 @@
kMaxBringupPower) /
12.0);
top_rollers_talon_->Set(-queue->voltage_top_rollers / 12.0);
+ intake_rollers_talon_->Set(-queue->voltage_intake_rollers / 12.0);
bottom_rollers_talon_->Set(-queue->voltage_bottom_rollers / 12.0);
}
@@ -409,7 +415,7 @@
}
::std::unique_ptr<Talon> intake_talon_, top_rollers_talon_,
- bottom_rollers_talon_;
+ bottom_rollers_talon_, intake_rollers_talon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -430,14 +436,14 @@
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
SensorReader reader;
- reader.set_drivetrain_left_encoder(make_encoder(5));
- reader.set_drivetrain_right_encoder(make_encoder(6));
+ reader.set_drivetrain_left_encoder(make_encoder(0));
+ reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_intake_encoder(make_encoder(0));
+ reader.set_intake_encoder(make_encoder(2));
reader.set_intake_index(make_unique<DigitalInput>(0));
- reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
+ reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
- reader.set_ball_detector(make_unique<AnalogInput>(7));
+ reader.set_ball_detector(make_unique<AnalogInput>(5));
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
@@ -446,23 +452,27 @@
::std::thread gyro_thread(::std::ref(gyro_sender));
DrivetrainWriter drivetrain_writer;
+ // 2 and 3 are right. 0 and 1 are left
drivetrain_writer.set_drivetrain_left_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
+ ::std::unique_ptr<Talon>(new Talon(0)),
+ ::std::unique_ptr<Talon>(new Talon(1)));
drivetrain_writer.set_drivetrain_right_talon(
- ::std::unique_ptr<Talon>(new Talon(4)));
+ ::std::unique_ptr<Talon>(new Talon(2)),
+ ::std::unique_ptr<Talon>(new Talon(3)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
IntakeWriter intake_writer;
- intake_writer.set_intake_talon(::std::unique_ptr<Talon>(new Talon(3)));
- intake_writer.set_top_rollers_talon(::std::unique_ptr<Talon>(new Talon(1)));
+ intake_writer.set_intake_talon(::std::unique_ptr<Talon>(new Talon(7)));
+ intake_writer.set_top_rollers_talon(::std::unique_ptr<Talon>(new Talon(6)));
+ intake_writer.set_intake_rollers_talon(
+ ::std::unique_ptr<Talon>(new Talon(5)));
intake_writer.set_bottom_rollers_talon(
- ::std::unique_ptr<Talon>(new Talon(0)));
+ ::std::unique_ptr<Talon>(new Talon(4)));
::std::thread intake_writer_thread(::std::ref(intake_writer));
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
solenoid_writer.set_compressor(make_unique<Compressor>());