Merge "Splines gui"
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index d597e19..8c44823 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -91,6 +91,8 @@
deps = [
":control_loop_queues",
"//aos:queues",
+ "//aos/events:event-loop",
+ "//aos/events:shm-event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/robot_state",
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 1834728..49bad45 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -3,7 +3,6 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
namespace aos {
namespace controls {
@@ -17,83 +16,100 @@
template <class T>
void ControlLoop<T>::ZeroOutputs() {
- aos::ScopedMessagePtr<OutputType> output =
- control_loop_->output.MakeMessage();
+ typename ::aos::Sender<OutputType>::Message output =
+ output_sender_.MakeMessage();
Zero(output.get());
output.Send();
}
template <class T>
void ControlLoop<T>::Iterate() {
+ if (!has_iterate_fetcher_) {
+ iterate_position_fetcher_ =
+ event_loop_->MakeFetcher<PositionType>(name_ + ".position");
+ has_iterate_fetcher_ = true;
+ }
+ const bool did_fetch = iterate_position_fetcher_.Fetch();
+ if (!did_fetch) {
+ LOG(FATAL, "Failed to fetch from position queue\n");
+ }
+ IteratePosition(*iterate_position_fetcher_);
+}
+
+template <class T>
+void ControlLoop<T>::IteratePosition(const PositionType &position) {
+ // Since Exit() isn't async safe, we want to call Exit from the periodic
+ // handler.
+ if (!run_) {
+ event_loop_->Exit();
+ }
no_goal_.Print();
no_sensor_state_.Print();
motors_off_log_.Print();
- control_loop_->position.FetchAnother();
- const PositionType *const position = control_loop_->position.get();
- LOG_STRUCT(DEBUG, "position", *position);
+ LOG_STRUCT(DEBUG, "position", position);
// Fetch the latest control loop goal. If there is no new
// goal, we will just reuse the old one.
- control_loop_->goal.FetchLatest();
- const GoalType *goal = control_loop_->goal.get();
+ goal_fetcher_.Fetch();
+ const GoalType *goal = goal_fetcher_.get();
if (goal) {
LOG_STRUCT(DEBUG, "goal", *goal);
} else {
LOG_INTERVAL(no_goal_);
}
- const bool new_robot_state = ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get()) {
+ const bool new_robot_state = robot_state_fetcher_.Fetch();
+ if (!robot_state_fetcher_.get()) {
LOG_INTERVAL(no_sensor_state_);
return;
}
- if (sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
- ::aos::robot_state->reader_pid, sensor_reader_pid_);
+ robot_state_fetcher_->reader_pid, sensor_reader_pid_);
reset_ = true;
- sensor_reader_pid_ = ::aos::robot_state->reader_pid;
+ sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
}
- bool outputs_enabled = ::aos::robot_state->outputs_enabled;
+ bool outputs_enabled = robot_state_fetcher_->outputs_enabled;
// Check to see if we got a driver station packet recently.
if (new_robot_state) {
- if (::aos::robot_state->outputs_enabled) {
+ if (robot_state_fetcher_->outputs_enabled) {
// If the driver's station reports being disabled, we're probably not
// actually going to send motor values regardless of what the FPGA
// reports.
- last_pwm_sent_ = ::aos::robot_state->sent_time;
+ last_pwm_sent_ = robot_state_fetcher_->sent_time;
}
}
const ::aos::monotonic_clock::time_point now = ::aos::monotonic_clock::now();
const bool motors_off = now >= kPwmDisableTime + last_pwm_sent_;
- ::aos::joystick_state.FetchLatest();
+ joystick_state_fetcher_.Fetch();
if (motors_off) {
- if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
LOG_INTERVAL(motors_off_log_);
}
outputs_enabled = false;
}
- aos::ScopedMessagePtr<StatusType> status =
- control_loop_->status.MakeMessage();
+ typename ::aos::Sender<StatusType>::Message status =
+ status_sender_.MakeMessage();
if (status.get() == nullptr) {
return;
}
if (outputs_enabled) {
- aos::ScopedMessagePtr<OutputType> output =
- control_loop_->output.MakeMessage();
- RunIteration(goal, position, output.get(), status.get());
+ typename ::aos::Sender<OutputType>::Message output =
+ output_sender_.MakeMessage();
+ RunIteration(goal, &position, output.get(), status.get());
output->SetTimeToNow();
LOG_STRUCT(DEBUG, "output", *output);
output.Send();
} else {
// The outputs are disabled, so pass nullptr in for the output.
- RunIteration(goal, position, nullptr, status.get());
+ RunIteration(goal, &position, nullptr, status.get());
ZeroOutputs();
}
@@ -113,9 +129,12 @@
PCHECK(sigaction(SIGQUIT, &action, nullptr));
PCHECK(sigaction(SIGINT, &action, nullptr));
- while (run_) {
- Iterate();
- }
+ event_loop_->MakeWatcher(name_ + ".position",
+ [this](const PositionType &position) {
+ this->IteratePosition(position);
+ });
+
+ event_loop_->Run();
LOG(INFO, "Shutting down\n");
}
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 335a1bb..58f92a2 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -4,7 +4,10 @@
#include <string.h>
#include <atomic>
+#include "aos/events/event-loop.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/queue.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/type_traits/type_traits.h"
#include "aos/util/log_interval.h"
@@ -49,7 +52,22 @@
decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
OutputType;
- ControlLoop(T *control_loop) : control_loop_(control_loop) {}
+ ControlLoop(EventLoop *event_loop, const ::std::string &name)
+ : event_loop_(event_loop), name_(name) {
+ output_sender_ = event_loop_->MakeSender<OutputType>(name_ + ".output");
+ status_sender_ = event_loop_->MakeSender<StatusType>(name_ + ".status");
+ goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_ + ".goal");
+ robot_state_fetcher_ =
+ event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
+ joystick_state_fetcher_ =
+ event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
+ }
+
+ const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
+ bool has_joystick_state() const { return joystick_state_fetcher_.get(); }
+ const ::aos::JoystickState &joystick_state() const {
+ return *joystick_state_fetcher_;
+ }
// Returns true if all the counters etc in the sensor data have been reset.
// This will return true only a single time per reset.
@@ -73,12 +91,17 @@
virtual void Zero(OutputType *output) { output->Zero(); }
// Runs the loop forever.
+ // TODO(austin): This should move to the event loop once it gets hoisted out.
void Run() override;
// Runs one cycle of the loop.
+ // TODO(austin): This should go away when all the tests use event loops
+ // directly.
void Iterate() override;
protected:
+ void IteratePosition(const PositionType &position);
+
static void Quit(int /*signum*/) {
run_ = false;
}
@@ -108,7 +131,20 @@
::std::chrono::milliseconds(100);
// Pointer to the queue group
- T *control_loop_;
+ ::std::unique_ptr<ShmEventLoop> shm_event_loop_;
+ EventLoop *event_loop_;
+ ::std::string name_;
+
+ ::aos::Sender<OutputType> output_sender_;
+ ::aos::Sender<StatusType> status_sender_;
+ ::aos::Fetcher<GoalType> goal_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+
+ // Fetcher only to be used for the Iterate method. If Iterate is called, Run
+ // can't be called.
+ bool has_iterate_fetcher_ = false;
+ ::aos::Fetcher<PositionType> iterate_position_fetcher_;
bool reset_ = false;
int32_t sensor_reader_pid_ = 0;
diff --git a/aos/events/BUILD b/aos/events/BUILD
index ad888f6..a5e83a9 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -1,74 +1,77 @@
-
cc_library(
- name = "raw-event-loop",
- hdrs = ["raw-event-loop.h"],
- deps = [
- "//aos/time:time",
- "//aos:queues",
- ],
+ name = "raw-event-loop",
+ hdrs = ["raw-event-loop.h"],
+ deps = [
+ "//aos:queues",
+ "//aos/time",
+ ],
)
cc_library(
- name = "event-loop",
- hdrs = ["event-loop.h", "raw-event-loop.h"],
- srcs = ["event-loop-tmpl.h"],
- deps = [
- ":raw-event-loop",
- "//aos/time:time",
- "//aos:queues",
- ],
+ name = "event-loop",
+ srcs = ["event-loop-tmpl.h"],
+ hdrs = [
+ "event-loop.h",
+ "raw-event-loop.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":raw-event-loop",
+ "//aos:queues",
+ "//aos/time",
+ ],
)
cc_library(
- name = "shm-event-loop",
- hdrs = ["shm-event-loop.h"],
- srcs = ["shm-event-loop.cc"],
- deps = [
- ":event-loop",
- "//aos:queues",
- "//aos/logging:logging",
- ],
+ name = "shm-event-loop",
+ srcs = ["shm-event-loop.cc"],
+ hdrs = ["shm-event-loop.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":event-loop",
+ "//aos:queues",
+ "//aos/logging",
+ ],
)
cc_test(
- name = "shm-event-loop_test",
- srcs = ["shm-event-loop_test.cc"],
- deps = [
- ":event-loop_param_test",
- ":shm-event-loop",
- "//aos/testing:test_shm",
- ],
+ name = "shm-event-loop_test",
+ srcs = ["shm-event-loop_test.cc"],
+ deps = [
+ ":event-loop_param_test",
+ ":shm-event-loop",
+ "//aos/testing:test_shm",
+ ],
)
cc_library(
- name = "event-loop_param_test",
- srcs = ["event-loop_param_test.cc"],
- hdrs = ["event-loop_param_test.h"],
- deps = [
- "event-loop",
- "//aos/testing:googletest",
- ],
- testonly = True,
+ name = "event-loop_param_test",
+ testonly = True,
+ srcs = ["event-loop_param_test.cc"],
+ hdrs = ["event-loop_param_test.h"],
+ deps = [
+ "event-loop",
+ "//aos/testing:googletest",
+ ],
)
cc_test(
- name = "simulated-event-loop_test",
- srcs = ["simulated-event-loop_test.cc"],
- deps = [
- "//aos/testing:googletest",
- ":event-loop_param_test",
- ":simulated-event-loop",
- ],
- testonly = True,
+ name = "simulated-event-loop_test",
+ testonly = True,
+ srcs = ["simulated-event-loop_test.cc"],
+ deps = [
+ ":event-loop_param_test",
+ ":simulated-event-loop",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = "simulated-event-loop",
- hdrs = ["simulated-event-loop.h"],
- srcs = ["simulated-event-loop.cc"],
- deps = [
- ":event-loop",
- "//aos:queues",
- ],
+ name = "simulated-event-loop",
+ srcs = ["simulated-event-loop.cc"],
+ hdrs = ["simulated-event-loop.h"],
+ deps = [
+ ":event-loop",
+ "//aos:queues",
+ ],
)
-
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index 57ff49b..a187981 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -73,6 +73,9 @@
// Constructs an above message.
Message MakeMessage();
+ // Returns the name of the underlying queue.
+ const char *name() const { return sender_->name(); }
+
private:
friend class EventLoop;
Sender(std::unique_ptr<RawSender> sender) : sender_(std::move(sender)) {}
@@ -107,6 +110,7 @@
//
// This will watch messages sent to path.
// Note that T needs to match both send and recv side.
+ // TODO(parker): Need to support ::std::bind. For now, use lambdas.
template <typename Watch>
void MakeWatcher(const std::string &path, Watch &&w);
diff --git a/aos/events/raw-event-loop.h b/aos/events/raw-event-loop.h
index ffc894b..a903e93 100644
--- a/aos/events/raw-event-loop.h
+++ b/aos/events/raw-event-loop.h
@@ -60,6 +60,8 @@
Free(reinterpret_cast<SendContext *>(t));
}
+ virtual const char *name() const = 0;
+
protected:
RawSender(const RawSender &) = delete;
RawSender &operator=(const RawSender &) = delete;
diff --git a/aos/events/shm-event-loop.cc b/aos/events/shm-event-loop.cc
index 6231df2..945ebc0 100644
--- a/aos/events/shm-event-loop.cc
+++ b/aos/events/shm-event-loop.cc
@@ -60,6 +60,8 @@
return queue_->WriteMessage(msg, RawQueue::kOverride);
}
+ const char *name() const override { return queue_->name(); }
+
private:
RawQueue *queue_;
};
@@ -218,6 +220,8 @@
void ShmEventLoop::Run() {
set_is_running(true);
for (const auto &run : on_run_) run();
+ // TODO(austin): epoll event loop in main thread (if needed), and async safe
+ // quit handler.
thread_state_->Run();
}
diff --git a/aos/events/shm-event-loop.h b/aos/events/shm-event-loop.h
index 9bbdb42..49d65f7 100644
--- a/aos/events/shm-event-loop.h
+++ b/aos/events/shm-event-loop.h
@@ -1,3 +1,6 @@
+#ifndef AOS_EVENTS_SHM_EVENT_LOOP_H_
+#define AOS_EVENTS_SHM_EVENT_LOOP_H_
+
#include <unordered_set>
#include <vector>
#include "aos/condition.h"
@@ -79,3 +82,5 @@
};
} // namespace aos
+
+#endif // AOS_EVENTS_SHM_EVENT_LOOP_H_
diff --git a/aos/events/simulated-event-loop.cc b/aos/events/simulated-event-loop.cc
index efce9ab..e82c381 100644
--- a/aos/events/simulated-event-loop.cc
+++ b/aos/events/simulated-event-loop.cc
@@ -43,6 +43,8 @@
return true; // Maybe false instead? :)
}
+ const char *name() const override { return queue_->name(); }
+
private:
SimulatedQueue *queue_;
};
@@ -93,8 +95,10 @@
const ::std::pair<::std::string, QueueTypeInfo> &type) {
auto it = queues_->find(type);
if (it == queues_->end()) {
- it = queues_->emplace(type, SimulatedQueue(type.second, scheduler_))
- .first;
+ it =
+ queues_
+ ->emplace(type, SimulatedQueue(type.second, type.first, scheduler_))
+ .first;
}
return &it->second;
}
diff --git a/aos/events/simulated-event-loop.h b/aos/events/simulated-event-loop.h
index 829ee8c..79b48b8 100644
--- a/aos/events/simulated-event-loop.h
+++ b/aos/events/simulated-event-loop.h
@@ -105,8 +105,9 @@
class SimulatedQueue {
public:
- explicit SimulatedQueue(const QueueTypeInfo &type, EventScheduler *scheduler)
- : type_(type), scheduler_(scheduler){};
+ explicit SimulatedQueue(const QueueTypeInfo &type, const ::std::string &name,
+ EventScheduler *scheduler)
+ : type_(type), name_(name), scheduler_(scheduler){};
std::unique_ptr<RawSender> MakeRawSender();
@@ -129,9 +130,12 @@
size_t size() { return type_.size; }
+ const char *name() const { return name_.c_str(); }
+
private:
int64_t index_ = -1;
QueueTypeInfo type_;
+ const ::std::string name_;
::std::vector<std::function<void(const aos::Message *message)>> watchers_;
RefCountedBuffer latest_message_;
EventScheduler *scheduler_;
diff --git a/aos/ipc_lib/queue.h b/aos/ipc_lib/queue.h
index f21295f..6fef1d5 100644
--- a/aos/ipc_lib/queue.h
+++ b/aos/ipc_lib/queue.h
@@ -144,6 +144,9 @@
// only 1 task is using this object (ie in tests).
int FreeMessages() const;
+ // Returns the name of the queue.
+ const char *name() const { return name_; }
+
private:
struct MessageHeader;
diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
index 252e01d..0fe9205 100644
--- a/frc971/codelab/BUILD
+++ b/frc971/codelab/BUILD
@@ -1,39 +1,40 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
cc_binary(
- name = 'basic_test',
- srcs = ['basic_test.cc'],
- deps = [
- ':basic_queue',
- ':basic',
- '//aos/testing:googletest',
- '//aos:queues',
- '//aos/controls:control_loop_test',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:team_number_test_environment',
- ],
- testonly = 1,
+ name = "basic_test",
+ testonly = 1,
+ srcs = ["basic_test.cc"],
+ deps = [
+ ":basic",
+ ":basic_queue",
+ "//aos:queues",
+ "//aos/controls:control_loop_test",
+ "//aos/events:shm-event-loop",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/control_loops:team_number_test_environment",
+ ],
)
cc_library(
- name = 'basic',
- srcs = ['basic.cc'],
- hdrs = ['basic.h'],
- deps = [
- ':basic_queue',
- '//aos/controls:control_loop',
- ],
+ name = "basic",
+ srcs = ["basic.cc"],
+ hdrs = ["basic.h"],
+ deps = [
+ ":basic_queue",
+ "//aos/controls:control_loop",
+ ],
)
queue_library(
- name = 'basic_queue',
- srcs = [
- 'basic.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "basic_queue",
+ srcs = [
+ "basic.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
diff --git a/frc971/codelab/basic.cc b/frc971/codelab/basic.cc
index 536384c..d06e285 100644
--- a/frc971/codelab/basic.cc
+++ b/frc971/codelab/basic.cc
@@ -3,8 +3,8 @@
namespace frc971 {
namespace codelab {
-Basic::Basic(BasicQueue *my_basic_queue)
- : aos::controls::ControlLoop<BasicQueue>(my_basic_queue) {}
+Basic::Basic(::aos::EventLoop *event_loop, const ::std::string &name)
+ : aos::controls::ControlLoop<BasicQueue>(event_loop, name) {}
void Basic::RunIteration(const BasicQueue::Goal *goal,
const BasicQueue::Position *position,
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
index 0332168..6f33718 100644
--- a/frc971/codelab/basic.h
+++ b/frc971/codelab/basic.h
@@ -43,7 +43,8 @@
// - Read basic.q, and familiarize yourself on the inputs and types involved.
class Basic : public ::aos::controls::ControlLoop<BasicQueue> {
public:
- explicit Basic(BasicQueue *my_basic_queue = &basic_queue);
+ explicit Basic(::aos::EventLoop *event_loop,
+ const ::std::string &name = ".frc971.codelab.basic_queue");
protected:
void RunIteration(const BasicQueue::Goal *goal,
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
index 5132407..2dc8dea 100644
--- a/frc971/codelab/basic_test.cc
+++ b/frc971/codelab/basic_test.cc
@@ -6,6 +6,7 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/queue.h"
#include "frc971/codelab/basic.q.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -65,7 +66,7 @@
".frc971.codelab.basic_queue.position",
".frc971.codelab.basic_queue.output",
".frc971.codelab.basic_queue.status"),
- basic_loop_(&basic_queue_) {
+ basic_loop_(&event_loop_, ".frc971.codelab.basic_queue") {
set_team_id(control_loops::testing::kTeamNumber);
}
@@ -81,6 +82,7 @@
}
BasicQueue basic_queue_;
+ ::aos::ShmEventLoop event_loop_;
Basic basic_loop_;
BasicSimulation basic_simulation_;
};
diff --git a/frc971/constants.h b/frc971/constants.h
index e14144e..fc3e12b 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -68,6 +68,27 @@
double allowable_encoder_error;
};
+struct AbsoluteEncoderZeroingConstants {
+ // The number of samples in the moving average filter.
+ size_t average_filter_size;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ double one_revolution_distance;
+ // Measured absolute position of the encoder when at zero.
+ double measured_absolute_position;
+ // Position of the middle of the range of motion in output coordinates.
+ double middle_position;
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ double zeroing_threshold;
+ // Buffer size for deciding if we are moving.
+ size_t moving_buffer_size;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
+};
+
// Defines a range of motion for a subsystem.
// These are all absolute positions in scaled units.
struct Range {
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index d46bca6..d12d3da 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -30,6 +30,24 @@
)
cc_library(
+ name = "pose",
+ hdrs = ["pose.h"],
+ deps = [
+ "//aos/util:math",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "pose_test",
+ srcs = ["pose_test.cc"],
+ deps = [
+ ":pose",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "hall_effect_tracker",
hdrs = [
"hall_effect_tracker.h",
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index bebecd0..44eb262 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -51,6 +51,16 @@
double pot;
};
+// Represents all of the data for an absolute and relative encoder pair.
+// The units on all of the positions are the same.
+// The relative encoder values are relative to where the encoder was at some
+// arbitrary point in time.
+struct AbsolutePosition {
+ // Current position read from each encoder.
+ double encoder;
+ double absolute_encoder;
+};
+
// The internal state of a zeroing estimator.
struct EstimatorState {
// If true, there has been a fatal error for the estimator.
@@ -82,6 +92,20 @@
};
// The internal state of a zeroing estimator.
+struct AbsoluteEncoderEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ bool error;
+ // If the joint has seen an index pulse and is zeroed.
+ bool zeroed;
+ // The estimated position of the joint.
+ double position;
+
+ // The estimated absolute position of the encoder. This is filtered, so it
+ // can be easily used when zeroing.
+ double absolute_position;
+};
+
+// The internal state of a zeroing estimator.
struct IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
bool error;
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 36f7c82..a06bc3b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -29,11 +29,11 @@
namespace control_loops {
namespace drivetrain {
-DrivetrainLoop::DrivetrainLoop(
- const DrivetrainConfig<double> &dt_config,
- ::frc971::control_loops::DrivetrainQueue *my_drivetrain)
+DrivetrainLoop::DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name)
: aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
- my_drivetrain),
+ event_loop, name),
dt_config_(dt_config),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
@@ -331,7 +331,7 @@
dt_openloop_.SetGoal(*goal);
}
- dt_openloop_.Update();
+ dt_openloop_.Update(robot_state().voltage_battery);
dt_closedloop_.Update(output != NULL && controller_type == 1);
@@ -393,7 +393,7 @@
right_high_requested_ = output->right_high;
}
- const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+ const double scalar = robot_state().voltage_battery / 12.0;
left_voltage *= scalar;
right_voltage *= scalar;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9396aed..a357eab 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -22,9 +22,8 @@
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
- const DrivetrainConfig<double> &dt_config,
- ::frc971::control_loops::DrivetrainQueue *my_drivetrain =
- &::frc971::control_loops::drivetrain_queue);
+ const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
int ControllerIndexFromGears();
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index a6711e4..5c1a8b4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -103,11 +103,11 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(new DrivetrainPlant(MakeDrivetrainPlant())),
- my_drivetrain_queue_(".frc971.control_loops.drivetrain",
- ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status"),
+ my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
+ ".frc971.control_loops.drivetrain_queue.goal",
+ ".frc971.control_loops.drivetrain_queue.position",
+ ".frc971.control_loops.drivetrain_queue.output",
+ ".frc971.control_loops.drivetrain_queue.status"),
gyro_reading_(::frc971::sensors::gyro_reading.name()) {
Reinitialize();
last_U_.setZero();
@@ -164,6 +164,7 @@
last_U_ << my_drivetrain_queue_.output->left_voltage,
my_drivetrain_queue_.output->right_voltage;
{
+ ::aos::robot_state.FetchLatest();
const double scalar = ::aos::robot_state->voltage_battery / 12.0;
last_U_ *= scalar;
}
@@ -218,17 +219,18 @@
// is no longer valid.
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
+ ::aos::ShmEventLoop event_loop_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
DrivetrainTest()
- : my_drivetrain_queue_(".frc971.control_loops.drivetrain",
- ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status"),
- drivetrain_motor_(GetDrivetrainConfig(), &my_drivetrain_queue_),
+ : my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
+ ".frc971.control_loops.drivetrain_queue.goal",
+ ".frc971.control_loops.drivetrain_queue.position",
+ ".frc971.control_loops.drivetrain_queue.output",
+ ".frc971.control_loops.drivetrain_queue.status"),
+ drivetrain_motor_(GetDrivetrainConfig(), &event_loop_),
drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
set_battery_voltage(12.0);
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 64b1aff..cc3468a 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -45,7 +45,7 @@
Scalar MaxVelocity();
- void Update();
+ void Update(Scalar voltage_battery);
void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
@@ -292,7 +292,7 @@
}
template <typename Scalar>
-void PolyDrivetrain<Scalar>::Update() {
+void PolyDrivetrain<Scalar>::Update(Scalar voltage_battery) {
if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
@@ -406,7 +406,9 @@
(R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-kTwelve, kTwelve);
#ifdef __linux__
- loop_->mutable_U() *= kTwelve / ::aos::robot_state->voltage_battery;
+ loop_->mutable_U() *= kTwelve / voltage_battery;
+#else
+ (void)voltage_battery;
#endif // __linux__
}
}
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
new file mode 100644
index 0000000..508a6a6
--- /dev/null
+++ b/frc971/control_loops/pose.h
@@ -0,0 +1,175 @@
+#ifndef FRC971_CONTROL_LOOPS_POSE_H_
+#define FRC971_CONTROL_LOOPS_POSE_H_
+
+#include "Eigen/Dense"
+#include "aos/util/math.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Provides a representation of a transformation on the field.
+// Currently, this is heavily geared towards things that occur in a 2-D plane.
+// The Z-axis is rarely used (but still relevant; e.g., in 2019 some of the
+// targets are at a different height).
+// For rotations, we currently just represent the yaw axis (the rotation about
+// the Z-axis).
+// As a convention, we use right-handed coordinate systems; the positive Z
+// axis will go up on the field, the positive X axis shall be "forwards" for
+// some relevant meaning of forwards, and the origin shall be chosen as
+// appropriate.
+// For 2019, at least, the global origin will be on the ground at the center
+// of the driver's station wall of your current alliance and the positive X-axis
+// will point straight into the field from the driver's station.
+// In future years this may need to change if the field's symmetry changes and
+// we can't interchangeably handle which side of the field we are on.
+// This means that if we had a Pose for the center of mass of the robot with a
+// position of (10, -5, 0) and a yaw of pi / 2, that suggests the robot is
+// facing straight to the left from the driver's perspective and is placed 10m
+// from the driver's station wall and 5m to the right of the center of the wall.
+//
+// Furthermore, Poses can be chained such that a Pose can be placed relative to
+// another Pose; the other Pose can dynamically update, thus allowing us to,
+// e.g., provide a Pose for a camera that is relative to the Pose of the robot.
+// Poses can also be in the global frame with no parent Pose.
+template <typename Scalar = double>
+class TypedPose {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ // The type that contains the translational (x, y, z) component of the Pose.
+ typedef Eigen::Matrix<Scalar, 3, 1> Pos;
+
+ // Construct a Pose in the absolute frame with a particular position and yaw.
+ TypedPose(const Pos &abs_pos, Scalar theta) : pos_(abs_pos), theta_(theta) {}
+ // Construct a Pose relative to another Pose (base).
+ // If you provide a base of nullptr, then this will
+ // construct a Pose in the global frame.
+ // Note that the lifetime of base should be greater than the lifetime of
+ // the object being constructed.
+ TypedPose(const TypedPose<Scalar> *base, const Pos &rel_pos, Scalar rel_theta)
+ : base_(base), pos_(rel_pos), theta_(rel_theta) {}
+
+ // Calculate the current global position of this Pose.
+ Pos abs_pos() const {
+ if (base_ == nullptr) {
+ return pos_;
+ }
+ Pos base_pos = base_->abs_pos();
+ Scalar base_theta = base_->abs_theta();
+ return base_pos + YawRotation(base_theta) * pos_;
+ }
+ // Calculate the absolute yaw of this Pose. Since we only have a single
+ // rotational axis, we can just sum the angle with that of the base Pose.
+ Scalar abs_theta() const {
+ if (base_ == nullptr) {
+ return theta_;
+ }
+ return aos::math::NormalizeAngle(theta_ + base_->abs_theta());
+ }
+ // Provide access to the position and yaw relative to the base Pose.
+ Pos rel_pos() const { return pos_; }
+ Scalar rel_theta() const { return theta_; }
+
+ Pos *mutable_pos() { return &pos_; }
+ void set_theta(Scalar theta) { theta_ = theta; }
+
+ // For 2-D calculation, provide the heading, which is distinct from the
+ // yaw/theta value. heading is the heading relative to the base Pose if you
+ // were to draw a line from the base to this Pose. i.e., if heading() is zero
+ // then you are directly in front of the base Pose.
+ Scalar heading() const { return ::std::atan2(pos_.y(), pos_.x()); }
+ // The 2-D distance from the base Pose to this Pose.
+ Scalar xy_norm() const { return pos_.template topRows<2>().norm(); }
+ // Return the absolute xy position.
+ Eigen::Matrix<Scalar, 2, 1> abs_xy() const {
+ return abs_pos().template topRows<2>();
+ }
+
+ // Provide a copy of this that is set to have the same
+ // current absolute Pose as this, but have a different base.
+ // This can be used, e.g., to compute a Pose for a vision target that is
+ // relative to the camera instead of relative to the field. You can then
+ // access the rel_* variables to get what the position of the target is
+ // relative to the robot/camera.
+ // If new_base == nullptr, provides a Pose referenced to the global frame.
+ // Note that the lifetime of new_base should be greater than the lifetime of
+ // the returned object (unless new_base == nullptr).
+ TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+
+ private:
+ // A rotation-matrix like representation of the rotation for a given angle.
+ inline static Eigen::AngleAxis<Scalar> YawRotation(double theta) {
+ return Eigen::AngleAxis<Scalar>(theta, Pos::UnitZ());
+ }
+
+ // A pointer to the base Pose. If uninitialized, then this Pose is in the
+ // global frame.
+ const TypedPose<Scalar> *base_ = nullptr;
+ // Position and yaw relative to base_.
+ Pos pos_;
+ Scalar theta_;
+}; // class TypedPose
+
+typedef TypedPose<double> Pose;
+
+template <typename Scalar>
+TypedPose<Scalar> TypedPose<Scalar>::Rebase(
+ const TypedPose<Scalar> *new_base) const {
+ if (new_base == nullptr) {
+ return TypedPose<Scalar>(nullptr, abs_pos(), abs_theta());
+ }
+ // Calculate the absolute position/yaws of this and of the new_base, and then
+ // calculate where we are relative to new_base, essentially reversing the
+ // calculation in abs_*.
+ Pos base_pos = new_base->abs_pos();
+ Scalar base_theta = new_base->abs_theta();
+ Pos self_pos = abs_pos();
+ Scalar self_theta = abs_theta();
+ Scalar diff_theta = ::aos::math::DiffAngle(self_theta, base_theta);
+ Pos diff_pos = YawRotation(-base_theta) * (self_pos - base_pos);
+ return TypedPose<Scalar>(new_base, diff_pos, diff_theta);
+}
+
+// Represents a 2D line segment constructed from a pair of Poses.
+// The line segment goes between the two Poses, but for calculating
+// intersections we use the 2D projection of the Poses onto the global X-Y
+// plane.
+template <typename Scalar = double>
+class TypedLineSegment {
+ public:
+ TypedLineSegment(const TypedPose<Scalar> &pose1,
+ const TypedPose<Scalar> &pose2)
+ : pose1_(pose1), pose2_(pose2) {}
+ // Detects if two line segments intersect.
+ // When at least one end of one line segment is collinear with the other,
+ // the line segments are treated as not intersecting.
+ bool Intersects(const TypedLineSegment<Scalar> &other) const {
+ // Source for algorithm:
+ // https://bryceboe.com/2006/10/23/line-segment-intersection-algorithm/
+ // Method:
+ // We will consider the four triangles that can be made out of any 3 points
+ // from the pair of line segments.
+ // Basically, if you consider one line segment the base of the triangle,
+ // then the two points of the other line segment should be on opposite
+ // sides of the first line segment (we use the PointsAreCCW function for
+ // this). This must hold when splitting off of both line segments.
+ Eigen::Matrix<Scalar, 2, 1> p1 = pose1_.abs_xy();
+ Eigen::Matrix<Scalar, 2, 1> p2 = pose2_.abs_xy();
+ Eigen::Matrix<Scalar, 2, 1> q1 = other.pose1_.abs_xy();
+ Eigen::Matrix<Scalar, 2, 1> q2 = other.pose2_.abs_xy();
+ return (::aos::math::PointsAreCCW<Scalar>(p1, q1, q2) !=
+ ::aos::math::PointsAreCCW<Scalar>(p2, q1, q2)) &&
+ (::aos::math::PointsAreCCW<Scalar>(p1, p2, q1) !=
+ ::aos::math::PointsAreCCW<Scalar>(p1, p2, q2));
+ }
+ private:
+ const TypedPose<Scalar> pose1_;
+ const TypedPose<Scalar> pose2_;
+}; // class TypedLineSegment
+
+typedef TypedLineSegment<double> LineSegment;
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_POSE_H_
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
new file mode 100644
index 0000000..cb994a2
--- /dev/null
+++ b/frc971/control_loops/pose_test.cc
@@ -0,0 +1,131 @@
+#include "frc971/control_loops/pose.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Test that basic accessors on an individual Pose object work as expected.
+TEST(PoseTest, BasicPoseTest) {
+ // Provide a basic Pose with non-zero components for everything.
+ Pose pose({1, 1, 0.5}, 0.5);
+ // The xy_norm should just be based on the x/y positions, not the Z; hence
+ // sqrt(2) rather than sqrt(1^2 + 1^2 + 0.5^2).
+ EXPECT_DOUBLE_EQ(::std::sqrt(2.0), pose.xy_norm());
+ // Similarly, heading should just be atan2(y, x).
+ EXPECT_DOUBLE_EQ(M_PI / 4.0, pose.heading());
+ // Global and relative poses should be the same since we did not construct
+ // this off of a separate Pose.
+ EXPECT_EQ(1.0, pose.rel_pos().x());
+ EXPECT_EQ(1.0, pose.rel_pos().y());
+ EXPECT_EQ(0.5, pose.rel_pos().z());
+
+ EXPECT_EQ(1.0, pose.abs_pos().x());
+ EXPECT_EQ(1.0, pose.abs_pos().y());
+ EXPECT_EQ(0.5, pose.abs_pos().z());
+
+ EXPECT_EQ(0.5, pose.rel_theta());
+ EXPECT_EQ(0.5, pose.abs_theta());
+
+ pose.set_theta(3.14);
+ EXPECT_EQ(3.14, pose.rel_theta());
+ pose.mutable_pos()->x() = 9.71;
+ EXPECT_EQ(9.71, pose.rel_pos().x());
+}
+
+// Check that Poses behave as expected when constructed relative to another
+// POse.
+TEST(PoseTest, BaseTest) {
+ // Tolerance for the EXPECT_NEARs. Because we are doing enough trig operations
+ // under the hood we actually start to lose some precision.
+ constexpr double kEps = 1e-15;
+ // The points we will construct have absolute positions at:
+ // base1: (1, 1)
+ // base2: (-1, 1)
+ // rel1: (0, 2)
+ // Where rel1 is expressed as compared to base1, noting that because base1
+ // has a yaw of M_PI, the position of rel1 compared to base1 is (1, -1)
+ // rather than (-1, 1).
+ Pose base1({1, 1, 0}, M_PI);
+ Pose base2({-1, 1, 0}, -M_PI / 2.0);
+ Pose rel1(&base1, {1, -1, 0}, 0.0);
+ EXPECT_NEAR(0.0, rel1.abs_pos().x(), kEps);
+ EXPECT_NEAR(2.0, rel1.abs_pos().y(), kEps);
+ EXPECT_NEAR(M_PI, rel1.abs_theta(), kEps);
+ // Check that, when rebasing to base2, the absolute position does not change
+ // and the relative POse changes to be relative to base2.
+ Pose rel2 = rel1.Rebase(&base2);
+ EXPECT_NEAR(rel1.abs_pos().x(), rel2.abs_pos().x(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().y(), rel2.abs_pos().y(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().z(), rel2.abs_pos().z(), kEps);
+ EXPECT_NEAR(rel1.abs_theta(), rel2.abs_theta(), kEps);
+ EXPECT_NEAR(-1.0, rel2.rel_pos().x(), kEps);
+ EXPECT_NEAR(1.0, rel2.rel_pos().y(), kEps);
+ EXPECT_NEAR(-M_PI / 2.0, rel2.rel_theta(), kEps);
+ // Check that rebasing onto nullptr results in a Pose based in the global
+ // frame.
+ Pose abs = rel1.Rebase(nullptr);
+ EXPECT_NEAR(rel1.abs_pos().x(), abs.abs_pos().x(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().y(), abs.abs_pos().y(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().z(), abs.abs_pos().z(), kEps);
+ EXPECT_NEAR(rel1.abs_theta(), abs.abs_theta(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().x(), abs.rel_pos().x(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().y(), abs.rel_pos().y(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().z(), abs.rel_pos().z(), kEps);
+ EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
+}
+
+// Tests that basic checks for intersection function as expected.
+TEST(LineSegmentTest, TrivialIntersectTest) {
+ Pose p1({0, 0, 0}, 0.0), p2({2, 0, 0}, 0.0);
+ // A line segment from (0, 0) to (0, 2).
+ LineSegment l1(p1, p2);
+ Pose q1({1, -1, 0}, 0.0), q2({1, 1, 0}, 0.0);
+ // A line segment from (1, -1) to (1, 1).
+ LineSegment l2(q1, q2);
+ // The two line segments should intersect.
+ EXPECT_TRUE(l1.Intersects(l2));
+ EXPECT_TRUE(l2.Intersects(l1));
+
+ // If we switch around the orderings such that the line segments are
+ // (0, 0) -> (1, -1) and (2, 0)->(1, 1) then the line segments do not
+ // intersect.
+ LineSegment l3(p1, q1);
+ LineSegment l4(p2, q2);
+ EXPECT_FALSE(l3.Intersects(l4));
+ EXPECT_FALSE(l4.Intersects(l3));
+}
+
+// Check that when we construct line segments that are collinear, both with
+// overlapping bits and without overlapping bits, they register as not
+// intersecting.
+// We may want this behavior to change in the future, but for now check for
+// consistency.
+TEST(LineSegmentTest, CollinearIntersectTest) {
+ Pose p1({0, 0, 0}, 0.0), p2({1, 0, 0}, 0.0), p3({2, 0, 0}, 0.0),
+ p4({3, 0, 0}, 0.0);
+ // These two line segments overlap and are collinear, one going from 0 to 2
+ // and the other from 1 to 3 on the X-axis.
+ LineSegment l1(p1, p3);
+ LineSegment l2(p2, p4);
+ EXPECT_FALSE(l1.Intersects(l2));
+ EXPECT_FALSE(l2.Intersects(l1));
+
+ // These two line segments do not overlap and are collinear, one going from 0
+ // to 1 and the other from 2 to 3 on the X-axis.
+ LineSegment l3(p1, p2);
+ LineSegment l4(p3, p4);
+ EXPECT_FALSE(l3.Intersects(l4));
+ EXPECT_FALSE(l4.Intersects(l3));
+
+ // Test when one line segment is completely contained within the other.
+ LineSegment l5(p1, p4);
+ LineSegment l6(p3, p2);
+ EXPECT_FALSE(l5.Intersects(l6));
+ EXPECT_FALSE(l6.Intersects(l5));
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 97a96d5..c875a0b 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -172,6 +172,18 @@
}
}
+void PositionSensorSimulator::GetSensorValues(AbsolutePosition *values) {
+ values->encoder = current_position_ - start_position_;
+ // TODO(phil): Create some lag here since this is a PWM signal it won't be
+ // instantaneous like the other signals. Better yet, its lag varies
+ // randomly with the distribution varying depending on the reading.
+ values->absolute_encoder = ::std::remainder(
+ current_position_ + known_absolute_encoder_, distance_per_revolution_);
+ if (values->absolute_encoder < 0) {
+ values->absolute_encoder += distance_per_revolution_;
+ }
+}
+
void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
values->current = lower_index_edge_.current_index_segment() !=
upper_index_edge_.current_index_segment();
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index b8a214b..2880429 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -60,6 +60,7 @@
// can be given in radians, meters, etc.
void GetSensorValues(IndexPosition *values);
void GetSensorValues(PotAndIndexPosition *values);
+ void GetSensorValues(AbsolutePosition *values);
void GetSensorValues(PotAndAbsolutePosition *values);
void GetSensorValues(HallEffectAndPosition *values);
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index bc3ac04..00cd77d 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -276,3 +276,22 @@
"//aos/logging",
],
)
+
+cc_library(
+ name = "sensor_reader",
+ srcs = [
+ "sensor_reader.cc",
+ ],
+ hdrs = [
+ "sensor_reader.h"
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ "//aos/stl_mutex",
+ "//aos/time:time",
+ "//aos:init",
+ "//third_party:wpilib",
+ ":dma",
+ ":dma_edge_counting",
+ ],
+)
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
new file mode 100644
index 0000000..24d33f8
--- /dev/null
+++ b/frc971/wpilib/sensor_reader.cc
@@ -0,0 +1,106 @@
+#include "frc971/wpilib/sensor_reader.h"
+
+#include "aos/init.h"
+#include "aos/util/compiler_memory_barrier.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
+#include "frc971/wpilib/ahal/Utility.h"
+
+namespace frc971 {
+namespace wpilib {
+
+SensorReader::SensorReader() {}
+
+void SensorReader::set_drivetrain_left_encoder(
+ ::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_left_encoder_ = ::std::move(encoder);
+}
+
+void SensorReader::set_drivetrain_right_encoder(
+ ::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_right_encoder_ = ::std::move(encoder);
+}
+
+// All of the DMA-related set_* calls must be made before this, and it
+// doesn't hurt to do all of them.
+void SensorReader::set_dma(::std::unique_ptr<DMA> dma) {
+ dma_synchronizer_.reset(
+ new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+}
+
+void SensorReader::set_pwm_trigger(
+ ::std::unique_ptr<frc::DigitalInput> pwm_trigger) {
+ medium_encoder_filter_.Add(pwm_trigger.get());
+ pwm_trigger_ = ::std::move(pwm_trigger);
+}
+
+void SensorReader::RunPWMDetecter() {
+ ::aos::SetCurrentThreadRealtimePriority(41);
+
+ pwm_trigger_->RequestInterrupts();
+ // Rising edge only.
+ pwm_trigger_->SetUpSourceEdge(true, false);
+
+ monotonic_clock::time_point last_posedge_monotonic =
+ monotonic_clock::min_time;
+
+ while (run_) {
+ auto ret = pwm_trigger_->WaitForInterrupt(1.0, true);
+ if (ret == frc::InterruptableSensorBase::WaitResult::kRisingEdge) {
+ // Grab all the clocks.
+ const double pwm_fpga_time = pwm_trigger_->ReadRisingTimestamp();
+
+ aos_compiler_memory_barrier();
+ const double fpga_time_before = frc::GetFPGATime() * 1e-6;
+ aos_compiler_memory_barrier();
+ const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ aos_compiler_memory_barrier();
+ const double fpga_time_after = frc::GetFPGATime() * 1e-6;
+ aos_compiler_memory_barrier();
+
+ const double fpga_offset =
+ (fpga_time_after + fpga_time_before) / 2.0 - pwm_fpga_time;
+
+ // Compute when the edge was.
+ const monotonic_clock::time_point monotonic_edge =
+ monotonic_now - chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(fpga_offset));
+
+ LOG(DEBUG, "Got PWM pulse %f spread, %f offset, %lld trigger\n",
+ fpga_time_after - fpga_time_before, fpga_offset,
+ monotonic_edge.time_since_epoch().count());
+
+ // Compute bounds on the timestep and sampling times.
+ const double fpga_sample_length = fpga_time_after - fpga_time_before;
+ const chrono::nanoseconds elapsed_time =
+ monotonic_edge - last_posedge_monotonic;
+
+ last_posedge_monotonic = monotonic_edge;
+
+ // Verify that the values are sane.
+ if (fpga_sample_length > 2e-5 || fpga_sample_length < 0) {
+ continue;
+ }
+ if (fpga_offset < 0 || fpga_offset > 0.00015) {
+ continue;
+ }
+ if (elapsed_time > chrono::microseconds(5050) + chrono::microseconds(4) ||
+ elapsed_time < chrono::microseconds(5050) - chrono::microseconds(4)) {
+ continue;
+ }
+ // Good edge!
+ {
+ ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
+ last_tick_time_monotonic_timepoint_ = last_posedge_monotonic;
+ last_period_ = elapsed_time;
+ }
+ } else {
+ LOG(INFO, "PWM triggered %d\n", ret);
+ }
+ }
+ pwm_trigger_->CancelInterrupts();
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
new file mode 100644
index 0000000..3abe1b0
--- /dev/null
+++ b/frc971/wpilib/sensor_reader.h
@@ -0,0 +1,66 @@
+#ifndef FRC971_WPILIB_SENSOR_READER_H_
+#define FRC971_WPILIB_SENSOR_READER_H_
+
+#include <atomic>
+#include <chrono>
+
+#include "aos/stl_mutex/stl_mutex.h"
+#include "aos/time/time.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/dma_edge_counting.h"
+
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
+namespace frc971 {
+namespace wpilib {
+
+class SensorReader {
+ public:
+ SensorReader();
+
+ // Sets the left drivetrain encoder.
+ void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder);
+
+ // Sets the right drivetrain encoder.
+ void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder);
+
+ // Sets the dma.
+ void set_dma(::std::unique_ptr<DMA> dma);
+
+ // Sets the pwm trigger.
+ void set_pwm_trigger(::std::unique_ptr<frc::DigitalInput> pwm_trigger);
+
+ // Stops the pwm trigger on the next iteration.
+ void Quit() { run_ = false; }
+
+ protected:
+ // Uses the pwm trigger to find the pwm cycle width and offset for that
+ // iteration.
+ void RunPWMDetecter();
+
+ ::std::unique_ptr<frc::DigitalInput> pwm_trigger_;
+
+ frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
+ hall_filter_;
+
+ // Mutex to manage access to the period and tick time variables.
+ ::aos::stl_mutex tick_time_mutex_;
+ monotonic_clock::time_point last_tick_time_monotonic_timepoint_ =
+ monotonic_clock::min_time;
+ chrono::nanoseconds last_period_ = chrono::microseconds(5050);
+
+ ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
+
+ ::std::atomic<bool> run_{true};
+
+ ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_SENSOR_READER_H_
diff --git a/frc971/zeroing/wrap.h b/frc971/zeroing/wrap.h
index c6ec085..976e605 100644
--- a/frc971/zeroing/wrap.h
+++ b/frc971/zeroing/wrap.h
@@ -9,6 +9,13 @@
double Wrap(double nearest, double value, double period);
float Wrap(float nearest, float value, float period);
+inline double UnWrap(double nearest, double value, double period) {
+ return Wrap(nearest, value, period);
+}
+inline float UnWrap(float nearest, float value, float period) {
+ return Wrap(nearest, value, period);
+}
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 114ef21..1ed98a3 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -332,19 +332,12 @@
// Now, compute the nearest absolute encoder value to the offset relative
// encoder position.
const double adjusted_absolute_encoder =
- Wrap(adjusted_incremental_encoder,
- sample.absolute_encoder - constants_.measured_absolute_position,
- constants_.one_revolution_distance);
+ UnWrap(adjusted_incremental_encoder,
+ sample.absolute_encoder - constants_.measured_absolute_position,
+ constants_.one_revolution_distance);
- // Reverse the math on the previous line to compute the absolute encoder.
- // Do this by taking the adjusted encoder, and then subtracting off the
- // second argument above, and the value that was added by Wrap.
- filtered_absolute_encoder_ =
- ((sample.encoder + average_relative_to_absolute_offset) -
- (-constants_.measured_absolute_position +
- (adjusted_absolute_encoder -
- (sample.absolute_encoder - constants_.measured_absolute_position))));
-
+ // We can now compute the offset now that we have unwrapped the absolute
+ // encoder.
const double relative_to_absolute_offset =
adjusted_absolute_encoder - sample.encoder;
@@ -386,10 +379,20 @@
::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
offset_samples_.size();
- offset_ = Wrap(sample.encoder + pot_relative_encoder_offset_,
- average_relative_to_absolute_offset + sample.encoder,
- constants_.one_revolution_distance) -
+ offset_ = UnWrap(sample.encoder + pot_relative_encoder_offset_,
+ average_relative_to_absolute_offset + sample.encoder,
+ constants_.one_revolution_distance) -
sample.encoder;
+
+ // Reverse the math for adjusted_absolute_encoder to compute the absolute
+ // encoder. Do this by taking the adjusted encoder, and then subtracting off
+ // the second argument above, and the value that was added by Wrap.
+ filtered_absolute_encoder_ =
+ ((sample.encoder + average_relative_to_absolute_offset) -
+ (-constants_.measured_absolute_position +
+ (adjusted_absolute_encoder -
+ (sample.absolute_encoder - constants_.measured_absolute_position))));
+
if (offset_ready()) {
if (!zeroed_) {
first_offset_ = offset_;
@@ -527,5 +530,170 @@
return r;
}
+AbsoluteEncoderZeroingEstimator::AbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void AbsoluteEncoderZeroingEstimator::Reset() {
+ zeroed_ = false;
+ error_ = false;
+ first_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ position_ = 0.0;
+ nan_samples_ = 0;
+ relative_to_absolute_offset_samples_.clear();
+ move_detector_.Reset();
+}
+
+
+// The math here is a bit backwards, but I think it'll be less error prone that
+// way and more similar to the version with a pot as well.
+//
+// We start by unwrapping the absolute encoder using the relative encoder. This
+// puts us in a non-wrapping space and lets us average a bit easier. From
+// there, we can compute an offset and wrap ourselves back such that we stay
+// close to the middle value.
+//
+// To guard against the robot moving while updating estimates, buffer a number
+// of samples and check that the buffered samples are not different than the
+// zeroing threshold. At any point that the samples differ too much, do not
+// update estimates based on those samples.
+void AbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const AbsolutePosition &info) {
+ // Check for Abs Encoder NaN value that would mess up the rest of the zeroing
+ // code below. NaN values are given when the Absolute Encoder is disconnected.
+ if (::std::isnan(info.absolute_encoder)) {
+ if (zeroed_) {
+ LOG(ERROR, "NAN on absolute encoder\n");
+ error_ = true;
+ } else {
+ ++nan_samples_;
+ LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
+ static_cast<int>(nan_samples_));
+ if (nan_samples_ >= constants_.average_filter_size) {
+ error_ = true;
+ zeroed_ = true;
+ }
+ }
+ // Throw some dummy values in for now.
+ filtered_absolute_encoder_ = info.absolute_encoder;
+ position_ = offset_ + info.encoder;
+ return;
+ }
+
+ const bool moving = move_detector_.Update(info, constants_.moving_buffer_size,
+ constants_.zeroing_threshold);
+
+ if (!moving) {
+ const AbsolutePosition &sample = move_detector_.GetSample();
+
+ // Compute the average offset between the absolute encoder and relative
+ // encoder. If we have 0 samples, assume it is 0.
+ double average_relative_to_absolute_offset =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? 0.0
+ : ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(),
+ 0.0) /
+ relative_to_absolute_offset_samples_.size();
+
+ // Now, compute the estimated absolute position using the previously
+ // estimated offset and the incremental encoder.
+ const double adjusted_incremental_encoder =
+ sample.encoder + average_relative_to_absolute_offset;
+
+ // Now, compute the absolute encoder value nearest to the offset relative
+ // encoder position.
+ const double adjusted_absolute_encoder =
+ UnWrap(adjusted_incremental_encoder,
+ sample.absolute_encoder - constants_.measured_absolute_position,
+ constants_.one_revolution_distance);
+
+ // We can now compute the offset now that we have unwrapped the absolute
+ // encoder.
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - sample.encoder;
+
+ // Add the sample and update the average with the new reading.
+ const size_t relative_to_absolute_offset_samples_size =
+ relative_to_absolute_offset_samples_.size();
+ if (relative_to_absolute_offset_samples_size <
+ constants_.average_filter_size) {
+ average_relative_to_absolute_offset =
+ (average_relative_to_absolute_offset *
+ relative_to_absolute_offset_samples_size +
+ relative_to_absolute_offset) /
+ (relative_to_absolute_offset_samples_size + 1);
+
+ relative_to_absolute_offset_samples_.push_back(
+ relative_to_absolute_offset);
+ } else {
+ average_relative_to_absolute_offset -=
+ relative_to_absolute_offset_samples_[samples_idx_] /
+ relative_to_absolute_offset_samples_size;
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset;
+ average_relative_to_absolute_offset +=
+ relative_to_absolute_offset /
+ relative_to_absolute_offset_samples_size;
+ }
+
+ // Drop the oldest sample when we run this function the next time around.
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ // And our offset is the offset that gives us the position within +- ord/2
+ // of the middle position.
+ offset_ = Wrap(constants_.middle_position,
+ average_relative_to_absolute_offset + sample.encoder,
+ constants_.one_revolution_distance) -
+ sample.encoder;
+
+ // Reverse the math for adjusted_absolute_encoder to compute the absolute
+ // encoder. Do this by taking the adjusted encoder, and then subtracting off
+ // the second argument above, and the value that was added by Wrap.
+ filtered_absolute_encoder_ =
+ ((sample.encoder + average_relative_to_absolute_offset) -
+ (-constants_.measured_absolute_position +
+ (adjusted_absolute_encoder -
+ (sample.absolute_encoder - constants_.measured_absolute_position))));
+
+ if (offset_ready()) {
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
+
+ if (::std::abs(first_offset_ - offset_) >
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance) {
+ LOG(ERROR,
+ "Offset moved too far. Initial: %f, current %f, allowable change: "
+ "%f\n",
+ first_offset_, offset_, constants_.allowable_encoder_error *
+ constants_.one_revolution_distance);
+ error_ = true;
+ }
+
+ zeroed_ = true;
+ }
+ }
+
+ // Update the position.
+ position_ = offset_ + info.encoder;
+}
+
+AbsoluteEncoderZeroingEstimator::State
+ AbsoluteEncoderZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.position = position_;
+ r.absolute_position = filtered_absolute_encoder_;
+ return r;
+}
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index e969a88..e0c6169 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -421,6 +421,74 @@
double position_;
};
+// Estimates the position with an absolute encoder which also reports
+// incremental counts. The absolute encoder can't spin more than one
+// revolution.
+class AbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<AbsolutePosition,
+ constants::AbsoluteEncoderZeroingConstants,
+ AbsoluteEncoderEstimatorState> {
+ public:
+ explicit AbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteEncoderZeroingConstants &constants);
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Updates the sensor values for the zeroing logic.
+ void UpdateEstimate(const AbsolutePosition &info) override;
+
+ void TriggerError() override { error_ = true; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const override {
+ return relative_to_absolute_offset_samples_.size() ==
+ constants_.average_filter_size;
+ }
+
+ // Returns information about our current state.
+ State GetEstimatorState() const override;
+
+ private:
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::AbsoluteEncoderZeroingConstants constants_;
+
+ // True if the mechanism is zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+ // The first valid offset we recorded. This is only set after zeroed_ first
+ // changes to true.
+ double first_offset_;
+
+ // The filtered absolute encoder. This is used in the status for calibration.
+ double filtered_absolute_encoder_ = 0.0;
+
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+
+ MoveDetector<AbsolutePosition> move_detector_;
+
+ // Estimated start position of the mechanism
+ double offset_ = 0;
+ // The next position in 'relative_to_absolute_offset_samples_' and
+ // 'encoder_samples_' to be used to store the next sample.
+ int samples_idx_ = 0;
+
+ // Number of NANs we've seen in a row.
+ size_t nan_samples_ = 0;
+
+ // The filtered position.
+ double position_ = 0.0;
+};
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 2a35764..cca0585 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -16,9 +16,10 @@
namespace zeroing {
using control_loops::PositionSensorSimulator;
-using constants::PotAndIndexPulseZeroingConstants;
-using constants::PotAndAbsoluteEncoderZeroingConstants;
+using constants::AbsoluteEncoderZeroingConstants;
using constants::EncoderPlusIndexZeroingConstants;
+using constants::PotAndAbsoluteEncoderZeroingConstants;
+using constants::PotAndIndexPulseZeroingConstants;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -39,6 +40,15 @@
}
void MoveTo(PositionSensorSimulator *simulator,
+ AbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ AbsolutePosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
+
+ void MoveTo(PositionSensorSimulator *simulator,
PotAndAbsoluteEncoderZeroingEstimator *estimator,
double new_position) {
PotAndAbsolutePosition sensor_values_;
@@ -324,7 +334,7 @@
}
// Makes sure that using an absolute encoder lets us zero without moving.
-TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithoutMovement) {
+TEST_F(ZeroingTest, TestPotAndAbsoluteEncoderZeroingWithoutMovement) {
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
@@ -351,8 +361,8 @@
}
// Makes sure that we ignore a NAN if we get it, but will correctly zero
-// afterwords.
-TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingIgnoresNAN) {
+// afterwards.
+TEST_F(ZeroingTest, TestPotAndAbsoluteEncoderZeroingIgnoresNAN) {
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
@@ -388,7 +398,7 @@
}
// Makes sure that using an absolute encoder doesn't let us zero while moving.
-TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithMovement) {
+TEST_F(ZeroingTest, TestPotAndAbsoluteEncoderZeroingWithMovement) {
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
@@ -415,7 +425,7 @@
}
// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
-TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
+TEST_F(ZeroingTest, TestPotAndAbsoluteEncoderZeroingWithNaN) {
PotAndAbsoluteEncoderZeroingConstants constants{
kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
@@ -656,5 +666,121 @@
EXPECT_FALSE(estimator.zeroed());
}
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double kMiddlePosition = 2.5;
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ kMiddlePosition, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ AbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwards.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+ const double kMiddlePosition = 2.5;
+
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ kMiddlePosition, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ AbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ AbsolutePosition sensor_values;
+ sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
+ sensor_values.encoder = 0.0;
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(sensor_values);
+ }
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 10 * index_diff;
+ double measured_absolute_position = 0.3 * index_diff;
+ const double kMiddlePosition = 2.5;
+
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ kMiddlePosition, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ AbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 1.0, 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ AbsoluteEncoderZeroingEstimator estimator(constants);
+
+ AbsolutePosition sensor_values;
+ sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
+ sensor_values.encoder = 0.0;
+ // We tolerate a couple NANs before we start.
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(sensor_values);
+ }
+ ASSERT_FALSE(estimator.error());
+
+ estimator.UpdateEstimate(sensor_values);
+ ASSERT_TRUE(estimator.error());
+}
+
} // namespace zeroing
} // namespace frc971
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index a78f1e0..0470c31 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -561,7 +561,7 @@
DrivetrainQueue_Output output;
polydrivetrain->SetGoal(goal);
- polydrivetrain->Update();
+ polydrivetrain->Update(12.0f);
polydrivetrain->SetOutput(&output);
vesc_set_duty(0, -output.left_voltage / 12.0f);
diff --git a/y2012/control_loops/accessories/accessories.cc b/y2012/control_loops/accessories/accessories.cc
index 0f9fc93..d320468 100644
--- a/y2012/control_loops/accessories/accessories.cc
+++ b/y2012/control_loops/accessories/accessories.cc
@@ -11,10 +11,10 @@
::y2012::control_loops::AccessoriesQueue> {
public:
explicit AccessoriesLoop(
- ::y2012::control_loops::AccessoriesQueue *my_accessories =
- &::y2012::control_loops::accessories_queue)
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2012.control_loops.accessories_queue")
: ::aos::controls::ControlLoop<::y2012::control_loops::AccessoriesQueue>(
- my_accessories) {}
+ event_loop, name) {}
void RunIteration(
const ::y2012::control_loops::AccessoriesQueue::Message *goal,
@@ -33,7 +33,8 @@
int main() {
::aos::Init();
- ::y2012::control_loops::accessories::AccessoriesLoop accessories;
+ ::aos::ShmEventLoop event_loop;
+ ::y2012::control_loops::accessories::AccessoriesLoop accessories(&event_loop);
accessories.Run();
::aos::Cleanup();
}
diff --git a/y2012/control_loops/drivetrain/BUILD b/y2012/control_loops/drivetrain/BUILD
index a0aa4cc..3be1e9a 100644
--- a/y2012/control_loops/drivetrain/BUILD
+++ b/y2012/control_loops/drivetrain/BUILD
@@ -78,6 +78,7 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2012/control_loops/drivetrain/drivetrain_main.cc b/y2012/control_loops/drivetrain/drivetrain_main.cc
index 6b7eca3..ef348a4 100644
--- a/y2012/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2012/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,9 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index f267251..0764895 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -1,105 +1,106 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
cc_binary(
- name = 'replay_claw',
- srcs = [
- 'replay_claw.cc',
- ],
- deps = [
- ':claw_queue',
- '//aos/controls:replay_control_loop',
- '//aos:init',
- ],
+ name = "replay_claw",
+ srcs = [
+ "replay_claw.cc",
+ ],
+ deps = [
+ ":claw_queue",
+ "//aos:init",
+ "//aos/controls:replay_control_loop",
+ ],
)
queue_library(
- name = 'claw_queue',
- srcs = [
- 'claw.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "claw_queue",
+ srcs = [
+ "claw.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
genrule(
- name = 'genrule_claw',
- visibility = ['//visibility:private'],
- cmd = '$(location //y2014/control_loops/python:claw) $(OUTS)',
- tools = [
- '//y2014/control_loops/python:claw',
- ],
- outs = [
- 'claw_motor_plant.h',
- 'claw_motor_plant.cc',
- ],
+ name = "genrule_claw",
+ outs = [
+ "claw_motor_plant.h",
+ "claw_motor_plant.cc",
+ ],
+ cmd = "$(location //y2014/control_loops/python:claw) $(OUTS)",
+ tools = [
+ "//y2014/control_loops/python:claw",
+ ],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'claw_lib',
- srcs = [
- 'claw.cc',
- 'claw_motor_plant.cc',
- ],
- hdrs = [
- 'claw.h',
- 'claw_motor_plant.h',
- ],
- deps = [
- ':claw_queue',
- '//aos/controls:control_loop',
- '//aos/controls:polytope',
- '//aos/logging:queue_logging',
- '//aos/logging:matrix_logging',
- '//aos:math',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:coerce_goal',
- '//frc971/control_loops:hall_effect_tracker',
- '//y2014:constants',
- ],
- linkopts = [
- '-lm',
- ],
+ name = "claw_lib",
+ srcs = [
+ "claw.cc",
+ "claw_motor_plant.cc",
+ ],
+ hdrs = [
+ "claw.h",
+ "claw_motor_plant.h",
+ ],
+ linkopts = [
+ "-lm",
+ ],
+ deps = [
+ ":claw_queue",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//aos/controls:polytope",
+ "//aos/logging:matrix_logging",
+ "//aos/logging:queue_logging",
+ "//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:hall_effect_tracker",
+ "//frc971/control_loops:state_feedback_loop",
+ "//y2014:constants",
+ ],
)
cc_test(
- name = 'claw_lib_test',
- srcs = [
- 'claw_lib_test.cc',
- ],
- deps = [
- ':claw_lib',
- ':claw_queue',
- '//aos/controls:control_loop_test',
- '//aos/testing:googletest',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:team_number_test_environment',
- ],
+ name = "claw_lib_test",
+ srcs = [
+ "claw_lib_test.cc",
+ ],
+ deps = [
+ ":claw_lib",
+ ":claw_queue",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/control_loops:team_number_test_environment",
+ ],
)
cc_binary(
- name = 'claw_calibration',
- srcs = [
- 'claw_calibration.cc',
- ],
- deps = [
- '//aos:init',
- ':claw_queue',
- '//aos/controls:control_loop',
- '//y2014:constants',
- ],
+ name = "claw_calibration",
+ srcs = [
+ "claw_calibration.cc",
+ ],
+ deps = [
+ ":claw_queue",
+ "//aos:init",
+ "//aos/controls:control_loop",
+ "//y2014:constants",
+ ],
)
cc_binary(
- name = 'claw',
- srcs = [
- 'claw_main.cc',
- ],
- deps = [
- '//aos:init',
- ':claw_lib',
- ],
+ name = "claw",
+ srcs = [
+ "claw_main.cc",
+ ],
+ deps = [
+ ":claw_lib",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ ],
)
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 088472c..7a28a23 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -373,8 +373,9 @@
return false;
}
-ClawMotor::ClawMotor(::y2014::control_loops::ClawQueue *my_claw)
- : aos::controls::ControlLoop<::y2014::control_loops::ClawQueue>(my_claw),
+ClawMotor::ClawMotor(::aos::EventLoop *event_loop, const ::std::string &name)
+ : aos::controls::ControlLoop<::y2014::control_loops::ClawQueue>(event_loop,
+ name),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -604,9 +605,7 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (((::aos::joystick_state.get() == NULL)
- ? true
- : ::aos::joystick_state->autonomous) &&
+ ((has_joystick_state() ? joystick_state().autonomous : true) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -674,12 +673,12 @@
}
bool autonomous, enabled;
- if (::aos::joystick_state.get() == nullptr) {
+ if (has_joystick_state()) {
+ autonomous = joystick_state().autonomous;
+ enabled = joystick_state().enabled;
+ } else {
autonomous = true;
enabled = false;
- } else {
- autonomous = ::aos::joystick_state->autonomous;
- enabled = ::aos::joystick_state->enabled;
}
double bottom_claw_velocity_ = 0.0;
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
index 7995512..082ddce 100644
--- a/y2014/control_loops/claw/claw.h
+++ b/y2014/control_loops/claw/claw.h
@@ -185,8 +185,9 @@
class ClawMotor
: public aos::controls::ControlLoop<::y2014::control_loops::ClawQueue> {
public:
- explicit ClawMotor(::y2014::control_loops::ClawQueue *my_claw =
- &::y2014::control_loops::claw_queue);
+ explicit ClawMotor(
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2014.control_loops.claw_queue");
// True if the state machine is ready.
bool capped_goal() const { return capped_goal_; }
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index f10e61e..6545dcb 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -245,6 +245,7 @@
// is no longer valid.
::y2014::control_loops::ClawQueue claw_queue;
+ ::aos::ShmEventLoop event_loop_;
// Create a loop and simulation plant.
ClawMotor claw_motor_;
ClawMotorSimulation claw_motor_plant_;
@@ -259,7 +260,7 @@
".y2014.control_loops.claw_queue.position",
".y2014.control_loops.claw_queue.output",
".y2014.control_loops.claw_queue.status"),
- claw_motor_(&claw_queue),
+ claw_motor_(&event_loop_),
claw_motor_plant_(0.4, 0.2),
min_separation_(constants::GetValues().claw.claw_min_separation) {}
diff --git a/y2014/control_loops/claw/claw_main.cc b/y2014/control_loops/claw/claw_main.cc
index 5bb61ba..85497d9 100644
--- a/y2014/control_loops/claw/claw_main.cc
+++ b/y2014/control_loops/claw/claw_main.cc
@@ -1,10 +1,12 @@
#include "y2014/control_loops/claw/claw.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2014::control_loops::ClawMotor claw;
+ ::aos::ShmEventLoop event_loop;
+ ::y2014::control_loops::ClawMotor claw(&event_loop);
claw.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index bed5cd9..858f7e0 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -77,6 +77,7 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index ae20bc8..19c2659 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -1,13 +1,16 @@
#include "aos/init.h"
-#include "y2014/control_loops/drivetrain/drivetrain_base.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2014/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
::aos::Init();
- DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig());
+ ::aos::ShmEventLoop event_loop;
+ DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
+ &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index 810d239..b58ff98 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -1,91 +1,92 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
cc_binary(
- name = 'replay_shooter',
- srcs = [
- 'replay_shooter.cc',
- ],
- deps = [
- ':shooter_queue',
- '//aos/controls:replay_control_loop',
- '//aos:init',
- ],
+ name = "replay_shooter",
+ srcs = [
+ "replay_shooter.cc",
+ ],
+ deps = [
+ ":shooter_queue",
+ "//aos:init",
+ "//aos/controls:replay_control_loop",
+ ],
)
queue_library(
- name = 'shooter_queue',
- srcs = [
- 'shooter.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "shooter_queue",
+ srcs = [
+ "shooter.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
genrule(
- name = 'genrule_shooter',
- visibility = ['//visibility:private'],
- cmd = '$(location //y2014/control_loops/python:shooter) $(OUTS)',
- tools = [
- '//y2014/control_loops/python:shooter',
- ],
- outs = [
- 'shooter_motor_plant.cc',
- 'shooter_motor_plant.h',
- 'unaugmented_shooter_motor_plant.cc',
- 'unaugmented_shooter_motor_plant.h',
- ],
+ name = "genrule_shooter",
+ outs = [
+ "shooter_motor_plant.cc",
+ "shooter_motor_plant.h",
+ "unaugmented_shooter_motor_plant.cc",
+ "unaugmented_shooter_motor_plant.h",
+ ],
+ cmd = "$(location //y2014/control_loops/python:shooter) $(OUTS)",
+ tools = [
+ "//y2014/control_loops/python:shooter",
+ ],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'shooter_lib',
- srcs = [
- 'shooter.cc',
- 'shooter_motor_plant.cc',
- 'unaugmented_shooter_motor_plant.cc',
- ],
- hdrs = [
- 'shooter.h',
- 'shooter_motor_plant.h',
- 'unaugmented_shooter_motor_plant.h',
- ],
- deps = [
- ':shooter_queue',
- '//aos/controls:control_loop',
- '//y2014:constants',
- '//frc971/control_loops:state_feedback_loop',
- '//aos/logging:queue_logging',
- ],
- linkopts = [
- '-lm',
- ],
+ name = "shooter_lib",
+ srcs = [
+ "shooter.cc",
+ "shooter_motor_plant.cc",
+ "unaugmented_shooter_motor_plant.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ "shooter_motor_plant.h",
+ "unaugmented_shooter_motor_plant.h",
+ ],
+ linkopts = [
+ "-lm",
+ ],
+ deps = [
+ ":shooter_queue",
+ "//aos/controls:control_loop",
+ "//aos/logging:queue_logging",
+ "//frc971/control_loops:state_feedback_loop",
+ "//y2014:constants",
+ ],
)
cc_test(
- name = 'shooter_lib_test',
- srcs = [
- 'shooter_lib_test.cc',
- ],
- deps = [
- ':shooter_lib',
- ':shooter_queue',
- '//aos/controls:control_loop_test',
- '//aos/testing:googletest',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:team_number_test_environment',
- ],
+ name = "shooter_lib_test",
+ srcs = [
+ "shooter_lib_test.cc",
+ ],
+ deps = [
+ ":shooter_lib",
+ ":shooter_queue",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/control_loops:team_number_test_environment",
+ ],
)
cc_binary(
- name = 'shooter',
- srcs = [
- 'shooter_main.cc',
- ],
- deps = [
- '//aos:init',
- ':shooter_lib',
- ],
+ name = "shooter",
+ srcs = [
+ "shooter_main.cc",
+ ],
+ deps = [
+ ":shooter_lib",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ ],
)
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index f61cc02..d87c638 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -116,9 +116,10 @@
previous_offset, offset_));
}
-ShooterMotor::ShooterMotor(::y2014::control_loops::ShooterQueue *my_shooter)
+ShooterMotor::ShooterMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name)
: aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue>(
- my_shooter),
+ event_loop, name),
shooter_(MakeShooterLoop()),
state_(STATE_INITIALIZE),
cycles_not_moved_(0),
@@ -249,8 +250,7 @@
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- const bool disabled =
- !::aos::joystick_state.get() || !::aos::joystick_state->enabled;
+ const bool disabled = !has_joystick_state() || !joystick_state().enabled;
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -549,7 +549,7 @@
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
monotonic_clock::now() > shot_end_time_) &&
- ::aos::robot_state->voltage_battery > 10.5) {
+ robot_state().voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index 75a05db..a9a255b 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -129,8 +129,9 @@
class ShooterMotor
: public aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue> {
public:
- explicit ShooterMotor(::y2014::control_loops::ShooterQueue *my_shooter =
- &::y2014::control_loops::shooter_queue);
+ explicit ShooterMotor(
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2014.control_loops.shooter_queue");
// True if the goal was moved to avoid goal windup.
bool capped_goal() const { return shooter_.capped_goal(); }
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 108502b..e88a33e 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -289,6 +289,7 @@
// is no longer valid.
::y2014::control_loops::ShooterQueue shooter_queue_;
+ ::aos::ShmEventLoop event_loop_;
// Create a loop and simulation plant.
ShooterMotor shooter_motor_;
ShooterSimulation shooter_motor_plant_;
@@ -303,7 +304,7 @@
".y2014.control_loops.shooter_queue.position",
".y2014.control_loops.shooter_queue.output",
".y2014.control_loops.shooter_queue.status"),
- shooter_motor_(&shooter_queue_),
+ shooter_motor_(&event_loop_),
shooter_motor_plant_(0.2) {}
void VerifyNearGoal() {
@@ -359,6 +360,7 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
EXPECT_NEAR(
shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
pos, 0.05);
@@ -400,6 +402,7 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
EXPECT_NEAR(
shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
pos, 0.05);
@@ -438,7 +441,9 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power), pos, 0.05);
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -472,8 +477,8 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_.goal.MakeWithBuilder().shot_power(14.0).Send();
- while (::aos::monotonic_clock::now() <
- ::aos::monotonic_clock::time_point(chrono::seconds(1))) {
+ while (monotonic_clock::now() <
+ monotonic_clock::time_point(chrono::seconds(2))) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -481,6 +486,7 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
EXPECT_NEAR(
shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
pos, 0.05);
@@ -650,6 +656,7 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
EXPECT_NEAR(
shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
pos, 0.05);
@@ -671,6 +678,7 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
EXPECT_NEAR(
shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
pos, 0.05);
@@ -702,6 +710,7 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
EXPECT_NEAR(
shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
pos, 0.05);
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
index b6f4a8c..a7b60f9 100644
--- a/y2014/control_loops/shooter/shooter_main.cc
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -1,10 +1,12 @@
#include "y2014/control_loops/shooter/shooter.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2014::control_loops::ShooterMotor shooter;
+ ::aos::ShmEventLoop event_loop;
+ ::y2014::control_loops::ShooterMotor shooter(&event_loop);
shooter.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/drivetrain/BUILD b/y2014_bot3/control_loops/drivetrain/BUILD
index 53605f3..18e0e90 100644
--- a/y2014_bot3/control_loops/drivetrain/BUILD
+++ b/y2014_bot3/control_loops/drivetrain/BUILD
@@ -77,6 +77,7 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index 3a2dc6d..a76b5a3 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,10 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
+ &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/rollers/BUILD b/y2014_bot3/control_loops/rollers/BUILD
index 0ca9e1b..9535489 100644
--- a/y2014_bot3/control_loops/rollers/BUILD
+++ b/y2014_bot3/control_loops/rollers/BUILD
@@ -1,40 +1,41 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'rollers_queue',
- srcs = [
- 'rollers.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "rollers_queue",
+ srcs = [
+ "rollers.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
cc_library(
- name = 'rollers_lib',
- hdrs = [
- 'rollers.h',
- ],
- srcs = [
- 'rollers.cc',
- ],
- deps = [
- ':rollers_queue',
- '//aos/logging',
- '//aos/controls:control_loop',
- ],
+ name = "rollers_lib",
+ srcs = [
+ "rollers.cc",
+ ],
+ hdrs = [
+ "rollers.h",
+ ],
+ deps = [
+ ":rollers_queue",
+ "//aos/controls:control_loop",
+ "//aos/logging",
+ ],
)
cc_binary(
- name = 'rollers',
- srcs = [
- 'rollers_main.cc',
- ],
- deps = [
- ':rollers_lib',
- '//aos:init',
- ],
+ name = "rollers",
+ srcs = [
+ "rollers_main.cc",
+ ],
+ deps = [
+ ":rollers_lib",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ ],
)
diff --git a/y2014_bot3/control_loops/rollers/rollers.cc b/y2014_bot3/control_loops/rollers/rollers.cc
index 573a41f..d2b4da9 100644
--- a/y2014_bot3/control_loops/rollers/rollers.cc
+++ b/y2014_bot3/control_loops/rollers/rollers.cc
@@ -5,8 +5,9 @@
namespace y2014_bot3 {
namespace control_loops {
-Rollers::Rollers(control_loops::RollersQueue *rollers)
- : aos::controls::ControlLoop<control_loops::RollersQueue>(rollers) {}
+Rollers::Rollers(::aos::EventLoop *event_loop, const ::std::string &name)
+ : aos::controls::ControlLoop<control_loops::RollersQueue>(event_loop,
+ name) {}
void Rollers::RunIteration(
const control_loops::RollersQueue::Goal *goal,
diff --git a/y2014_bot3/control_loops/rollers/rollers.h b/y2014_bot3/control_loops/rollers/rollers.h
index edbea7a..9890871 100644
--- a/y2014_bot3/control_loops/rollers/rollers.h
+++ b/y2014_bot3/control_loops/rollers/rollers.h
@@ -12,8 +12,9 @@
public:
// Constructs a control loops which can take a rollers or defaults to the
// rollers at ::2014_bot3::control_loops::rollers.
- explicit Rollers(control_loops::RollersQueue *rollers_queue =
- &control_loops::rollers_queue);
+ explicit Rollers(
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2014_bot3.control_loops.rollers_queue");
protected:
// Executes one cycle of the control loop.
diff --git a/y2014_bot3/control_loops/rollers/rollers_main.cc b/y2014_bot3/control_loops/rollers/rollers_main.cc
index b2bb7c9..d783d98 100644
--- a/y2014_bot3/control_loops/rollers/rollers_main.cc
+++ b/y2014_bot3/control_loops/rollers/rollers_main.cc
@@ -1,10 +1,12 @@
#include "y2014_bot3/control_loops/rollers/rollers.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2014_bot3::control_loops::Rollers rollers;
+ ::aos::ShmEventLoop event_loop;
+ ::y2014_bot3::control_loops::Rollers rollers(&event_loop);
rollers.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index f461581..e74ddea 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,9 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index 4f62c7a..8938a2b 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -1,87 +1,88 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'shooter_queue',
- srcs = [
- 'shooter.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "shooter_queue",
+ srcs = [
+ "shooter.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
genrule(
- name = 'genrule_shooter',
- visibility = ['//visibility:private'],
- cmd = '$(location //y2016/control_loops/python:shooter) $(OUTS)',
- tools = [
- '//y2016/control_loops/python:shooter',
- ],
- outs = [
- 'shooter_plant.h',
- 'shooter_plant.cc',
- 'shooter_integral_plant.h',
- 'shooter_integral_plant.cc',
- ],
+ name = "genrule_shooter",
+ outs = [
+ "shooter_plant.h",
+ "shooter_plant.cc",
+ "shooter_integral_plant.h",
+ "shooter_integral_plant.cc",
+ ],
+ cmd = "$(location //y2016/control_loops/python:shooter) $(OUTS)",
+ tools = [
+ "//y2016/control_loops/python:shooter",
+ ],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'shooter_plants',
- srcs = [
- 'shooter_plant.cc',
- 'shooter_integral_plant.cc',
- ],
- hdrs = [
- 'shooter_plant.h',
- 'shooter_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "shooter_plants",
+ srcs = [
+ "shooter_integral_plant.cc",
+ "shooter_plant.cc",
+ ],
+ hdrs = [
+ "shooter_integral_plant.h",
+ "shooter_plant.h",
+ ],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'shooter_lib',
- srcs = [
- 'shooter.cc',
- ],
- hdrs = [
- 'shooter.h',
- ],
- deps = [
- ':shooter_queue',
- ':shooter_plants',
- '//aos/controls:control_loop',
- ],
+ name = "shooter_lib",
+ srcs = [
+ "shooter.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ ],
+ deps = [
+ ":shooter_plants",
+ ":shooter_queue",
+ "//aos/controls:control_loop",
+ ],
)
cc_test(
- name = 'shooter_lib_test',
- srcs = [
- 'shooter_lib_test.cc',
- ],
- deps = [
- ':shooter_queue',
- ':shooter_lib',
- '//aos/testing:googletest',
- '//aos:queues',
- '//aos/controls:control_loop_test',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:team_number_test_environment',
- ],
+ name = "shooter_lib_test",
+ srcs = [
+ "shooter_lib_test.cc",
+ ],
+ deps = [
+ ":shooter_lib",
+ ":shooter_queue",
+ "//aos:queues",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/control_loops:team_number_test_environment",
+ ],
)
cc_binary(
- name = 'shooter',
- srcs = [
- 'shooter_main.cc',
- ],
- deps = [
- '//aos:init',
- ':shooter_lib',
- ':shooter_queue',
- ],
+ name = "shooter",
+ srcs = [
+ "shooter_main.cc",
+ ],
+ deps = [
+ ":shooter_lib",
+ ":shooter_queue",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ ],
)
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index a047f55..6ae7f0e 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -72,8 +72,8 @@
loop_->next_R(1, 0) > 1.0);
}
-Shooter::Shooter(ShooterQueue *my_shooter)
- : aos::controls::ControlLoop<ShooterQueue>(my_shooter),
+Shooter::Shooter(::aos::EventLoop *event_loop, const ::std::string &name)
+ : aos::controls::ControlLoop<ShooterQueue>(event_loop, name),
shots_(0),
last_pre_shot_timeout_(::aos::monotonic_clock::min_time) {}
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index ac5b2d7..9e6968f 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -4,11 +4,11 @@
#include <memory>
#include "aos/controls/control_loop.h"
+#include "aos/events/event-loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2016/control_loops/shooter/shooter_integral_plant.h"
#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter_integral_plant.h"
namespace y2016 {
namespace control_loops {
@@ -56,7 +56,8 @@
class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
public:
explicit Shooter(
- ShooterQueue *shooter_queue = &control_loops::shooter::shooter_queue);
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2016.control_loops.shooter.shooter_queue");
enum class ShooterLatchState {
// Any shoot commands will be passed through without modification.
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index bba603f..3c8be30 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -53,11 +53,11 @@
::y2016::control_loops::shooter::MakeShooterPlant())),
shooter_plant_right_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
- shooter_queue_(".y2016.control_loops.shooter",
- ".y2016.control_loops.shooter.goal",
- ".y2016.control_loops.shooter.position",
- ".y2016.control_loops.shooter.output",
- ".y2016.control_loops.shooter.status") {}
+ shooter_queue_(".y2016.control_loops.shooter.shooter_queue",
+ ".y2016.control_loops.shooter.shooter_queue.goal",
+ ".y2016.control_loops.shooter.shooter_queue.position",
+ ".y2016.control_loops.shooter.shooter_queue.output",
+ ".y2016.control_loops.shooter.shooter_queue.status") {}
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
@@ -101,12 +101,12 @@
class ShooterTest : public ::aos::testing::ControlLoopTest {
protected:
ShooterTest()
- : shooter_queue_(".y2016.control_loops.shooter",
- ".y2016.control_loops.shooter.goal",
- ".y2016.control_loops.shooter.position",
- ".y2016.control_loops.shooter.output",
- ".y2016.control_loops.shooter.status"),
- shooter_(&shooter_queue_),
+ : shooter_queue_(".y2016.control_loops.shooter.shooter_queue",
+ ".y2016.control_loops.shooter.shooter_queue.goal",
+ ".y2016.control_loops.shooter.shooter_queue.position",
+ ".y2016.control_loops.shooter.shooter_queue.output",
+ ".y2016.control_loops.shooter.shooter_queue.status"),
+ shooter_(&event_loop_),
shooter_plant_() {
set_team_id(kTeamNumber);
}
@@ -153,6 +153,7 @@
// is no longer valid.
ShooterQueue shooter_queue_;
+ ::aos::ShmEventLoop event_loop_;
// Create a control loop and simulation.
Shooter shooter_;
ShooterSimulation shooter_plant_;
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index 2e1d875..8070dde 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -1,10 +1,12 @@
#include "y2016/control_loops/shooter/shooter.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2016::control_loops::shooter::Shooter shooter;
+ ::aos::ShmEventLoop event_loop;
+ ::y2016::control_loops::shooter::Shooter shooter(&event_loop);
shooter.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 7f08651..6a14a5f 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -1,118 +1,119 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'superstructure_queue',
- srcs = [
- 'superstructure.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:queues',
- ],
+ name = "superstructure_queue",
+ srcs = [
+ "superstructure.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
genrule(
- name = 'genrule_intake',
- visibility = ['//visibility:private'],
- cmd = '$(location //y2016/control_loops/python:intake) $(OUTS)',
- tools = [
- '//y2016/control_loops/python:intake',
- ],
- outs = [
- 'intake_plant.h',
- 'intake_plant.cc',
- 'integral_intake_plant.h',
- 'integral_intake_plant.cc',
- ],
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "integral_intake_plant.h",
+ "integral_intake_plant.cc",
+ ],
+ cmd = "$(location //y2016/control_loops/python:intake) $(OUTS)",
+ tools = [
+ "//y2016/control_loops/python:intake",
+ ],
+ visibility = ["//visibility:private"],
)
genrule(
- name = 'genrule_arm',
- visibility = ['//visibility:private'],
- cmd = '$(location //y2016/control_loops/python:arm) $(OUTS)',
- tools = [
- '//y2016/control_loops/python:arm',
- ],
- outs = [
- 'arm_plant.h',
- 'arm_plant.cc',
- 'integral_arm_plant.h',
- 'integral_arm_plant.cc',
- ],
+ name = "genrule_arm",
+ outs = [
+ "arm_plant.h",
+ "arm_plant.cc",
+ "integral_arm_plant.h",
+ "integral_arm_plant.cc",
+ ],
+ cmd = "$(location //y2016/control_loops/python:arm) $(OUTS)",
+ tools = [
+ "//y2016/control_loops/python:arm",
+ ],
+ visibility = ["//visibility:private"],
)
cc_library(
- name = 'superstructure_plants',
- srcs = [
- 'intake_plant.cc',
- 'arm_plant.cc',
- 'integral_intake_plant.cc',
- 'integral_arm_plant.cc',
- ],
- hdrs = [
- 'intake_plant.h',
- 'arm_plant.h',
- 'integral_intake_plant.h',
- 'integral_arm_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "superstructure_plants",
+ srcs = [
+ "arm_plant.cc",
+ "intake_plant.cc",
+ "integral_arm_plant.cc",
+ "integral_intake_plant.cc",
+ ],
+ hdrs = [
+ "arm_plant.h",
+ "intake_plant.h",
+ "integral_arm_plant.h",
+ "integral_intake_plant.h",
+ ],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'superstructure_lib',
- srcs = [
- 'superstructure.cc',
- 'superstructure_controls.cc',
- ],
- hdrs = [
- 'superstructure.h',
- 'superstructure_controls.h',
- ],
- deps = [
- ':superstructure_queue',
- ':superstructure_plants',
- '//aos/controls:control_loop',
- '//aos/util:trapezoid_profile',
- '//aos:math',
- '//frc971/control_loops:profiled_subsystem',
- '//frc971/control_loops:simple_capped_state_feedback_loop',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/zeroing',
- '//y2016/queues:ball_detector',
- '//y2016:constants',
- ],
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ "superstructure_controls.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ "superstructure_controls.h",
+ ],
+ deps = [
+ ":superstructure_plants",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//aos/util:trapezoid_profile",
+ "//frc971/control_loops:profiled_subsystem",
+ "//frc971/control_loops:simple_capped_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/zeroing",
+ "//y2016:constants",
+ "//y2016/queues:ball_detector",
+ ],
)
cc_test(
- name = 'superstructure_lib_test',
- srcs = [
- 'superstructure_lib_test.cc',
- ],
- deps = [
- ':superstructure_queue',
- ':superstructure_lib',
- '//aos/testing:googletest',
- '//aos:queues',
- '//aos/controls:control_loop_test',
- '//aos:math',
- '//aos/time:time',
- '//frc971/control_loops:position_sensor_sim',
- '//frc971/control_loops:team_number_test_environment',
- ],
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos:queues",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ ],
)
cc_binary(
- name = 'superstructure',
- srcs = [
- 'superstructure_main.cc',
- ],
- deps = [
- '//aos:init',
- ':superstructure_lib',
- ':superstructure_queue',
- ],
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ ],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 1bcac52..984bcc3 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -225,10 +225,10 @@
constexpr double CollisionAvoidance::kMaxWristAngleForSafeArmStowing;
constexpr double CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing;
-Superstructure::Superstructure(
- control_loops::SuperstructureQueue *superstructure_queue)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
- superstructure_queue),
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
+ name),
collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 17a1547..7215067 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -107,8 +107,8 @@
: public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
public:
explicit Superstructure(
- control_loops::SuperstructureQueue *my_superstructure =
- &control_loops::superstructure_queue);
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2016.control_loops.superstructure_queue");
static constexpr double kZeroingVoltage = 6.0;
static constexpr double kShooterHangingVoltage = 6.0;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index e4d1301..d0c4362 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -88,11 +88,11 @@
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
- superstructure_queue_(".y2016.control_loops.superstructure",
- ".y2016.control_loops.superstructure.goal",
- ".y2016.control_loops.superstructure.status",
- ".y2016.control_loops.superstructure.output",
- ".y2016.control_loops.superstructure.status") {
+ superstructure_queue_(".y2016.control_loops.superstructure_queue",
+ ".y2016.control_loops.superstructure_queue.goal",
+ ".y2016.control_loops.superstructure_queue.position",
+ ".y2016.control_loops.superstructure_queue.output",
+ ".y2016.control_loops.superstructure_queue.status") {
InitializeIntakePosition(0.0);
InitializeShoulderPosition(0.0);
InitializeRelativeWristPosition(0.0);
@@ -246,12 +246,12 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : superstructure_queue_(".y2016.control_loops.superstructure",
- ".y2016.control_loops.superstructure.goal",
- ".y2016.control_loops.superstructure.status",
- ".y2016.control_loops.superstructure.output",
- ".y2016.control_loops.superstructure.status"),
- superstructure_(&superstructure_queue_),
+ : superstructure_queue_(".y2016.control_loops.superstructure_queue",
+ ".y2016.control_loops.superstructure_queue.goal",
+ ".y2016.control_loops.superstructure_queue.position",
+ ".y2016.control_loops.superstructure_queue.output",
+ ".y2016.control_loops.superstructure_queue.status"),
+ superstructure_(&event_loop_),
superstructure_plant_() {}
void VerifyNearGoal() {
@@ -375,6 +375,7 @@
// shared memory that is no longer valid.
SuperstructureQueue superstructure_queue_;
+ ::aos::ShmEventLoop event_loop_;
// Create a control loop and simulation.
Superstructure superstructure_;
SuperstructureSimulation superstructure_plant_;
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index a08074d..435724b 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -1,10 +1,13 @@
#include "y2016/control_loops/superstructure/superstructure.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2016::control_loops::superstructure::Superstructure superstructure;
+ ::aos::ShmEventLoop event_loop;
+ ::y2016::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
superstructure.Run();
::aos::Cleanup();
return 0;
diff --git a/y2017/control_loops/drivetrain/BUILD b/y2017/control_loops/drivetrain/BUILD
index 2eb2a0f..409fc93 100644
--- a/y2017/control_loops/drivetrain/BUILD
+++ b/y2017/control_loops/drivetrain/BUILD
@@ -78,6 +78,7 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
index 4507a4a..ffe479a 100644
--- a/y2017/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,9 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 732c6dc..e7e6f73 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -65,6 +65,7 @@
":superstructure_lib",
":superstructure_queue",
"//aos:init",
+ "//aos/events:shm-event-loop",
],
)
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 054fab4..730cd24 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -23,10 +23,10 @@
typedef ::y2017::constants::Values::ShotParams ShotParams;
using ::frc971::control_loops::drivetrain_queue;
-Superstructure::Superstructure(
- control_loops::SuperstructureQueue *superstructure_queue)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
- superstructure_queue) {
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
+ name) {
shot_interpolation_table_ =
::frc971::shooter_interpolation::InterpolationTable<ShotParams>({
// { distance_to_target, { shot_angle, shot_power, indexer_velocity }},
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 399e166..580b6a5 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -20,8 +20,8 @@
: public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
public:
explicit Superstructure(
- control_loops::SuperstructureQueue *my_superstructure =
- &control_loops::superstructure_queue);
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2017.control_loops.superstructure_queue");
const hood::Hood &hood() const { return hood_; }
const intake::Intake &intake() const { return intake_; }
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index de47690..c8f55ff 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -134,11 +134,12 @@
column_plant_(new ColumnPlant(
::y2017::control_loops::superstructure::column::MakeColumnPlant())),
- superstructure_queue_(".y2017.control_loops.superstructure",
- ".y2017.control_loops.superstructure.goal",
- ".y2017.control_loops.superstructure.position",
- ".y2017.control_loops.superstructure.output",
- ".y2017.control_loops.superstructure.status") {
+ superstructure_queue_(
+ ".y2017.control_loops.superstructure_queue",
+ ".y2017.control_loops.superstructure_queue.goal",
+ ".y2017.control_loops.superstructure_queue.position",
+ ".y2017.control_loops.superstructure_queue.output",
+ ".y2017.control_loops.superstructure_queue.status") {
// Start the hood out in the middle by default.
InitializeHoodPosition((constants::Values::kHoodRange.lower +
constants::Values::kHoodRange.upper) /
@@ -432,12 +433,13 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : superstructure_queue_(".y2017.control_loops.superstructure",
- ".y2017.control_loops.superstructure.goal",
- ".y2017.control_loops.superstructure.position",
- ".y2017.control_loops.superstructure.output",
- ".y2017.control_loops.superstructure.status"),
- superstructure_(&superstructure_queue_) {
+ : superstructure_queue_(
+ ".y2017.control_loops.superstructure_queue",
+ ".y2017.control_loops.superstructure_queue.goal",
+ ".y2017.control_loops.superstructure_queue.position",
+ ".y2017.control_loops.superstructure_queue.output",
+ ".y2017.control_loops.superstructure_queue.status"),
+ superstructure_(&event_loop_) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
@@ -562,6 +564,7 @@
// that is no longer valid.
SuperstructureQueue superstructure_queue_;
+ ::aos::ShmEventLoop event_loop_;
// Create a control loop and simulation.
Superstructure superstructure_;
SuperstructureSimulation superstructure_plant_;
diff --git a/y2017/control_loops/superstructure/superstructure_main.cc b/y2017/control_loops/superstructure/superstructure_main.cc
index 6ab834b..6ebf91a 100644
--- a/y2017/control_loops/superstructure/superstructure_main.cc
+++ b/y2017/control_loops/superstructure/superstructure_main.cc
@@ -1,10 +1,13 @@
#include "y2017/control_loops/superstructure/superstructure.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2017::control_loops::superstructure::Superstructure superstructure;
+ ::aos::ShmEventLoop event_loop;
+ ::y2017::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
superstructure.Run();
::aos::Cleanup();
return 0;
diff --git a/y2017_bot3/control_loops/drivetrain/BUILD b/y2017_bot3/control_loops/drivetrain/BUILD
index 679e4bf..08aaab2 100644
--- a/y2017_bot3/control_loops/drivetrain/BUILD
+++ b/y2017_bot3/control_loops/drivetrain/BUILD
@@ -77,6 +77,7 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
index f24b949..ac581eb 100644
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,10 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
+ &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2017_bot3/control_loops/superstructure/BUILD b/y2017_bot3/control_loops/superstructure/BUILD
index 17cfc20..9c9f750 100644
--- a/y2017_bot3/control_loops/superstructure/BUILD
+++ b/y2017_bot3/control_loops/superstructure/BUILD
@@ -1,47 +1,48 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'superstructure_queue',
- srcs = [
- 'superstructure.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:profiled_subsystem_queue',
- '//frc971/control_loops:queues',
- ],
+ name = "superstructure_queue",
+ srcs = [
+ "superstructure.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:profiled_subsystem_queue",
+ "//frc971/control_loops:queues",
+ ],
)
cc_library(
- name = 'superstructure_lib',
- srcs = [
- 'superstructure.cc',
- ],
- hdrs = [
- 'superstructure.h',
- ],
- deps = [
- ':superstructure_queue',
- '//aos/controls:control_loop',
- '//aos/util:trapezoid_profile',
- '//aos:math',
- '//frc971/control_loops:profiled_subsystem',
- '//frc971/control_loops:simple_capped_state_feedback_loop',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/zeroing',
- ],
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//aos/util:trapezoid_profile",
+ "//frc971/control_loops:profiled_subsystem",
+ "//frc971/control_loops:simple_capped_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/zeroing",
+ ],
)
cc_binary(
- name = 'superstructure',
- srcs = [
- 'superstructure_main.cc',
- ],
- deps = [
- '//aos:init',
- ':superstructure_lib',
- ':superstructure_queue',
- ],
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ ],
)
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.cc b/y2017_bot3/control_loops/superstructure/superstructure.cc
index 1b91c69..e75ffed 100644
--- a/y2017_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2017_bot3/control_loops/superstructure/superstructure.cc
@@ -8,10 +8,10 @@
namespace control_loops {
namespace superstructure {
-Superstructure::Superstructure(
- control_loops::SuperstructureQueue *superstructure_queue)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
- superstructure_queue) {}
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
+ name) {}
void Superstructure::RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.h b/y2017_bot3/control_loops/superstructure/superstructure.h
index 99a75e3..a9378eb 100644
--- a/y2017_bot3/control_loops/superstructure/superstructure.h
+++ b/y2017_bot3/control_loops/superstructure/superstructure.h
@@ -16,9 +16,9 @@
class Superstructure
: public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
public:
- explicit Superstructure(
- control_loops::SuperstructureQueue *my_superstructure =
- &control_loops::superstructure_queue);
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name =
+ ".y2017_bot3.control_loops.superstructure_queue");
static constexpr double kOperatingVoltage = 12.0;
diff --git a/y2017_bot3/control_loops/superstructure/superstructure_main.cc b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
index 378b498..38df40e 100644
--- a/y2017_bot3/control_loops/superstructure/superstructure_main.cc
+++ b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
@@ -4,7 +4,9 @@
int main() {
::aos::Init();
- ::y2017_bot3::control_loops::superstructure::Superstructure superstructure;
+ ::aos::ShmEventLoop event_loop;
+ ::y2017_bot3::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
superstructure.Run();
::aos::Cleanup();
return 0;
diff --git a/y2018/control_loops/drivetrain/drivetrain_main.cc b/y2018/control_loops/drivetrain/drivetrain_main.cc
index 15d99be..f0c7f0e 100644
--- a/y2018/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,9 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2018::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index c2d0bcf..9474b6d 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -35,10 +35,10 @@
}
}
-Superstructure::Superstructure(
- control_loops::SuperstructureQueue *superstructure_queue)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
- superstructure_queue),
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
+ name),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 4f8f4d5..3d1a6af 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -17,8 +17,8 @@
: public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
public:
explicit Superstructure(
- control_loops::SuperstructureQueue *my_superstructure =
- &control_loops::superstructure_queue);
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name = ".y2018.control_loops.superstructure_queue");
const intake::IntakeSide &intake_left() const { return intake_left_; }
const intake::IntakeSide &intake_right() const { return intake_right_; }
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 0856a7d..89cfaa0 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -198,9 +198,9 @@
constants::GetValues().arm_distal.zeroing),
superstructure_queue_(".y2018.control_loops.superstructure",
".y2018.control_loops.superstructure.goal",
- ".y2018.control_loops.superstructure.position",
".y2018.control_loops.superstructure.output",
- ".y2018.control_loops.superstructure.status") {
+ ".y2018.control_loops.superstructure.status",
+ ".y2018.control_loops.superstructure.position") {
// Start the intake out in the middle by default.
InitializeIntakePosition((constants::Values::kIntakeRange().lower +
constants::Values::kIntakeRange().upper) /
@@ -245,8 +245,8 @@
// Simulates the intake for a single timestep.
void Simulate() {
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
- EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+ ASSERT_TRUE(superstructure_queue_.output.FetchLatest());
+ ASSERT_TRUE(superstructure_queue_.status.FetchLatest());
left_intake_.Simulate(superstructure_queue_.output->left_intake);
right_intake_.Simulate(superstructure_queue_.output->right_intake);
@@ -270,10 +270,10 @@
SuperstructureTest()
: superstructure_queue_(".y2018.control_loops.superstructure",
".y2018.control_loops.superstructure.goal",
- ".y2018.control_loops.superstructure.position",
".y2018.control_loops.superstructure.output",
- ".y2018.control_loops.superstructure.status"),
- superstructure_(&superstructure_queue_) {
+ ".y2018.control_loops.superstructure.status",
+ ".y2018.control_loops.superstructure.position"),
+ superstructure_(&event_loop_, ".y2018.control_loops.superstructure") {
status_light.Clear();
::y2018::vision::vision_status.Clear();
::frc971::control_loops::drivetrain_queue.output.Clear();
@@ -323,6 +323,7 @@
}
}
+ ::aos::ShmEventLoop event_loop_;
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory
// that is no longer valid.
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
index fca04e8..f5b026c 100644
--- a/y2018/control_loops/superstructure/superstructure_main.cc
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -1,10 +1,13 @@
#include "y2018/control_loops/superstructure/superstructure.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::y2018::control_loops::superstructure::Superstructure superstructure;
+ ::aos::ShmEventLoop event_loop;
+ ::y2018::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
::aos::GoRT();
superstructure.Run();
::aos::Cleanup();
diff --git a/y2018_bot3/control_loops/drivetrain/BUILD b/y2018_bot3/control_loops/drivetrain/BUILD
index d3fabe1..62484c0 100644
--- a/y2018_bot3/control_loops/drivetrain/BUILD
+++ b/y2018_bot3/control_loops/drivetrain/BUILD
@@ -74,6 +74,7 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
index cd91f7c..e481f6c 100644
--- a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2018_bot3/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,10 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
+ &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2019/BUILD b/y2019/BUILD
index 52c0264..7c987f6 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -16,8 +16,46 @@
],
)
+cc_binary(
+ name = "wpilib_interface",
+ srcs = [
+ "wpilib_interface.cc",
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ "//aos:init",
+ "//aos:make_unique",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//aos/logging",
+ "//aos/logging:queue_logging",
+ "//aos/robot_state",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//aos/util:phased_loop",
+ "//aos/util:wrapping_counter",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/control_loops:queues",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/wpilib:ADIS16448",
+ "//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:encoder_and_potentiometer",
+ "//frc971/wpilib:interrupt_edge_counting",
+ "//frc971/wpilib:joystick_sender",
+ "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:loop_output_handler",
+ "//frc971/wpilib:pdp_fetcher",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:wpilib_interface",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//third_party:wpilib",
+ "//third_party/Phoenix-frc-lib:phoenix",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
visibility = ["//visibility:public"],
-)
\ No newline at end of file
+)
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 4d7e616..959e839 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -76,6 +76,25 @@
deps = [
":drivetrain_base",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
+
+cc_library(
+ name = "camera",
+ srcs = ["camera.h"],
+ deps = [
+ "//aos/containers:sized_array",
+ "//frc971/control_loops:pose",
+ ],
+)
+
+cc_test(
+ name = "camera_test",
+ srcs = ["camera_test.cc"],
+ deps = [
+ ":camera",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
new file mode 100644
index 0000000..7135e8b
--- /dev/null
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -0,0 +1,289 @@
+#ifndef Y2019_CONTROL_LOOPS_DRIVETRAIN_CAMERA_H_
+#define Y2019_CONTROL_LOOPS_DRIVETRAIN_CAMERA_H_
+
+#include <vector>
+
+#include "aos/containers/sized_array.h"
+#include "frc971/control_loops/pose.h"
+
+namespace y2019 {
+namespace control_loops {
+
+// Represents a target on the field. Currently just consists of a pose and a
+// indicator for whether it is occluded (occlusion is only used by the simulator
+// for testing).
+// Orientation convention:
+// -The absolute position of the pose is the center of the vision target on the
+// field.
+// -The yaw of the pose shall be such that the positive X-axis in the Target's
+// frame wil be pointed straight through the target--i.e., if you are looking
+// at the target head-on, then you will be facing in the same direction as the
+// positive X-axis.
+// E.g., if the Target has a global position of (1, 1, 0) and yaw of pi / 2,
+// then it is at position (1, 1, 0) on the field and is oriented so that if
+// someone were to stand at (1, 0, 0) and turn themselves to have a yaw of
+// pi / 2, they would see the target 1 meter straight ahead of them.
+//
+// Generally, the position of a target should not change dynamically; if we do
+// start having targets that move, we may want to start optimizing certain
+// things (e.g., caching the position of the Target--currently, if the Pose of a
+// target is in an absolute frame, then calling abs_pos will be inexpensive; if
+// that changes, then we start having to recalculate transformations on every
+// frame).
+template <typename Scalar = double>
+class TypedTarget {
+ public:
+ typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
+ TypedTarget(const Pose &pose) : pose_(pose) {}
+ Pose pose() const { return pose_; }
+
+ bool occluded() const { return occluded_; }
+ void set_occluded(bool occluded) { occluded_ = occluded; }
+
+ // Get a list of points for plotting. These points should be plotted on
+ // an x/y plane in the global frame with lines connecting the points.
+ // Essentially, this provides a Polygon that is a reasonable representation
+ // of a Target.
+ // This should not be called from real-time code, as it will probably
+ // dynamically allocate memory.
+ ::std::vector<Pose> PlotPoints() const {
+ // For the actual representation, we will use a triangle such that the
+ // base of the triangle corresponds to the surface the target is on.
+ // The third point is shown behind the target, so that the user can
+ // visually identify which side of the target is the front.
+ Pose base1(&pose_, {0, 0.125, 0}, 0);
+ Pose base2(&pose_, {0, -0.125, 0}, 0);
+ Pose behind(&pose_, {0.05, 0, 0}, 0);
+ // Include behind at the start and end to indicate that we want to draw
+ // a closed polygon.
+ return {behind, base1, base2, behind};
+ }
+
+ private:
+ Pose pose_;
+ bool occluded_ = false;
+}; // class TypedTarget
+
+typedef TypedTarget<double> Target;
+
+// Represents a camera that can see targets and provide information about their
+// relative positions.
+// Note on coordinate systems:
+// -The camera's Pose shall be such that the camera frame's positive X-axis is
+// pointed straight out of the lens (as always, positive Z will be up; we
+// assume that all cameras mounted level, or compensated for such that this
+// code won't care).
+// -The origin of the camera shall be "at" the camera. For this code, I don't
+// think we care too much about the details of the camera model, so we can just
+// assume that it is an idealized pinhole camera with the pinhole being the
+// location of the camera.
+//
+// Template parameters:
+// -num_targets: The number of targets on the field, should be the same for
+// all the actual cameras on the robot (although it may change in tests).
+// -Scalar: The floating point type to use (double vs. float).
+// -num_obstacles: The number of obstacles on the field to account for; like
+// the number of targets, it should be consistent across actual cameras,
+// although for simulation we may add extra obstacles for testing.
+template <int num_targets, int num_obstacles, typename Scalar = double>
+class TypedCamera {
+ public:
+ typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
+ typedef ::frc971::control_loops::TypedLineSegment<Scalar> LineSegment;
+
+ // TargetView contains the information associated with a sensor reading
+ // from the camera--the readings themselves and noise values, *from the
+ // Camera's persective* for each reading.
+ // Note that the noise terms are just accounting for the inaccuracy you
+ // expect to get due to visual noise, pixel-level resolution, etc. These
+ // do not account for the fact that, e.g., there is noise in the Pose of the
+ // robot which can translate into noise in the target reading.
+ // The noise terms are standard deviations, and so have units identical
+ // to that of the actual readings.
+ struct TargetView {
+ struct Reading {
+ // The heading as reported from the camera; zero = straight ahead,
+ // positive = target in the left half of the image.
+ Scalar heading; // radians
+ // The distance from the camera to the target.
+ Scalar distance; // meters
+ // Height of the target from the floor.
+ Scalar height; // meters
+ // The angle of the target relative to the frame of the camera.
+ Scalar skew; // radians
+ };
+ Reading reading;
+ Reading noise;
+
+ // The target that this view corresponds to.
+ const TypedTarget<Scalar> *target;
+ };
+
+ // Important parameters for dealing with camera noise calculations.
+ // Ultimately, this should end up coming from the constants file.
+ struct NoiseParameters {
+ // The maximum distance from which we can see a target head-on (when the
+ // target is not head-on, we adjust for that).
+ Scalar max_viewable_distance; // meters
+
+ // All noises are standard deviations of the noise, assuming an ~normal
+ // distribution.
+
+ // Noise in the heading measurement, which should be constant regardless of
+ // other factors.
+ Scalar heading_noise; // radians
+ // Noise in the distance measurement when the target is 1m away and head-on
+ // to us. This is adjusted by assuming the noise is proportional to the
+ // apparent width of the target (because the target also has height, this
+ // may not be strictly correct).
+ // TODO(james): Is this a good model? It should be reasonable, but there
+ // may be more complexity somewhere.
+ Scalar nominal_distance_noise; // meters
+ // The noise in the skew measurement when the target is 1m away and head-on
+ // to us. Calculated in the same manner with the same caveats as the
+ // distance noise.
+ Scalar nominal_skew_noise; // radians
+ // Noise in the height measurement, same rules as for skew and distance.
+ // TODO(james): Figure out how much noise we will actually get in the
+ // height, since there will be extremely low resolution on it.
+ Scalar nominal_height_noise; // meters
+ };
+
+ // Creates a camera:
+ // pose: The Pose of the camera, relative to the robot at least transitively.
+ // fov: The field-of-view of the camera, in radians. Note that this is the
+ // *total* field-of-view in the horizontal plane (left-right), so the angle
+ // from the left edge of the image to the right edge.
+ // targets: The list of targets on the field that could be seen by the camera.
+ // obstacles: The list of known obstacles on the field.
+ TypedCamera(const Pose &pose, Scalar fov,
+ const NoiseParameters &noise_parameters,
+ const ::std::array<TypedTarget<Scalar>, num_targets> &targets,
+ const ::std::array<LineSegment, num_obstacles> &obstacles)
+ : pose_(pose),
+ fov_(fov),
+ noise_parameters_(noise_parameters),
+ targets_(targets),
+ obstacles_(obstacles) {}
+
+ // Returns a list of TargetViews for all the currently visible targets.
+ // These will contain ground-truth TargetViews, so the readings will be
+ // perfect; a pseudo-random number generator should be used to add noise
+ // separately for simulation.
+ ::aos::SizedArray<TargetView, num_targets> target_views() const {
+ ::aos::SizedArray<TargetView, num_targets> views;
+ // Because there are num_targets in targets_ and because AddTargetIfVisible
+ // adds at most 1 view to views, we should never exceed the size of
+ // SizedArray.
+ for (const auto &target : targets_) {
+ AddTargetIfVisible(target, &views);
+ }
+ return views;
+ }
+
+ // Returns a list of list of points for plotting. Each list of points should
+ // be plotted as a line; currently, each list is just a pair drawing a line
+ // from the camera aperture to the target location.
+ // This should not be called from real-time code, as it will probably
+ // dynamically allocate memory.
+ ::std::vector<::std::vector<Pose>> PlotPoints() const {
+ ::std::vector<::std::vector<Pose>> list_of_lists;
+ for (const auto &view : target_views()) {
+ list_of_lists.push_back({pose_, view.target.pose()});
+ }
+ return list_of_lists;
+ }
+
+ const Pose pose() const { return pose_; }
+ Scalar fov() const { return fov_; }
+
+ private:
+
+ // If the specified target is visible from the current camera Pose, adds it to
+ // the views array.
+ void AddTargetIfVisible(
+ const TypedTarget<Scalar> &target,
+ ::aos::SizedArray<TargetView, num_targets> *views) const;
+
+ // The Pose of this camera.
+ const Pose pose_;
+ // Field of view of the camera, in radians.
+ const Scalar fov_;
+
+ // Various constants to calclate sensor noise; see definition of
+ // NoiseParameters for more detail.
+ const NoiseParameters noise_parameters_;
+
+ // A list of all the targets on the field.
+ const ::std::array<TypedTarget<Scalar>, num_targets> targets_;
+ // Known obstacles on the field which can interfere with our view of the
+ // targets. An "obstacle" is a line segment which we cannot see through, as
+ // such a logical obstacle (e.g., the cargo ship) may consist of many
+ // obstacles in this list to account for all of its sides.
+ const ::std::array<LineSegment, num_obstacles> obstacles_;
+}; // class TypedCamera
+
+template <int num_targets, int num_obstacles, typename Scalar>
+void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
+ const TypedTarget<Scalar> &target,
+ ::aos::SizedArray<TargetView, num_targets> *views) const {
+ if (target.occluded()) {
+ return;
+ }
+
+ // Precompute the current absolute pose of the camera, because we will reuse
+ // it a bunch.
+ const Pose camera_abs_pose = pose_.Rebase(nullptr);
+
+ const Pose relative_pose = target.pose().Rebase(&camera_abs_pose);
+ const Scalar heading = relative_pose.heading();
+ const Scalar distance = relative_pose.xy_norm();
+ const Scalar skew = ::aos::math::NormalizeAngle(relative_pose.rel_theta());
+
+ // Check if the camera is in the angular FOV.
+ if (::std::abs(heading) > fov_ / 2.0) {
+ return;
+ }
+
+ // Calculate the width of the target as it appears in the image.
+ // This number is unitless and if greater than 1, implies that the target is
+ // visible to the camera and if less than 1 implies it is too small to be
+ // registered on the camera.
+ const Scalar apparent_width =
+ ::std::max(0.0, ::std::cos(skew) *
+ noise_parameters_.max_viewable_distance / distance);
+
+ if (apparent_width < 1.0) {
+ return;
+ }
+
+ // Final visibility check is for whether there are any obstacles blocking or
+ // line of sight.
+ for (const auto &obstacle : obstacles_) {
+ if (obstacle.Intersects({camera_abs_pose, target.pose()})) {
+ return;
+ }
+ }
+
+ Scalar normalized_width =
+ apparent_width / noise_parameters_.max_viewable_distance;
+ Scalar distance_noise =
+ noise_parameters_.nominal_distance_noise / normalized_width;
+ Scalar skew_noise = noise_parameters_.nominal_skew_noise / normalized_width;
+ Scalar height_noise =
+ noise_parameters_.nominal_height_noise / normalized_width;
+
+ // If we've made it here, the target is visible to the camera and we should
+ // provide the actual TargetView in question.
+ TargetView view;
+ view.reading = {heading, distance, target.pose().abs_pos().z(), skew};
+ view.noise = {noise_parameters_.heading_noise, distance_noise, height_noise,
+ skew_noise};
+ view.target = ⌖
+ views->push_back(view);
+}
+
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_DRIVETRAIN_CAMERA_H_
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
new file mode 100644
index 0000000..9a73ccf
--- /dev/null
+++ b/y2019/control_loops/drivetrain/camera_test.cc
@@ -0,0 +1,166 @@
+#include "y2019/control_loops/drivetrain/camera.h"
+
+#include "gtest/gtest.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace testing {
+
+// Check that a Target's basic operations work.
+TEST(TargetTest, BasicTargetTest) {
+ Target target({{1, 2, 3}, M_PI / 2.0});
+
+ EXPECT_EQ(1.0, target.pose().abs_pos().x());
+ EXPECT_EQ(2.0, target.pose().abs_pos().y());
+ EXPECT_EQ(3.0, target.pose().abs_pos().z());
+ EXPECT_EQ(M_PI / 2.0, target.pose().abs_theta());
+
+ EXPECT_FALSE(target.occluded());
+ target.set_occluded(true);
+ EXPECT_TRUE(target.occluded());
+
+ ::std::vector<Target::Pose> plot_pts = target.PlotPoints();
+ ASSERT_EQ(4, plot_pts.size());
+ for (const Target::Pose &pt : plot_pts) {
+ EXPECT_EQ(3.0, pt.abs_pos().z());
+ EXPECT_EQ(M_PI / 2.0, pt.abs_theta());
+ // We don't particularly care about the plot point details, just check that
+ // they are all roughly in the right vicinity:
+ EXPECT_LT((pt.abs_pos() - target.pose().abs_pos()).norm(), 0.25);
+ }
+ EXPECT_EQ(plot_pts[0].abs_pos(), plot_pts[3].abs_pos());
+}
+
+typedef TypedCamera</*num_targets=*/3, /*num_obstacles=*/1, double> TestCamera;
+
+class CameraTest : public ::testing::Test {
+ public:
+ // Set up three targets in a row, at (-1, 0), (0, 0), and (1, 0).
+ // Make the right-most target (1, 0) be facing away from the camera, and give
+ // the middle target some skew.
+ // Place the camera at (0, -5) so the targets are a few meters away.
+ // Place one obstacle in a place where it blocks the left-most target (-1, 0).
+ CameraTest()
+ : targets_{{Target(Target::Pose({-1.0, 0.0, 0.0}, M_PI_2)),
+ Target(Target::Pose({0.0, 0.0, kMiddleHeight},
+ M_PI_2 + kMiddleSkew)),
+ Target(Target::Pose({1.0, 0.0, 0.0}, -M_PI_2))}},
+ obstacles_{{TestCamera::LineSegment({{-2.0, -0.5, 0.0}, 0.0},
+ {{-0.5, -0.5, 0.0}, 0.0})}},
+ base_pose_({0.0, -5.0, 0.0}, M_PI_2),
+ camera_({&base_pose_, {0.0, 0.0, 0.0}, 0.0}, M_PI_2, noise_parameters_,
+ targets_, obstacles_) {}
+
+ protected:
+ static constexpr double kMiddleSkew = 0.1;
+ static constexpr double kMiddleHeight = 0.5;
+ ::std::array<Target, 3> targets_;
+ ::std::array<TestCamera::LineSegment, 1> obstacles_;
+
+ TestCamera::NoiseParameters noise_parameters_ = {
+ .max_viewable_distance = 10.0,
+ .heading_noise = 0.03,
+ .nominal_distance_noise = 0.06,
+ .nominal_skew_noise = 0.1,
+ .nominal_height_noise = 0.01};
+
+ // Provide base_pose_ as the base for the Pose used in the camera, to make it
+ // so that we can easily move the camera around for testing.
+ TestCamera::Pose base_pose_;
+ TestCamera camera_;
+};
+
+constexpr double CameraTest::kMiddleSkew;
+constexpr double CameraTest::kMiddleHeight;
+
+constexpr double kEps = 1e-15;
+
+// Check that, in the default setup we have, the correct targets are visible in
+// the expected locations.
+TEST_F(CameraTest, BasicCameraTest) {
+ const auto views = camera_.target_views();
+ // We should only be able to see one target (the middle one).
+ ASSERT_EQ(1u, views.size());
+ // And, check the actual result for correctness.
+ EXPECT_NEAR(0.0, views[0].reading.heading, kEps);
+ EXPECT_NEAR(5.0, views[0].reading.distance, kEps);
+ EXPECT_NEAR(kMiddleSkew, views[0].reading.skew, kEps);
+ EXPECT_NEAR(kMiddleHeight, views[0].reading.height, kEps);
+ // Check that the noise outputs are sane; leave other tests to check the exact
+ // values of the noise outputs.
+ // All noise values should be strictly positive.
+ EXPECT_GT(views[0].noise.heading, 0.0);
+ EXPECT_GT(views[0].noise.distance, 0.0);
+ EXPECT_GT(views[0].noise.skew, 0.0);
+ EXPECT_GT(views[0].noise.height, 0.0);
+}
+
+// Check that occluding the middle target makes it invisible.
+TEST_F(CameraTest, OcclusionTest) {
+ auto views = camera_.target_views();
+ // We should only be able to see one target (the middle one).
+ ASSERT_EQ(1u, views.size());
+ targets_[1].set_occluded(true);
+ TestCamera occluded_camera(camera_.pose(), camera_.fov(), noise_parameters_,
+ targets_, obstacles_);
+ views = occluded_camera.target_views();
+ // We should no longer see any targets.
+ ASSERT_EQ(0u, views.size());
+}
+
+// Checks that targets outside of the field-of-view don't show up.
+TEST_F(CameraTest, FovTest) {
+ // Initially, we should still see just the middle target.
+ EXPECT_EQ(1u, camera_.target_views().size());
+ // Point camera so that the middle target is just barely in its field of view.
+ base_pose_.set_theta(3.0 * M_PI / 4.0 - 0.01);
+ EXPECT_EQ(1u, camera_.target_views().size());
+ // Point camera so that the middle target is just outside of its FoV.
+ base_pose_.set_theta(3.0 * M_PI / 4.0 + 0.01);
+ EXPECT_EQ(0u, camera_.target_views().size());
+ // Check the same things, but on the other edge of the FoV:
+ base_pose_.set_theta(M_PI / 4.0 + 0.01);
+ EXPECT_EQ(1u, camera_.target_views().size());
+ base_pose_.set_theta(M_PI / 4.0 - 0.01);
+ EXPECT_EQ(0u, camera_.target_views().size());
+}
+
+// Checks that targets don't show up when very far away.
+TEST_F(CameraTest, FarAwayTest) {
+ EXPECT_EQ(1u, camera_.target_views().size());
+ // If we move the camera really far away we can't see it any more:
+ base_pose_.mutable_pos()->y() = -1000.0;
+ EXPECT_EQ(0u, camera_.target_views().size());
+}
+
+// Checks that targets which are highly skewed only show up if we are
+// arbitrarily close.
+TEST_F(CameraTest, HighlySkewedTest) {
+ // Skew the target a bunch.
+ targets_[1] = Target({{0.0, 0.0, 0.0}, 0.01});
+ TestCamera occluded_camera(camera_.pose(), camera_.fov(), noise_parameters_,
+ targets_, obstacles_);
+ EXPECT_EQ(0u, occluded_camera.target_views().size());
+ // But if we get really close we should still see it...
+ base_pose_.mutable_pos()->y() = -0.1;
+ EXPECT_EQ(1u, camera_.target_views().size());
+}
+
+// Checks that reading noises have the expected characteristics (mostly, going
+// up linearly with distance):
+TEST_F(CameraTest, DistanceNoiseTest) {
+ using Reading = TestCamera::TargetView::Reading;
+ const Reading normal_noise = camera_.target_views()[0].noise;
+ // Get twice as close:
+ base_pose_.mutable_pos()->y() /= 2.0;
+ const Reading closer_noise = camera_.target_views()[0].noise;
+ EXPECT_EQ(normal_noise.distance / 2.0, closer_noise.distance);
+ EXPECT_EQ(normal_noise.skew / 2.0, closer_noise.skew);
+ EXPECT_EQ(normal_noise.height / 2.0, closer_noise.height);
+ // Heading reading should be equally good.
+ EXPECT_EQ(normal_noise.heading, closer_noise.heading);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index c872622..4e23987 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_main.cc
@@ -1,5 +1,6 @@
#include "aos/init.h"
+#include "aos/events/shm-event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
@@ -7,8 +8,9 @@
int main() {
::aos::Init();
+ ::aos::ShmEventLoop event_loop;
DrivetrainLoop drivetrain(
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 79d851a..5d07e17 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -15,7 +15,7 @@
)
cc_library(
- name = 'superstructure_lib',
+ name = "superstructure_lib",
srcs = [
"superstructure.cc",
],
@@ -25,7 +25,7 @@
deps = [
":superstructure_queue",
"//aos/controls:control_loop",
- ]
+ ],
)
cc_binary(
@@ -36,5 +36,6 @@
deps = [
":superstructure_lib",
"//aos:init",
- ]
-)
\ No newline at end of file
+ "//aos/events:shm-event-loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 2f0832c..e87ef6e 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -7,10 +7,9 @@
namespace control_loops {
namespace superstructure {
-Superstructure::Superstructure(
- SuperstructureQueue *superstructure_queue)
- : aos::controls::ControlLoop<SuperstructureQueue>(
- superstructure_queue) {}
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name) {}
void Superstructure::RunIteration(
const SuperstructureQueue::Goal *unsafe_goal,
@@ -29,4 +28,4 @@
} // namespace superstructure
} // namespace control_loops
-} // namespace y2019
\ No newline at end of file
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 1faa6b4..0d6765e 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -12,8 +12,9 @@
: public ::aos::controls::ControlLoop<SuperstructureQueue> {
public:
explicit Superstructure(
- SuperstructureQueue *my_superstructure =
- &superstructure_queue);
+ ::aos::EventLoop *event_loop,
+ const ::std::string &name =
+ ".y2019.control_loops.superstructure.superstructure_queue");
protected:
virtual void RunIteration(
@@ -31,4 +32,4 @@
} // namespace control_loops
} // namespace y2019
-#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
\ No newline at end of file
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
index f3dd1ad..fe7be09 100644
--- a/y2019/control_loops/superstructure/superstructure_main.cc
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -1,10 +1,13 @@
#include "y2019/control_loops/superstructure/superstructure.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
int main() {
::aos::Init();
- ::y2019::control_loops::superstructure::Superstructure superstructure;
+ ::aos::ShmEventLoop event_loop;
+ ::y2019::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
superstructure.Run();
::aos::Cleanup();
return 0;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
new file mode 100644
index 0000000..dcdf00b
--- /dev/null
+++ b/y2019/wpilib_interface.cc
@@ -0,0 +1,306 @@
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <mutex>
+#include <thread>
+
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "aos/commonmath.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
+#include "aos/make_unique.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/wpilib_interface.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+using aos::make_unique;
+
+namespace y2019 {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
+// DMA stuff and then removing the * 2.0 in *_translate.
+// The low bit is direction.
+
+// TODO(brian): Use ::std::max instead once we have C++14 so that can be
+// constexpr.
+template <typename T>
+constexpr T max(T a, T b) {
+ return (a > b) ? a : b;
+}
+
+template <typename T, typename... Rest>
+constexpr T max(T a, T b, T c, Rest... rest) {
+ return max(max(a, b), c, rest...);
+}
+
+double drivetrain_translate(int32_t in) {
+ return ((static_cast<double>(in)
+ /* / Values::kDrivetrainEncoderCountsPerRevolution()) *
+ (2.0 * M_PI)) *
+ Values::kDrivetrainEncoderRatio() *
+ control_loops::drivetrain::kWheelRadius*/));
+}
+
+double drivetrain_velocity_translate(double in) {
+ return (((1.0 / in) /* / Values::kDrivetrainCyclesPerRevolution()) *
+ (2.0 * M_PI)) *
+ Values::kDrivetrainEncoderRatio() *
+ control_loops::drivetrain::kWheelRadius*/));
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond =
+ max(/*Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxIntakeMotorEncoderPulsesPerSecond()*/ 1.0, 1.0);
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+
+constexpr double kMaxMediumEncoderPulsesPerSecond =
+ max(/*Values::kMaxProximalEncoderPulsesPerSecond(),
+ Values::kMaxDistalEncoderPulsesPerSecond()*/ 1.0, 1.0);
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+ "medium encoders are too fast");
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+ SensorReader() {
+ // Set to filter out anything shorter than 1/4 of the minimum pulse width
+ // we should ever see.
+ fast_encoder_filter_.SetPeriodNanoSeconds(
+ static_cast<int>(1 / 4.0 /* built-in tolerance */ /
+ kMaxFastEncoderPulsesPerSecond * 1e9 +
+ 0.5));
+ medium_encoder_filter_.SetPeriodNanoSeconds(
+ static_cast<int>(1 / 4.0 /* built-in tolerance */ /
+ kMaxMediumEncoderPulsesPerSecond * 1e9 +
+ 0.5));
+ hall_filter_.SetPeriodNanoSeconds(100000);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("SensorReader");
+
+ my_pid_ = getpid();
+
+ dma_synchronizer_->Start();
+
+ ::aos::time::PhasedLoop phased_loop(last_period_,
+ ::std::chrono::milliseconds(3));
+ chrono::nanoseconds filtered_period = last_period_;
+
+ ::std::thread pwm_detecter_thread(
+ ::std::bind(&SensorReader::RunPWMDetecter, this));
+
+ ::aos::SetCurrentThreadRealtimePriority(40);
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ }
+ }
+ RunIteration();
+
+ monotonic_clock::time_point last_tick_timepoint;
+ chrono::nanoseconds period;
+ {
+ ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
+ last_tick_timepoint = last_tick_time_monotonic_timepoint_;
+ period = last_period_;
+ }
+
+ if (last_tick_timepoint == monotonic_clock::min_time) {
+ continue;
+ }
+ chrono::nanoseconds new_offset = phased_loop.OffsetFromIntervalAndTime(
+ period, last_tick_timepoint + chrono::microseconds(2050));
+
+ // TODO(austin): If this is the first edge in a while, skip to it (plus
+ // an offset). Otherwise, slowly drift time to line up.
+
+ phased_loop.set_interval_and_offset(period, new_offset);
+ }
+ pwm_detecter_thread.join();
+ }
+
+ void RunIteration() {
+ ::frc971::wpilib::SendRobotState(my_pid_);
+
+ {
+ auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ drivetrain_message->left_encoder =
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+ drivetrain_message->left_speed =
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+
+ drivetrain_message->right_encoder =
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+ drivetrain_message->right_speed = -drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod());
+
+ drivetrain_message.Send();
+ }
+
+ dma_synchronizer_->RunIteration();
+ }
+
+
+ private:
+ int32_t my_pid_;
+
+ ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+};
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_left_victor_ = ::std::move(t);
+ }
+
+ void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_right_victor_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::frc971::control_loops::drivetrain_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ drivetrain_left_victor_->SetSpeed(
+ ::aos::Clip(queue->left_voltage, -12.0, 12.0) / 12.0);
+ drivetrain_right_victor_->SetSpeed(
+ ::aos::Clip(-queue->right_voltage, -12.0, 12.0) / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "drivetrain output too old\n");
+ drivetrain_left_victor_->SetDisabled();
+ drivetrain_right_victor_->SetDisabled();
+ }
+
+ ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
+ drivetrain_right_victor_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ frc::Encoder::k4X);
+ }
+
+ void Run() override {
+ ::aos::InitNRT();
+ ::aos::SetCurrentThreadName("StartCompetition");
+
+ ::frc971::wpilib::JoystickSender joystick_sender;
+ ::std::thread joystick_thread(::std::ref(joystick_sender));
+
+ ::frc971::wpilib::PDPFetcher pdp_fetcher;
+ ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
+ SensorReader reader;
+
+ // TODO(Sabina): Update port numbers(Sensors and Victors)
+ reader.set_drivetrain_left_encoder(make_encoder(0));
+ reader.set_drivetrain_right_encoder(make_encoder(1));
+
+ reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
+
+ reader.set_dma(make_unique<DMA>());
+ ::std::thread reader_thread(::std::ref(reader));
+
+ auto imu_trigger = make_unique<frc::DigitalInput>(5);
+ ::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,
+ imu_trigger.get());
+ imu.SetDummySPI(frc::SPI::Port::kOnboardCS2);
+ auto imu_reset = make_unique<frc::DigitalOutput>(6);
+ imu.set_reset(imu_reset.get());
+ ::std::thread imu_thread(::std::ref(imu));
+
+ // While as of 2/9/18 the drivetrain Victors are SPX, it appears as though
+ // they are identical, as far as DrivetrainWriter is concerned, to the SP
+ // variety so all the Victors are written as SPs.
+
+ DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_drivetrain_left_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
+ drivetrain_writer.set_drivetrain_right_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+
+ // Wait forever. Not much else to do...
+ while (true) {
+ const int r = select(0, nullptr, nullptr, nullptr, nullptr);
+ if (r != 0) {
+ PLOG(WARNING, "infinite select failed");
+ } else {
+ PLOG(WARNING, "infinite select succeeded??\n");
+ }
+ }
+
+ LOG(ERROR, "Exiting WPILibRobot\n");
+
+ joystick_sender.Quit();
+ joystick_thread.join();
+ pdp_fetcher.Quit();
+ pdp_fetcher_thread.join();
+ reader.Quit();
+ reader_thread.join();
+ imu.Quit();
+ imu_thread.join();
+
+ drivetrain_writer.Quit();
+ drivetrain_writer_thread.join();
+
+ ::aos::Cleanup();
+ }
+};
+
+} // namespace
+} // namespace wpilib
+} // namespace y2019
+
+AOS_ROBOT_CLASS(::y2019::wpilib::WPILibRobot);