Add all the third robot code to the master branch.
There are some changes to the main robot code that Brian thought
should be in the mainline.
Change-Id: I5e08392c3f874c1c5da2c33d70da1fb359db964c
diff --git a/aos/build/aos_all.gyp b/aos/build/aos_all.gyp
index 1caac63..3d02b84 100644
--- a/aos/build/aos_all.gyp
+++ b/aos/build/aos_all.gyp
@@ -19,6 +19,7 @@
'<(AOS)/linux_code/starter/starter.gyp:starter_exe',
'<(AOS)/linux_code/starter/starter.gyp:netconsole',
'<(AOS)/linux_code/linux_code.gyp:complex_thread_local_test',
+ '<(AOS)/linux_code/linux_code.gyp:dump_rtprio',
],
},
{
diff --git a/aos/build/externals.gyp b/aos/build/externals.gyp
index 5e3a64f..ae5a038 100644
--- a/aos/build/externals.gyp
+++ b/aos/build/externals.gyp
@@ -37,7 +37,6 @@
'-li2c',
'/opt/wpilib_4.8.3/lib/libwpilib_nonshared.a',
'/opt/wpilib_4.8.3/lib/libHALAthena.a',
- '/opt/wpilib_4.8.3/lib/libNetworkTables.a',
],
},
'direct_dependent_settings': {
diff --git a/aos/common/logging/logging_printf_formats.h b/aos/common/logging/logging_printf_formats.h
index be186bc..0bf253c 100644
--- a/aos/common/logging/logging_printf_formats.h
+++ b/aos/common/logging/logging_printf_formats.h
@@ -24,7 +24,7 @@
// These 2 define how many digits we use to print out the nseconds fields of
// times. They have to stay matching.
-#define AOS_TIME_NSECONDS_DIGITS 5
+#define AOS_TIME_NSECONDS_DIGITS 6
#define AOS_TIME_NSECONDS_DENOMINATOR 10000
#endif // AOS_COMMON_LOGGING_LOGGING_PRINTF_FORMATS_H_
diff --git a/aos/common/queue_types.cc b/aos/common/queue_types.cc
index f04e365..4dd7d43 100644
--- a/aos/common/queue_types.cc
+++ b/aos/common/queue_types.cc
@@ -90,7 +90,8 @@
for (int i = 0; i < number_fields; ++i) {
uint16_t field_name_length;
- if (*bytes < sizeof(fields[i]->type) + sizeof(field_name_length)) {
+ if (*bytes < sizeof(fields[i]->type) + sizeof(field_name_length) +
+ (deserialize_length ? sizeof(fields[i]->length) : 0)) {
return nullptr;
}
*bytes -= sizeof(fields[i]->type) + sizeof(field_name_length);
@@ -100,6 +101,7 @@
if (deserialize_length) {
to_host(buffer, &fields[i]->length);
buffer += sizeof(fields[i]->length);
+ *bytes -= sizeof(fields[i]->length);
}
to_host(buffer, &field_name_length);
buffer += sizeof(field_name_length);
diff --git a/aos/common/util/string_to_num.h b/aos/common/util/string_to_num.h
index c6dcb30..d99ed91 100644
--- a/aos/common/util/string_to_num.h
+++ b/aos/common/util/string_to_num.h
@@ -7,11 +7,11 @@
namespace aos {
namespace util {
-// Converts a string into a specified integral type. If it can't be converted
+// Converts a string into a specified numeric type. If it can't be converted
// completely or at all, or if the converted number would overflow the
-// specified integral type, it returns false.
+// specified type, it returns false.
template<typename T>
-inline bool StringToInteger(const ::std::string &input, T *out_num) {
+inline bool StringToNumber(const ::std::string &input, T *out_num) {
::std::istringstream stream(input);
stream >> *out_num;
diff --git a/aos/common/util/string_to_num_test.cc b/aos/common/util/string_to_num_test.cc
index e1ee1cb..705de42 100644
--- a/aos/common/util/string_to_num_test.cc
+++ b/aos/common/util/string_to_num_test.cc
@@ -12,30 +12,36 @@
TEST(StringToNumTest, CorrectNumber) {
int result;
- ASSERT_TRUE(StringToInteger<int>(::std::string("42"), &result));
+ ASSERT_TRUE(StringToNumber<int>(::std::string("42"), &result));
EXPECT_EQ(result, 42);
}
TEST(StringToNumTest, NegativeTest) {
int result;
- ASSERT_TRUE(StringToInteger<int>(::std::string("-42"), &result));
+ ASSERT_TRUE(StringToNumber<int>(::std::string("-42"), &result));
EXPECT_EQ(result, -42);
}
TEST(StringToNumTest, NonNumber) {
int result;
- ASSERT_FALSE(StringToInteger<int>(::std::string("Daniel"), &result));
+ ASSERT_FALSE(StringToNumber<int>(::std::string("Daniel"), &result));
}
TEST(StringToNumTest, NumberWithText) {
int result;
- ASSERT_FALSE(StringToInteger<int>(::std::string("42Daniel"), &result));
+ ASSERT_FALSE(StringToNumber<int>(::std::string("42Daniel"), &result));
}
TEST(StringToNumTest, OverflowTest) {
uint32_t result;
// 2 << 32 should overflow.
- ASSERT_FALSE(StringToInteger<uint32_t>(::std::string("4294967296"), &result));
+ ASSERT_FALSE(StringToNumber<uint32_t>(::std::string("4294967296"), &result));
+}
+
+TEST(StringToNumTest, FloatingPointTest) {
+ double result;
+ ASSERT_TRUE(StringToNumber<double>(::std::string("3.1415927")));
+ EXPECT_EQ(result, 3.1415927);
}
} // testing
diff --git a/aos/linux_code/dump_rtprio.cc b/aos/linux_code/dump_rtprio.cc
new file mode 100644
index 0000000..cae69f1
--- /dev/null
+++ b/aos/linux_code/dump_rtprio.cc
@@ -0,0 +1,282 @@
+// This is a utility program that prints out realtime priorities for processes
+// on a system. It is useful both because the standard tools don't format that
+// information very well and the roboRIO's busybox ones don't seem to do it at
+// all.
+//
+// The output format is the following comma-separated columns:
+// exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid,cpu
+
+#include <sched.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+#include <unistd.h>
+
+#include <string>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/common/time.h"
+
+namespace {
+
+const char *policy_string(uint32_t policy) {
+ switch (policy) {
+ case SCHED_OTHER:
+ return "OTHER";
+ case SCHED_BATCH:
+ return "BATCH";
+ case SCHED_IDLE:
+ return "IDLE";
+ case SCHED_FIFO:
+ return "FIFO";
+ case SCHED_RR:
+ return "RR";
+#ifdef SCHED_DEADLINE
+ case SCHED_DEADLINE:
+ return "DEADLINE";
+#endif
+ default:
+ return "???";
+ }
+}
+
+::std::string strip_string_prefix(size_t length, ::std::string str) {
+ str = str.substr(length);
+ while (str[0] == ' ' || str[0] == '\t') {
+ str = str.substr(1);
+ }
+ return str.substr(0, str.size() - 1);
+}
+
+int find_pid_max() {
+ int r;
+ FILE *pid_max_file = fopen("/proc/sys/kernel/pid_max", "r");
+ if (pid_max_file == nullptr) {
+ PLOG(FATAL, "fopen(\"/proc/sys/kernel/pid_max\")");
+ }
+ CHECK_EQ(1, fscanf(pid_max_file, "%d", &r));
+ PCHECK(fclose(pid_max_file));
+ return r;
+}
+
+cpu_set_t find_all_cpus() {
+ long nproc = sysconf(_SC_NPROCESSORS_CONF);
+ if (nproc == -1) {
+ PLOG(FATAL, "sysconf(_SC_NPROCESSORS_CONF)");
+ }
+ cpu_set_t r;
+ CPU_ZERO(&r);
+ for (long i = 0; i < nproc; ++i) {
+ CPU_SET(i, &r);
+ }
+ return r;
+}
+
+cpu_set_t find_cpu_mask(int process, bool *not_there) {
+ cpu_set_t r;
+ const int result = sched_getaffinity(process, sizeof(r), &r);
+ if (result == -1 && errno == ESRCH) {
+ *not_there = true;
+ return cpu_set_t();
+ }
+ if (result != 0) {
+ PLOG(FATAL, "sched_getaffinity(%d, %zu, %p)", process, sizeof(r), &r);
+ }
+ return r;
+}
+
+sched_param find_sched_param(int process, bool *not_there) {
+ sched_param r;
+ const int result = sched_getparam(process, &r);
+ if (result == -1 && errno == ESRCH) {
+ *not_there = true;
+ return sched_param();
+ }
+ if (result != 0) {
+ PLOG(FATAL, "sched_getparam(%d)", process);
+ }
+ return r;
+}
+
+int find_scheduler(int process, bool *not_there) {
+ int scheduler = sched_getscheduler(process);
+ if (scheduler == -1 && errno == ESRCH) {
+ *not_there = true;
+ return 0;
+ }
+ if (scheduler == -1) {
+ PLOG(FATAL, "sched_getscheduler(%d)", process);
+ }
+ return scheduler;
+}
+
+::std::string find_exe(int process, bool *not_there) {
+ ::std::string exe_filename = "/proc/" + ::std::to_string(process) + "/exe";
+ char exe_buffer[1024];
+ ssize_t exe_size =
+ readlink(exe_filename.c_str(), exe_buffer, sizeof(exe_buffer));
+ if (exe_size == -1 && errno == ENOENT) {
+ return "ENOENT";
+ } else {
+ if (exe_size == -1 && errno == ESRCH) {
+ *not_there = true;
+ return "";
+ }
+ if (exe_size == -1) {
+ PLOG(FATAL, "readlink(%s, %p, %zu)", exe_filename.c_str(), exe_buffer,
+ sizeof(exe_buffer));
+ }
+ return ::std::string(exe_buffer, exe_size);
+ }
+}
+
+int find_nice_value(int process, bool *not_there) {
+ errno = 0;
+ int nice_value = getpriority(PRIO_PROCESS, process);
+ if (errno == ESRCH) {
+ *not_there = true;
+ return 0;
+ }
+ if (errno != 0) {
+ PLOG(FATAL, "getpriority(PRIO_PROCESS, %d)", process);
+ }
+ return nice_value;
+}
+
+void read_stat(int process, int *ppid, int *sid, bool *not_there) {
+ ::std::string stat_filename = "/proc/" + ::std::to_string(process) + "/stat";
+ FILE *stat = fopen(stat_filename.c_str(), "r");
+ if (stat == nullptr && errno == ENOENT) {
+ *not_there = true;
+ return;
+ }
+ if (stat == nullptr) {
+ PLOG(FATAL, "fopen(%s, \"r\")", stat_filename.c_str());
+ }
+
+ char buffer[2048];
+ if (fgets(buffer, sizeof(buffer), stat) == nullptr) {
+ if (ferror(stat)) {
+ if (errno == ESRCH) {
+ *not_there = true;
+ return;
+ }
+ PLOG(FATAL, "fgets(%p, %zu, %p)", buffer, sizeof(buffer), stat);
+ }
+ }
+
+ int pid = 0;
+
+ int field = 0;
+ size_t field_start = 0;
+ int parens = 0;
+ for (size_t i = 0; i < sizeof(buffer); ++i) {
+ if (buffer[i] == '\0') break;
+ if (buffer[i] == '(') ++parens;
+ if (parens > 0) {
+ if (buffer[i] == ')') --parens;
+ } else if (buffer[i] == ' ') {
+ ::std::string field_string(buffer, field_start, i - field_start);
+ switch (field) {
+ case 0:
+ pid = ::std::stoi(field_string);
+ break;
+ case 3:
+ *ppid = ::std::stoi(field_string);
+ break;
+ case 4:
+ *sid = ::std::stoi(field_string);
+ break;
+ default:
+ break;
+ }
+ ++field;
+ field_start = i + 1;
+ }
+ }
+ PCHECK(fclose(stat));
+
+ if (field < 4) {
+ LOG(FATAL, "couldn't get fields from /proc/%d/stat\n", process);
+ }
+ CHECK_EQ(pid, process);
+}
+
+void read_status(int process, int ppid, int *pgrp, ::std::string *name,
+ bool *not_there) {
+ ::std::string status_filename =
+ "/proc/" + ::std::to_string(process) + "/status";
+ FILE *status = fopen(status_filename.c_str(), "r");
+ if (status == nullptr && errno == ENOENT) {
+ *not_there = true;
+ return;
+ }
+ if (status == nullptr) {
+ PLOG(FATAL, "fopen(%s, \"r\")", status_filename.c_str());
+ }
+
+ int pid = 0, status_ppid = 0;
+ while (true) {
+ char buffer[1024];
+ if (fgets(buffer, sizeof(buffer), status) == nullptr) {
+ if (ferror(status)) {
+ PLOG(FATAL, "fgets(%p, %zu, %p)", buffer, sizeof(buffer), status);
+ } else {
+ break;
+ }
+ }
+ ::std::string line(buffer);
+ if (line.substr(0, 5) == "Name:") {
+ *name = strip_string_prefix(5, line);
+ } else if (line.substr(0, 4) == "Pid:") {
+ pid = ::std::stoi(strip_string_prefix(4, line));
+ } else if (line.substr(0, 5) == "PPid:") {
+ status_ppid = ::std::stoi(strip_string_prefix(5, line));
+ } else if (line.substr(0, 5) == "Tgid:") {
+ *pgrp = ::std::stoi(strip_string_prefix(5, line));
+ }
+ }
+ PCHECK(fclose(status));
+ CHECK_EQ(pid, process);
+ CHECK_EQ(status_ppid, ppid);
+}
+
+} // namespace
+
+int main() {
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+
+ const int pid_max = find_pid_max();
+ const cpu_set_t all_cpus = find_all_cpus();
+
+ for (int i = 0; i < pid_max; ++i) {
+ bool not_there = false;
+
+ const cpu_set_t cpu_mask = find_cpu_mask(i, ¬_there);
+ const sched_param param = find_sched_param(i, ¬_there);
+ const int scheduler = find_scheduler(i, ¬_there);
+ const ::std::string exe = find_exe(i, ¬_there);
+ const int nice_value = find_nice_value(i, ¬_there);
+
+ int ppid = 0, sid = 0;
+ read_stat(i, &ppid, &sid, ¬_there);
+
+ int pgrp = 0;
+ ::std::string name;
+ read_status(i, ppid, &pgrp, &name, ¬_there);
+
+ if (not_there) continue;
+
+ const char *cpu_mask_string =
+ CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+
+ printf("%s,%s,%s,%s,%d,%d,%d,%d,%d,%d\n", exe.c_str(), name.c_str(),
+ cpu_mask_string, policy_string(scheduler), nice_value,
+ param.sched_priority, i, pgrp, ppid, sid);
+ }
+}
diff --git a/aos/linux_code/init.cc b/aos/linux_code/init.cc
index d7501af..a4e72db 100644
--- a/aos/linux_code/init.cc
+++ b/aos/linux_code/init.cc
@@ -109,4 +109,12 @@
SetSoftRLimit(RLIMIT_CORE, RLIM_INFINITY, true);
}
+void SetCurrentThreadRealtimePriority(int priority) {
+ struct sched_param param;
+ param.sched_priority = priority;
+ if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
+ PLOG(FATAL, "sched_setscheduler(0, SCHED_FIFO, %d) failed", priority);
+ }
+}
+
} // namespace aos
diff --git a/aos/linux_code/init.h b/aos/linux_code/init.h
index 538684e..ab1aab5 100644
--- a/aos/linux_code/init.h
+++ b/aos/linux_code/init.h
@@ -25,6 +25,9 @@
// behavior without calling Init*.
void WriteCoreDumps();
+// Sets the current thread's realtime priority.
+void SetCurrentThreadRealtimePriority(int priority);
+
} // namespace aos
#endif // AOS_LINUX_CODE_INIT_H_
diff --git a/aos/linux_code/linux_code.gyp b/aos/linux_code/linux_code.gyp
index c34b42e..16f3fd6 100644
--- a/aos/linux_code/linux_code.gyp
+++ b/aos/linux_code/linux_code.gyp
@@ -1,6 +1,17 @@
{
'targets': [
{
+ 'target_name': 'dump_rtprio',
+ 'type': 'executable',
+ 'sources': [
+ 'dump_rtprio.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ {
'target_name': 'complex_thread_local',
'type': 'static_library',
'sources': [
diff --git a/aos/linux_code/logging/binary_log_file.cc b/aos/linux_code/logging/binary_log_file.cc
index 0142db3..210d443 100644
--- a/aos/linux_code/logging/binary_log_file.cc
+++ b/aos/linux_code/logging/binary_log_file.cc
@@ -63,6 +63,9 @@
if (result == -1) {
PLOG(FATAL, "read(%d, %p, %zu) failed", fd_,
current_ + (kPageSize - todo), todo);
+ } else if (result == 0) {
+ memset(current_, 0, todo);
+ result = todo;
}
todo -= result;
}
diff --git a/aos/linux_code/logging/log_displayer.cc b/aos/linux_code/logging/log_displayer.cc
index 7da0dc4..b2f4d45 100644
--- a/aos/linux_code/logging/log_displayer.cc
+++ b/aos/linux_code/logging/log_displayer.cc
@@ -154,7 +154,7 @@
}
break;
case 'p':
- if (!::aos::util::StringToInteger(::std::string(optarg), &source_pid)) {
+ if (!::aos::util::StringToNumber(::std::string(optarg), &source_pid)) {
fprintf(stderr, "ERROR: -p expects a number, not '%s'.\n", optarg);
exit(EXIT_FAILURE);
}
@@ -177,7 +177,7 @@
skip_to_end = true;
break;
case 'm':
- if (!::aos::util::StringToInteger(::std::string(optarg), &display_max)) {
+ if (!::aos::util::StringToNumber(::std::string(optarg), &display_max)) {
fprintf(stderr, "ERROR: -m expects a number, not '%s'.\n", optarg);
exit(EXIT_FAILURE);
}
diff --git a/bot3/actions/actions.gyp b/bot3/actions/actions.gyp
index 55af1aa..a1c0130 100644
--- a/bot3/actions/actions.gyp
+++ b/bot3/actions/actions.gyp
@@ -76,10 +76,12 @@
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
},
{
diff --git a/doc/building-the-code.txt b/doc/building-the-code.txt
index fc3a7ae..c2676d1 100644
--- a/doc/building-the-code.txt
+++ b/doc/building-the-code.txt
@@ -1,7 +1,7 @@
This file contains instructions on how to set up a computer to build the code.
[OS]
-Most of these instructions assume 64 bit Debian Squeeze.
+Most of these instructions assume 64 bit Debian Wheezy.
[Install Packages]
First, you have to download and follow the directions in
@@ -18,17 +18,19 @@
[Running Locally]
If you want to be able to run the realtime code on your development machine
without just disabling the realtime part (set the AOS_NO_REALTIME environment
- variable), follow the directions in /src/aos/config/aos.conf.
+ variable), follow the directions in //aos/config/aos.conf.
+If you want to run the clang or gcc_4.8 amd64 code, add /opt/clang-3.5/lib64/ to
+ LD_LIBRARY_PATH. Using the system gcc-compiled versions should just work.
[Compiling and Downloading]
-Run src/frc971/{atom_code,crio}/build.sh.
+Run //frc971/*/build.sh.
Give it clean, tests, or deploy as a first argument to do something other
than just build.
Each action (build (the default), clean, tests, or deploy) has a different
default set of versions of the code it builds. You can change those by
passing another argument. Some popular ones are 'all' (build everything),
'clang-amd64-none' (the most basic one for local testing), and
- 'gcc-arm-nodebug' (the one that gets downloaded). See its --help for
+ 'gcc_frc-arm-nodebug' (the one that gets downloaded). See its --help for
more details.
[Communicating with the cRIO]
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 1243e58..51da4cf 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -7,6 +7,7 @@
#include <functional>
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "aos/common/time.h"
namespace frc971 {
@@ -24,15 +25,18 @@
action_q_->goal.FetchLatest();
if (action_q_->goal.get()) {
+ LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
const uint32_t initially_running = action_q_->goal->run;
if (initially_running != 0) {
while (action_q_->goal->run == initially_running) {
LOG(INFO, "run is still %" PRIx32 "\n", initially_running);
action_q_->goal.FetchNextBlocking();
+ LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
}
}
} else {
action_q_->goal.FetchNextBlocking();
+ LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
}
LOG(DEBUG, "actually starting\n");
@@ -44,6 +48,7 @@
while (!action_q_->goal->run) {
LOG(INFO, "Waiting for an action request.\n");
action_q_->goal.FetchNextBlocking();
+ LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
if (!action_q_->goal->run) {
if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
LOG(ERROR, "Failed to send the status.\n");
@@ -73,6 +78,7 @@
LOG(INFO, "Waiting for the action (%" PRIx32 ") to be stopped.\n",
running_id);
action_q_->goal.FetchNextBlocking();
+ LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
}
LOG(DEBUG, "action %" PRIx32 " was stopped\n", running_id);
}
@@ -110,7 +116,9 @@
// Returns true if the action should be canceled.
bool ShouldCancel() {
- action_q_->goal.FetchLatest();
+ if (action_q_->goal.FetchNext()) {
+ LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
+ }
bool ans = !action_q_->goal->run;
if (ans) {
LOG(INFO, "Time to stop action\n");
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index ff18204..1d75b02 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -141,10 +141,12 @@
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
},
{
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index 1d3b4a1..fa2afdf 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -114,29 +114,25 @@
(::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 %sready zeroed %d bottom_velocity %f bottom %f sep %f\n",
- ans ? "" : "not ", 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));
- }
+ LOG(DEBUG, "Claw is %sready zeroed %d bottom_velocity %f bottom %f sep %f\n",
+ ans ? "" : "not ", 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 ShooterIsReady() {
control_loops::shooter_queue_group.goal.FetchLatest();
- if (control_loops::shooter_queue_group.status->ready) {
- LOG(INFO, "Power error is %f - %f -> %f, ready %d\n",
- control_loops::shooter_queue_group.status->hard_stop_power,
- control_loops::shooter_queue_group.goal->shot_power,
- ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
- control_loops::shooter_queue_group.goal->shot_power),
- control_loops::shooter_queue_group.status->ready);
- }
+ LOG(DEBUG, "Power error is %f - %f -> %f, ready %d\n",
+ control_loops::shooter_queue_group.status->hard_stop_power,
+ control_loops::shooter_queue_group.goal->shot_power,
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power),
+ control_loops::shooter_queue_group.status->ready);
return (::std::abs(
control_loops::shooter_queue_group.status->hard_stop_power -
control_loops::shooter_queue_group.goal->shot_power) < 1.0) &&
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index ba7adf6..32fe567 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -49,7 +49,7 @@
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
-const double kRezeroThreshold = 0.03;
+const double kRezeroThreshold = 0.07;
ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> &&loop)
: StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 8e283ff..2a35c62 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -449,9 +449,8 @@
LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
shooter_.absolute_position(), PowerToPosition(goal->shot_power));
if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) +
- ::std::abs(shooter_.absolute_velocity()) <
- 0.001) {
+ PowerToPosition(goal->shot_power)) < 0.001 &&
+ ::std::abs(shooter_.absolute_velocity()) < 0.005) {
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 1b3792e..edae002 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -56,28 +56,5 @@
'<(AOS)/common/controls/controls.gyp:output_check',
],
},
- {
- 'target_name': 'wpilib_interface',
- 'type': 'executable',
- 'sources': [
- 'wpilib_interface.cc'
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/build/externals.gyp:WPILib_roboRIO',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/controls/controls.gyp:sensor_generation',
- '<(AOS)/common/util/util.gyp:log_interval',
- '../frc971.gyp:constants',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(AOS)/common/controls/controls.gyp:output_check',
- ],
- },
],
}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 5a9382a..8455ba9 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -64,7 +64,7 @@
'conditions': [
['FULL_COMPILER=="gcc_frc"', {
'dependencies': [
- '../output/output.gyp:wpilib_interface',
+ '../wpilib/wpilib.gyp:wpilib_interface',
],
}, {
'dependencies': [
diff --git a/frc971/wpilib/hall_effect.h b/frc971/wpilib/hall_effect.h
new file mode 100644
index 0000000..8a088c3
--- /dev/null
+++ b/frc971/wpilib/hall_effect.h
@@ -0,0 +1,18 @@
+#ifndef FRC971_WPILIB_HALL_EFFECT_H_
+#define FRC971_WPILIB_HALL_EFFECT_H_
+
+#include "DigitalInput.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class HallEffect : public DigitalInput {
+ public:
+ HallEffect(int index) : DigitalInput(index) {}
+ bool GetHall() { return !Get(); }
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_HALL_EFFECT_H_
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
new file mode 100644
index 0000000..d2a687b
--- /dev/null
+++ b/frc971/wpilib/joystick_sender.cc
@@ -0,0 +1,48 @@
+#include "frc971/wpilib/joystick_sender.h"
+
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "DriverStation.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void JoystickSender::operator()() {
+ DriverStation *ds = DriverStation::GetInstance();
+ ::aos::SetCurrentThreadRealtimePriority(29);
+ uint16_t team_id = ::aos::network::GetTeamNumber();
+
+ while (run_) {
+ ds->WaitForData();
+ auto new_state = ::aos::robot_state.MakeMessage();
+
+ new_state->test_mode = ds->IsAutonomous();
+ new_state->fms_attached = ds->IsFMSAttached();
+ new_state->enabled = ds->IsEnabled();
+ new_state->autonomous = ds->IsAutonomous();
+ new_state->team_id = team_id;
+ new_state->fake = false;
+
+ for (int i = 0; i < 4; ++i) {
+ new_state->joysticks[i].buttons = 0;
+ for (int button = 0; button < 16; ++button) {
+ new_state->joysticks[i].buttons |= ds->GetStickButton(i, button + 1)
+ << button;
+ }
+ for (int j = 0; j < 4; ++j) {
+ new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ }
+ }
+ LOG_STRUCT(DEBUG, "robot_state", *new_state);
+
+ if (!new_state.Send()) {
+ LOG(WARNING, "sending robot_state failed\n");
+ }
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
new file mode 100644
index 0000000..205f9c4
--- /dev/null
+++ b/frc971/wpilib/joystick_sender.h
@@ -0,0 +1,22 @@
+#ifndef FRC971_WPILIB_JOYSTICK_SENDER_H_
+#define FRC971_WPILIB_JOYSTICK_SENDER_H_
+
+#include <atomic>
+
+namespace frc971 {
+namespace wpilib {
+
+class JoystickSender {
+ public:
+ void operator()();
+
+ void Quit() { run_ = false; }
+
+ private:
+ ::std::atomic<bool> run_{true};
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_JOYSTICK_SENDER_H_
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
new file mode 100644
index 0000000..32a213a
--- /dev/null
+++ b/frc971/wpilib/wpilib.gyp
@@ -0,0 +1,57 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'wpilib_interface',
+ 'type': 'executable',
+ 'sources': [
+ 'wpilib_interface.cc'
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(EXTERNALS):WPILib_roboRIO',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/controls/controls.gyp:sensor_generation',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '../frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(AOS)/common/controls/controls.gyp:output_check',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ 'hall_effect',
+ 'joystick_sender',
+ ],
+ },
+ {
+ 'target_name': 'joystick_sender',
+ 'type': 'static_library',
+ 'sources': [
+ 'joystick_sender.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ ],
+ },
+ {
+ 'target_name': 'hall_effect',
+ 'type': 'static_library',
+ 'sources': [
+ #'hall_effect.h',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ ],
+ },
+ ],
+}
diff --git a/frc971/output/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
similarity index 85%
rename from frc971/output/wpilib_interface.cc
rename to frc971/wpilib/wpilib_interface.cc
index 6c45324..2d66833 100644
--- a/frc971/output/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -7,15 +7,14 @@
#include "aos/prime/output/motor_output.h"
#include "aos/common/controls/output_check.q.h"
+#include "aos/common/messages/robot_state.q.h"
#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/common/messages/robot_state.q.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
#include "aos/common/util/phased_loop.h"
#include "aos/common/util/wrapping_counter.h"
-#include "aos/common/network/team_number.h"
#include "aos/linux_code/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -26,7 +25,16 @@
#include "frc971/queues/to_log.q.h"
#include "frc971/shifter_hall_effect.h"
-#include <WPILib.h>
+#include "frc971/wpilib/hall_effect.h"
+#include "frc971/wpilib/joystick_sender.h"
+
+#include "Encoder.h"
+#include "Talon.h"
+#include "DriverStation.h"
+#include "AnalogInput.h"
+#include "Solenoid.h"
+#include "Compressor.h"
+#include "RobotBase.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -39,15 +47,7 @@
using ::aos::util::WrappingCounter;
namespace frc971 {
-namespace output {
-
-void SetThreadRealtimePriority(int priority) {
- struct sched_param param;
- param.sched_priority = priority;
- if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
- PLOG(FATAL, "sched_setscheduler failed");
- }
-}
+namespace wpilib {
class priority_mutex {
public:
@@ -57,10 +57,10 @@
// constexpr.
priority_mutex() {
pthread_mutexattr_t attr;
- // Turn on priority inheritance.
#ifdef NDEBUG
-#error "Won't let perror be no-op ed"
+#error "Won't let assert_perror be no-op ed"
#endif
+ // Turn on priority inheritance.
assert_perror(pthread_mutexattr_init(&attr));
assert_perror(pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL));
assert_perror(pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT));
@@ -92,12 +92,7 @@
pthread_mutex_t handle_;
};
-class HallEffect : public DigitalInput {
- public:
- HallEffect(int index) : DigitalInput(index) {}
- bool GetHall() { return !Get(); }
-};
-
+// TODO(brian): Split this out into a separate file once DMA is in.
class EdgeCounter {
public:
EdgeCounter(int priority, Encoder *encoder, HallEffect *input,
@@ -114,8 +109,8 @@
// Waits for interrupts, locks the mutex, and updates the internal state.
// Updates the any_interrupt_count count when the interrupt comes in without
// the lock.
- void operator ()() {
- SetThreadRealtimePriority(priority_);
+ void operator()() {
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
input_->RequestInterrupts();
input_->SetUpSourceEdge(true, true);
@@ -211,11 +206,13 @@
::std::unique_ptr<::std::thread> thread_;
};
-// This class will synchronize sampling edges on a bunch of DigitalInputs with
+// This class will synchronize sampling edges on a bunch of HallEffects with
// the periodic poll.
//
// The data is provided to subclasses by calling SaveState when the state is
// consistent and ready.
+//
+// TODO(brian): Split this out into a separate file once DMA is in.
template <int num_sensors>
class PeriodicHallSynchronizer {
public:
@@ -293,7 +290,7 @@
}
void operator()() {
- SetThreadRealtimePriority(priority_);
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
while (run_) {
::aos::time::PhasedLoopXMS(10, 9000);
RunIteration();
@@ -341,11 +338,12 @@
};
double drivetrain_translate(int32_t in) {
- return static_cast<double>(in)
- / (256.0 /*cpr*/ * 2.0 /*2x. Stupid WPILib*/)
- * (18.0 / 50.0 /*output stage*/) * (56.0 / 30.0 /*encoder gears*/)
- // * constants::GetValues().drivetrain_encoder_ratio
- * (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+ return static_cast<double>(in) /
+ (256.0 /*cpr*/ * 2.0 /*2x. Stupid WPILib*/) *
+ (18.0 / 50.0 /*output stage*/) * (56.0 / 30.0 /*encoder gears*/)
+ // * constants::GetValues().drivetrain_encoder_ratio
+ *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
static const double kVcc = 5.15;
@@ -372,22 +370,18 @@
}
double claw_translate(int32_t in) {
- return static_cast<double>(in)
- / (256.0 /*cpr*/ * 4.0 /*quad*/)
- / (18.0 / 48.0 /*encoder gears*/)
- / (12.0 / 60.0 /*chain reduction*/)
- * (M_PI / 180.0)
- * 2.0 /*TODO(austin): Debug this, encoders read too little*/;
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) /
+ (18.0 / 48.0 /*encoder gears*/) / (12.0 / 60.0 /*chain reduction*/) *
+ (M_PI / 180.0) *
+ 2.0 /*TODO(austin): Debug this, encoders read too little*/;
}
double shooter_translate(int32_t in) {
- return static_cast<double>(in)
- / (256.0 /*cpr*/ * 4.0 /*quad*/)
- * 16 /*sprocket teeth*/ * 0.375 /*chain pitch*/
- * (2.54 / 100.0 /*in to m*/);
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ 16 /*sprocket teeth*/ * 0.375 /*chain pitch*/
+ * (2.54 / 100.0 /*in to m*/);
}
-
// This class sends out half of the claw position state at 100 hz.
class HalfClawHallSynchronizer : public PeriodicHallSynchronizer<3> {
public:
@@ -397,7 +391,8 @@
// interrupt_priority is the priority of the interrupt threads.
// encoder is the encoder to read.
// sensors is an array of hall effect sensors. The sensors[0] is the front
- // sensor, sensors[1] is the calibration sensor, sensors[2] is the back sensor.
+ // sensor, sensors[1] is the calibration sensor, sensors[2] is the back
+ // sensor.
HalfClawHallSynchronizer(
const char *name, int priority, int interrupt_priority,
::std::unique_ptr<Encoder> encoder,
@@ -514,7 +509,6 @@
bool reversed_;
};
-
// This class sends out the shooter position state at 100 hz.
class ShooterHallSynchronizer : public PeriodicHallSynchronizer<2> {
public:
@@ -559,15 +553,15 @@
}
{
- const auto &distal_edge_counter = edge_counters()[1];
- shooter_position->pusher_distal.current =
- distal_edge_counter->polled_value();
- shooter_position->pusher_distal.posedge_count =
- distal_edge_counter->positive_interrupt_count();
- shooter_position->pusher_distal.negedge_count =
- distal_edge_counter->negative_interrupt_count();
- shooter_position->pusher_distal.posedge_value =
- shooter_translate(distal_edge_counter->last_positive_encoder_value());
+ const auto &distal_edge_counter = edge_counters()[1];
+ shooter_position->pusher_distal.current =
+ distal_edge_counter->polled_value();
+ shooter_position->pusher_distal.posedge_count =
+ distal_edge_counter->positive_interrupt_count();
+ shooter_position->pusher_distal.negedge_count =
+ distal_edge_counter->negative_interrupt_count();
+ shooter_position->pusher_distal.posedge_value =
+ shooter_translate(distal_edge_counter->last_positive_encoder_value());
}
shooter_position.Send();
@@ -578,33 +572,32 @@
::std::unique_ptr<HallEffect> shooter_latch_;
};
-
class SensorReader {
public:
SensorReader()
: auto_selector_analog_(new AnalogInput(4)),
- left_encoder_(new Encoder(10, 11, false, Encoder::k2X)), // E0
- right_encoder_(new Encoder(12, 13, false, Encoder::k2X)), // E1
- low_left_drive_hall_(new AnalogInput(2)),
- high_left_drive_hall_(new AnalogInput(3)),
- low_right_drive_hall_(new AnalogInput(1)),
- high_right_drive_hall_(new AnalogInput(0)),
- shooter_plunger_(new HallEffect(1)), // S3
- shooter_latch_(new HallEffect(0)), // S4
- shooter_distal_(new HallEffect(2)), // S2
- shooter_proximal_(new HallEffect(3)), // S1
- shooter_encoder_(new Encoder(19, 18)), // E2
- claw_top_front_hall_(new HallEffect(5)), // R2
- claw_top_calibration_hall_(new HallEffect(6)), // R3
- claw_top_back_hall_(new HallEffect(4)), // R2
- claw_top_encoder_(new Encoder(20, 21)), // E3
+ left_encoder_(new Encoder(11, 10, false, Encoder::k2X)), // E0
+ right_encoder_(new Encoder(13, 12, false, Encoder::k2X)), // E1
+ low_left_drive_hall_(new AnalogInput(1)),
+ high_left_drive_hall_(new AnalogInput(0)),
+ low_right_drive_hall_(new AnalogInput(2)),
+ high_right_drive_hall_(new AnalogInput(3)),
+ shooter_plunger_(new HallEffect(8)), // S3
+ shooter_latch_(new HallEffect(9)), // S4
+ shooter_distal_(new HallEffect(7)), // S2
+ shooter_proximal_(new HallEffect(6)), // S1
+ shooter_encoder_(new Encoder(14, 15)), // E2
+ claw_top_front_hall_(new HallEffect(4)), // R2
+ claw_top_calibration_hall_(new HallEffect(3)), // R3
+ claw_top_back_hall_(new HallEffect(5)), // R1
+ claw_top_encoder_(new Encoder(17, 16)), // E3
// L2 Middle Left hall effect sensor.
- claw_bottom_front_hall_(new HallEffect(8)),
+ claw_bottom_front_hall_(new HallEffect(1)),
// L3 Bottom Left hall effect sensor
- claw_bottom_calibration_hall_(new HallEffect(9)),
+ claw_bottom_calibration_hall_(new HallEffect(0)),
// L1 Top Left hall effect sensor
- claw_bottom_back_hall_(new HallEffect(7)),
- claw_bottom_encoder_(new Encoder(22, 23)), // E5
+ claw_bottom_back_hall_(new HallEffect(2)),
+ claw_bottom_encoder_(new Encoder(21, 20)), // E5
run_(true) {
filter_.SetPeriodNanoSeconds(100000);
filter_.Add(shooter_plunger_.get());
@@ -625,7 +618,7 @@
void operator()() {
const int kPriority = 30;
const int kInterruptPriority = 55;
- SetThreadRealtimePriority(kPriority);
+ ::aos::SetCurrentThreadRealtimePriority(kPriority);
::std::array<::std::unique_ptr<HallEffect>, 2> shooter_sensors;
shooter_sensors[0] = ::std::move(shooter_proximal_);
@@ -668,7 +661,8 @@
// to go from visible to software to having triggered an interrupt.
::aos::time::SleepFor(::aos::time::Time::InUS(120));
- if (bottom_claw.TryFinishingIteration() && top_claw.TryFinishingIteration()) {
+ if (bottom_claw.TryFinishingIteration() &&
+ top_claw.TryFinishingIteration()) {
break;
}
}
@@ -709,7 +703,7 @@
gyro_reading.MakeWithBuilder().angle(0).Send();
}
- if (ds->SystemActive()) {
+ if (ds->IsSysActive()) {
auto message = ::aos::controls::output_check_received.MakeMessage();
// TODO(brians): Actually read a pulse value from the roboRIO.
message->pwm_value = 0;
@@ -794,15 +788,15 @@
shooter_brake_(new Solenoid(4)),
compressor_(new Compressor()) {
compressor_->SetClosedLoopControl(true);
- //right_drivetrain_talon_->EnableDeadbandElimination(true);
- //left_drivetrain_talon_->EnableDeadbandElimination(true);
- //shooter_talon_->EnableDeadbandElimination(true);
- //top_claw_talon_->EnableDeadbandElimination(true);
- //bottom_claw_talon_->EnableDeadbandElimination(true);
- //left_tusk_talon_->EnableDeadbandElimination(true);
- //right_tusk_talon_->EnableDeadbandElimination(true);
- //intake1_talon_->EnableDeadbandElimination(true);
- //intake2_talon_->EnableDeadbandElimination(true);
+ // right_drivetrain_talon_->EnableDeadbandElimination(true);
+ // left_drivetrain_talon_->EnableDeadbandElimination(true);
+ // shooter_talon_->EnableDeadbandElimination(true);
+ // top_claw_talon_->EnableDeadbandElimination(true);
+ // bottom_claw_talon_->EnableDeadbandElimination(true);
+ // left_tusk_talon_->EnableDeadbandElimination(true);
+ // right_tusk_talon_->EnableDeadbandElimination(true);
+ // intake1_talon_->EnableDeadbandElimination(true);
+ // intake2_talon_->EnableDeadbandElimination(true);
}
// Maximum age of an output packet before the motors get zeroed instead.
@@ -937,57 +931,17 @@
constexpr ::aos::time::Time MotorWriter::kOldLogInterval;
-class JoystickSender {
- public:
- JoystickSender() : run_(true) {}
-
- void operator()() {
- DriverStation *ds = DriverStation::GetInstance();
- SetThreadRealtimePriority(29);
- uint16_t team_id = ::aos::network::GetTeamNumber();
-
- while (run_) {
- ds->WaitForData();
- auto new_state = ::aos::robot_state.MakeMessage();
-
- new_state->test_mode = ds->IsAutonomous();
- new_state->fms_attached = ds->IsFMSAttached();
- new_state->enabled = ds->IsEnabled();
- new_state->autonomous = ds->IsAutonomous();
- new_state->team_id = team_id;
- new_state->fake = false;
-
- for (int i = 0; i < 4; ++i) {
- new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 4; ++j) {
- new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j + 1);
- }
- }
- LOG_STRUCT(DEBUG, "robot_state", *new_state);
-
- if (!new_state.Send()) {
- LOG(WARNING, "sending robot_state failed\n");
- }
- }
- }
-
- void Quit() { run_ = false; }
-
- private:
- ::std::atomic<bool> run_;
-};
-
-} // namespace output
+} // namespace wpilib
} // namespace frc971
class WPILibRobot : public RobotBase {
public:
virtual void StartCompetition() {
::aos::Init();
- ::frc971::output::MotorWriter writer;
- ::frc971::output::SensorReader reader;
+ ::frc971::wpilib::MotorWriter writer;
+ ::frc971::wpilib::SensorReader reader;
::std::thread reader_thread(::std::ref(reader));
- ::frc971::output::JoystickSender joystick_sender;
+ ::frc971::wpilib::JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
writer.Run();
LOG(ERROR, "Exiting WPILibRobot\n");
@@ -999,4 +953,5 @@
}
};
+
START_ROBOT_CLASS(WPILibRobot);