merging in matrix logging support
diff --git a/aos/build/aos.gyp b/aos/build/aos.gyp
index 12ac2a1..a0412c3 100644
--- a/aos/build/aos.gyp
+++ b/aos/build/aos.gyp
@@ -40,6 +40,7 @@
],
'dependencies': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:queue',
+ '<(AOS)/common/common.gyp:time',
],
'export_dependent_settings': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:queue',
diff --git a/aos/build/build.sh b/aos/build/build.sh
index cf79fc3..6f1bf02 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -76,11 +76,11 @@
fi
${NINJA} -C ${OUTDIR} ${NINJA_ACTION} "$@"
if [[ ${ACTION} == deploy ]]; then
- [[ ${PLATFORM} == linux ]] && \
+ [[ ${PLATFORM} == linux ]] && ( \
rsync --progress -c -r \
${OUTDIR}/outputs/* \
driver@`${AOS}/build/get_ip prime`:/home/driver/robot_code/bin
- ssh driver@`${AOS}/build/get_ip prime` "sync; sync; sync"
+ ssh driver@`${AOS}/build/get_ip prime` "sync; sync; sync" )
[ ${PLATFORM} == crio ] && \
ncftpput `${AOS}/build/get_ip robot` / \
${OUTDIR}/lib/FRC_UserProgram.out
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index e96ae66..4735051 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -12,20 +12,21 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position, bool fail_no_position>
-constexpr ::aos::time::Time
- ControlLoop<T, has_position, fail_no_position>::kStaleLogInterval;
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+constexpr ::aos::time::Time ControlLoop<T, has_position, fail_no_position,
+ fail_no_goal>::kStaleLogInterval;
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::ZeroOutputs() {
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+void
+ControlLoop<T, has_position, fail_no_position, fail_no_goal>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Iterate() {
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Iterate() {
no_prior_goal_.Print();
no_sensor_generation_.Print();
very_stale_position_.Print();
@@ -43,8 +44,10 @@
const GoalType *goal = control_loop_->goal.get();
if (goal == NULL) {
LOG_INTERVAL(no_prior_goal_);
- ZeroOutputs();
- return;
+ if (fail_no_goal) {
+ ZeroOutputs();
+ return;
+ }
}
::bbb::sensor_generation.FetchLatest();
@@ -56,18 +59,18 @@
if (!has_sensor_reset_counters_ ||
::bbb::sensor_generation->reader_pid != reader_pid_ ||
::bbb::sensor_generation->cape_resets != cape_resets_) {
- char buffer[128];
- size_t characters = ::bbb::sensor_generation->Print(buffer, sizeof(buffer));
- LOG(INFO, "new sensor_generation message %.*s\n",
- static_cast<int>(characters), buffer);
+ LOG_STRUCT(INFO, "new sensor_generation message",
+ *::bbb::sensor_generation.get());
reader_pid_ = ::bbb::sensor_generation->reader_pid;
cape_resets_ = ::bbb::sensor_generation->cape_resets;
has_sensor_reset_counters_ = true;
reset_ = true;
}
-
- LOG_STRUCT(DEBUG, "goal", *goal);
+
+ if (goal) {
+ LOG_STRUCT(DEBUG, "goal", *goal);
+ }
// Only pass in a position if we got one this cycle.
const PositionType *position = NULL;
@@ -141,8 +144,8 @@
status.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Run() {
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Run() {
while (true) {
time::SleepUntil(NextLoopTime());
Iterate();
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index f1109e6..0e5c144 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -51,7 +51,8 @@
// If has_position is false, the control loop will always use NULL as the
// position and not check the queue. This is used for "loops" that control
// motors open loop.
-template <class T, bool has_position = true, bool fail_no_position = true>
+template <class T, bool has_position = true, bool fail_no_position = true,
+ bool fail_no_goal = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
@@ -79,9 +80,12 @@
decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
OutputType;
- // Constructs and sends a message on the output queue which will stop all the
- // motors. Calls Zero to clear all the state.
- void ZeroOutputs();
+ // Constructs and sends a message on the output queue which sets everything to
+ // a safe state (generally motors off). For some subclasses, this will be a
+ // bit different (ie pistons).
+ // The implementation here creates a new Output message, calls Zero() on it,
+ // and then sends it.
+ virtual void ZeroOutputs();
// Returns true if the device reading the sensors reset and potentially lost
// track of encoder counts. Calling this read method clears the flag. After
@@ -139,6 +143,9 @@
OutputType *output,
StatusType *status) = 0;
+ T *queue_group() { return control_loop_; }
+ const T *queue_group() const { return control_loop_; }
+
private:
// Pointer to the queue group
T *control_loop_;
diff --git a/aos/common/logging/logging_impl.h b/aos/common/logging/logging_impl.h
index 1d68a16..312a977 100644
--- a/aos/common/logging/logging_impl.h
+++ b/aos/common/logging/logging_impl.h
@@ -292,6 +292,14 @@
void FillInMessage(log_level level, const char *format, va_list ap,
LogMessage *message);
+static inline void FillInMessageVarargs(log_level level, LogMessage *message,
+ const char *format, ...) {
+ va_list ap;
+ va_start(ap, format);
+ FillInMessage(level, format, ap, message);
+ va_end(ap);
+}
+
// Prints message to output.
void PrintMessage(FILE *output, const LogMessage &message);
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 65bfa95..6c33822 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -214,8 +214,6 @@
// Fetches the next message from the queue.
// Returns true if there was a new message available and we successfully
// fetched it. This removes the message from the queue for all readers.
- // TODO(aschuh): Fix this to use a different way of fetching messages so other
- // readers can also FetchNext.
bool FetchNext();
bool FetchNextBlocking();
@@ -287,6 +285,7 @@
T *MakeRawMessage();
// Pointer to the queue that this object fetches from.
RawQueue *queue_;
+ int index_ = 0;
#endif
// Scoped pointer holding the latest message or NULL.
ScopedMessagePtr<const T> queue_msg_;
diff --git a/aos/linux_code/camera/Buffers.h b/aos/linux_code/camera/Buffers.h
index aedd79f..c73b3d8 100644
--- a/aos/linux_code/camera/Buffers.h
+++ b/aos/linux_code/camera/Buffers.h
@@ -8,7 +8,7 @@
#include "aos/linux_code/ipc_lib/queue.h"
#include "aos/common/type_traits.h"
-#include "aos/atom_code/ipc_lib/unique_message_ptr.h"
+#include "aos/linux_code/ipc_lib/unique_message_ptr.h"
namespace aos {
namespace camera {
diff --git a/aos/linux_code/ipc_lib/ipc_lib.gyp b/aos/linux_code/ipc_lib/ipc_lib.gyp
index 08e8df8..fe8b2e0 100644
--- a/aos/linux_code/ipc_lib/ipc_lib.gyp
+++ b/aos/linux_code/ipc_lib/ipc_lib.gyp
@@ -51,7 +51,7 @@
'target_name': 'raw_queue_test',
'type': 'executable',
'sources': [
- 'queue_test.cc',
+ 'raw_queue_test.cc',
],
'dependencies': [
'<(EXTERNALS):gtest',
diff --git a/aos/linux_code/ipc_lib/queue.cc b/aos/linux_code/ipc_lib/queue.cc
index 6c8b046..39fd558 100644
--- a/aos/linux_code/ipc_lib/queue.cc
+++ b/aos/linux_code/ipc_lib/queue.cc
@@ -22,6 +22,7 @@
const bool kWriteDebug = false;
const bool kRefDebug = false;
const bool kFetchDebug = false;
+const bool kReadIndexDebug = false;
// The number of extra messages the pool associated with each queue will be able
// to hold (for readers who are slow about freeing them or who leak one when
@@ -37,6 +38,8 @@
const int RawQueue::kOverride;
struct RawQueue::MessageHeader {
+ // This gets incremented and decremented with atomic instructions without any
+ // locks held.
int ref_count;
int index; // in pool_
// Gets the message header immediately preceding msg.
@@ -59,18 +62,22 @@
bool writable_start;
};
-// TODO(brians) maybe do this with atomic integer instructions so it doesn't
-// have to lock/unlock pool_lock_
void RawQueue::DecrementMessageReferenceCount(const void *msg) {
- MutexLocker locker(&pool_lock_);
MessageHeader *header = MessageHeader::Get(msg);
- --header->ref_count;
- assert(header->ref_count >= 0);
+ __atomic_sub_fetch(&header->ref_count, 1, __ATOMIC_RELAXED);
if (kRefDebug) {
printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
}
+
+ // The only way it should ever be 0 is if we were the last one to decrement,
+ // in which case nobody else should have it around to re-increment it or
+ // anything in the middle, so this is safe to do not atomically with the
+ // decrement.
if (header->ref_count == 0) {
+ MutexLocker locker(&pool_lock_);
DoFreeMessage(msg);
+ } else {
+ assert(header->ref_count > 0);
}
}
@@ -226,6 +233,7 @@
}
bool RawQueue::WriteMessage(void *msg, int options) {
+ // TODO(brians): Test this function.
if (kWriteDebug) {
printf("queue: %p->WriteMessage(%p, %x)\n", this, msg, options);
}
@@ -251,6 +259,7 @@
if (kWriteDebug) {
printf("queue: not blocking on %p. returning false\n", this);
}
+ DecrementMessageReferenceCount(msg);
return false;
} else if (options & kOverride) {
if (kWriteDebug) {
@@ -325,7 +334,7 @@
}
return true;
}
-void *RawQueue::ReadPeek(int options, int start) {
+void *RawQueue::ReadPeek(int options, int start) const {
void *ret;
if (options & kFromEnd) {
int pos = data_end_ - 1;
@@ -337,19 +346,21 @@
}
ret = data_[pos];
} else {
+ assert(start != -1);
if (kReadDebug) {
printf("queue: %p reading from line %d: %d\n", this, __LINE__, start);
}
ret = data_[start];
}
MessageHeader *const header = MessageHeader::Get(ret);
- ++header->ref_count;
+ __atomic_add_fetch(&header->ref_count, 1, __ATOMIC_RELAXED);
if (kRefDebug) {
- printf("ref inc count: %p\n", ret);
+ printf("ref inc count1: %p\n", ret);
}
return ret;
}
const void *RawQueue::ReadMessage(int options) {
+ // TODO(brians): Test this function.
if (kReadDebug) {
printf("queue: %p->ReadMessage(%x)\n", this, options);
}
@@ -423,20 +434,43 @@
// Where we're going to start reading.
int my_start;
- const int unread_messages = messages_ - *index;
- const int current_messages = ::std::abs(data_start_ - data_end_);
- if (unread_messages > current_messages) { // If we're behind the available messages.
- // Catch index up to the last available message.
- *index = messages_ - current_messages;
- // And that's the one we're going to read.
- my_start = data_start_;
+ if (options & kFromEnd) {
+ my_start = -1;
} else {
- // Just start reading at the first available message that we haven't yet
- // read.
- my_start = (data_start_ + unread_messages - 1) % data_length_;
+ const int unread_messages = messages_ - *index;
+ assert(unread_messages > 0);
+ int current_messages = data_end_ - data_start_;
+ if (current_messages < 0) current_messages += data_length_;
+ if (kReadIndexDebug) {
+ printf("queue: %p start=%d end=%d current=%d\n",
+ this, data_start_, data_end_, current_messages);
+ }
+ assert(current_messages > 0);
+ // If we're behind the available messages.
+ if (unread_messages > current_messages) {
+ // Catch index up to the last available message.
+ *index = messages_ - current_messages;
+ // And that's the one we're going to read.
+ my_start = data_start_;
+ if (kReadIndexDebug) {
+ printf("queue: %p jumping ahead to message %d (have %d) (at %d)\n",
+ this, *index, messages_, data_start_);
+ }
+ } else {
+ // Just start reading at the first available message that we haven't yet
+ // read.
+ my_start = data_end_ - unread_messages;
+ if (kReadIndexDebug) {
+ printf("queue: %p original read from %d\n", this, my_start);
+ }
+ if (data_start_ < data_end_) {
+ assert(my_start >= data_start_);
+ } else {
+ if (my_start < 0) my_start += data_length_;
+ }
+ }
}
-
if (options & kPeek) {
msg = ReadPeek(options, my_start);
} else {
@@ -445,6 +479,9 @@
printf("queue: %p start of c1\n", this);
}
int pos = data_end_ - 1;
+ if (kReadIndexDebug) {
+ printf("queue: %p end pos start %d\n", this, pos);
+ }
if (pos < 0) { // If it wrapped.
pos = data_length_ - 1; // Unwrap it.
}
@@ -458,16 +495,18 @@
printf("queue: %p reading from d1: %d\n", this, my_start);
}
// This assert checks that we're either within both endpoints (duh) or
- // outside of both of them (if the queue is wrapped around).
+ // not between them (if the queue is wrapped around).
assert((my_start >= data_start_ && my_start < data_end_) ||
- (my_start > data_end_ && my_start <= data_start_));
+ ((my_start >= data_start_) == (my_start > data_end_)));
+ // More sanity checking.
+ assert((my_start >= 0) && (my_start < data_length_));
msg = data_[my_start];
++(*index);
}
MessageHeader *const header = MessageHeader::Get(msg);
- ++header->ref_count;
+ __atomic_add_fetch(&header->ref_count, 1, __ATOMIC_RELAXED);
if (kRefDebug) {
- printf("ref_inc_count: %p\n", msg);
+ printf("ref_inc_count2: %p\n", msg);
}
}
ReadCommonEnd(&read_data);
@@ -475,6 +514,7 @@
}
void *RawQueue::GetMessage() {
+ // TODO(brians): Test this function.
MutexLocker locker(&pool_lock_);
MessageHeader *header;
if (pool_length_ - messages_used_ > 0) {
@@ -489,6 +529,9 @@
}
void *msg = reinterpret_cast<uint8_t *>(header) + sizeof(MessageHeader);
header->ref_count = 1;
+ static_assert(
+ __atomic_always_lock_free(sizeof(header->ref_count), &header->ref_count),
+ "we access this using not specifically atomic loads and stores");
if (kRefDebug) {
printf("%p ref alloc: %p\n", this, msg);
}
diff --git a/aos/linux_code/ipc_lib/queue.h b/aos/linux_code/ipc_lib/queue.h
index 4d00cde..e7737e1 100644
--- a/aos/linux_code/ipc_lib/queue.h
+++ b/aos/linux_code/ipc_lib/queue.h
@@ -55,20 +55,22 @@
// Constants for passing to options arguments.
// The non-conflicting ones can be combined with bitwise-or.
- // Causes the returned message to be left in the queue.
+ // Doesn't update the currently read index (the read messages in the queue or
+ // the index). This means the returned message (and any others skipped with
+ // kFromEnd) will be left in the queue.
// For reading only.
static const int kPeek = 0x0001;
// Reads the last message in the queue instead of just the next one.
// NOTE: This removes all of the messages until the last one from the queue
- // (which means that nobody else will read them). However, PEEK means to not
- // remove any from the queue, including the ones that are skipped.
+ // (which means that nobody else will read them).
// For reading only.
static const int kFromEnd = 0x0002;
// Causes reads to return NULL and writes to fail instead of waiting.
// For reading and writing.
static const int kNonBlock = 0x0004;
// Causes things to block.
- // IMPORTANT: Has a value of 0 so that it is the default. This has to stay.
+ // IMPORTANT: Has a value of 0 so that it is the default. This has to stay
+ // this way.
// For reading and writing.
static const int kBlock = 0x0000;
// Causes writes to overwrite the oldest message in the queue instead of
@@ -94,7 +96,7 @@
// same message twice with the same index argument. However, it may not
// return some messages that pass through the queue.
// *index should start as 0. index does not have to be in shared memory, but
- // it can be
+ // it can be.
const void *ReadMessageIndex(int options, int *index);
// Retrieves ("allocates") a message that can then be written to the queue.
@@ -111,6 +113,10 @@
if (msg != NULL) DecrementMessageReferenceCount(msg);
}
+ // Returns the number of messages from this queue that are currently used (in
+ // the queue and/or given out as references).
+ int messages_used() const { return messages_used_; }
+
private:
struct MessageHeader;
struct ReadData;
@@ -151,16 +157,17 @@
// Calls DoFreeMessage if appropriate.
void DecrementMessageReferenceCount(const void *msg);
- // Should be called with data_lock_ locked.
+ // Must be called with data_lock_ locked.
// *read_data will be initialized.
// Returns with a readable message in data_ or false.
bool ReadCommonStart(int options, int *index, ReadData *read_data);
// Deals with setting/unsetting readable_ and writable_.
- // Should be called after data_lock_ has been unlocked.
+ // Must be called after data_lock_ has been unlocked.
// read_data should be the same thing that was passed in to ReadCommonStart.
void ReadCommonEnd(ReadData *read_data);
// Handles reading with kPeek.
- void *ReadPeek(int options, int start);
+ // start can be -1 if options has kFromEnd set.
+ void *ReadPeek(int options, int start) const;
// Gets called by Fetch when necessary (with placement new).
RawQueue(const char *name, size_t length, int hash, int queue_length);
diff --git a/aos/linux_code/ipc_lib/queue_test.cc b/aos/linux_code/ipc_lib/queue_test.cc
deleted file mode 100644
index 65527c8..0000000
--- a/aos/linux_code/ipc_lib/queue_test.cc
+++ /dev/null
@@ -1,453 +0,0 @@
-#include "aos/common/queue.h"
-
-#include <unistd.h>
-#include <sys/mman.h>
-#include <inttypes.h>
-
-#include <ostream>
-#include <memory>
-#include <map>
-
-#include "gtest/gtest.h"
-
-#include "aos/linux_code/ipc_lib/core_lib.h"
-#include "aos/common/type_traits.h"
-#include "aos/common/queue_testutils.h"
-#include "aos/common/time.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/die.h"
-
-using ::testing::AssertionResult;
-using ::testing::AssertionSuccess;
-using ::testing::AssertionFailure;
-using ::aos::common::testing::GlobalCoreInstance;
-
-namespace aos {
-namespace testing {
-
-class QueueTest : public ::testing::Test {
- protected:
- static const size_t kFailureSize = 400;
- static char *fatal_failure;
- private:
- enum class ResultType : uint8_t {
- NotCalled,
- Called,
- Returned,
- };
- const std::string ResultTypeString(volatile const ResultType &result) {
- switch (result) {
- case ResultType::Returned:
- return "Returned";
- case ResultType::Called:
- return "Called";
- case ResultType::NotCalled:
- return "NotCalled";
- default:
- return std::string("unknown(" + static_cast<uint8_t>(result)) + ")";
- }
- }
- static_assert(aos::shm_ok<ResultType>::value,
- "this will get put in shared memory");
- template<typename T>
- struct FunctionToCall {
- FunctionToCall() : result(ResultType::NotCalled) {
- started.Lock();
- }
-
- volatile ResultType result;
- bool expected;
- void (*function)(T*, char*);
- T *arg;
- volatile char failure[kFailureSize];
- Mutex started;
- };
- template<typename T>
- static void Hangs_(FunctionToCall<T> *const to_call) {
- to_call->started.Unlock();
- to_call->result = ResultType::Called;
- to_call->function(to_call->arg, const_cast<char *>(to_call->failure));
- to_call->result = ResultType::Returned;
- }
-
- // How long until a function is considered to have hung.
- static constexpr time::Time kHangTime = time::Time::InSeconds(0.035);
- // How long to sleep after forking (for debugging).
- static constexpr time::Time kForkSleep = time::Time::InSeconds(0);
-
- // Represents a process that has been forked off. The destructor kills the
- // process and wait(2)s for it.
- class ForkedProcess {
- public:
- ForkedProcess(pid_t pid, mutex *lock) : pid_(pid), lock_(lock) {};
- ~ForkedProcess() {
- if (kill(pid_, SIGINT) == -1) {
- if (errno == ESRCH) {
- printf("process %jd was already dead\n", static_cast<intmax_t>(pid_));
- } else {
- fprintf(stderr, "kill(SIGKILL, %jd) failed with %d: %s\n",
- static_cast<intmax_t>(pid_), errno, strerror(errno));
- }
- return;
- }
- const pid_t ret = wait(NULL);
- if (ret == -1) {
- LOG(WARNING, "wait(NULL) failed."
- " child %jd might still be alive\n",
- static_cast<intmax_t>(pid_));
- } else if (ret == 0) {
- LOG(WARNING, "child %jd wasn't waitable. it might still be alive\n",
- static_cast<intmax_t>(pid_));
- } else if (ret != pid_) {
- LOG(WARNING, "child %d is now confirmed dead"
- ", but child %jd might still be alive\n",
- ret, static_cast<intmax_t>(pid_));
- }
- }
-
- enum class JoinResult {
- Finished, Hung, Error
- };
- JoinResult Join(time::Time timeout = kHangTime) {
- timespec lock_timeout = (kForkSleep + timeout).ToTimespec();
- switch (mutex_lock_timeout(lock_, &lock_timeout)) {
- case 2:
- return JoinResult::Hung;
- case 0:
- return JoinResult::Finished;
- default:
- return JoinResult::Error;
- }
- }
-
- private:
- const pid_t pid_;
- mutex *const lock_;
- } __attribute__((unused));
-
- // State for HangsFork and HangsCheck.
- typedef uint8_t ChildID;
- static void ReapExitHandler() {
- for (auto it = children_.begin(); it != children_.end(); ++it) {
- delete it->second;
- }
- }
- static std::map<ChildID, ForkedProcess *> children_;
- std::map<ChildID, FunctionToCall<void> *> to_calls_;
-
- void SetUp() override {
- ::testing::Test::SetUp();
-
- SetDieTestMode(true);
-
- fatal_failure = static_cast<char *>(shm_malloc(sizeof(fatal_failure)));
- static bool registered = false;
- if (!registered) {
- atexit(ReapExitHandler);
- registered = true;
- }
- }
-
- protected:
- // function gets called with arg in a forked process.
- // Leaks shared memory.
- template<typename T> __attribute__((warn_unused_result))
- std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
- mutex *lock = static_cast<mutex *>(shm_malloc_aligned(
- sizeof(*lock), sizeof(int)));
- assert(mutex_lock(lock) == 0);
- const pid_t pid = fork();
- switch (pid) {
- case 0: // child
- if (kForkSleep != time::Time(0, 0)) {
- LOG(INFO, "pid %jd sleeping for %ds%dns\n",
- static_cast<intmax_t>(getpid()),
- kForkSleep.sec(), kForkSleep.nsec());
- time::SleepFor(kForkSleep);
- }
- ::aos::common::testing::PreventExit();
- function(arg);
- mutex_unlock(lock);
- exit(EXIT_SUCCESS);
- case -1: // parent failure
- LOG(ERROR, "fork() failed with %d: %s\n", errno, strerror(errno));
- return std::unique_ptr<ForkedProcess>();
- default: // parent
- return std::unique_ptr<ForkedProcess>(new ForkedProcess(pid, lock));
- }
- }
-
- // Checks whether or not the given function hangs.
- // expected is whether to return success or failure if the function hangs
- // NOTE: There are other reasons for it to return a failure than the function
- // doing the wrong thing.
- // Leaks shared memory.
- template<typename T>
- AssertionResult Hangs(void (*function)(T*, char*), T *arg, bool expected) {
- AssertionResult fork_result(HangsFork<T>(function, arg, expected, 0));
- if (!fork_result) {
- return fork_result;
- }
- return HangsCheck(0);
- }
- // Starts the first part of Hangs.
- // Use HangsCheck to get the result.
- // Returns whether the fork succeeded or not, NOT whether or not the hang
- // check succeeded.
- template<typename T>
- AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
- bool expected, ChildID id) {
- static_assert(aos::shm_ok<FunctionToCall<T>>::value,
- "this is going into shared memory");
- FunctionToCall<T> *const to_call =
- static_cast<FunctionToCall<T> *>(
- shm_malloc_aligned(sizeof(*to_call), alignof(FunctionToCall<T>)));
- new (to_call) FunctionToCall<T>();
- to_call->function = function;
- to_call->arg = arg;
- to_call->expected = expected;
- to_call->failure[0] = '\0';
- static_cast<char *>(fatal_failure)[0] = '\0';
- children_[id] = ForkExecute(Hangs_, to_call).release();
- if (!children_[id]) return AssertionFailure() << "ForkExecute failed";
- to_calls_[id] = reinterpret_cast<FunctionToCall<void> *>(to_call);
- to_call->started.Lock();
- return AssertionSuccess();
- }
- // Checks whether or not a function hung like it was supposed to.
- // Use HangsFork first.
- // NOTE: calls to HangsFork and HangsCheck with the same id argument will
- // correspond, but they do not nest. Also, id 0 is used by Hangs.
- // Return value is the same as Hangs.
- AssertionResult HangsCheck(ChildID id) {
- std::unique_ptr<ForkedProcess> child(children_[id]);
- children_.erase(id);
- const ForkedProcess::JoinResult result = child->Join();
- if (to_calls_[id]->failure[0] != '\0') {
- return AssertionFailure() << "function says: "
- << const_cast<char *>(to_calls_[id]->failure);
- }
- if (result == ForkedProcess::JoinResult::Finished) {
- return !to_calls_[id]->expected ? AssertionSuccess() : (AssertionFailure()
- << "something happened and the the test only got to "
- << ResultTypeString(to_calls_[id]->result));
- } else {
- if (to_calls_[id]->result == ResultType::Called) {
- return to_calls_[id]->expected ? AssertionSuccess() :
- AssertionFailure();
- } else if (result == ForkedProcess::JoinResult::Error) {
- return AssertionFailure() << "error joining child";
- } else {
- abort();
- return AssertionFailure() << "something weird happened";
- }
- }
- }
-#define EXPECT_HANGS(function, arg) \
- EXPECT_HANGS_COND(function, arg, true, EXPECT_TRUE)
-#define EXPECT_RETURNS(function, arg) \
- EXPECT_HANGS_COND(function, arg, false, EXPECT_TRUE)
-#define EXPECT_RETURNS_FAILS(function, arg) \
- EXPECT_HANGS_COND(function, arg, false, EXPECT_FALSE)
-#define EXPECT_HANGS_COND(function, arg, hangs, cond) do { \
- cond(Hangs(function, arg, hangs)); \
- if (fatal_failure[0] != '\0') { \
- FAIL() << fatal_failure; \
- } \
-} while (false)
-
- struct TestMessage {
- // Some contents because we don't really want to test empty messages.
- int16_t data;
- };
- struct MessageArgs {
- RawQueue *const queue;
- int flags;
- int16_t data; // -1 means NULL expected
- };
- static void WriteTestMessage(MessageArgs *args, char *failure) {
- TestMessage *msg = static_cast<TestMessage *>(args->queue->GetMessage());
- if (msg == NULL) {
- snprintf(fatal_failure, kFailureSize,
- "couldn't get_msg from %p", args->queue);
- return;
- }
- msg->data = args->data;
- if (!args->queue->WriteMessage(msg, args->flags)) {
- snprintf(failure, kFailureSize, "write_msg_free(%p, %p, %d) failed",
- args->queue, msg, args->flags);
- }
- }
- static void ReadTestMessage(MessageArgs *args, char *failure) {
- const TestMessage *msg = static_cast<const TestMessage *>(
- args->queue->ReadMessage(args->flags));
- if (msg == NULL) {
- if (args->data != -1) {
- snprintf(failure, kFailureSize,
- "expected data of %" PRId16 " but got NULL message",
- args->data);
- }
- } else {
- if (args->data != msg->data) {
- snprintf(failure, kFailureSize,
- "expected data of %" PRId16 " but got %" PRId16 " instead",
- args->data, msg->data);
- }
- args->queue->FreeMessage(msg);
- }
- }
-
- private:
- GlobalCoreInstance my_core;
-};
-char *QueueTest::fatal_failure;
-std::map<QueueTest::ChildID, QueueTest::ForkedProcess *> QueueTest::children_;
-constexpr time::Time QueueTest::kHangTime;
-constexpr time::Time QueueTest::kForkSleep;
-
-TEST_F(QueueTest, Reading) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
- MessageArgs args{queue, 0, -1};
-
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kNonBlock | RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- EXPECT_HANGS(ReadTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_HANGS(ReadTestMessage, &args);
- args.data = 254;
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kPeek | RawQueue::kNonBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- args.data = -1;
- EXPECT_HANGS(ReadTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- args.data = 971;
- EXPECT_RETURNS_FAILS(ReadTestMessage, &args);
-}
-TEST_F(QueueTest, Writing) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
- MessageArgs args{queue, 0, 973};
-
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kBlock;
- EXPECT_HANGS(WriteTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.data = 971;
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
-}
-
-TEST_F(QueueTest, MultiRead) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
- MessageArgs args{queue, 0, 1323};
-
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kBlock;
- ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 1));
- ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 2));
- EXPECT_TRUE(HangsCheck(1) != HangsCheck(2));
- // TODO(brians) finish this
-}
-
-// There used to be a bug where reading first without an index and then with an
-// index would crash. This test makes sure that's fixed.
-TEST_F(QueueTest, ReadIndexAndNot) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
-
- // Write a message, read it (with ReadMessage), and then write another
- // message (before freeing the read one so the queue allocates a distinct
- // message to use for it).
- TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
- ASSERT_NE(nullptr, msg);
- ASSERT_TRUE(queue->WriteMessage(msg, 0));
- const void *read_msg = queue->ReadMessage(0);
- EXPECT_NE(nullptr, read_msg);
- msg = static_cast<TestMessage *>(queue->GetMessage());
- queue->FreeMessage(read_msg);
- ASSERT_NE(nullptr, msg);
- ASSERT_TRUE(queue->WriteMessage(msg, 0));
-
- int index = 0;
- const void *second_read_msg = queue->ReadMessageIndex(0, &index);
- EXPECT_NE(nullptr, second_read_msg);
- EXPECT_NE(read_msg, second_read_msg)
- << "We already took that message out of the queue.";
-}
-
-TEST_F(QueueTest, Recycle) {
- // TODO(brians) basic test of recycle queue
- // include all of the ways a message can get into the recycle queue
- RawQueue *recycle_queue = reinterpret_cast<RawQueue *>(23);
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage),
- 1, 2, 2, 2, &recycle_queue);
- ASSERT_NE(reinterpret_cast<RawQueue *>(23), recycle_queue);
- MessageArgs args{queue, 0, 973}, recycle{recycle_queue, 0, 973};
-
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- EXPECT_HANGS(ReadTestMessage, &recycle);
- args.data = 254;
- EXPECT_RETURNS(WriteTestMessage, &args);
- EXPECT_HANGS(ReadTestMessage, &recycle);
- args.data = 971;
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- recycle.flags = RawQueue::kBlock;
- EXPECT_RETURNS(ReadTestMessage, &recycle);
-
- EXPECT_HANGS(ReadTestMessage, &recycle);
-
- TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
- ASSERT_TRUE(msg != NULL);
- msg->data = 341;
- queue->FreeMessage(msg);
- recycle.data = 341;
- EXPECT_RETURNS(ReadTestMessage, &recycle);
-
- EXPECT_HANGS(ReadTestMessage, &recycle);
-
- args.data = 254;
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- recycle.flags = RawQueue::kBlock;
- EXPECT_HANGS(ReadTestMessage, &recycle);
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- recycle.data = 254;
- EXPECT_RETURNS(ReadTestMessage, &recycle);
-}
-
-} // namespace testing
-} // namespace aos
diff --git a/aos/linux_code/ipc_lib/raw_queue_test.cc b/aos/linux_code/ipc_lib/raw_queue_test.cc
new file mode 100644
index 0000000..6778df3
--- /dev/null
+++ b/aos/linux_code/ipc_lib/raw_queue_test.cc
@@ -0,0 +1,878 @@
+#include "aos/common/queue.h"
+
+#include <unistd.h>
+#include <sys/mman.h>
+#include <inttypes.h>
+
+#include <ostream>
+#include <memory>
+#include <map>
+
+#include "gtest/gtest.h"
+
+#include "aos/linux_code/ipc_lib/core_lib.h"
+#include "aos/common/type_traits.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/die.h"
+
+using ::testing::AssertionResult;
+using ::testing::AssertionSuccess;
+using ::testing::AssertionFailure;
+using ::aos::common::testing::GlobalCoreInstance;
+
+namespace aos {
+namespace testing {
+
+class RawQueueTest : public ::testing::Test {
+ protected:
+ static const size_t kFailureSize = 400;
+ static char *fatal_failure;
+ private:
+ enum class ResultType : uint8_t {
+ NotCalled,
+ Called,
+ Returned,
+ };
+ const std::string ResultTypeString(volatile const ResultType &result) {
+ switch (result) {
+ case ResultType::Returned:
+ return "Returned";
+ case ResultType::Called:
+ return "Called";
+ case ResultType::NotCalled:
+ return "NotCalled";
+ default:
+ return std::string("unknown(" + static_cast<uint8_t>(result)) + ")";
+ }
+ }
+ static_assert(aos::shm_ok<ResultType>::value,
+ "this will get put in shared memory");
+ template<typename T>
+ struct FunctionToCall {
+ FunctionToCall() : result(ResultType::NotCalled) {
+ started.Lock();
+ }
+
+ volatile ResultType result;
+ bool expected;
+ void (*function)(T*, char*);
+ T *arg;
+ volatile char failure[kFailureSize];
+ Mutex started;
+ };
+ template<typename T>
+ static void Hangs_(FunctionToCall<T> *const to_call) {
+ to_call->started.Unlock();
+ to_call->result = ResultType::Called;
+ to_call->function(to_call->arg, const_cast<char *>(to_call->failure));
+ to_call->result = ResultType::Returned;
+ }
+
+ // How long until a function is considered to have hung.
+ static constexpr time::Time kHangTime = time::Time::InSeconds(0.035);
+ // How long to sleep after forking (for debugging).
+ static constexpr time::Time kForkSleep = time::Time::InSeconds(0);
+
+ // Represents a process that has been forked off. The destructor kills the
+ // process and wait(2)s for it.
+ class ForkedProcess {
+ public:
+ ForkedProcess(pid_t pid, mutex *lock) : pid_(pid), lock_(lock) {};
+ ~ForkedProcess() {
+ if (kill(pid_, SIGINT) == -1) {
+ if (errno == ESRCH) {
+ printf("process %jd was already dead\n", static_cast<intmax_t>(pid_));
+ } else {
+ fprintf(stderr, "kill(SIGKILL, %jd) failed with %d: %s\n",
+ static_cast<intmax_t>(pid_), errno, strerror(errno));
+ }
+ return;
+ }
+ const pid_t ret = wait(NULL);
+ if (ret == -1) {
+ LOG(WARNING, "wait(NULL) failed."
+ " child %jd might still be alive\n",
+ static_cast<intmax_t>(pid_));
+ } else if (ret == 0) {
+ LOG(WARNING, "child %jd wasn't waitable. it might still be alive\n",
+ static_cast<intmax_t>(pid_));
+ } else if (ret != pid_) {
+ LOG(WARNING, "child %d is now confirmed dead"
+ ", but child %jd might still be alive\n",
+ ret, static_cast<intmax_t>(pid_));
+ }
+ }
+
+ enum class JoinResult {
+ Finished, Hung, Error
+ };
+ JoinResult Join(time::Time timeout = kHangTime) {
+ timespec lock_timeout = (kForkSleep + timeout).ToTimespec();
+ switch (mutex_lock_timeout(lock_, &lock_timeout)) {
+ case 2:
+ return JoinResult::Hung;
+ case 0:
+ return JoinResult::Finished;
+ default:
+ return JoinResult::Error;
+ }
+ }
+
+ private:
+ const pid_t pid_;
+ mutex *const lock_;
+ } __attribute__((unused));
+
+ // State for HangsFork and HangsCheck.
+ typedef uint8_t ChildID;
+ static void ReapExitHandler() {
+ for (auto it = children_.begin(); it != children_.end(); ++it) {
+ delete it->second;
+ }
+ }
+ static std::map<ChildID, ForkedProcess *> children_;
+ std::map<ChildID, FunctionToCall<void> *> to_calls_;
+
+ void SetUp() override {
+ ::testing::Test::SetUp();
+
+ SetDieTestMode(true);
+
+ fatal_failure = static_cast<char *>(shm_malloc(sizeof(fatal_failure)));
+ static bool registered = false;
+ if (!registered) {
+ atexit(ReapExitHandler);
+ registered = true;
+ }
+ }
+
+ protected:
+ // function gets called with arg in a forked process.
+ // Leaks shared memory.
+ template<typename T> __attribute__((warn_unused_result))
+ std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
+ mutex *lock = static_cast<mutex *>(shm_malloc_aligned(
+ sizeof(*lock), sizeof(int)));
+ assert(mutex_lock(lock) == 0);
+ const pid_t pid = fork();
+ switch (pid) {
+ case 0: // child
+ if (kForkSleep != time::Time(0, 0)) {
+ LOG(INFO, "pid %jd sleeping for %ds%dns\n",
+ static_cast<intmax_t>(getpid()),
+ kForkSleep.sec(), kForkSleep.nsec());
+ time::SleepFor(kForkSleep);
+ }
+ ::aos::common::testing::PreventExit();
+ function(arg);
+ mutex_unlock(lock);
+ exit(EXIT_SUCCESS);
+ case -1: // parent failure
+ LOG(ERROR, "fork() failed with %d: %s\n", errno, strerror(errno));
+ return std::unique_ptr<ForkedProcess>();
+ default: // parent
+ return std::unique_ptr<ForkedProcess>(new ForkedProcess(pid, lock));
+ }
+ }
+
+ // Checks whether or not the given function hangs.
+ // expected is whether to return success or failure if the function hangs
+ // NOTE: There are other reasons for it to return a failure than the function
+ // doing the wrong thing.
+ // Leaks shared memory.
+ template<typename T>
+ AssertionResult Hangs(void (*function)(T*, char*), T *arg, bool expected) {
+ AssertionResult fork_result(HangsFork<T>(function, arg, expected, 0));
+ if (!fork_result) {
+ return fork_result;
+ }
+ return HangsCheck(0);
+ }
+ // Starts the first part of Hangs.
+ // Use HangsCheck to get the result.
+ // Returns whether the fork succeeded or not, NOT whether or not the hang
+ // check succeeded.
+ template<typename T>
+ AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
+ bool expected, ChildID id) {
+ static_assert(aos::shm_ok<FunctionToCall<T>>::value,
+ "this is going into shared memory");
+ FunctionToCall<T> *const to_call =
+ static_cast<FunctionToCall<T> *>(
+ shm_malloc_aligned(sizeof(*to_call), alignof(FunctionToCall<T>)));
+ new (to_call) FunctionToCall<T>();
+ to_call->function = function;
+ to_call->arg = arg;
+ to_call->expected = expected;
+ to_call->failure[0] = '\0';
+ static_cast<char *>(fatal_failure)[0] = '\0';
+ children_[id] = ForkExecute(Hangs_, to_call).release();
+ if (!children_[id]) return AssertionFailure() << "ForkExecute failed";
+ to_calls_[id] = reinterpret_cast<FunctionToCall<void> *>(to_call);
+ to_call->started.Lock();
+ return AssertionSuccess();
+ }
+ // Checks whether or not a function hung like it was supposed to.
+ // Use HangsFork first.
+ // NOTE: calls to HangsFork and HangsCheck with the same id argument will
+ // correspond, but they do not nest. Also, id 0 is used by Hangs.
+ // Return value is the same as Hangs.
+ AssertionResult HangsCheck(ChildID id) {
+ std::unique_ptr<ForkedProcess> child(children_[id]);
+ children_.erase(id);
+ const ForkedProcess::JoinResult result = child->Join();
+ if (to_calls_[id]->failure[0] != '\0') {
+ return AssertionFailure() << "function says: "
+ << const_cast<char *>(to_calls_[id]->failure);
+ }
+ if (result == ForkedProcess::JoinResult::Finished) {
+ return !to_calls_[id]->expected ? AssertionSuccess() : (AssertionFailure()
+ << "something happened and the the test only got to "
+ << ResultTypeString(to_calls_[id]->result));
+ } else {
+ if (to_calls_[id]->result == ResultType::Called) {
+ return to_calls_[id]->expected ? AssertionSuccess() :
+ AssertionFailure();
+ } else if (result == ForkedProcess::JoinResult::Error) {
+ return AssertionFailure() << "error joining child";
+ } else {
+ abort();
+ return AssertionFailure() << "something weird happened";
+ }
+ }
+ }
+#define EXPECT_HANGS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, true, EXPECT_TRUE)
+#define EXPECT_RETURNS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, false, EXPECT_TRUE)
+#define EXPECT_RETURNS_FAILS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, false, EXPECT_FALSE)
+#define EXPECT_HANGS_COND(function, arg, hangs, cond) do { \
+ cond(Hangs(function, arg, hangs)); \
+ if (fatal_failure[0] != '\0') { \
+ FAIL() << fatal_failure; \
+ } \
+} while (false)
+
+ struct TestMessage {
+ // Some contents because we don't really want to test empty messages.
+ int16_t data;
+ };
+ struct MessageArgs {
+ RawQueue *const queue;
+ int flags;
+ int16_t data; // -1 means NULL expected
+ };
+ static void WriteTestMessage(MessageArgs *args, char *failure) {
+ TestMessage *msg = static_cast<TestMessage *>(args->queue->GetMessage());
+ if (msg == NULL) {
+ snprintf(fatal_failure, kFailureSize,
+ "couldn't get_msg from %p", args->queue);
+ return;
+ }
+ msg->data = args->data;
+ if (!args->queue->WriteMessage(msg, args->flags)) {
+ snprintf(failure, kFailureSize, "write_msg_free(%p, %p, %d) failed",
+ args->queue, msg, args->flags);
+ }
+ }
+ static void ReadTestMessage(MessageArgs *args, char *failure) {
+ const TestMessage *msg = static_cast<const TestMessage *>(
+ args->queue->ReadMessage(args->flags));
+ if (msg == NULL) {
+ if (args->data != -1) {
+ snprintf(failure, kFailureSize,
+ "expected data of %" PRId16 " but got NULL message",
+ args->data);
+ }
+ } else {
+ if (args->data != msg->data) {
+ snprintf(failure, kFailureSize,
+ "expected data of %" PRId16 " but got %" PRId16 " instead",
+ args->data, msg->data);
+ }
+ args->queue->FreeMessage(msg);
+ }
+ }
+
+ void PushMessage(RawQueue *queue, uint16_t data) {
+ TestMessage *message = static_cast<TestMessage *>(queue->GetMessage());
+ message->data = data;
+ ASSERT_TRUE(queue->WriteMessage(message, RawQueue::kOverride));
+ }
+
+ private:
+ GlobalCoreInstance my_core;
+};
+char *RawQueueTest::fatal_failure;
+std::map<RawQueueTest::ChildID, RawQueueTest::ForkedProcess *>
+ RawQueueTest::children_;
+constexpr time::Time RawQueueTest::kHangTime;
+constexpr time::Time RawQueueTest::kForkSleep;
+
+TEST_F(RawQueueTest, Reading) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+ MessageArgs args{queue, 0, -1};
+
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kNonBlock | RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.data = 254;
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kPeek | RawQueue::kNonBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ args.data = -1;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ args.data = 971;
+ EXPECT_RETURNS_FAILS(ReadTestMessage, &args);
+}
+TEST_F(RawQueueTest, Writing) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+ MessageArgs args{queue, 0, 973};
+
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kBlock;
+ EXPECT_HANGS(WriteTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.data = 971;
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+}
+
+TEST_F(RawQueueTest, MultiRead) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+ MessageArgs args{queue, 0, 1323};
+
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kBlock;
+ ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 1));
+ ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 2));
+ EXPECT_TRUE(HangsCheck(1) != HangsCheck(2));
+ // TODO(brians) finish this
+}
+
+// There used to be a bug where reading first without an index and then with an
+// index would crash. This test makes sure that's fixed.
+TEST_F(RawQueueTest, ReadIndexAndNot) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+
+ // Write a message, read it (with ReadMessage), and then write another
+ // message (before freeing the read one so the queue allocates a distinct
+ // message to use for it).
+ TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
+ ASSERT_NE(nullptr, msg);
+ ASSERT_TRUE(queue->WriteMessage(msg, 0));
+ const void *read_msg = queue->ReadMessage(0);
+ EXPECT_NE(nullptr, read_msg);
+ msg = static_cast<TestMessage *>(queue->GetMessage());
+ queue->FreeMessage(read_msg);
+ ASSERT_NE(nullptr, msg);
+ ASSERT_TRUE(queue->WriteMessage(msg, 0));
+
+ int index = 0;
+ const void *second_read_msg = queue->ReadMessageIndex(0, &index);
+ EXPECT_NE(nullptr, second_read_msg);
+ EXPECT_NE(read_msg, second_read_msg)
+ << "We already took that message out of the queue.";
+}
+
+TEST_F(RawQueueTest, Recycle) {
+ // TODO(brians) basic test of recycle queue
+ // include all of the ways a message can get into the recycle queue
+ RawQueue *recycle_queue = reinterpret_cast<RawQueue *>(23);
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage),
+ 1, 2, 2, 2, &recycle_queue);
+ ASSERT_NE(reinterpret_cast<RawQueue *>(23), recycle_queue);
+ MessageArgs args{queue, 0, 973}, recycle{recycle_queue, 0, 973};
+
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.data = 254;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.data = 971;
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ recycle.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+
+ TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
+ ASSERT_TRUE(msg != NULL);
+ msg->data = 341;
+ queue->FreeMessage(msg);
+ recycle.data = 341;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+
+ args.data = 254;
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ recycle.flags = RawQueue::kBlock;
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ recycle.data = 254;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+}
+
+// Makes sure that when a message doesn't get written with kNonBlock it does get
+// freed.
+TEST_F(RawQueueTest, NonBlockFailFree) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+
+ void *message1 = queue->GetMessage();
+ void *message2 = queue->GetMessage();
+ ASSERT_TRUE(queue->WriteMessage(message1, RawQueue::kNonBlock));
+ ASSERT_FALSE(queue->WriteMessage(message2, RawQueue::kNonBlock));
+ EXPECT_EQ(message2, queue->GetMessage());
+}
+
+TEST_F(RawQueueTest, ReadIndexNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ EXPECT_EQ(0, queue->messages_used());
+ PushMessage(queue, 971);
+ EXPECT_EQ(1, queue->messages_used());
+
+ int index = 0;
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(971, message->data);
+ EXPECT_EQ(1, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ PushMessage(queue, 1768);
+ EXPECT_EQ(2, queue->messages_used());
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ PushMessage(queue, 254);
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ EXPECT_EQ(2, queue->messages_used());
+}
+
+TEST_F(RawQueueTest, ReadIndexNotBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ EXPECT_EQ(0, queue->messages_used());
+ PushMessage(queue, 971);
+ EXPECT_EQ(1, queue->messages_used());
+ PushMessage(queue, 1768);
+ EXPECT_EQ(2, queue->messages_used());
+
+ int index = 0;
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(971, message->data);
+ EXPECT_EQ(1, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexLittleBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexMoreBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexMoreBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexLotBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ {
+ const void *message1, *message2;
+ message1 = queue->ReadMessage(RawQueue::kNonBlock);
+ ASSERT_NE(nullptr, message1);
+ PushMessage(queue, 254);
+ message2 = queue->ReadMessage(RawQueue::kNonBlock);
+ ASSERT_NE(nullptr, message2);
+ PushMessage(queue, 973);
+ EXPECT_EQ(4, queue->messages_used());
+ queue->FreeMessage(message1);
+ EXPECT_EQ(3, queue->messages_used());
+ queue->FreeMessage(message2);
+ EXPECT_EQ(2, queue->messages_used());
+ }
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexLotBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 973);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexEvenMoreBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 973);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 1114);
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexEvenMoreBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 973);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 1114);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, MessageReferenceCounts) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const void *message1, *message2;
+
+ EXPECT_EQ(0, queue->messages_used());
+ message1 = queue->GetMessage();
+ EXPECT_NE(nullptr, message1);
+ EXPECT_EQ(1, queue->messages_used());
+ message2 = queue->GetMessage();
+ EXPECT_NE(nullptr, message2);
+ EXPECT_EQ(2, queue->messages_used());
+ queue->FreeMessage(message1);
+ EXPECT_EQ(1, queue->messages_used());
+ queue->FreeMessage(message2);
+ EXPECT_EQ(0, queue->messages_used());
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/linux_code/ipc_lib/shared_mem.c b/aos/linux_code/ipc_lib/shared_mem.c
index 850253d..4071ca2 100644
--- a/aos/linux_code/ipc_lib/shared_mem.c
+++ b/aos/linux_code/ipc_lib/shared_mem.c
@@ -36,6 +36,7 @@
struct aos_core *global_core = NULL;
+// TODO(brians): madvise(2) it to put this shm in core dumps.
int aos_core_create_shared_mem(enum aos_core_create to_create) {
static struct aos_core global_core_data;
global_core = &global_core_data;
@@ -103,7 +104,10 @@
(void *)SHM_START, shm_address);
return -1;
}
- return aos_core_use_address_as_shared_mem(shm_address, SIZEOFSHMSEG);
+ int r = aos_core_use_address_as_shared_mem(shm_address, SIZEOFSHMSEG);
+ fprintf(stderr, "shared_mem: end of create_shared_mem owner=%d r=%d\n",
+ global_core->owner, r);
+ return r;
}
int aos_core_use_address_as_shared_mem(void *address, size_t size) {
@@ -123,8 +127,6 @@
return -1;
}
}
- fprintf(stderr, "shared_mem: end of create_shared_mem owner=%d\n",
- global_core->owner);
return 0;
}
diff --git a/aos/atom_code/ipc_lib/unique_message_ptr.h b/aos/linux_code/ipc_lib/unique_message_ptr.h
similarity index 100%
rename from aos/atom_code/ipc_lib/unique_message_ptr.h
rename to aos/linux_code/ipc_lib/unique_message_ptr.h
diff --git a/aos/linux_code/logging/binary_log_file.cc b/aos/linux_code/logging/binary_log_file.cc
index 007ed9b..896422c 100644
--- a/aos/linux_code/logging/binary_log_file.cc
+++ b/aos/linux_code/logging/binary_log_file.cc
@@ -6,6 +6,8 @@
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>
+#include <signal.h>
+#include <setjmp.h>
namespace aos {
namespace logging {
@@ -111,11 +113,20 @@
if (current_ == MAP_FAILED) {
LOG(FATAL,
"mmap(NULL, %zd, PROT_READ [ | PROT_WRITE], MAP_SHARED, %d, %jd)"
- " failed with %d: %s. aborting\n",
+ " failed with %d: %s\n",
kPageSize, fd_, static_cast<intmax_t>(offset_), errno,
strerror(errno));
}
+ if (madvise(current_, kPageSize, MADV_SEQUENTIAL | MADV_WILLNEED) == -1) {
+ LOG(WARNING, "madvise(%p, %zd, MADV_SEQUENTIAL | MADV_WILLNEED)"
+ " failed with %d: %s\n",
+ current_, kPageSize, errno, strerror(errno));
+ }
offset_ += kPageSize;
+
+ if (!writable_) {
+ CheckCurrentPageReadable();
+ }
}
void LogFileAccessor::Unmap(void *location) {
@@ -126,6 +137,59 @@
is_last_page_ = 0;
}
+// This mess is because the only not completely hackish way to do this is to set
+// up a handler for SIGBUS/SIGSEGV that siglongjmps out to avoid either the
+// instruction being repeated infinitely (and more signals being delivered) or
+// (with SA_RESETHAND) the signal killing the process.
+namespace {
+
+void *volatile fault_address;
+sigjmp_buf jump_context;
+
+void CheckCurrentPageReadableHandler(int /*signal*/, siginfo_t *info, void *) {
+ fault_address = info->si_addr;
+
+ siglongjmp(jump_context, 1);
+}
+
+} // namespace
+void LogFileAccessor::CheckCurrentPageReadable() {
+ if (sigsetjmp(jump_context, 1) == 0) {
+ struct sigaction action;
+ action.sa_sigaction = CheckCurrentPageReadableHandler;
+ sigemptyset(&action.sa_mask);
+ action.sa_flags = SA_RESETHAND | SA_SIGINFO;
+ struct sigaction previous_bus, previous_segv;
+ if (sigaction(SIGBUS, &action, &previous_bus) == -1) {
+ LOG(FATAL, "sigaction(SIGBUS(=%d), %p, %p) failed with %d: %s\n",
+ SIGBUS, &action, &previous_bus, errno, strerror(errno));
+ }
+ if (sigaction(SIGSEGV, &action, &previous_segv) == -1) {
+ LOG(FATAL, "sigaction(SIGSEGV(=%d), %p, %p) failed with %d: %s\n",
+ SIGSEGV, &action, &previous_segv, errno, strerror(errno));
+ }
+
+ char __attribute__((unused)) c = current_[0];
+
+ if (sigaction(SIGBUS, &previous_bus, NULL) == -1) {
+ LOG(FATAL, "sigaction(SIGBUS(=%d), %p, NULL) failed with %d: %s\n",
+ SIGBUS, &previous_bus, errno, strerror(errno));
+ }
+ if (sigaction(SIGSEGV, &previous_segv, NULL) == -1) {
+ LOG(FATAL, "sigaction(SIGSEGV(=%d), %p, NULL) failed with %d: %s\n",
+ SIGSEGV, &previous_segv, errno, strerror(errno));
+ }
+ } else {
+ if (fault_address == current_) {
+ LOG(FATAL, "could not read 1 byte at offset 0x%jx into log file\n",
+ static_cast<uintmax_t>(offset_));
+ } else {
+ LOG(FATAL, "faulted at %p, not %p like we were (maybe) supposed to\n",
+ fault_address, current_);
+ }
+ }
+}
+
} // namespace linux_code
} // namespace logging
} // namespace aos
diff --git a/aos/linux_code/logging/binary_log_file.h b/aos/linux_code/logging/binary_log_file.h
index 0f8c3fb..72aac09 100644
--- a/aos/linux_code/logging/binary_log_file.h
+++ b/aos/linux_code/logging/binary_log_file.h
@@ -118,6 +118,9 @@
void MapNextPage();
void Unmap(void *location);
+ // Tries reading from the current page to see if it fails because the file
+ // isn't big enough.
+ void CheckCurrentPageReadable();
// Advances position to the next (aligned) location.
void AlignPosition() {
diff --git a/aos/linux_code/logging/binary_log_writer.cc b/aos/linux_code/logging/binary_log_writer.cc
index c404c39..76b53ef 100644
--- a/aos/linux_code/logging/binary_log_writer.cc
+++ b/aos/linux_code/logging/binary_log_writer.cc
@@ -89,7 +89,7 @@
LOG(INFO, "Could not find aos_log-current\n");
printf("Could not find aos_log-current\n");
}
- if (asprintf(filename, "%s/aos_log-%d", directory, fileindex) == -1) {
+ if (asprintf(filename, "%s/aos_log-%03d", directory, fileindex) == -1) {
aos::Die("couldn't create final name because of %d (%s)\n",
errno, strerror(errno));
}
diff --git a/aos/linux_code/logging/linux_logging.cc b/aos/linux_code/logging/linux_logging.cc
index 0adce79..dc322ed 100644
--- a/aos/linux_code/logging/linux_logging.cc
+++ b/aos/linux_code/logging/linux_logging.cc
@@ -8,33 +8,35 @@
#include <errno.h>
#include <unistd.h>
#include <limits.h>
+#include <inttypes.h>
#include <algorithm>
#include "aos/common/die.h"
#include "aos/common/logging/logging_impl.h"
#include "aos/linux_code/ipc_lib/queue.h"
+#include "aos/common/time.h"
namespace aos {
namespace logging {
-namespace {
-
-RawQueue *queue;
-
-} // namespace
namespace linux_code {
namespace {
-class LinuxQueueLogImplementation : public LogImplementation {
- LogMessage *GetMessageOrDie() {
- LogMessage *message = static_cast<LogMessage *>(queue->GetMessage());
- if (message == NULL) {
- LOG(FATAL, "%p->GetMessage() failed\n", queue);
- } else {
- return message;
- }
- }
+RawQueue *queue = NULL;
+int dropped_messages = 0;
+int32_t dropped_start_seconds, dropped_start_nseconds;
+
+LogMessage *GetMessageOrDie() {
+ LogMessage *message = static_cast<LogMessage *>(queue->GetMessage());
+ if (message == NULL) {
+ LOG(FATAL, "%p->GetMessage() failed\n", queue);
+ } else {
+ return message;
+ }
+}
+
+class LinuxQueueLogImplementation : public LogImplementation {
virtual void DoLog(log_level level, const char *format, va_list ap) override {
LogMessage *message = GetMessageOrDie();
internal::FillInMessage(level, format, ap, message);
@@ -91,9 +93,30 @@
}
void Write(LogMessage *msg) {
- // TODO(brians): Keep track of if we overflow the queue.
- if (!queue->WriteMessage(msg, RawQueue::kOverride)) {
- LOG(FATAL, "writing failed\n");
+ if (__builtin_expect(dropped_messages > 0, 0)) {
+ LogMessage *dropped_message = GetMessageOrDie();
+ internal::FillInMessageVarargs(
+ ERROR, dropped_message,
+ "%d logs starting at %" PRId32 ".%" PRId32 " dropped\n",
+ dropped_messages, dropped_start_seconds, dropped_start_nseconds);
+ if (queue->WriteMessage(dropped_message, RawQueue::kNonBlock)) {
+ dropped_messages = 0;
+ internal::PrintMessage(stderr, *dropped_message);
+ } else {
+ // Don't even bother trying to write this message because it's not likely
+ // to work and it would be confusing to have one log in the middle of a
+ // string of failures get through.
+ ++dropped_messages;
+ queue->FreeMessage(msg);
+ return;
+ }
+ }
+ if (!queue->WriteMessage(msg, RawQueue::kNonBlock)) {
+ if (dropped_messages == 0) {
+ dropped_start_seconds = msg->seconds;
+ dropped_start_nseconds = msg->nseconds;
+ }
+ ++dropped_messages;
}
}
diff --git a/aos/linux_code/logging/linux_logging.h b/aos/linux_code/logging/linux_logging.h
index 928a480..3dc8763 100644
--- a/aos/linux_code/logging/linux_logging.h
+++ b/aos/linux_code/logging/linux_logging.h
@@ -10,7 +10,7 @@
// Calls AddImplementation to register the usual linux logging implementation
// which sends the messages through a queue. This implementation relies on
// another process(es) to read the log messages that it puts into the queue.
-// It gets called by aos::Init*.
+// This function is usually called by aos::Init*.
void Register();
// Fairly simple wrappers around the raw queue calls.
diff --git a/aos/linux_code/queue-tmpl.h b/aos/linux_code/queue-tmpl.h
index 84eb02c..98091cf 100644
--- a/aos/linux_code/queue-tmpl.h
+++ b/aos/linux_code/queue-tmpl.h
@@ -182,11 +182,8 @@
template <class T>
bool Queue<T>::FetchNext() {
Init();
- // TODO(aschuh): Use RawQueue::ReadMessageIndex so that multiple readers
- // reading don't randomly get only part of the messages.
- // Document here the tradoffs that are part of each method.
const T *msg = static_cast<const T *>(
- queue_->ReadMessage(RawQueue::kNonBlock));
+ queue_->ReadMessageIndex(RawQueue::kNonBlock, &index_));
// Only update the internal pointer if we got a new message.
if (msg != NULL) {
queue_msg_.reset(msg);
@@ -197,7 +194,7 @@
template <class T>
bool Queue<T>::FetchNextBlocking() {
Init();
- const T *msg = static_cast<const T *>(queue_->ReadMessage(RawQueue::kBlock));
+ const T *msg = static_cast<const T *>(queue_->ReadMessageIndex(RawQueue::kBlock, &index_));
queue_msg_.reset(msg);
assert (msg != NULL);
return true;
@@ -206,8 +203,8 @@
template <class T>
bool Queue<T>::FetchLatest() {
Init();
- const T *msg = static_cast<const T *>(queue_->ReadMessage(
- RawQueue::kFromEnd | RawQueue::kNonBlock | RawQueue::kPeek));
+ const T *msg = static_cast<const T *>(queue_->ReadMessageIndex(
+ RawQueue::kFromEnd | RawQueue::kNonBlock, &index_));
// Only update the internal pointer if we got a new message.
if (msg != NULL && msg != queue_msg_.get()) {
queue_msg_.reset(msg);
diff --git a/aos/prime/output/motor_output.cc b/aos/prime/output/motor_output.cc
index a532183..0bea8b2 100644
--- a/aos/prime/output/motor_output.cc
+++ b/aos/prime/output/motor_output.cc
@@ -36,6 +36,7 @@
MotorOutput::MotorOutput()
: socket_(NetworkPort::kMotors, ::aos::NetworkAddress::kCRIO) {
+ values_.solenoid_values = 0;
}
void MotorOutput::Run() {
@@ -50,7 +51,6 @@
values_.pressure_switch_channel = 0;
values_.compressor_channel = 0;
values_.solenoid_module = -1;
- values_.solenoid_values = 0;
RunIteration();
diff --git a/bbb_cape/src/bbb/bbb.gyp b/bbb_cape/src/bbb/bbb.gyp
index 8efe553..1da4732 100644
--- a/bbb_cape/src/bbb/bbb.gyp
+++ b/bbb_cape/src/bbb/bbb.gyp
@@ -11,6 +11,16 @@
},
'targets': [
{
+ 'target_name': 'led',
+ 'type': 'static_library',
+ 'sources': [
+ 'led.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'crc',
'type': 'static_library',
'sources': [
diff --git a/bbb_cape/src/bbb/gpios.h b/bbb_cape/src/bbb/gpios.h
index 1471b43..0ba3323 100644
--- a/bbb_cape/src/bbb/gpios.h
+++ b/bbb_cape/src/bbb/gpios.h
@@ -1,5 +1,5 @@
-#ifndef BBB_CAPE_SRC_BBB_CAPE_CONTROL_H_
-#define BBB_CAPE_SRC_BBB_CAPE_CONTROL_H_
+#ifndef BBB_CAPE_SRC_BBB_GPIOS_H_
+#define BBB_CAPE_SRC_BBB_GPIOS_H_
#include <stdio.h>
@@ -31,4 +31,4 @@
} // namespace bbb
-#endif
+#endif // BBB_CAPE_SRC_BBB_GPIOS_H_
diff --git a/bbb_cape/src/bbb/led.cc b/bbb_cape/src/bbb/led.cc
new file mode 100644
index 0000000..5dc5e71
--- /dev/null
+++ b/bbb_cape/src/bbb/led.cc
@@ -0,0 +1,60 @@
+#include "bbb_cape/src/bbb/led.h"
+
+#include "aos/common/logging/logging.h"
+
+#include <string.h>
+#include <errno.h>
+
+#define DIRECTORY "/sys/class/leds/beaglebone:green:usr%d/"
+
+namespace bbb {
+
+LED::LED(int number) : number_(number) {
+ char trigger_path[64];
+ snprintf(trigger_path, sizeof(trigger_path), DIRECTORY "trigger", number_);
+ FILE *trigger_handle = fopen(trigger_path, "w");
+ if (trigger_handle == nullptr) {
+ LOG(FATAL, "couldn't open trigger file for LED %d because of %d: %s\n",
+ number_, errno, strerror(errno));
+ }
+ if (fputs("none", trigger_handle) < 0) {
+ LOG(FATAL,
+ "writing 'none' to file %p (trigger for LED %d) failed with %d: %s\n",
+ trigger_handle, number_, errno, strerror(errno));
+ }
+ if (fclose(trigger_handle) == -1) {
+ LOG(WARNING, "fclose(%p) failed with %d: %s\n",
+ trigger_handle, errno, strerror(errno));
+ }
+
+ char brightness_path[64];
+ snprintf(brightness_path, sizeof(brightness_path),
+ DIRECTORY "brightness", number_);
+
+ brightness_handle_ = fopen(brightness_path, "w");
+ if (brightness_handle_ == nullptr) {
+ LOG(FATAL, "fopen('%s', 'w') failed with %d: %s\n",
+ brightness_path, errno, strerror(errno));
+ }
+}
+
+LED::~LED() {
+ if (fclose(brightness_handle_) == -1) {
+ LOG(WARNING, "fclose(%p) failed with %d: %s\n",
+ brightness_handle_, errno, strerror(errno));
+ }
+}
+
+void LED::Set(bool on) {
+ rewind(brightness_handle_);
+ if (fputs(on ? "255" : "0", brightness_handle_) == EOF) {
+ LOG(FATAL, "fputs(255|0, %p) for LED %d failed with %d: %s\n",
+ brightness_handle_, number_, errno, strerror(errno));
+ }
+ if (fflush(brightness_handle_) == EOF) {
+ LOG(FATAL, "fflush(%p) for LED %d failed with %d: %s\n",
+ brightness_handle_, number_, errno, strerror(errno));
+ }
+}
+
+} // namespace bbb
diff --git a/bbb_cape/src/bbb/led.h b/bbb_cape/src/bbb/led.h
new file mode 100644
index 0000000..19976e6
--- /dev/null
+++ b/bbb_cape/src/bbb/led.h
@@ -0,0 +1,27 @@
+#ifndef BBB_CAPE_SRC_BBB_LED_H_
+#define BBB_CAPE_SRC_BBB_LED_H_
+
+#include <stdio.h>
+
+#include "aos/common/macros.h"
+
+namespace bbb {
+
+// Allows easily controlling one of the LEDs.
+class LED {
+ public:
+ LED(int number);
+ ~LED();
+
+ void Set(bool on);
+
+ private:
+ FILE *brightness_handle_ = nullptr;
+ const int number_;
+
+ DISALLOW_COPY_AND_ASSIGN(LED);
+};
+
+} // namespace bbb
+
+#endif // BBB_CAPE_SRC_BBB_LED_H_
diff --git a/bbb_cape/src/cape/analog.c b/bbb_cape/src/cape/analog.c
index 855098c..297afd6 100644
--- a/bbb_cape/src/cape/analog.c
+++ b/bbb_cape/src/cape/analog.c
@@ -2,8 +2,6 @@
#include <string.h>
-#include <STM32F2XX.h>
-
#include "cape/util.h"
#include "cape/led.h"
@@ -20,27 +18,13 @@
#define NUM_CHANNELS 8
+// This file handles reading values from the MCP3008-I/SL ADC.
+
uint16_t analog_readings[NUM_CHANNELS] __attribute__((aligned(8)));
static volatile int current_channel;
static volatile int partial_reading;
static volatile int frame;
-
-static void start_read(int channel) {
- // This needs to wait 13 cycles between enabling the CSEL pin and starting to
- // send data.
- // (100ns+8ns)*120MHz = 12.96
-
- // Clear the CSEL pin to select it.
- for (int i = 0; i < 9; ++i) gpio_off(CSEL_GPIO, CSEL_NUM);
- current_channel = channel;
- partial_reading = 0;
- frame = 0;
- SPI->DR = 1; // start bit
- uint16_t data = (1 << 15) /* not differential */ |
- (channel << 12);
- while (!(SPI->SR & SPI_SR_TXE));
- SPI->DR = data;
-}
+static volatile int analog_errors;
void SPI_IRQHandler(void) {
uint32_t status = SPI->SR;
@@ -50,15 +34,15 @@
frame = 1;
partial_reading = value;
} else {
+ frame = 2;
// Masking off the high bits is important because there's nothing driving
// the MISO line during the time the MCU receives them.
- analog_readings[current_channel] = (partial_reading << 16 | value) & 0x3FF;
+ analog_readings[current_channel] =
+ (partial_reading << 16 | value) & 0x3FF;
for (int i = 0; i < 100; ++i) gpio_off(CSEL_GPIO, CSEL_NUM);
gpio_on(CSEL_GPIO, CSEL_NUM);
- TIM->CR1 = TIM_CR1_OPM;
- TIM->EGR = TIM_EGR_UG;
- TIM->CR1 |= TIM_CR1_CEN;
+ current_channel = (current_channel + 1) % NUM_CHANNELS;
}
}
}
@@ -66,7 +50,27 @@
void TIM_IRQHandler(void) {
TIM->SR = ~TIM_SR_CC1IF;
- start_read((current_channel + 1) % NUM_CHANNELS);
+ if (frame != 2) {
+ // We're not done with the previous reading yet, so we're going to reset and
+ // try again.
+ // 270ns*120MHz = 32.4
+ for (int i = 0; i < 33; ++i) gpio_on(CSEL_GPIO, CSEL_NUM);
+ ++analog_errors;
+ }
+
+ // This needs to wait 13 cycles between enabling the CSEL pin and starting to
+ // send data.
+ // (100ns+8ns)*120MHz = 12.96
+
+ // Clear the CSEL pin to select it.
+ for (int i = 0; i < 9; ++i) gpio_off(CSEL_GPIO, CSEL_NUM);
+ partial_reading = 0;
+ frame = 0;
+ SPI->DR = 1; // start bit
+ uint16_t data = (1 << 15) /* not differential */ |
+ (current_channel << 12);
+ while (!(SPI->SR & SPI_SR_TXE));
+ SPI->DR = data;
}
void analog_init(void) {
@@ -87,7 +91,7 @@
NVIC_SetPriority(TIM_IRQn, 6);
NVIC_EnableIRQ(TIM_IRQn);
- TIM->CR1 = TIM_CR1_OPM;
+ TIM->CR1 = 0;
TIM->DIER = TIM_DIER_CC1IE;
TIM->CCMR1 = 0;
// Make each tick take 1500ns.
@@ -104,5 +108,17 @@
SPI->CR2 = SPI_CR2_RXNEIE;
SPI->CR1 |= SPI_CR1_SPE; // enable it
- start_read(0);
+ current_channel = 0;
+ analog_errors = 0;
+
+ TIM->EGR = TIM_EGR_UG;
+ TIM->CR1 |= TIM_CR1_CEN;
+}
+
+int analog_get_errors(void) {
+ NVIC_DisableIRQ(TIM_IRQn);
+ int r = analog_errors;
+ analog_errors = 0;
+ NVIC_EnableIRQ(TIM_IRQn);
+ return r;
}
diff --git a/bbb_cape/src/cape/analog.h b/bbb_cape/src/cape/analog.h
index 50038d5..cd9ce82 100644
--- a/bbb_cape/src/cape/analog.h
+++ b/bbb_cape/src/cape/analog.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+#include <STM32F2XX.h>
+
// Starts up constantly reading analog values and storing them in an array to
// be retrieved by analog_get.
void analog_init(void);
@@ -14,4 +16,9 @@
return analog_readings[num];
}
+// Returns the number of errors since last called.
+// Must be called from something with priority equal to or lower than our
+// timer's IRQ.
+int analog_get_errors(void);
+
#endif // CAPE_ANALOG_H_
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index 4aa2a10..943e8c2 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -38,6 +38,8 @@
// contents of flash for the main code (aka what's in the .hex file).
uint32_t flash_checksum;
+ uint8_t analog_errors;
+
struct {
// If the current gyro_angle has been not updated because of a bad
// reading from the sensor.
diff --git a/bbb_cape/src/cape/fill_packet.c b/bbb_cape/src/cape/fill_packet.c
index 97f9d8a..8d1f72a 100644
--- a/bbb_cape/src/cape/fill_packet.c
+++ b/bbb_cape/src/cape/fill_packet.c
@@ -40,6 +40,7 @@
packet->uninitialized_gyro = !gyro_output.initialized;
packet->zeroing_gyro = !gyro_output.zeroed;
packet->bad_gyro = gyro_output.gyro_bad;
+ packet->analog_errors = analog_get_errors();
robot_fill_packet(packet);
//counter_update_u64_u16(×tamp, TIMESTAMP_TIM->CNT);
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 7336b12..fe17676 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -1,3 +1,6 @@
+#ifndef FRC971_ACTIONS_ACTION_H_
+#define FRC971_ACTIONS_ACTION_H_
+
#include <stdio.h>
#include <functional>
@@ -32,6 +35,11 @@
while (!action_q_->goal->run) {
LOG(INFO, "Waiting for an action request.\n");
action_q_->goal.FetchNextBlocking();
+ if (!action_q_->goal->run) {
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ }
}
LOG(INFO, "Starting action\n");
if (!action_q_->status.MakeWithBuilder().running(true).Send()) {
@@ -57,13 +65,13 @@
bool WaitUntil(::std::function<bool(void)> done_condition) {
while (!done_condition()) {
if (ShouldCancel() || abort_) {
- // clear abort bit as we have just aqborted
+ // Clear abort bit as we have just aborted.
abort_ = false;
return true;
}
}
if (ShouldCancel() || abort_) {
- // clear abort bit as we have just aqborted
+ // Clear abort bit as we have just aborted.
abort_ = false;
return true;
} else {
@@ -76,7 +84,7 @@
action_q_->goal.FetchLatest();
bool ans = !action_q_->goal->run;
if (ans) {
- LOG(INFO, "Time to exit auto mode\n");
+ LOG(INFO, "Time to stop action\n");
}
return ans;
}
@@ -93,3 +101,4 @@
} // namespace actions
} // namespace frc971
+#endif // FRC971_ACTIONS_ACTION_H_
diff --git a/frc971/actions/action_client.h b/frc971/actions/action_client.h
new file mode 100644
index 0000000..2a7d65f
--- /dev/null
+++ b/frc971/actions/action_client.h
@@ -0,0 +1,117 @@
+#ifndef FRC971_ACTIONS_ACTION_CLIENT_H_
+#define FRC971_ACTIONS_ACTION_CLIENT_H_
+
+#include <type_traits>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+
+namespace frc971 {
+
+class Action {
+ public:
+ // Cancels the action.
+ void Cancel() { DoCancel(); }
+ // Returns true if the action is currently running.
+ bool Running() { return DoRunning(); }
+ // Starts the action.
+ void Start() { DoStart(); }
+
+ // Waits until the action has finished.
+ void WaitUntilDone() { DoWaitUntilDone(); }
+
+ virtual ~Action() {}
+
+ private:
+ virtual void DoCancel() = 0;
+ virtual bool DoRunning() = 0;
+ virtual void DoStart() = 0;
+ virtual void DoWaitUntilDone() = 0;
+};
+
+// Templated subclass to hold the type information.
+template <typename T>
+class TypedAction : public Action {
+ public:
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+
+ TypedAction(T *queue_group)
+ : queue_group_(queue_group),
+ goal_(queue_group_->goal.MakeMessage()),
+ has_started_(false) {}
+
+ // Returns the current goal that will be sent when the action is sent.
+ GoalType *GetGoal() { return goal_.get(); }
+
+ virtual ~TypedAction() {
+ LOG(INFO, "Calling destructor\n");
+ DoCancel();
+ }
+
+ private:
+ // Cancels the action.
+ virtual void DoCancel() {
+ LOG(INFO, "Canceling action on queue %s\n", queue_group_->goal.name());
+ queue_group_->goal.MakeWithBuilder().run(false).Send();
+ }
+
+ // Returns true if the action is running or we don't have an initial response
+ // back from it to signal whether or not it is running.
+ virtual bool DoRunning() {
+ if (has_started_) {
+ queue_group_->status.FetchLatest();
+ } else if (queue_group_->status.FetchLatest()) {
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
+ }
+ return !has_started_ ||
+ (queue_group_->status.get() && queue_group_->status->running);
+ }
+
+ // Returns true if the action is running or we don't have an initial response
+ // back from it to signal whether or not it is running.
+ virtual void DoWaitUntilDone() {
+ queue_group_->status.FetchLatest();
+ while (true) {
+ if (has_started_) {
+ queue_group_->status.FetchNextBlocking();
+ } else {
+ queue_group_->status.FetchNextBlocking();
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
+ }
+ if (has_started_ &&
+ (queue_group_->status.get() && !queue_group_->status->running)) {
+ return;
+ }
+ }
+ }
+
+ // Starts the action if a goal has been created.
+ virtual void DoStart() {
+ if (goal_) {
+ goal_->run = true;
+ goal_.Send();
+ has_started_ = false;
+ LOG(INFO, "Starting action\n");
+ } else {
+ has_started_ = true;
+ }
+ }
+
+ T *queue_group_;
+ ::aos::ScopedMessagePtr<GoalType> goal_;
+ // Track if we have seen a response to the start message.
+ // If we haven't, we are considered running regardless.
+ bool has_started_;
+};
+
+} // namespace frc971
+
+#endif // FRC971_ACTIONS_ACTION_CLIENT_H_
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index a0dca81..7cfb7a5 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -1,6 +1,21 @@
{
'targets': [
{
+ 'target_name': 'action_client',
+ 'type': 'static_library',
+ 'sources': [
+ #'action_client.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ },
+ {
'target_name': 'shoot_action_queue',
'type': 'static_library',
'sources': ['shoot_action.q'],
@@ -15,20 +30,51 @@
],
'includes': ['../../aos/build/queues.gypi'],
},
- {
- 'target_name': 'action',
+ {
+ 'target_name': 'drivetrain_action_queue',
'type': 'static_library',
+ 'sources': ['drivetrain_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
'dependencies': [
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/build/aos.gyp:logging',
- ],
+ '<(AOS)/common/common.gyp:queues',
+ ],
'export_dependent_settings': [
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/build/aos.gyp:logging',
- ]
- },
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'selfcatch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['selfcatch_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'catch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['catch_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
{
'target_name': 'shoot_action_lib',
'type': 'static_library',
@@ -44,6 +90,90 @@
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ 'action_client',
+ 'action',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'shoot_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_action.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ 'action_client',
+ 'action',
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'drivetrain_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'action',
+ 'type': 'static_library',
+ 'sources': [
+ #'action.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ },
+ {
+ 'target_name': 'selfcatch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'selfcatch_action.cc',
+ ],
+ 'dependencies': [
+ 'selfcatch_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ ],
+ },
+ {
+ 'target_name': 'catch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'catch_action.cc',
+ ],
+ 'dependencies': [
+ 'catch_action_queue',
+ 'action',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
],
},
{
@@ -56,7 +186,46 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'shoot_action_queue',
'shoot_action_lib',
- 'action',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'selfcatch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'selfcatch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'selfcatch_action_queue',
+ 'selfcatch_action_lib',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'catch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'catch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'catch_action_queue',
+ 'catch_action_lib',
+ 'action',
],
},
],
diff --git a/frc971/actions/catch_action.cc b/frc971/actions/catch_action.cc
new file mode 100644
index 0000000..1fddd30
--- /dev/null
+++ b/frc971/actions/catch_action.cc
@@ -0,0 +1,154 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/catch_action.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/queues/other_sensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+CatchAction::CatchAction(actions::CatchActionGroup* s)
+ : actions::ActionBase<actions::CatchActionGroup>(s) {}
+
+void CatchAction::RunAction() {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (control_loops::claw_queue_group.goal.get() == nullptr) {
+ LOG(WARNING, "no claw goal\n");
+ return;
+ }
+
+ // Set claw angle.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle - kCatchSeparation / 2.0)
+ .separation_angle(kCatchSeparation)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ LOG(INFO, "Waiting for the claw to be ready\n");
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&CatchAction::DoneSetupCatch, this))) return;
+ LOG(INFO, "Waiting for the sonar\n");
+
+ close_count_ = 0;
+
+ // wait for the sonar to trigger
+ if (WaitUntil(::std::bind(&CatchAction::DoneFoundSonar, this))) return;
+
+ LOG(INFO, "Closing the claw\n");
+
+ // close the claw
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle)
+ .separation_angle(0.0)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
+
+ for (int i = 0; i < 5; ++i) {
+ aos::time::SleepFor(aos::time::Time::InSeconds(0.05));
+ if (ShouldCancel()) return;
+ }
+}
+
+
+/*bool CatchAction::DoneBallIn() {
+ if (!sensors::othersensors.FetchLatest()) {
+ sensors::othersensors.FetchNextBlocking();
+ }
+ if (sensors::othersensors->travis_hall_effect_distance > 0.005) {
+ LOG(INFO, "Ball in at %.2f.\n",
+ sensors::othersensors->travis_hall_effect_distance);
+ return true;
+ }
+ return false;
+}*/
+
+bool CatchAction::DoneClawWithBall() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 1.0) &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.10) &&
+ (::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle) <
+ 0.4);
+
+ if (!ans) {
+ LOG(INFO,
+ "Claw is ready %d zeroed %d bottom_velocity %f bottom %f sep %f\n", ans,
+ control_loops::claw_queue_group.status->zeroed,
+ ::std::abs(control_loops::claw_queue_group.status->bottom_velocity),
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle),
+ ::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle));
+ }
+ return ans;
+}
+
+bool CatchAction::DoneFoundSonar() {
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ LOG(DEBUG, "Sonar at %.2f.\n", sensors::other_sensors->sonar_distance);
+ if (sensors::other_sensors->sonar_distance > 0.1 &&
+ sensors::other_sensors->sonar_distance < kSonarTriggerDist) {
+ ++close_count_;
+ } else {
+ close_count_ = 0;
+ }
+ if (close_count_ > 50) {
+ return true;
+ }
+ return false;
+}
+
+bool CatchAction::DoneSetupCatch() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ // Make sure that the shooter and claw has reached the necessary state.
+ // Check the current positions of the various mechanisms to make sure that we
+ // avoid race conditions where we send it a new goal but it still thinks that
+ // it has the old goal and thinks that it is already done.
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.15;
+ bool open_enough =
+ control_loops::claw_queue_group.status->separation > kCatchMinSeparation;
+
+ if (claw_angle_correct && open_enough) {
+ LOG(INFO, "Claw ready for catching.\n");
+ return true;
+ }
+
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.h b/frc971/actions/catch_action.h
new file mode 100644
index 0000000..56c5f42
--- /dev/null
+++ b/frc971/actions/catch_action.h
@@ -0,0 +1,42 @@
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class CatchAction : public ActionBase<CatchActionGroup> {
+ public:
+
+ explicit CatchAction(CatchActionGroup* s);
+
+ // Actually executes the action of moving the claw into position and closing
+ // it.
+ virtual void RunAction();
+
+ static constexpr double kCatchSeparation = 0.8;
+ static constexpr double kCatchMinSeparation = 0.65;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.725;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupCatch();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+
+ private:
+ int close_count_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.q b/frc971/actions/catch_action.q
new file mode 100644
index 0000000..33b21f0
--- /dev/null
+++ b/frc971/actions/catch_action.q
@@ -0,0 +1,19 @@
+package frc971.actions;
+
+queue_group CatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double catch_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group CatchActionGroup catch_action;
diff --git a/frc971/actions/catch_action_main.cc b/frc971/actions/catch_action_main.cc
new file mode 100644
index 0000000..88fa48b
--- /dev/null
+++ b/frc971/actions/catch_action_main.cc
@@ -0,0 +1,20 @@
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/catch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::CatchAction action_catch(
+ &::frc971::actions::catch_action);
+ action_catch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
new file mode 100644
index 0000000..1ca6219
--- /dev/null
+++ b/frc971/actions/drivetrain_action.cc
@@ -0,0 +1,65 @@
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/actions/drivetrain_action.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace actions {
+
+DrivetrainAction::DrivetrainAction(actions::DrivetrainActionQueueGroup* s)
+ : actions::ActionBase<actions::DrivetrainActionQueueGroup>(s) {}
+
+void DrivetrainAction::RunAction() {
+ const double yoffset = action_q_->goal->y_offset;
+ LOG(INFO, "Going to move %f\n", yoffset);
+
+ // Measured conversion to get the distance right.
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
+ ::Eigen::Matrix<double, 2, 1> profile_goal_state;
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+
+ profile.set_maximum_acceleration(2.2);
+ profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
+
+ while (true) {
+ // wait until next 10ms tick
+ ::aos::time::PhasedLoop10MS(5000);
+ profile_goal_state = profile.Update(yoffset, goal_velocity);
+
+ if (::std::abs(profile_goal_state(0, 0) - yoffset) < epsilon) break;
+ if (ShouldCancel()) return;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ profile_goal_state(0, 0) + action_q_->goal->left_initial_position,
+ profile_goal_state(0, 0) + action_q_->goal->right_initial_position);
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .highgear(false)
+ .left_goal(profile_goal_state(0, 0) + action_q_->goal->left_initial_position)
+ .right_goal(profile_goal_state(0, 0) + action_q_->goal->right_initial_position)
+ .left_velocity_goal(profile_goal_state(1, 0))
+ .right_velocity_goal(profile_goal_state(1, 0))
+ .Send();
+ }
+ LOG(INFO, "Done moving\n");
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction() {
+ return ::std::unique_ptr<
+ TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
+ new TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
+ &::frc971::actions::drivetrain_action));
+}
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/drivetrain_action.h b/frc971/actions/drivetrain_action.h
new file mode 100644
index 0000000..b164e22
--- /dev/null
+++ b/frc971/actions/drivetrain_action.h
@@ -0,0 +1,22 @@
+#include <memory>
+
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
+
+namespace frc971 {
+namespace actions {
+
+class DrivetrainAction : public ActionBase<actions::DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainAction(actions::DrivetrainActionQueueGroup* s);
+
+ virtual void RunAction();
+};
+
+// Makes a new DrivetrainAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction();
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
new file mode 100644
index 0000000..cde518c
--- /dev/null
+++ b/frc971/actions/drivetrain_action.q
@@ -0,0 +1,22 @@
+package frc971.actions;
+
+queue_group DrivetrainActionQueueGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double left_initial_position;
+ double right_initial_position;
+ double y_offset;
+ double maximum_velocity;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/frc971/actions/drivetrain_action_main.cc b/frc971/actions/drivetrain_action_main.cc
new file mode 100644
index 0000000..0251e25
--- /dev/null
+++ b/frc971/actions/drivetrain_action_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/drivetrain_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/selfcatch_action.cc b/frc971/actions/selfcatch_action.cc
index fc9a06c..eef0a05 100644
--- a/frc971/actions/selfcatch_action.cc
+++ b/frc971/actions/selfcatch_action.cc
@@ -8,7 +8,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/control_loops/claw/claw.q.h"
-#include "frc971/queues/othersensors.q.h"
+#include "frc971/queues/other_sensors.q.h"
namespace frc971 {
namespace actions {
@@ -110,7 +110,7 @@
// head to a finshed pose
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
kFinishAngle)
- .separation_angle(kFinishAngle).intake(0.0).centering(0.0).Send()) {
+ .separation_angle(0.0).intake(0.0).centering(0.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
}
@@ -124,12 +124,12 @@
bool SelfCatchAction::DoneBallIn() {
- if (!queues::othersensors.FetchLatest()) {
- queues::othersensors.FetchNextBlocking();
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
}
- if (queues::othersensors->travis_hall_effect_distance > 0.005)
+ if (sensors::other_sensors->plunger_hall_effect_distance > 0.005) {
LOG(INFO, "Ball in at %.2f.\n",
- queues::othersensors->travis_hall_effect_distance);
+ sensors::other_sensors->plunger_hall_effect_distance);
return true;
}
return false;
@@ -149,12 +149,12 @@
}
bool SelfCatchAction::DoneFoundSonar() {
- if (!queues::othersensors.FetchLatest()) {
- queues::othersensors.FetchNextBlocking();
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
}
- if (queues::othersensors->sonar_distance > 0.3 &&
- queues::othersensors->sonar_distance < kSonarTriggerDist) {
- LOG(INFO, "Hit Sonar at %.2f.\n", queues::othersensors->sonar_distance);
+ if (sensors::other_sensors->sonar_distance > 0.3 &&
+ sensors::other_sensors->sonar_distance < kSonarTriggerDist) {
+ LOG(INFO, "Hit Sonar at %.2f.\n", sensors::other_sensors->sonar_distance);
return true;
}
return false;
@@ -167,10 +167,30 @@
if (!control_loops::claw_queue_group.status.FetchLatest()) {
control_loops::claw_queue_group.status.FetchNextBlocking();
}
+ if (!control_loops::shooter_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch shooter goal.\n");
+ }
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch claw goal.\n");
+ }
// Make sure that both the shooter and claw have reached the necessary
// states.
+ // Check the current positions of the various mechanisms to make sure that we
+ // avoid race conditions where we send it a new goal but it still thinks that
+ // it has the old goal and thinks that it is already done.
+ bool shooter_power_correct =
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power) <
+ 0.005;
+
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.005;
+
if (control_loops::shooter_queue_group.status->ready &&
- control_loops::claw_queue_group.status->done_with_ball) {
+ control_loops::claw_queue_group.status->done_with_ball &&
+ shooter_power_correct && claw_angle_correct) {
LOG(INFO, "Claw and Shooter ready for shooting.\n");
// TODO(james): Get realer numbers for shooter_action.
return true;
@@ -181,7 +201,7 @@
// continually changing, we will need testing to see
control_loops::drivetrain.status.FetchLatest();
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle +
+ selfcatch_action.goal->shot_angle +
SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
.separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
@@ -199,6 +219,7 @@
if (!control_loops::claw_queue_group.status.FetchLatest()) {
control_loops::claw_queue_group.status.FetchNextBlocking();
}
+
if (control_loops::claw_queue_group.status->separation >
values.shooter_action.claw_shooting_separation) {
LOG(INFO, "Opened up enough to shoot.\n");
diff --git a/frc971/actions/selfcatch_action.h b/frc971/actions/selfcatch_action.h
index 0b34e8c..c57d24a 100644
--- a/frc971/actions/selfcatch_action.h
+++ b/frc971/actions/selfcatch_action.h
@@ -5,14 +5,14 @@
namespace frc971 {
namespace actions {
-class SelfCatchAction : public ActionBase<actions::SelfCatchActionGroup> {
+class SelfCatchAction : public ActionBase<SelfCatchActionGroup> {
public:
- explicit SelfCatchAction(actions::SelfCatchActionGroup* s);
+ explicit SelfCatchAction(SelfCatchActionGroup* s);
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
- void RunAction();
+ virtual void RunAction();
// calc an offset to our requested shot based on robot speed
double SpeedToAngleOffset(double speed);
@@ -21,6 +21,7 @@
static constexpr double kShotPower = 100.0;
static constexpr double kCatchSeperation = 1.0;
static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.8;
static constexpr double kCatchCentering = 12.0;
static constexpr double kFinishAngle = 0.2;
@@ -40,7 +41,6 @@
// to track when shot is complete
int previous_shots_;
- aos::Time::Time action_step_start_;
};
} // namespace actions
diff --git a/frc971/actions/selfcatch_action.q b/frc971/actions/selfcatch_action.q
index b0254f7..9ba21ad 100644
--- a/frc971/actions/selfcatch_action.q
+++ b/frc971/actions/selfcatch_action.q
@@ -8,7 +8,8 @@
message Goal {
// If true, run this action. If false, cancel the action if it is
// currently running.
- bool run; // Shot power in joules.
+ bool run;
+ double shot_angle;
};
queue Goal goal;
diff --git a/frc971/actions/selfcatch_action_main.cc b/frc971/actions/selfcatch_action_main.cc
index b56ed50..53600ac 100644
--- a/frc971/actions/selfcatch_action_main.cc
+++ b/frc971/actions/selfcatch_action_main.cc
@@ -5,7 +5,7 @@
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
#include "frc971/actions/selfcatch_action.q.h"
-#include "frc971/actions/self_catch_action.h"
+#include "frc971/actions/selfcatch_action.h"
using ::aos::time::Time;
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index adb6773..b1e27b4 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -20,63 +20,39 @@
// scale speed to a [0.0-1.0] on something close to the max
return (speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
}
-
void ShootAction::RunAction() {
- LOG(INFO, "Shooting at angle %f, power %f\n", shoot_action.goal->shot_angle,
- shoot_action.goal->shot_power);
+ InnerRunAction();
- // Set shot power
+ // Now do our 'finally' block and make sure that we aren't requesting shots
+ // continually.
+ control_loops::shooter_queue_group.goal.FetchLatest();
+ if (control_loops::shooter_queue_group.goal.get() == nullptr) {
+ return;
+ }
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
- .shot_power(shoot_action.goal->shot_power)
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
.shot_requested(false)
.unload_requested(false)
.load_requested(false)
.Send()) {
- LOG(ERROR, "Failed to send the shoot action\n");
+ LOG(WARNING, "sending shooter goal failed\n");
return;
}
+}
- // Set claw angle
- control_loops::drivetrain.status.FetchLatest();
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(shoot_action.goal->shot_angle +
- SpeedToAngleOffset(
- control_loops::drivetrain.status->robot_speed))
- .separation_angle(0.0)
- .intake(2.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
+void ShootAction::InnerRunAction() {
+ LOG(INFO, "Shooting at the original angle and power.\n");
// wait for claw to be ready
if (WaitUntil(::std::bind(&ShootAction::DoneSetupShot, this))) return;
- // Open up the claw in preparation for shooting.
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(shoot_action.goal->shot_angle +
- SpeedToAngleOffset(
- control_loops::drivetrain.status->robot_speed))
- .separation_angle(kClawShootingSeparationGoal)
- .intake(2.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
-
- // wait for the claw to open up a little before we shoot
- if (WaitUntil(::std::bind(&ShootAction::DonePreShotOpen, this))) return;
-
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.2));
-
- // Open up the claw in preparation for shooting.
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(shoot_action.goal->shot_angle +
- SpeedToAngleOffset(
- control_loops::drivetrain.status->robot_speed))
- .separation_angle(kClawShootingSeparationGoal)
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
.intake(0.0)
.centering(0.0)
.Send()) {
@@ -90,30 +66,48 @@
// again for another few cycles.
previous_shots_ = control_loops::shooter_queue_group.status->shots;
// Shoot!
- if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
- shoot_action.goal->shot_power)
- .shot_requested(true).unload_requested(false).load_requested(false)
- .Send()) {
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
+ .shot_requested(true)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
LOG(WARNING, "sending shooter goal failed\n");
return;
}
// wait for record of shot having been fired
if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
+
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
}
bool ClawIsReady() {
- control_loops::claw_queue_group.goal.FetchLatest();
- control_loops::claw_queue_group.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ }
+
bool ans =
control_loops::claw_queue_group.status->zeroed &&
(::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
0.5) &&
(::std::abs(control_loops::claw_queue_group.status->bottom -
- control_loops::claw_queue_group.goal->bottom_angle) < 0.04) &&
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.10) &&
(::std::abs(control_loops::claw_queue_group.status->separation -
control_loops::claw_queue_group.goal->separation_angle) <
- 0.2);
+ 0.4);
if (!ans) {
LOG(INFO,
"Claw is ready %d zeroed %d bottom_velocity %f bottom %f sep %f\n", ans,
@@ -158,23 +152,6 @@
return true;
}
- // update the claw position to track velocity
- // TODO(ben): the claw may never reach the goal if the velocity is
- // continually changing, we will need testing to see
- control_loops::drivetrain.status.FetchLatest();
- if (ShouldCancel()) return false;
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(shoot_action.goal->shot_angle +
- SpeedToAngleOffset(
- control_loops::drivetrain.status->robot_speed))
- .separation_angle(0.0)
- .intake(2.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- abort_ = true;
- return true;
- }
return false;
}
@@ -201,6 +178,14 @@
return false;
}
+::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
+MakeShootAction() {
+ return ::std::unique_ptr<
+ TypedAction< ::frc971::actions::ShootActionQueueGroup>>(
+ new TypedAction< ::frc971::actions::ShootActionQueueGroup>(
+ &::frc971::actions::shoot_action));
+}
+
} // namespace actions
} // namespace frc971
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
index 733af4e..6d29ca7 100644
--- a/frc971/actions/shoot_action.h
+++ b/frc971/actions/shoot_action.h
@@ -1,5 +1,8 @@
+#include <memory>
+
#include "frc971/actions/shoot_action.q.h"
#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
namespace frc971 {
namespace actions {
@@ -11,7 +14,8 @@
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
- void RunAction();
+ virtual void RunAction();
+ void InnerRunAction();
// calc an offset to our requested shot based on robot speed
double SpeedToAngleOffset(double speed);
@@ -32,6 +36,9 @@
int previous_shots_;
};
+// Makes a new ShootAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
+MakeShootAction();
+
} // namespace actions
} // namespace frc971
-
diff --git a/frc971/actions/shoot_action_main.cc b/frc971/actions/shoot_action_main.cc
index 31cea1f..18df9aa 100644
--- a/frc971/actions/shoot_action_main.cc
+++ b/frc971/actions/shoot_action_main.cc
@@ -1,7 +1,5 @@
-#include "stdio.h"
+#include <stdio.h>
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/time.h"
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
#include "frc971/actions/shoot_action.q.h"
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index ea2a74b..107b380 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -1,4 +1,6 @@
-#include "stdio.h"
+#include <stdio.h>
+
+#include <memory>
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
@@ -9,12 +11,19 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/shoot_action.h"
+#include "frc971/actions/drivetrain_action.h"
using ::aos::time::Time;
namespace frc971 {
namespace autonomous {
+namespace time = ::aos::time;
+
static double left_initial_position, right_initial_position;
bool ShouldExitAuto() {
@@ -48,42 +57,6 @@
.Send();
}
-void SetDriveGoal(double yoffset, double maximum_velocity = 1.5) {
- LOG(INFO, "Going to move %f\n", yoffset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::Eigen::Matrix<double, 2, 1> driveTrainState;
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
-
- profile.set_maximum_acceleration(2.0);
- profile.set_maximum_velocity(maximum_velocity);
-
- while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
- driveTrainState = profile.Update(yoffset, goal_velocity);
-
- if (::std::abs(driveTrainState(0, 0) - yoffset) < epsilon) break;
- if (ShouldExitAuto()) return;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- driveTrainState(0, 0) + left_initial_position,
- driveTrainState(0, 0) + right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(driveTrainState(0, 0) + left_initial_position)
- .right_goal(driveTrainState(0, 0) + right_initial_position)
- .left_velocity_goal(driveTrainState(1, 0))
- .right_velocity_goal(driveTrainState(1, 0))
- .Send();
- }
- left_initial_position += yoffset;
- right_initial_position += yoffset;
- LOG(INFO, "Done moving\n");
-}
-
void DriveSpin(double radians) {
LOG(INFO, "going to spin %f\n", radians);
@@ -123,13 +96,85 @@
LOG(INFO, "Done moving\n");
}
-void HandleAuto() {
- LOG(INFO, "Handling auto mode\n");
+void PositionClawVertically(double intake_power = 0.0, double centering_power = 0.0) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.0)
+ .separation_angle(0.0)
+ .intake(intake_power)
+ .centering(centering_power)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
- ResetDrivetrain();
+void PositionClawBackIntake() {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(-2.273474)
+ .separation_angle(0.0)
+ .intake(12.0)
+ .centering(12.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
- if (ShouldExitAuto()) return;
-
+void PositionClawForShot() {
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.86)
+ .separation_angle(0.10)
+ .intake(4.0)
+ .centering(1.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void SetShotPower(double power) {
+ LOG(INFO, "Setting shot power to %f\n", power);
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ }
+}
+
+void WaitUntilDoneOrCanceled(Action *action) {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ if (!action->Running() || ShouldExitAuto()) {
+ return;
+ }
+ }
+}
+
+void Shoot() {
+ // Shoot.
+ auto shoot_action = actions::MakeShootAction();
+ shoot_action->Start();
+ WaitUntilDoneOrCanceled(shoot_action.get());
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+SetDriveGoal(double distance, double maximum_velocity = 1.7) {
+ LOG(INFO, "Driving to %f\n", distance);
+ auto drivetrain_action = actions::MakeDrivetrainAction();
+ drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
+ drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
+ drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
+ drivetrain_action->Start();
+ // Uncomment to make relative again.
+ left_initial_position += distance;
+ right_initial_position += distance;
+ return ::std::move(drivetrain_action);
+}
+
+void InitializeEncoders() {
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
@@ -140,7 +185,107 @@
right_initial_position =
control_loops::drivetrain.position->right_encoder;
- StopDrivetrain();
+}
+
+void WaitUntilClawDone() {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ control_loops::claw_queue_group.status.FetchLatest();
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (ShouldExitAuto()) {
+ return;
+ }
+ if (control_loops::claw_queue_group.status.get() == nullptr ||
+ control_loops::claw_queue_group.goal.get() == nullptr) {
+ continue;
+ }
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 1.0) &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.10) &&
+ (::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle) <
+ 0.4);
+ if (ans) {
+ return;
+ }
+ }
+}
+
+void HandleAuto() {
+ // The front of the robot is 1.854 meters from the wall
+ const double kShootDistance = 3.15;
+ const double kPickupDistance = 0.5;
+ LOG(INFO, "Handling auto mode\n");
+ ResetDrivetrain();
+
+ if (ShouldExitAuto()) return;
+ InitializeEncoders();
+
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ LOG(INFO, "Claw going up\n");
+ PositionClawVertically(12.0, 4.0);
+ SetShotPower(115.0);
+
+ // Wait for the ball to enter the claw.
+ time::SleepFor(time::Time::InSeconds(0.25));
+ if (ShouldExitAuto()) return;
+ LOG(INFO, "Readying claw for shot\n");
+
+ {
+ if (ShouldExitAuto()) return;
+ // Drive to the goal.
+ auto drivetrain_action = SetDriveGoal(-kShootDistance);
+ time::SleepFor(time::Time::InSeconds(0.75));
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Shooting\n");
+
+ // Shoot.
+ Shoot();
+ time::SleepFor(time::Time::InSeconds(0.1));
+
+ {
+ if (ShouldExitAuto()) return;
+ // Intake the new ball.
+ LOG(INFO, "Claw ready for intake\n");
+ PositionClawBackIntake();
+ auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ LOG(INFO, "Wait for the claw.\n");
+ WaitUntilClawDone();
+ if (ShouldExitAuto()) return;
+ }
+
+ // Drive back.
+ {
+ LOG(INFO, "Driving back\n");
+ auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
+ time::SleepFor(time::Time::InSeconds(0.7));
+ if (ShouldExitAuto()) return;
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ WaitUntilClawDone();
+ if (ShouldExitAuto()) return;
+ }
+
+ LOG(INFO, "Shooting\n");
+ // Shoot
+ Shoot();
+ if (ShouldExitAuto()) return;
+
+ // Get ready to zero when we come back up.
+ PositionClawVertically(0.0, 0.0);
}
} // namespace autonomous
diff --git a/frc971/autonomous/auto_main.cc b/frc971/autonomous/auto_main.cc
index e8914c6..36494bc 100644
--- a/frc971/autonomous/auto_main.cc
+++ b/frc971/autonomous/auto_main.cc
@@ -12,6 +12,7 @@
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init();
+ LOG(INFO, "Auto main started\n");
::frc971::autonomous::autonomous.FetchLatest();
while (!::frc971::autonomous::autonomous.get()) {
::frc971::autonomous::autonomous.FetchNextBlocking();
@@ -24,9 +25,13 @@
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Starting auto mode\n");
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
::frc971::autonomous::HandleAuto();
- LOG(INFO, "Auto mode exited, waiting for it to finish.\n");
+
+ ::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
+ LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
+ elapsed_time.ToSeconds());
while (::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 91b1b2f..980425f 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -25,11 +25,16 @@
'auto_queue',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
+ '<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:controls',
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 9d29107..a61deee 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -9,9 +9,7 @@
#include "aos/common/network/team_number.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -21,6 +19,9 @@
namespace constants {
namespace {
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+
const double kCompDrivetrainEncoderRatio =
(15.0 / 50.0) /*output reduction*/ * (36.0 / 24.0) /*encoder gears*/;
const double kCompLowGearRatio = 14.0 / 60.0 * 15.0 / 50.0;
@@ -31,11 +32,12 @@
const double kPracticeLowGearRatio = 16.0 / 60.0 * 19.0 / 50.0;
const double kPracticeHighGearRatio = 28.0 / 48.0 * 19.0 / 50.0;
-const ShifterHallEffect kCompRightDriveShifter{525, 635, 603, 529, 0.3, 0.7};
-const ShifterHallEffect kCompLeftDriveShifter{525, 645, 620, 533, 0.3, 0.7};
+const ShifterHallEffect kCompRightDriveShifter{555, 657, 660, 560, 0.2, 0.7};
+const ShifterHallEffect kCompLeftDriveShifter{555, 660, 644, 552, 0.2, 0.7};
const ShifterHallEffect kPracticeRightDriveShifter{550, 640, 635, 550, 0.2, 0.7};
const ShifterHallEffect kPracticeLeftDriveShifter{540, 620, 640, 550, 0.2, 0.7};
+
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
@@ -49,8 +51,8 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
{-0.00127, 0.298196, -0.001524, 0.305054, 0.0149098,
@@ -83,28 +85,35 @@
kCompHighGearRatio,
kCompLeftDriveShifter,
kCompRightDriveShifter,
- false,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
+ true,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
- // TODO(ben): make these real numbers
- {-0.00127, 0.298196, -0.001524, 0.305054, 0.0149098,
- {-0.001778, 0.000762, 0, 0}, {-0.001778, 0.009906, 0, 0}, {0.006096, 0.026416, 0, 0},
+ {-0.001041, 0.296019, -0.001488, 0.302717, 0.0149098,
+ {-0.002, 0.000446, -0.002, 0.000446},
+ {-0.002, 0.009078, -0.002, 0.009078},
+ {0.003870, 0.026194, 0.003869, 0.026343},
shooter_zeroing_speed,
shooter_unload_speed
},
- {0.5,
- 0.1,
- 0.1,
- 0.0,
- 1.57,
- 0,
- 0,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
- 4.0,
+ {0.800000,
+ 0.400000,
+ 0.000000,
+ -1.220821,
+ 1.822142,
+ -0.849484,
+ 1.42309,
+ {-3.328397, 2.091668, -3.166136, 1.95,
+ {-3.4, -3.092051, -3.4, -3.093414},
+ {-0.249073, -0.035452, -0.251800, -0.033179},
+ {1.889410, 2.2, 1.883956, 2.2}},
+ {-2.453460, 3.082960, -2.453460, 3.082960,
+ {-2.6, -2.185752, -2.6, -2.184843},
+ {-0.280434, -0.049087, -0.277707, -0.047724},
+ {2.892065, 3.2, 2.888429, 3.2}},
+ 0.040000, // claw_unimportant_epsilon
+ -0.400000, // start_fine_tune_pos
+ 4.000000,
},
//TODO(james): Get realer numbers for shooter_action.
{0.07, 0.15}, // shooter_action
@@ -120,8 +129,8 @@
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
- control_loops::MakeVDogDrivetrainLoop,
- control_loops::MakeDogDrivetrainLoop,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
{-0.001042, 0.294084, -0.001935, 0.303460, 0.0138401,
{-0.002, 0.000446, -0.002, 0.000446},
@@ -137,8 +146,14 @@
0.912207 * 2.0,
-0.849484,
1.42308,
- {-1.682379 * 2.0, 1.043334 * 2.0, -3.166136, 0.643334 * 2.0, {-1.7 * 2.0, -1.544662 * 2.0, -1.7 * 2.0, -1.547616 * 2.0}, {-0.130218 * 2.0, -0.019771 * 2.0, -0.132036 * 2.0, -0.018862 * 2.0}, {0.935842 * 2.0, 1.1 * 2.0, 0.932660 * 2.0, 1.1 * 2.0}},
- {-1.225821 * 2.0, 1.553752 * 2.0, -2.273474, 1.153752 * 2.0, {-1.3 * 2.0, -1.088331 * 2.0, -1.3 * 2.0, -1.088331 * 2.0}, {-0.134536 * 2.0, -0.018408 * 2.0, -0.136127 * 2.0, -0.019771 * 2.0}, {1.447396 * 2.0, 1.6 * 2.0, 1.443987 * 2.0, 1.6 * 2.0}},
+ {-3.364758, 2.086668, -3.166136, 1.95,
+ {-1.7 * 2.0, -1.544662 * 2.0, -1.7 * 2.0, -1.547616 * 2.0},
+ {-0.130218 * 2.0, -0.019771 * 2.0, -0.132036 * 2.0, -0.018862 * 2.0},
+ {0.935842 * 2.0, 1.1 * 2.0, 0.932660 * 2.0, 1.1 * 2.0}},
+ {-2.451642, 3.107504, -2.273474, 2.750,
+ {-1.3 * 2.0, -1.088331 * 2.0, -1.3 * 2.0, -1.088331 * 2.0},
+ {-0.134536 * 2.0, -0.018408 * 2.0, -0.136127 * 2.0, -0.019771 * 2.0},
+ {1.447396 * 2.0, 1.6 * 2.0, 1.443987 * 2.0, 1.6 * 2.0}},
0.020000 * 2.0, // claw_unimportant_epsilon
-0.200000 * 2.0, // start_fine_tune_pos
4.000000,
diff --git a/frc971/constants.h b/frc971/constants.h
index 75e7e0c..3a37aa7 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,9 +11,6 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-const uint16_t kCompTeamNumber = 8971;
-const uint16_t kPracticeTeamNumber = 971;
-
// Contains the voltages for an analog hall effect sensor on a shifter.
struct ShifterHallEffect {
// The numbers to use for scaling raw voltages to 0-1.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 7334166..417df9f 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -6,6 +6,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -75,9 +76,9 @@
const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (max_value > k_max_voltage) {
- LOG(DEBUG, "Capping U because max is %f\n", max_value);
U = U * k_max_voltage / max_value;
- LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
+ LOG(DEBUG, "Capping U is now %f %f (max is %f)\n",
+ U(0, 0), U(1, 0), max_value);
}
}
@@ -176,7 +177,8 @@
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
- : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup, true, true,
+ false>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -238,7 +240,7 @@
if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- LOG(INFO, "%s: Uncertain which side, rejecting posedge\n", name_);
+ LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
@@ -260,7 +262,7 @@
if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- LOG(INFO, "%s: Uncertain which side, rejecting negedge\n", name_);
+ LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
@@ -385,7 +387,8 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (::aos::robot_state->autonomous &&
+ (((::aos::robot_state.get() == NULL) ? true
+ : ::aos::robot_state->autonomous) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -412,10 +415,12 @@
output->tusk_voltage = 0;
}
- if (::std::isnan(goal->bottom_angle) ||
- ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
- ::std::isnan(goal->centering)) {
- return;
+ if (goal) {
+ if (::std::isnan(goal->bottom_angle) ||
+ ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
+ ::std::isnan(goal->centering)) {
+ return;
+ }
}
if (reset()) {
@@ -423,10 +428,6 @@
bottom_claw_.Reset(position->bottom);
}
- if (::aos::robot_state.get() == nullptr) {
- return;
- }
-
const frc971::constants::Values &values = constants::GetValues();
if (position) {
@@ -450,25 +451,33 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- LOG(DEBUG, "Claw position is (top: %f bottom: %f\n",
- top_claw_.absolute_position(), bottom_claw_.absolute_position());
+ LOG_STRUCT(DEBUG, "absolute position",
+ ClawPositionToLog(top_claw_.absolute_position(),
+ bottom_claw_.absolute_position()));
}
- const bool autonomous = ::aos::robot_state->autonomous;
- const bool enabled = ::aos::robot_state->enabled;
+ bool autonomous, enabled;
+ if (::aos::robot_state.get() == nullptr) {
+ autonomous = true;
+ enabled = false;
+ } else {
+ autonomous = ::aos::robot_state->autonomous;
+ enabled = ::aos::robot_state->enabled;
+ }
double bottom_claw_velocity_ = 0.0;
double top_claw_velocity_ = 0.0;
- if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
- bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (autonomous &&
- ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- top_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
- (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- bottom_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
+ if (goal != NULL &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
bottom_claw_goal_ = goal->bottom_angle;
@@ -604,8 +613,8 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position;
- bottom_claw_goal_ = position->bottom.position;
+ top_claw_goal_ = position->top.position + top_claw_.offset();
+ bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
initial_separation_ =
position->top.position - position->bottom.position;
} else {
@@ -675,8 +684,8 @@
if (position != nullptr) {
separation = position->top.position - position->bottom.position;
}
- LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
- claw_.R(1, 0), separation);
+ LOG_STRUCT(DEBUG, "actual goal",
+ ClawGoalToLog(claw_.R(0, 0), claw_.R(1, 0), separation));
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -702,8 +711,9 @@
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
- LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
+ LOG(DEBUG, "Moving the goal by %f to prevent windup."
+ " Uncapped is %f, max is %f, difference is %f\n",
+ dx,
claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
(claw_.uncapped_average_voltage() -
values.claw.max_zeroing_voltage));
@@ -752,22 +762,33 @@
status->bottom_velocity = claw_.X_hat(2, 0);
status->separation_velocity = claw_.X_hat(3, 0);
- bool bottom_done =
- ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
- bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
- bool separation_done =
- ::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.020;
- bool separation_done_with_ball =
- ::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.06;
- status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
- status->done_with_ball =
- is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ if (goal) {
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.020;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
+ status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ } else {
+ status->done = status->done_with_ball = false;
+ }
status->zeroed = is_ready();
+ status->zeroed_for_auto =
+ (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
- was_enabled_ = ::aos::robot_state->enabled;
+ was_enabled_ = enabled;
}
} // namespace control_loops
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 8bb94c0..588ce79 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -29,6 +29,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 37a1787..ced9b80 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -168,8 +168,8 @@
JointZeroingState zeroing_state);
};
-class ClawMotor
- : public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
+class ClawMotor : public aos::control_loops::ControlLoop<
+ control_loops::ClawGroup, true, true, false> {
public:
explicit ClawMotor(control_loops::ClawGroup *my_claw =
&control_loops::claw_queue_group);
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 38e7efb..116a182 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -52,8 +52,10 @@
};
message Status {
- // True if zeroed.
+ // True if zeroed enough for the current period (autonomous or teleop).
bool zeroed;
+ // True if zeroed as much as we will force during autonomous.
+ bool zeroed_for_auto;
// True if zeroed and within tolerance for separation and bottom angle.
bool done;
// True if zeroed and within tolerance for separation and bottom angle.
@@ -73,3 +75,14 @@
};
queue_group ClawGroup claw_queue_group;
+
+struct ClawPositionToLog {
+ double top;
+ double bottom;
+};
+
+struct ClawGoalToLog {
+ double bottom_pos;
+ double bottom_vel;
+ double separation;
+};
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 9f3b94d..946d7e3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -16,10 +16,10 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/othersensors.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
-using frc971::sensors::othersensors;
+using frc971::sensors::gyro_reading;
namespace frc971 {
namespace control_loops {
@@ -617,10 +617,10 @@
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
- if (othersensors.FetchLatest()) {
- LOG(DEBUG, "using %.3f", othersensors->gyro_angle);
- dt_closedloop.SetPosition(left_encoder, right_encoder, othersensors->gyro_angle,
- control_loop_driving);
+ if (gyro_reading.FetchLatest()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro_reading->angle, control_loop_driving);
} else {
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index ccabc30..d5d5640 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -22,9 +22,7 @@
'type': 'static_library',
'sources': [
'polydrivetrain_dog_motor_plant.cc',
- 'polydrivetrain_clutch_motor_plant.cc',
'drivetrain_dog_motor_plant.cc',
- 'drivetrain_clutch_motor_plant.cc',
],
'dependencies': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
deleted file mode 100644
index b3aa088..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00876671940282, 0.0, 0.000204905465153, 0.0, 0.764245148008, 0.0, 0.0373841350548, 0.0, 0.000204905465153, 1.0, 0.00876671940282, 0.0, 0.0373841350548, 0.0, 0.764245148008;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000157874070659, -2.62302512161e-05, 0.0301793267864, -0.00478559834045, -2.62302512161e-05, 0.000157874070659, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.60424514801, 0.0373841350548, 53.4463554671, 4.58647914599, 0.0373841350548, 1.60424514801, 4.58647914599, 53.4463554671;
- Eigen::Matrix<double, 2, 4> K;
- K << 292.330461448, 10.4890095334, -85.5980253252, -0.517234397951, -58.0206391358, -1.5636023242, 153.384904309, 5.5616531565;
- return StateFeedbackController<4, 2, 2>(L, K, MakeClutchDrivetrainPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClutchDrivetrainPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeClutchDrivetrainController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
deleted file mode 100644
index e9444e6..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController();
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 7822056..231eefb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00923787174605, 0.0, 0.000131162317098, 0.0, 0.851672729447, 0.0, 0.0248457251406, 0.0, 0.000131162317098, 1.0, 0.00923787174605, 0.0, 0.0248457251406, 0.0, 0.851672729447;
+ A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000126364935405, -2.17474127771e-05, 0.0245934537462, -0.00411955394149, -2.17474127771e-05, 0.000126364935405, -0.00411955394149, 0.0245934537462;
+ B << 0.000272244648044, -4.46778919705e-05, 0.0517213538779, -0.00804353916233, -4.46778919705e-05, 0.000272244648044, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController() {
+StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.69167272945, 0.0248457251406, 64.4706646869, 3.2355304474, 0.0248457251406, 1.69167272945, 3.2355304474, 64.4706646869;
+ L << 1.57584167586, 0.0410810558113, 50.0130674801, 4.93325200717, 0.0410810558113, 1.57584167586, 4.93325200717, 50.0130674801;
Eigen::Matrix<double, 2, 4> K;
- K << 248.918529922, 14.4460993245, 41.6953764051, 3.43594323497, 41.6953764051, 3.43594323497, 248.918529922, 14.4460993245;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDogDrivetrainPlantCoefficients());
+ K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
}
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant() {
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDogDrivetrainPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
return StateFeedbackPlant<4, 2, 2>(plants);
}
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop() {
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDogDrivetrainController());
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
return StateFeedbackLoop<4, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index ba3d584..3829e9a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -6,13 +6,13 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController();
+StateFeedbackController<4, 2, 2> MakeDrivetrainController();
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant();
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop();
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 275d33f..e03ef59 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -12,7 +12,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/othersensors.q.h"
+#include "frc971/queues/other_sensors.q.h"
using ::aos::time::Time;
@@ -49,7 +49,7 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDogDrivetrainPlant())),
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
my_drivetrain_loop_(".frc971.control_loops.drivetrain",
0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
@@ -133,7 +133,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
- ::frc971::sensors::othersensors.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
SendDSPacket(true);
}
@@ -157,7 +157,7 @@
virtual ~DrivetrainTest() {
::aos::robot_state.Clear();
- ::frc971::sensors::othersensors.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
deleted file mode 100644
index 82962f0..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,125 +0,0 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.764245148008, 0.0373841350548, 0.0373841350548, 0.764245148008;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0301793267864, -0.00478559834045, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.763446428918, 0.00494258902788, 0.042202491067, 0.968991856576;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0302815719967, -0.00184882243178, -0.00540240320973, 0.011598890947;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968991856576, 0.042202491067, 0.00494258902788, 0.763446428918;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.011598890947, -0.00540240320973, -0.00184882243178, 0.0302815719967;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968881997557, 0.00555499847336, 0.00555499847336, 0.968881997557;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0116399847578, -0.0020779000091, -0.0020779000091, 0.0116399847578;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.744245148008, 0.0373841350548, 0.0373841350548, 0.744245148008;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78417881324, 2.15594244513, 2.15594244513, 5.78417881324;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.742469928763, 0.0421768815418, 0.0421768815418, 0.949968356732;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78418649682, 2.16715237139, 6.33258809821, 32.8220766317;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.954934950673, 0.00591596315544, 0.00591596315544, 0.737503334821;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220766317, 6.33258809821, 2.16715237139, 5.78418649682;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.948881997557, 0.00555499847336, 0.00555499847336, 0.948881997557;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220767657, 6.33643373411, 6.33643373411, 32.8220767657;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
-}
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
deleted file mode 100644
index 85c87c1..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController();
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant();
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index b3d4277..b31cf85 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851672729447, 0.0248457251406, 0.0248457251406, 0.851672729447;
+ A << 0.735841675858, 0.0410810558113, 0.0410810558113, 0.735841675858;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0245934537462, -0.00411955394149, -0.00411955394149, 0.0245934537462;
+ B << 0.0517213538779, -0.00804353916233, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,11 +23,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851389310398, 0.00553670185935, 0.0264939835067, 0.967000817219;
+ A << 0.735048848179, 0.0131811893199, 0.045929121897, 0.915703853642;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0246404461385, -0.00200815724925, -0.00439284398274, 0.0119687766843;
+ B << 0.0518765869984, -0.00481755802263, -0.00899277497558, 0.0308091755839;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -39,11 +39,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.967000817219, 0.0264939835067, 0.00553670185935, 0.851389310398;
+ A << 0.915703853642, 0.045929121897, 0.0131811893199, 0.735048848179;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119687766843, -0.00439284398274, -0.00200815724925, 0.0246404461385;
+ B << 0.0308091755839, -0.00899277497558, -0.00481755802263, 0.0518765869984;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -55,11 +55,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.966936300149, 0.00589655754287, 0.00589655754287, 0.966936300149;
+ A << 0.915439806567, 0.0146814193986, 0.0146814193986, 0.915439806567;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119921769728, -0.00213867661221, -0.00213867661221, 0.0119921769728;
+ B << 0.0309056814511, -0.00536587314624, -0.00536587314624, 0.0309056814511;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -71,53 +71,53 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831672729447, 0.0248457251406, 0.0248457251406, 0.831672729447;
+ L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028500331, 2.80305051463, 2.80305051463, 10.7028500331;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowLowPlantCoefficients());
+ K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831852326508, 0.0264837489415, 0.0264837489415, 0.946537801108;
+ L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028511964, 2.80768406175, 6.14180888507, 31.6936764099;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowHighPlantCoefficients());
+ K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.951097545753, 0.0063707209266, 0.0063707209266, 0.827292581863;
+ L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764099, 6.14180888507, 2.80768406175, 10.7028511964;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighLowPlantCoefficients());
+ K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.946936300149, 0.00589655754287, 0.00589655754287, 0.946936300149;
+ L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764663, 6.14392885659, 6.14392885659, 31.6936764663;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant() {
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
return StateFeedbackPlant<2, 2, 2>(plants);
}
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop() {
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighHighController());
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
return StateFeedbackLoop<2, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 613bff4..27aa4dd 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -6,25 +6,25 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant();
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant();
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop();
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 8b1d9ad..3d6e441 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
import control_loop
+import controls
import numpy
import sys
from matplotlib import pylab
@@ -90,8 +91,6 @@
# Control loop time step
self.dt = 0.01
- print "THE NUMBER I WANT" + str((self.free_speed / 60.0 * 2.0 * numpy.pi) * self.G_high * self.r)
-
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
self.msp = 1.0 / self.m + self.rb * self.rb / self.J
@@ -127,7 +126,22 @@
# Poles from last year.
self.hp = 0.65
self.lp = 0.83
- self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+ self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+ print self.K
+ q_pos = 0.07
+ q_vel = 1.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print self.A
+ print self.B
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.07
self.llp = 0.09
@@ -203,6 +217,7 @@
#pylab.show()
# Write the generated constants out to a file.
+ print "Output one"
drivetrain = Drivetrain()
if len(argv) != 5:
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 280db16..f2dfdbe 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -396,10 +396,10 @@
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
- "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 8b9d427..9de06a9 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -6,6 +6,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
@@ -26,17 +27,17 @@
// against last cycle's voltage.
if (X_hat(2, 0) > last_voltage_ + 4.0) {
voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
} else if (X_hat(2, 0) < last_voltage_ - 4.0) {
voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
U(0, 0) = voltage_ - old_voltage;
- LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -55,7 +56,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (controller_index() == 0) {
@@ -68,7 +69,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -84,13 +85,10 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- LOG(INFO, "Setting calibration such that %f -> %f\n", encoder_val,
- known_position);
- LOG(INFO, "Position was %f\n", absolute_position());
+ double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
- LOG(INFO, "Changing offset from %f to %f\n", previous_offset, offset_);
X_hat(0, 0) += doffset;
// Offset our measurements because the offset is baked into them.
Y_(0, 0) += doffset;
@@ -99,7 +97,10 @@
if (controller_index() == 0) {
R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
}
- LOG(INFO, "Validation: position is %f\n", absolute_position());
+ LOG_STRUCT(
+ DEBUG, "sensor edge (fake?)",
+ ShooterChangeCalibration(encoder_val, known_position, old_position,
+ absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
@@ -115,7 +116,9 @@
shot_count_(0),
zeroed_(false),
distal_posedge_validation_cycles_left_(0),
- proximal_posedge_validation_cycles_left_(0) {}
+ proximal_posedge_validation_cycles_left_(0),
+ last_distal_current_(true),
+ last_proximal_current_(true) {}
double ShooterMotor::PowerToPosition(double power) {
const frc971::constants::Values &values = constants::GetValues();
@@ -162,7 +165,7 @@
if (::std::isnan(goal->shot_power)) {
state_ = STATE_ESTOP;
- LOG(ERROR, "Got a shot power of NAN.\n");
+ LOG(ERROR, "Estopping because got a shot power of NAN.\n");
}
// we must always have these or we have issues.
@@ -175,6 +178,8 @@
if (reset()) {
state_ = STATE_INITIALIZE;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
if (position) {
shooter_.CorrectPosition(position->position);
@@ -231,13 +236,18 @@
values.shooter.upper_limit);
}
- state_ = STATE_REQUEST_LOAD;
-
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
latch_piston_ = position->plunger;
brake_piston_ = false;
+ if (position->latch == latch_piston_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ shooter_loop_disable = true;
+ LOG(DEBUG,
+ "Not moving on until the latch has moved to avoid a crash\n");
+ }
} else {
// If we can't start yet because we don't know where we are, set the
// latch and brake to their defaults.
@@ -318,7 +328,8 @@
// TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
if (position->pusher_proximal.posedge_count !=
- last_proximal_posedge_count_) {
+ last_proximal_posedge_count_ &&
+ !last_proximal_current_) {
proximal_posedge_validation_cycles_left_ = 2;
}
if (proximal_posedge_validation_cycles_left_ > 0) {
@@ -338,7 +349,8 @@
}
if (position->pusher_distal.posedge_count !=
- last_distal_posedge_count_) {
+ last_distal_posedge_count_ &&
+ !last_distal_current_) {
distal_posedge_validation_cycles_left_ = 2;
}
if (distal_posedge_validation_cycles_left_ > 0) {
@@ -383,6 +395,7 @@
if (!position->pusher_distal.current ||
!position->pusher_proximal.current) {
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to load.\n");
}
}
} else if (goal->unload_requested) {
@@ -548,6 +561,7 @@
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to unload.\n");
}
brake_piston_ = false;
@@ -593,6 +607,7 @@
break;
case STATE_ESTOP:
+ LOG(DEBUG, "estopped\n");
// Totally lost, go to a safe state.
shooter_loop_disable = true;
latch_piston_ = true;
@@ -601,8 +616,9 @@
}
if (!shooter_loop_disable) {
- LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
- shooter_.goal_position(), shooter_.absolute_position());
+ LOG_STRUCT(DEBUG, "running the loop",
+ ShooterStatusToLog(shooter_.goal_position(),
+ shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -625,21 +641,31 @@
}
if (position) {
+ LOG_STRUCT(DEBUG, "internal state",
+ ShooterStateToLog(
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
+ state_, position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current, position->plunger,
+ brake_piston_, latch_piston_));
+
last_position_ = *position;
- LOG(DEBUG, "pos > absolute: %f velocity: %f state= %d l= %d pp= %d, pd= %d "
- "p= %d b=%d\n",
- shooter_.absolute_position(), shooter_.absolute_velocity(),
- state_, position->latch, position->pusher_proximal.current,
- position->pusher_distal.current,
- position->plunger, brake_piston_);
- }
- if (position) {
+
last_distal_posedge_count_ = position->pusher_distal.posedge_count;
last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
status->shots = shot_count_;
}
+void ShooterMotor::ZeroOutputs() {
+ queue_group()->output.MakeWithBuilder()
+ .voltage(0)
+ .latch_piston(latch_piston_)
+ .brake_piston(brake_piston_)
+ .Send();
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index efcc0e7..10e0f4e 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -30,6 +30,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'shooter_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index d5fa2a9..1ab224b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -108,7 +108,7 @@
const Time kUnloadTimeout = Time::InSeconds(10);
const Time kLoadTimeout = Time::InSeconds(2);
-const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
// Time to wait after releasing the latch piston before winching back again.
const Time kShotEndTimeout = Time::InSeconds(0.2);
@@ -150,6 +150,9 @@
ShooterGroup::Output *output, ShooterGroup::Status *status);
private:
+ // We have to override this to keep the pistons in the correct positions.
+ virtual void ZeroOutputs();
+
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
friend class testing::ShooterTest_UnloadWindupNegative_Test;
@@ -201,6 +204,8 @@
bool zeroed_;
int distal_posedge_validation_cycles_left_;
int proximal_posedge_validation_cycles_left_;
+ bool last_distal_current_;
+ bool last_proximal_current_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 7475873..8613669 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -58,3 +58,38 @@
};
queue_group ShooterGroup shooter_queue_group;
+
+struct ShooterStateToLog {
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
+ bool latch_sensor;
+ bool proximal;
+ bool distal;
+ bool plunger;
+ bool brake;
+ bool latch_piston;
+};
+
+struct ShooterVoltageToLog {
+ double X_hat;
+ double applied;
+};
+
+struct ShooterMovingGoal {
+ double dx;
+};
+
+struct ShooterChangeCalibration {
+ double encoder;
+ double real_position;
+ double old_position;
+ double new_position;
+ double old_offset;
+ double new_offset;
+};
+
+struct ShooterStatusToLog {
+ double goal;
+ double position;
+};
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index bcf2cf0..8033591 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -13,10 +13,14 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/actions/actions.gyp:shoot_action_queue',
+ '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(DEPTH)/frc971/actions/actions.gyp:catch_action_queue',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
],
},
{
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index c0daa9e..55274f8 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -9,14 +9,18 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/othersensors.q.h"
+#include "frc971/constants.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/shoot_action.h"
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::othersensors;
+using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -32,120 +36,77 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kFire(3, 11);
+const ButtonLocation kCatch(3, 10);
+
+const ButtonLocation kFire(3, 9);
const ButtonLocation kUnload(2, 11);
const ButtonLocation kReload(2, 6);
-const ButtonLocation kRollersOut(3, 12);
-const ButtonLocation kRollersIn(3, 10);
+const ButtonLocation kRollersOut(3, 8);
+const ButtonLocation kRollersIn(3, 3);
-const ButtonLocation kTuck(3, 8);
-const ButtonLocation kIntakeOpenPosition(3, 9);
-const ButtonLocation kIntakePosition(3, 7);
+const ButtonLocation kTuck(3, 4);
+const ButtonLocation kIntakePosition(3, 5);
+const ButtonLocation kIntakeOpenPosition(3, 11);
+const JoystickAxis kFlipRobot(3, 3);
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
+const ButtonLocation kLongShot(3, 7);
+const ButtonLocation kMediumShot(3, 6);
+const ButtonLocation kShortShot(3, 2);
+const ButtonLocation kTrussShot(3, 1);
+
+const JoystickAxis kAdjustClawGoal(3, 2);
+const JoystickAxis kAdjustClawSeparation(3, 1);
struct ClawGoal {
double angle;
double separation;
};
+struct ShotGoal {
+ ClawGoal claw;
+ double shot_power;
+ bool velocity_compensation;
+ double intake_power;
+};
+
+const double kIntakePower = 4.0;
+const double kGrabSeparation = -0.04;
+const double kShootSeparation = 0.11 + kGrabSeparation;
+
const ClawGoal kTuckGoal = {-2.273474, -0.749484};
-const ClawGoal kIntakeGoal = {-2.273474, 0.0};
+const ClawGoal kIntakeGoal = {-2.273474, kGrabSeparation};
const ClawGoal kIntakeOpenGoal = {-2.0, 1.2};
-const ClawGoal kLongShotGoal = {-M_PI / 2.0 + 0.43, 0.10};
-const ClawGoal kMediumShotGoal = {-0.9, 0.10};
-const ClawGoal kShortShotGoal = {-0.04, 0.11};
+// TODO(austin): Tune these by hand...
+const ClawGoal kFlippedTuckGoal = {2.733474, -0.75};
+const ClawGoal kFlippedIntakeGoal = {2.0, kGrabSeparation};
+const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
-class Action {
- public:
- // Cancels the action.
- void Cancel() { DoCancel(); }
- // Returns true if the action is currently running.
- bool Running() { return DoRunning(); }
- // Starts the action.
- void Start() { DoStart(); }
+//const ShotGoal kLongShotGoal = {
+ //{-M_PI / 2.0 + 0.46, kShootSeparation}, 120, false, kIntakePower};
+const ShotGoal kLongShotGoal = {
+ {-0.60, kShootSeparation}, 80, false, kIntakePower};
+const ShotGoal kMediumShotGoal = {
+ {-0.90, kShootSeparation}, 105, true, kIntakePower};
+const ShotGoal kShortShotGoal = {
+ {-0.670, kShootSeparation}, 71.0, false, kIntakePower};
+const ShotGoal kTrussShotGoal = {
+ {-0.05, kShootSeparation}, 61.0, false, kIntakePower};
- virtual ~Action() {}
-
- private:
- virtual void DoCancel() = 0;
- virtual bool DoRunning() = 0;
- virtual void DoStart() = 0;
-};
-
-// Templated subclass to hold the type information.
-template <typename T>
-class TypedAction : public Action {
- public:
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
-
- TypedAction(T *queue_group)
- : queue_group_(queue_group),
- goal_(queue_group_->goal.MakeMessage()),
- has_started_(false) {}
-
- // Returns the current goal that will be sent when the action is sent.
- GoalType *GetGoal() { return goal_.get(); }
-
- virtual ~TypedAction() {
- LOG(INFO, "Calling destructor\n");
- DoCancel();
- }
-
- private:
- // Cancels the action.
- virtual void DoCancel() {
- LOG(INFO, "Canceling action\n");
- queue_group_->goal.MakeWithBuilder().run(false).Send();
- }
-
- // Returns true if the action is running or we don't have an initial response
- // back from it to signal whether or not it is running.
- virtual bool DoRunning() {
- if (has_started_) {
- queue_group_->status.FetchLatest();
- } else if (queue_group_->status.FetchLatest()) {
- if (queue_group_->status->running) {
- // Wait until it reports that it is running to start.
- has_started_ = true;
- }
- }
- return !has_started_ ||
- (queue_group_->status.get() && queue_group_->status->running);
- }
-
- // Starts the action if a goal has been created.
- virtual void DoStart() {
- if (goal_) {
- goal_->run = true;
- goal_.Send();
- has_started_ = false;
- LOG(INFO, "Starting action\n");
- } else {
- has_started_ = true;
- }
- }
-
- T *queue_group_;
- ::aos::ScopedMessagePtr<GoalType> goal_;
- // Track if we have seen a response to the start message.
- // If we haven't, we are considered running regardless.
- bool has_started_;
-};
+const ShotGoal kFlippedLongShotGoal = {
+ {0.55, kShootSeparation}, 80, false, kIntakePower};
+const ShotGoal kFlippedMediumShotGoal = {
+ {0.85, kShootSeparation}, 105, true, kIntakePower};
+const ShotGoal kFlippedShortShotGoal = {
+ {0.57, kShootSeparation}, 80.0, false, kIntakePower};
// Makes a new ShootAction action.
-::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
-MakeShootAction() {
- return ::std::unique_ptr<
- TypedAction< ::frc971::actions::ShootActionQueueGroup>>(
- new TypedAction< ::frc971::actions::ShootActionQueueGroup>(
- &::frc971::actions::shoot_action));
+::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>
+MakeCatchAction() {
+ return ::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>(
+ new TypedAction< ::frc971::actions::CatchActionGroup>(
+ &::frc971::actions::catch_action));
}
// A queue which queues Actions and cancels them.
@@ -175,7 +136,7 @@
// Cancels all running actions.
void CancelAllActions() {
- LOG(INFO, "Canceling all actions\n");
+ LOG(DEBUG, "Cancelling all actions\n");
if (current_action_) {
current_action_->Cancel();
}
@@ -205,24 +166,30 @@
::std::unique_ptr<Action> next_action_;
};
+
class Reader : public ::aos::input::JoystickInput {
public:
Reader()
: is_high_gear_(false),
shot_power_(80.0),
goal_angle_(0.0),
- separation_angle_(0.0) {}
+ separation_angle_(kGrabSeparation),
+ velocity_compensation_(false),
+ intake_power_(0.0),
+ was_running_(false) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
LOG(INFO, "Starting auto mode\n");
::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(true).Send();
+ .run_auto(true)
+ .Send();
} else if (data.NegEdge(ControlBit::kEnabled)) {
LOG(INFO, "Stopping auto mode\n");
::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(false).Send();
+ .run_auto(false)
+ .Send();
}
} else {
HandleTeleop(data);
@@ -244,12 +211,12 @@
static double filtered_goal_distance = 0.0;
if (data.PosEdge(kDriveControlLoopEnable1) ||
data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && othersensors.FetchLatest()) {
+ if (drivetrain.position.FetchLatest() && gyro_reading.FetchLatest()) {
distance = (drivetrain.position->left_encoder +
drivetrain.position->right_encoder) /
2.0 -
throttle * kThrottleGain / 2.0;
- angle = othersensors->gyro_angle;
+ angle = gyro_reading->angle;
filtered_goal_distance = distance;
}
}
@@ -296,68 +263,172 @@
void SetGoal(ClawGoal goal) {
goal_angle_ = goal.angle;
separation_angle_ = goal.separation;
+ velocity_compensation_ = false;
+ intake_power_ = 0.0;
+ }
+
+ void SetGoal(ShotGoal goal) {
+ goal_angle_ = goal.claw.angle;
+ separation_angle_ = goal.claw.separation;
+ shot_power_ = goal.shot_power;
+ velocity_compensation_ = goal.velocity_compensation;
+ intake_power_ = goal.intake_power;
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
HandleDrivetrain(data);
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
+ intake_power_ = 0.0;
+ separation_angle_ = kGrabSeparation;
}
- if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
- SetGoal(kIntakeOpenGoal);
- } else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
- SetGoal(kIntakeGoal);
- } else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
- SetGoal(kTuckGoal);
+ static const double kAdjustClawGoalDeadband = 0.08;
+ double claw_goal_adjust = data.GetAxis(kAdjustClawGoal);
+ if (::std::abs(claw_goal_adjust) < kAdjustClawGoalDeadband) {
+ claw_goal_adjust = 0;
+ } else {
+ claw_goal_adjust = (claw_goal_adjust -
+ ((claw_goal_adjust < 0) ? -kAdjustClawGoalDeadband
+ : kAdjustClawGoalDeadband)) *
+ 0.035;
+ }
+ double claw_separation_adjust = data.GetAxis(kAdjustClawSeparation);
+ if (::std::abs(claw_separation_adjust) < kAdjustClawGoalDeadband) {
+ claw_separation_adjust = 0;
+ } else {
+ claw_separation_adjust =
+ (claw_separation_adjust -
+ ((claw_separation_adjust < 0) ? -kAdjustClawGoalDeadband
+ : kAdjustClawGoalDeadband)) *
+ -0.035;
}
- if (data.PosEdge(kLongShot)) {
- auto shoot_action = MakeShootAction();
+ if (data.GetAxis(kFlipRobot) > 0.5) {
+ claw_goal_adjust += claw_separation_adjust;
+ claw_goal_adjust *= -1;
- shot_power_ = 145.0;
- shoot_action->GetGoal()->shot_power = shot_power_;
- shoot_action->GetGoal()->shot_angle = kLongShotGoal.angle;
- SetGoal(kLongShotGoal);
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedLongShotGoal);
+ } else if (data.PosEdge(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedShortShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ } else {
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kLongShotGoal);
+ } else if (data.PosEdge(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kShortShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ }
- action_queue_.QueueAction(::std::move(shoot_action));
- } else if (data.PosEdge(kMediumShot)) {
- auto shoot_action = MakeShootAction();
+ /*
+ if (data.PosEdge(kCatch)) {
+ auto catch_action = MakeCatchAction();
+ catch_action->GetGoal()->catch_angle = goal_angle_;
+ action_queue_.QueueAction(::std::move(catch_action));
+ }
+ */
- shot_power_ = 100.0;
- shoot_action->GetGoal()->shot_power = shot_power_;
- shoot_action->GetGoal()->shot_angle = kMediumShotGoal.angle;
- SetGoal(kMediumShotGoal);
-
- action_queue_.QueueAction(::std::move(shoot_action));
- } else if (data.PosEdge(kShortShot)) {
- auto shoot_action = MakeShootAction();
-
- shot_power_ = 20.0;
- shoot_action->GetGoal()->shot_power = shot_power_;
- shoot_action->GetGoal()->shot_angle = kShortShotGoal.angle;
- SetGoal(kShortShotGoal);
-
- action_queue_.QueueAction(::std::move(shoot_action));
+ if (data.PosEdge(kFire)) {
+ action_queue_.QueueAction(actions::MakeShootAction());
}
action_queue_.Tick();
if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ intake_power_ = 0.0;
+ velocity_compensation_ = false;
}
// Send out the claw and shooter goals if no actions are running.
if (!action_queue_.Running()) {
+ goal_angle_ += claw_goal_adjust;
+ separation_angle_ += claw_separation_adjust;
+
+ // If the action just ended, turn the intake off and stop velocity
+ // compensating.
+ if (was_running_) {
+ intake_power_ = 0.0;
+ velocity_compensation_ = false;
+ }
+
+ control_loops::drivetrain.status.FetchLatest();
+ double goal_angle =
+ goal_angle_ +
+ (velocity_compensation_
+ ? SpeedToAngleOffset(
+ control_loops::drivetrain.status->robot_speed)
+ : 0.0);
+ double separation_angle = separation_angle_;
+
+ if (data.IsPressed(kCatch)) {
+ const double kCatchSeparation = 1.0;
+ goal_angle -= kCatchSeparation / 2.0;
+ separation_angle = kCatchSeparation;
+ }
+
+ bool intaking =
+ data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
+ data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(goal_angle_)
- .separation_angle(separation_angle_)
- .intake(data.IsPressed(kRollersIn)
- ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0 : 0.0))
- .centering(data.IsPressed(kRollersIn) ? 12.0 : 0.0)
+ .bottom_angle(goal_angle)
+ .separation_angle(separation_angle)
+ .intake(intaking ? 12.0
+ : (data.IsPressed(kRollersOut) ? -12.0
+ : intake_power_))
+ .centering(intaking ? 12.0 : 0.0)
.Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
@@ -371,6 +442,14 @@
LOG(WARNING, "sending shooter goal failed\n");
}
}
+ was_running_ = action_queue_.Running();
+ }
+
+ double SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values &values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ // TODO(austin): Change the scale factor for different shots.
+ return (speed / values.drivetrain_max_speed) * 0.2;
}
private:
@@ -378,6 +457,9 @@
double shot_power_;
double goal_angle_;
double separation_angle_;
+ bool velocity_compensation_;
+ double intake_power_;
+ bool was_running_;
ActionQueue action_queue_;
};
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index abd31dc..b44bb8d 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -9,7 +9,7 @@
#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/othersensors.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
#include "frc971/queues/to_log.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
@@ -20,7 +20,8 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::othersensors;
+using ::frc971::sensors::other_sensors;
+using ::frc971::sensors::gyro_reading;
using ::aos::util::WrappingCounter;
namespace frc971 {
@@ -64,6 +65,10 @@
return adc_translate(in) * kDividerBig / kDividerSmall;
}
+double sonar_translate(uint32_t in) {
+ return static_cast<double>(in) / 1000.0 * 2.0;
+}
+
double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
uint16_t in_high) {
const double low_ratio =
@@ -129,9 +134,12 @@
State *state) {
::frc971::logging_structs::CapeReading reading_to_log(
cape_timestamp.sec(), cape_timestamp.nsec(),
- data->main.ultrasonic_pulse_length, sizeof(*data));
+ sizeof(*data), sonar_translate(data->main.ultrasonic_pulse_length),
+ data->main.low_left_drive_hall, data->main.high_left_drive_hall,
+ data->main.low_right_drive_hall, data->main.high_right_drive_hall);
LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
bool bad_gyro;
+ // TODO(brians): Switch to LogInterval for these things.
if (data->uninitialized_gyro) {
LOG(DEBUG, "uninitialized gyro\n");
bad_gyro = true;
@@ -141,7 +149,6 @@
} else if (data->bad_gyro) {
LOG(ERROR, "bad gyro\n");
bad_gyro = true;
- othersensors.MakeWithBuilder().gyro_angle(0).Send();
} else if (data->old_gyro_reading) {
LOG(WARNING, "old/bad gyro reading\n");
bad_gyro = true;
@@ -150,11 +157,19 @@
}
if (!bad_gyro) {
- othersensors.MakeWithBuilder()
- .gyro_angle(gyro_translate(data->gyro_angle))
+ gyro_reading.MakeWithBuilder()
+ .angle(gyro_translate(data->gyro_angle))
.Send();
}
+ if (data->analog_errors != 0) {
+ LOG(WARNING, "%" PRIu8 " analog errors\n", data->analog_errors);
+ }
+
+ other_sensors.MakeWithBuilder()
+ .sonar_distance(sonar_translate(data->main.ultrasonic_pulse_length))
+ .Send();
+
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(data->main.right_drive))
.left_encoder(-drivetrain_translate(data->main.left_drive))
diff --git a/frc971/output/led_setter.cc b/frc971/output/led_setter.cc
new file mode 100644
index 0000000..a69183d
--- /dev/null
+++ b/frc971/output/led_setter.cc
@@ -0,0 +1,19 @@
+#include "aos/common/logging/logging.h"
+#include "aos/linux_code/init.h"
+
+#include "bbb/led.h"
+
+#include "frc971/control_loops/claw/claw.q.h"
+
+using ::frc971::control_loops::claw_queue_group;
+
+int main() {
+ ::aos::InitNRT();
+
+ ::bbb::LED claw_zeroed(3);
+
+ while (true) {
+ CHECK(claw_queue_group.status.FetchNextBlocking());
+ claw_zeroed.Set(claw_queue_group.status->zeroed_for_auto);
+ }
+}
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index 54ff7d9..dca221b 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -26,6 +26,12 @@
static constexpr ::aos::time::Time kOldLogInterval =
::aos::time::Time::InSeconds(0.5);
+ double Cap(double value, double max) {
+ if (value > max) return max;
+ if (value < -max) return -max;
+ return value;
+ }
+
virtual void RunIteration() {
values_.digital_module = 0;
values_.pressure_switch_channel = 1;
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 67bd641..bc2d604 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -1,6 +1,19 @@
{
'targets': [
{
+ 'target_name': 'led_setter',
+ 'type': 'executable',
+ 'sources': [
+ 'led_setter.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:led',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ },
+ {
'target_name': 'CameraServer',
'type': 'executable',
'sources': [
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index f445b64..c4db795 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -14,6 +14,9 @@
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../autonomous/autonomous.gyp:auto',
'../actions/actions.gyp:shoot_action',
+ '../actions/actions.gyp:selfcatch_action',
+ '../actions/actions.gyp:catch_action',
+ '../actions/actions.gyp:drivetrain_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',
@@ -22,12 +25,13 @@
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:packet_finder_test',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:cows_test',
'<(DEPTH)/bbb_cape/src/flasher/flasher.gyp:stm32_flasher',
+ '../output/output.gyp:led_setter',
],
'copies': [
{
'destination': '<(rsync_dir)',
'files': [
- 'scripts/start_list.txt',
+ 'start_list.txt',
],
},
],
diff --git a/frc971/prime/scripts/start_list.txt b/frc971/prime/start_list.txt
similarity index 77%
rename from frc971/prime/scripts/start_list.txt
rename to frc971/prime/start_list.txt
index 043f616..94bf6bb 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -7,3 +7,5 @@
claw
shooter
shoot_action
+drivetrain_action
+catch_action
diff --git a/frc971/queues/other_sensors.q b/frc971/queues/other_sensors.q
new file mode 100644
index 0000000..7181ce9
--- /dev/null
+++ b/frc971/queues/other_sensors.q
@@ -0,0 +1,12 @@
+package frc971.sensors;
+
+message OtherSensors {
+ double sonar_distance;
+ double plunger_hall_effect_distance;
+};
+queue OtherSensors other_sensors;
+
+message GyroReading {
+ double angle;
+};
+queue GyroReading gyro_reading;
diff --git a/frc971/queues/othersensors.q b/frc971/queues/othersensors.q
deleted file mode 100644
index 8541b94..0000000
--- a/frc971/queues/othersensors.q
+++ /dev/null
@@ -1,9 +0,0 @@
-package frc971.sensors;
-
-message OtherSensors {
- double sonar_distance;
- double gyro_angle;
- double travis_hall_effect_distance;
-};
-
-queue OtherSensors othersensors;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index 57d2e60..8434437 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -1,7 +1,7 @@
{
'variables': {
'queue_files': [
- 'othersensors.q',
+ 'other_sensors.q',
'to_log.q',
]
},
diff --git a/frc971/queues/to_log.q b/frc971/queues/to_log.q
index a8bee3f..ded9b64 100644
--- a/frc971/queues/to_log.q
+++ b/frc971/queues/to_log.q
@@ -3,6 +3,11 @@
struct CapeReading {
uint32_t sec;
uint32_t nsec;
- uint32_t sonar;
uint64_t struct_size;
+ double sonar;
+
+ uint16_t left_low;
+ uint16_t left_high;
+ uint16_t right_low;
+ uint16_t right_high;
};