Merge "Enable y2020 Localizer in drivetrain_main"
diff --git a/aos/BUILD b/aos/BUILD
index 47de872..12d25b2 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -1,11 +1,10 @@
load("//tools:environments.bzl", "mcu_cpus")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library", "flatbuffer_ts_library")
filegroup(
name = "prime_binaries",
srcs = [
"//aos:aos_dump",
- "//aos:core",
"//aos/starter",
],
visibility = ["//visibility:public"],
@@ -23,7 +22,6 @@
name = "prime_binaries_stripped",
srcs = [
# starter is hard coded to look for a non-stripped core...
- "//aos:core",
"//aos:aos_dump.stripped",
"//aos/starter",
],
@@ -287,6 +285,12 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "configuration_ts_fbs",
+ srcs = ["configuration.fbs"],
+ visibility = ["//visibility:public"],
+)
+
flatbuffer_py_library(
name = "configuration_fbs_python",
srcs = ["configuration.fbs"],
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 6047cbc..d0ca196 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -344,8 +344,9 @@
protected:
TimerHandler(EventLoop *event_loop, std::function<void()> fn);
- void Call(std::function<monotonic_clock::time_point()> get_time,
- monotonic_clock::time_point event_time);
+ monotonic_clock::time_point Call(
+ std::function<monotonic_clock::time_point()> get_time,
+ monotonic_clock::time_point event_time);
private:
friend class EventLoop;
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index dfb5a6d..1daf88c 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -100,7 +100,7 @@
return false;
}
-inline void TimerHandler::Call(
+inline monotonic_clock::time_point TimerHandler::Call(
std::function<monotonic_clock::time_point()> get_time,
monotonic_clock::time_point event_time) {
CHECK_NOTNULL(timing_.timer);
@@ -131,6 +131,7 @@
monotonic_end_time - monotonic_start_time)
.count();
timing_.handler_time.Add(handler_latency);
+ return monotonic_start_time;
}
inline void PhasedLoopHandler::Call(
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index b3026fa..dad62cd 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -470,8 +470,14 @@
: TimerHandler(shm_event_loop, std::move(fn)),
shm_event_loop_(shm_event_loop),
event_(this) {
- shm_event_loop_->epoll_.OnReadable(
- timerfd_.fd(), [this]() { shm_event_loop_->HandleEvent(); });
+ shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
+ // The timer may fire spurriously. HandleEvent on the event loop will
+ // call the callback if it is needed. It may also have called it when
+ // processing some other event, and the kernel decided to deliver this
+ // wakeup anyways.
+ timerfd_.Read();
+ shm_event_loop_->HandleEvent();
+ });
}
~TimerHandlerState() {
@@ -480,22 +486,29 @@
}
void HandleEvent() {
- uint64_t elapsed_cycles = timerfd_.Read();
- if (elapsed_cycles == 0u) {
- // We got called before the timer interrupt could happen, but because we
- // are checking the time, we got called on time. Push the timer out by 1
- // cycle.
- elapsed_cycles = 1u;
- timerfd_.SetTime(base_ + repeat_offset_, repeat_offset_);
+ CHECK(!event_.valid());
+ const auto monotonic_now = Call(monotonic_clock::now, base_);
+ if (event_.valid()) {
+ // If someone called Setup inside Call, rescheduling is already taken care
+ // of. Bail.
+ return;
}
- Call(monotonic_clock::now, base_);
+ if (repeat_offset_ == chrono::seconds(0)) {
+ timerfd_.Disable();
+ } else {
+ // Compute how many cycles have elapsed and schedule the next iteration
+ // for the next iteration in the future.
+ const int elapsed_cycles =
+ std::max<int>(0, (monotonic_now - base_ + repeat_offset_ -
+ std::chrono::nanoseconds(1)) /
+ repeat_offset_);
+ base_ += repeat_offset_ * elapsed_cycles;
- base_ += repeat_offset_ * elapsed_cycles;
-
- if (repeat_offset_ != chrono::seconds(0)) {
+ // Update the heap and schedule the timerfd wakeup.
event_.set_event_time(base_);
shm_event_loop_->AddEvent(&event_);
+ timerfd_.SetTime(base_, chrono::seconds(0));
}
}
diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index 22db82a..83a39c8 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -101,7 +101,7 @@
char test_device[10];
for (char i = 'a'; i < 'z'; ++i) {
snprintf(test_device, sizeof(test_device), "/dev/sd%c", i);
- LOG(INFO) << "Trying to access" << test_device;
+ VLOG(1) << "Trying to access" << test_device;
if (access(test_device, F_OK) != -1) {
snprintf(device, device_size, "sd%c", i);
return true;
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 96d561f..89853f0 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -13,6 +13,14 @@
],
)
+flatbuffer_ts_library(
+ name = "connect_ts_fbs",
+ srcs = ["connect.fbs"],
+ includes = [
+ "//aos:configuration_fbs_includes",
+ ],
+)
+
flatbuffer_cc_library(
name = "timestamp_fbs",
srcs = ["timestamp.fbs"],
diff --git a/aos/network/message_bridge_client.cc b/aos/network/message_bridge_client.cc
index c44eee0..891a8a1 100644
--- a/aos/network/message_bridge_client.cc
+++ b/aos/network/message_bridge_client.cc
@@ -3,7 +3,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-DEFINE_string(config, "multinode_pingpong_config.json", "Path to the config.");
+DEFINE_string(config, "config.json", "Path to the config.");
namespace aos {
namespace message_bridge {
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index 263a757..64b9cff 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -146,6 +146,8 @@
<< ": " << FlatbufferToJson(connect_message_);
connect_timer_ = event_loop_->AddTimer([this]() { SendConnect(); });
+ connect_timer_->set_name(std::string("connect_") +
+ remote_node_->name()->str());
event_loop_->OnRun(
[this]() { connect_timer_->Setup(event_loop_->monotonic_now()); });
@@ -347,6 +349,7 @@
// And kick it all off.
statistics_timer_ = event_loop_->AddTimer([this]() { SendStatistics(); });
+ statistics_timer_->set_name("statistics");
event_loop_->OnRun([this]() {
statistics_timer_->Setup(
event_loop_->monotonic_now() + chrono::milliseconds(100),
diff --git a/aos/network/message_bridge_server.cc b/aos/network/message_bridge_server.cc
index fa5e7c1..ee276d6 100644
--- a/aos/network/message_bridge_server.cc
+++ b/aos/network/message_bridge_server.cc
@@ -4,7 +4,7 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
-DEFINE_string(config, "multinode_pingpong_config.json", "Path to the config.");
+DEFINE_string(config, "config.json", "Path to the config.");
namespace aos {
namespace message_bridge {
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index f54e2bf..53afe84 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -227,17 +227,24 @@
server_connection_.resize(event_loop->configuration()->nodes()->size());
// Seed up all the per-node connection state.
+ // We are making the assumption here that every connection is bidirectional
+ // (data is being sent both ways). This is pretty safe because we are
+ // forwarding timestamps between nodes.
for (std::string_view destination_node_name :
configuration::DestinationNodeNames(event_loop->configuration(),
event_loop->node())) {
// Find the largest connection message so we can size our buffers big enough
- // to receive a connection message.
- max_size = std::max(
- max_size,
- static_cast<int32_t>(MakeConnectMessage(event_loop->configuration(),
- event_loop->node(),
- destination_node_name)
- .size()));
+ // to receive a connection message. The connect message comes from the
+ // client to the server, so swap the node arguments.
+ const int32_t connect_size = static_cast<int32_t>(
+ MakeConnectMessage(event_loop->configuration(),
+ configuration::GetNode(event_loop->configuration(),
+ destination_node_name),
+ event_loop->node()->name()->string_view())
+ .size());
+ VLOG(1) << "Connection to " << destination_node_name << " has size "
+ << connect_size;
+ max_size = std::max(max_size, connect_size);
const Node *destination_node = configuration::GetNode(
event_loop->configuration(), destination_node_name);
@@ -315,6 +322,7 @@
}
// Buffer up the max size a bit so everything fits nicely.
+ LOG(INFO) << "Max message size for all clients is " << max_size;
server_.SetMaxSize(max_size + 100);
statistics_timer_ = event_loop_->AddTimer([this]() { Tick(); });
diff --git a/aos/network/www/BUILD b/aos/network/www/BUILD
index 76e8ef4..bab5198 100644
--- a/aos/network/www/BUILD
+++ b/aos/network/www/BUILD
@@ -18,6 +18,8 @@
],
deps = [
"//aos/network:web_proxy_ts_fbs",
+ "//aos/network:connect_ts_fbs",
+ "//aos:configuration_ts_fbs",
],
visibility=["//visibility:public"],
)
diff --git a/aos/network/www/config_handler.ts b/aos/network/www/config_handler.ts
index 0af78b6..72b496e 100644
--- a/aos/network/www/config_handler.ts
+++ b/aos/network/www/config_handler.ts
@@ -1,10 +1,11 @@
-import {aos} from 'aos/network/web_proxy_generated';
+import {Configuration, Channel} from 'aos/configuration_generated';
+import {Connect} from 'aos/network/connect_generated';
export class ConfigHandler {
private readonly root_div = document.getElementById('config');
constructor(
- private readonly config: aos.Configuration,
+ private readonly config: Configuration,
private readonly dataChannel: RTCDataChannel) {}
printConfig() {
@@ -46,19 +47,18 @@
const channel = this.config.channels(index);
const namefb = builder.createString(channel.name());
const typefb = builder.createString(channel.type());
- aos.Channel.startChannel(builder);
- aos.Channel.addName(builder, namefb);
- aos.Channel.addType(builder, typefb);
- const channelfb = aos.Channel.endChannel(builder);
+ Channel.startChannel(builder);
+ Channel.addName(builder, namefb);
+ Channel.addType(builder, typefb);
+ const channelfb = Channel.endChannel(builder);
channels.push(channelfb);
}
const channelsfb =
- aos.message_bridge.Connect.createChannelsToTransferVector(
- builder, channels);
- aos.message_bridge.Connect.startConnect(builder);
- aos.message_bridge.Connect.addChannelsToTransfer(builder, channelsfb);
- const connect = aos.message_bridge.Connect.endConnect(builder);
+ Connect.createChannelsToTransferVector(builder, channels);
+ Connect.startConnect(builder);
+ Connect.addChannelsToTransfer(builder, channelsfb);
+ const connect = Connect.endConnect(builder);
builder.finish(connect);
const array = builder.asUint8Array();
console.log('connect', array);
diff --git a/aos/network/www/ping_handler.ts b/aos/network/www/ping_handler.ts
index 9b37d70..df7635d 100644
--- a/aos/network/www/ping_handler.ts
+++ b/aos/network/www/ping_handler.ts
@@ -1,8 +1,8 @@
-import {aos} from 'aos/events/ping_generated';
+import {Ping} from 'aos/events/ping_generated';
export function HandlePing(data: Uint8Array) {
const fbBuffer = new flatbuffers.ByteBuffer(data);
- const ping = aos.examples.Ping.getRootAsPing(fbBuffer);
+ const ping = Ping.getRootAsPing(fbBuffer);
document.getElementById('val').innerHTML = ping.value();
document.getElementById('time').innerHTML = ping.sendTime().low;
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 27ffbfe..80c71b2 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -1,5 +1,5 @@
-import {aos} from 'aos/network/web_proxy_generated';
import {ConfigHandler} from './config_handler';
+import {Configuration} from 'aos/configuration_generated';
// There is one handler for each DataChannel, it maintains the state of
// multi-part messages and delegates to a callback when the message is fully
@@ -16,7 +16,7 @@
handleMessage(e: MessageEvent): void {
const fbBuffer = new flatbuffers.ByteBuffer(new Uint8Array(e.data));
const messageHeader =
- aos.web_proxy.MessageHeader.getRootAsMessageHeader(fbBuffer);
+ WebProxy.MessageHeader.getRootAsMessageHeader(fbBuffer);
// Short circuit if only one packet
if (messageHeader.packetCount === 1) {
this.handlerFunc(messageHeader.dataArray());
@@ -44,8 +44,8 @@
private rtcPeerConnection: RTCPeerConnection|null = null;
private dataChannel: DataChannel|null = null;
private webSocketUrl: string;
- private configHandler: ConfigHandler|null =
- null private config: aos.Configuration|null = null;
+ private configHandler: ConfigHandler|null = null;
+ private config: Configuration|null = null;
private readonly handlerFuncs = new Map<string, (data: Uint8Array) => void>();
private readonly handlers = new Set<Handler>();
@@ -98,10 +98,10 @@
const candidateString = builder.createString(candidate.candidate);
const sdpMidString = builder.createString(candidate.sdpMid);
- const iceFb = aos.web_proxy.WebSocketIce.createWebSocketIce(
+ const iceFb = WebProxy.WebSocketIce.createWebSocketIce(
builder, candidateString, sdpMidString, candidate.sdpMLineIndex);
- const messageFb = aos.web_proxy.WebSocketMessage.createWebSocketMessage(
- builder, aos.web_proxy.Payload.WebSocketIce, iceFb);
+ const messageFb = WebProxy.WebSocketMessage.createWebSocketMessage(
+ builder, WebProxy.Payload.WebSocketIce, iceFb);
builder.finish(messageFb);
const array = builder.asUint8Array();
this.webSocketConnection.send(array.buffer.slice(array.byteOffset));
@@ -113,10 +113,10 @@
const builder = new flatbuffers.Builder(512);
const offerString = builder.createString(description.sdp);
- const webSocketSdp = aos.web_proxy.WebSocketSdp.createWebSocketSdp(
- builder, aos.web_proxy.SdpType.OFFER, offerString);
- const message = aos.web_proxy.WebSocketMessage.createWebSocketMessage(
- builder, aos.web_proxy.Payload.WebSocketSdp, webSocketSdp);
+ const webSocketSdp = WebProxy.WebSocketSdp.createWebSocketSdp(
+ builder, WebProxy.SdpType.OFFER, offerString);
+ const message = WebProxy.WebSocketMessage.createWebSocketMessage(
+ builder, WebProxy.Payload.WebSocketSdp, webSocketSdp);
builder.finish(message);
const array = builder.asUint8Array();
this.webSocketConnection.send(array.buffer.slice(array.byteOffset));
@@ -145,19 +145,19 @@
const buffer = new Uint8Array(e.data)
const fbBuffer = new flatbuffers.ByteBuffer(buffer);
const message =
- aos.web_proxy.WebSocketMessage.getRootAsWebSocketMessage(fbBuffer);
+ WebProxy.WebSocketMessage.getRootAsWebSocketMessage(fbBuffer);
switch (message.payloadType()) {
- case aos.web_proxy.Payload.WebSocketSdp:
- const sdpFb = message.payload(new aos.web_proxy.WebSocketSdp());
- if (sdpFb.type() !== aos.web_proxy.SdpType.ANSWER) {
+ case WebProxy.Payload.WebSocketSdp:
+ const sdpFb = message.payload(new WebProxy.WebSocketSdp());
+ if (sdpFb.type() !== WebProxy.SdpType.ANSWER) {
console.log('got something other than an answer back');
break;
}
this.rtcPeerConnection.setRemoteDescription(new RTCSessionDescription(
{'type': 'answer', 'sdp': sdpFb.payload()}));
break;
- case aos.web_proxy.Payload.WebSocketIce:
- const iceFb = message.payload(new aos.web_proxy.WebSocketIce());
+ case WebProxy.Payload.WebSocketIce:
+ const iceFb = message.payload(new WebProxy.WebSocketIce());
const candidate = {} as RTCIceCandidateInit;
candidate.candidate = iceFb.candidate();
candidate.sdpMid = iceFb.sdpMid();
diff --git a/aos/starter/starter.cc b/aos/starter/starter.cc
index 6860cc4..5d78b5a 100644
--- a/aos/starter/starter.cc
+++ b/aos/starter/starter.cc
@@ -73,7 +73,7 @@
void operator()(event *evt) {
if (evt == NULL) return;
if (event_del(evt) != 0) {
- AOS_LOG(WARNING, "event_del(%p) failed\n", evt);
+ LOG(WARNING) << "event_del(" << evt << ") failed";
}
}
};
@@ -140,16 +140,16 @@
void RemoveWatchFromMap() {
int watch = watch_to_remove_;
if (watch == -1) {
- AOS_CHECK_NE(watch_, -1);
+ CHECK_NE(watch_, -1);
watch = watch_;
}
if (watchers[watch] != this) {
- AOS_LOG(WARNING, "watcher for %s (%p) didn't find itself in the map\n",
- filename_.c_str(), this);
+ LOG(WARNING) << "watcher for " << filename_ << " (" << this
+ << ") didn't find itself in the map";
} else {
watchers.erase(watch);
}
- AOS_LOG(DEBUG, "removed watch ID %d\n", watch);
+ VLOG(1) << "removed watch ID " << watch;
if (watch_to_remove_ == -1) {
watch_ = -1;
} else {
@@ -158,20 +158,19 @@
}
void CreateWatch() {
- AOS_CHECK_EQ(watch_, -1);
+ CHECK_EQ(watch_, -1);
watch_ = inotify_add_watch(notify_fd, filename_.c_str(),
create_ ? IN_CREATE : (IN_ATTRIB |
IN_MODIFY |
IN_DELETE_SELF |
IN_MOVE_SELF));
if (watch_ == -1) {
- AOS_PLOG(FATAL,
- "inotify_add_watch(%d, %s,"
- " %s ? IN_CREATE : (IN_ATTRIB | IN_MODIFY)) failed",
- notify_fd, filename_.c_str(), create_ ? "true" : "false");
+ PLOG(FATAL) << "inotify_add_watch(" << notify_fd << ", " << filename_
+ << ", " << (create_ ? "true" : "false")
+ << " ? IN_CREATE : (IN_ATTRIB | IN_MODIFY)) failed";
}
watchers[watch_] = this;
- AOS_LOG(DEBUG, "watch for %s is %d\n", filename_.c_str(), watch_);
+ VLOG(1) << "watch for " << filename_ << " is " << watch_;
}
// This gets set up as the callback for EV_READ on the inotify file
@@ -180,7 +179,7 @@
unsigned int to_read;
// Use FIONREAD to figure out how many bytes there are to read.
if (ioctl(notify_fd, FIONREAD, &to_read) < 0) {
- AOS_PLOG(FATAL, "FIONREAD(%d, %p) failed", notify_fd, &to_read);
+ PLOG(FATAL) << "FIONREAD(" << notify_fd << ", " << &to_read << ") failed";
}
inotify_event *notifyevt = static_cast<inotify_event *>(malloc(to_read));
const char *end = reinterpret_cast<char *>(notifyevt) + to_read;
@@ -191,8 +190,8 @@
AOS_PLOG(FATAL, "read(%d, %p, %u) failed", notify_fd, notifyevt, to_read);
}
if (static_cast<size_t>(ret) != to_read) {
- AOS_LOG(ERROR, "read(%d, %p, %u) returned %zd instead of %u\n", notify_fd,
- notifyevt, to_read, ret, to_read);
+ LOG(ERROR) << "read(" << notify_fd << ", " << notifyevt << ", " << to_read
+ << ") returned " << ret << " instead of " << to_read;
return;
}
@@ -200,9 +199,10 @@
// multiple events at once.
while (reinterpret_cast<char *>(notifyevt) < end) {
if (watchers.count(notifyevt->wd) != 1) {
- AOS_LOG(WARNING, "couldn't find whose watch ID %d is\n", notifyevt->wd);
+ LOG(WARNING) << "couldn't find whose watch ID " << notifyevt->wd
+ << " is";
} else {
- AOS_LOG(DEBUG, "mask=%" PRIu32 "\n", notifyevt->mask);
+ VLOG(1) << "mask=" << notifyevt->mask;
// If the watch was removed.
if (notifyevt->mask & IN_IGNORED) {
watchers[notifyevt->wd]->WatchDeleted();
@@ -222,7 +222,7 @@
// INotifyReadable calls this method whenever the watch for our file gets
// removed somehow.
void WatchDeleted() {
- AOS_LOG(DEBUG, "watch for %s deleted\n", filename_.c_str());
+ VLOG(1) << "watch for " << filename_ << " deleted";
RemoveWatchFromMap();
CreateWatch();
}
@@ -230,7 +230,7 @@
// INotifyReadable calls this method whenever the watch for our file triggers.
void FileNotified(const char *filename) {
AOS_CHECK_NE(watch_, -1);
- AOS_LOG(DEBUG, "got a notification for %s\n", filename_.c_str());
+ VLOG(1) << "got a notification for " << filename_;
if (!check_filename_.empty()) {
if (filename == NULL) {
@@ -315,8 +315,7 @@
}
if (feof(pipe)) {
- AOS_LOG(FATAL, "`%s` failed. didn't print a whole line\n",
- command.c_str());
+ LOG(FATAL) << "`" << command << "` failed. didn't print a whole line";
}
}
@@ -329,7 +328,7 @@
}
if (child_status != 0) {
- AOS_LOG(FATAL, "`%s` failed. return %d\n", command.c_str(), child_status);
+ LOG(FATAL) << "`" << command << "` failed. return " << child_status;
}
return std::string(result.get());
@@ -349,16 +348,14 @@
time_timeval.tv_usec = usec.count();
}
if (evtimer_add(timeout.release(), &time_timeval) != 0) {
- AOS_LOG(FATAL, "evtimer_add(%p, %p) failed\n", timeout.release(),
- &time_timeval);
+ LOG(FATAL) << "evtimer_add(" << timeout.release() << ", " << &time_timeval
+ << ") failed";
}
}
class Child;
-// This is where all of the Child instances except core live.
+// This is where all of the Child instances live.
std::vector<unique_ptr<Child>> children;
-// A global place to hold on to which child is core.
-unique_ptr<Child> core;
// Represents a child process. It will take care of restarting itself etc.
class Child {
@@ -401,7 +398,7 @@
monotonic_clock::time_point oldest = restarts_.front();
restarts_.pop();
if (monotonic_clock::now() <= kMaxRestartsTime + oldest) {
- AOS_LOG(WARNING, "process %s getting restarted too often\n", name());
+ LOG(WARNING) << "process " << name() << " getting restarted too often";
Timeout(kResumeWait, StaticStart, this);
return;
}
@@ -441,7 +438,7 @@
}
void FileModified() {
- AOS_LOG(DEBUG, "file for %s modified\n", name());
+ LOG(INFO) << "file for " << name() << " modified";
struct timeval restart_time_timeval;
{
::std::chrono::seconds sec =
@@ -455,18 +452,14 @@
}
// This will reset the timeout again if it hasn't run yet.
if (evtimer_add(restart_timeout.get(), &restart_time_timeval) != 0) {
- AOS_LOG(FATAL, "evtimer_add(%p, %p) failed\n", restart_timeout.get(),
- &restart_time_timeval);
+ LOG(FATAL) << "evtimer_add(" << restart_timeout.get() << ", "
+ << &restart_time_timeval << ") failed";
}
waiting_to_restart.insert(this);
}
static void StaticDoRestart(int, short, void *) {
- AOS_LOG(DEBUG, "restarting everything that needs it\n");
- if (waiting_to_restart.find(core.get()) != waiting_to_restart.end()) {
- core->DoRestart();
- waiting_to_restart.erase(core.get());
- }
+ LOG(INFO) << "restarting everything that needs it";
for (auto c : waiting_to_restart) {
c->DoRestart();
}
@@ -483,18 +476,14 @@
¤t_stat);
}
if (current_stat.st_mtime == stat_at_start_.st_mtime) {
- AOS_LOG(DEBUG, "ignoring trigger for %s because mtime didn't change\n",
- name());
+ LOG(INFO) << "ignoring trigger for " << name()
+ << " because mtime didn't change";
return;
}
}
- if (this == core.get()) {
- fprintf(stderr, "Restarting core -> exiting now.\n");
- exit(0);
- }
if (pid_ != -1) {
- AOS_LOG(DEBUG, "sending SIGTERM to child %d to restart it\n", pid_);
+ LOG(INFO) << "sending SIGTERM to child " << pid_ << " to restart it";
if (kill(pid_, SIGTERM) == -1) {
AOS_PLOG(WARNING, "kill(%d, SIGTERM) failed", pid_);
}
@@ -503,7 +492,7 @@
status->old_pid = pid_;
Timeout(kProcessDieTime, StaticCheckDied, status);
} else {
- AOS_LOG(WARNING, "%s restart attempted but not running\n", name());
+ LOG(WARNING) << name() << " restart attempted but not running";
}
}
@@ -516,9 +505,9 @@
// Checks to see if the child using the PID old_pid is still running.
void CheckDied(pid_t old_pid) {
if (pid_ == old_pid) {
- AOS_LOG(WARNING, "child %d refused to die\n", old_pid);
+ LOG(WARNING) << "child " << old_pid << " refused to die";
if (kill(old_pid, SIGKILL) == -1) {
- AOS_PLOG(WARNING, "kill(%d, SIGKILL) failed", old_pid);
+ LOG(WARNING) << "kill(" << old_pid << ", SIGKILL) failed";
}
}
}
@@ -530,8 +519,8 @@
// Actually starts the child.
void Start() {
if (pid_ != -1) {
- AOS_LOG(WARNING, "calling Start() but already have child %d running\n",
- pid_);
+ LOG(WARNING) << "calling Start() but already have child " << pid_
+ << " running";
if (kill(pid_, SIGKILL) == -1) {
AOS_PLOG(WARNING, "kill(%d, SIGKILL) failed", pid_);
return;
@@ -571,7 +560,7 @@
if (pid_ == -1) {
AOS_PLOG(FATAL, "forking to run \"%s\" failed", binary_.c_str());
}
- AOS_LOG(DEBUG, "started \"%s\" successfully\n", binary_.c_str());
+ LOG(INFO) << "started \"" << binary_ << "\" successfully";
}
// A history of the times that this process has been restarted.
@@ -658,10 +647,6 @@
}
}
- if (pid == core->pid()) {
- return core;
- }
-
static const unique_ptr<Child> kNothing;
return kNothing;
}
@@ -688,44 +673,40 @@
if (child) {
switch (infop.si_code) {
case CLD_EXITED:
- AOS_LOG(WARNING, "child %d (%s) exited with status %d\n", pid,
- child->name(), status);
+ LOG(WARNING) << "child " << pid << " (" << child->name()
+ << ") exited with status " << status;
break;
case CLD_DUMPED:
- AOS_LOG(INFO,
- "child %d actually dumped core. "
- "falling through to killed by signal case\n",
- pid);
+ LOG(INFO) << "child " << pid
+ << " actually dumped core. falling through to killed by "
+ "signal case";
[[fallthrough]];
/* FALLTHRU */
case CLD_KILLED:
// If somebody (possibly us) sent it SIGTERM that means that they just
// want it to stop, so it stopping isn't a WARNING.
- AOS_LOG((status == SIGTERM) ? DEBUG : WARNING,
- "child %d (%s) was killed by signal %d (%s)\n", pid,
- child->name(), status, aos_strsignal(status));
+ ((status == SIGTERM) ? LOG(INFO) : LOG(WARNING))
+ << "child " << pid << " (" << child->name()
+ << ") was killed by signal " << status << " ("
+ << aos_strsignal(status) << ")";
break;
case CLD_STOPPED:
- AOS_LOG(WARNING,
- "child %d (%s) was stopped by signal %d "
- "(giving it a SIGCONT(%d))\n",
- pid, child->name(), status, SIGCONT);
+ LOG(WARNING) << "child " << pid << " (" << child->name()
+ << ") was stopped by signal " << status
+ << " (giving it a SIGCONT(" << SIGCONT << "))";
kill(pid, SIGCONT);
continue;
default:
- AOS_LOG(WARNING, "something happened to child %d (%s) (killing it)\n",
- pid, child->name());
+ LOG(WARNING) << "something happened to child " << pid << " ("
+ << child->name() << ") (killing it)";
kill(pid, SIGKILL);
continue;
}
} else {
- AOS_LOG(WARNING, "couldn't find a Child for pid %d\n", pid);
+ LOG(WARNING) << "couldn't find a Child for pid " << pid;
return;
}
- if (child == core) {
- AOS_LOG(FATAL, "core died\n");
- }
child->ProcessDied();
}
}
@@ -734,7 +715,7 @@
// start from main to Run.
const char *child_list_file;
-void Run(void *watch);
+void Run();
void Main() {
logging::Init();
@@ -777,44 +758,15 @@
libevent_base = EventBaseUniquePtr(event_base_new());
- std::string core_touch_file = "/tmp/starter.";
- core_touch_file += std::to_string(static_cast<intmax_t>(getpid()));
- core_touch_file += ".core_touch_file";
- const int result =
- ::aos::util::RunCommand(("touch '" + core_touch_file + "'").c_str());
- if (result == -1) {
- AOS_PLOG(FATAL, "running `touch '%s'` failed\n", core_touch_file.c_str());
- } else if (!WIFEXITED(result) || WEXITSTATUS(result) != 0) {
- AOS_LOG(FATAL, "`touch '%s'` gave result %x\n", core_touch_file.c_str(),
- result);
- }
- FileWatch core_touch_file_watch(core_touch_file, Run, NULL);
- core = unique_ptr<Child>(
- new Child("core " + core_touch_file));
-
- FILE *pid_file = fopen("/tmp/starter.pid", "w");
- if (pid_file == NULL) {
- AOS_PLOG(FATAL, "fopen(\"/tmp/starter.pid\", \"w\") failed");
- } else {
- if (fprintf(pid_file, "%d", core->pid()) == -1) {
- AOS_PLOG(WARNING, "fprintf(%p, \"%%d\", %d) failed", pid_file,
- core->pid());
- }
- fclose(pid_file);
- }
-
- AOS_LOG(INFO, "waiting for %s to appear\n", core_touch_file.c_str());
+ Run();
event_base_dispatch(libevent_base.get());
- AOS_LOG(FATAL, "event_base_dispatch(%p) returned\n", libevent_base.get());
+ LOG(FATAL) << "event_base_dispatch(" << libevent_base.get() << ") returned";
}
// This is the callback for when core creates the file indicating that it has
// started.
-void Run(void *watch) {
- // Make it so it doesn't keep on seeing random changes in /tmp.
- static_cast<FileWatch *>(watch)->RemoveWatch();
-
+void Run() {
// It's safe now because core is up.
aos::InitNRT();
@@ -827,7 +779,7 @@
break;
}
if (list_file.rdstate() != 0) {
- AOS_LOG(FATAL, "reading input file %s failed\n", child_list_file);
+ LOG(FATAL) << "reading input file " << child_list_file << " failed";
}
children.push_back(unique_ptr<Child>(new Child(child_name)));
}
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index c5af65a..f5b14a5 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -1,10 +1,15 @@
#!/bin/bash
-# NI already has a core pattern, so we probably shouldn't change it.
-#echo '/home/driver/tmp/robot_logs/%e-%s-%p-%t.coredump' > /proc/sys/kernel/core_pattern
-ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
-ROBOT_CODE="/home/admin/robot_code"
+if [[ "$(hostname)" == "roboRIO"* ]]; then
+ ROBOT_CODE="/home/admin/robot_code"
+
+ ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
+ ln -s /var/local/natinst/log/FRC_UserProgram.log "${ROBOT_CODE}/FRC_UserProgram.log"
+else
+ ROBOT_CODE="${HOME}/robot_code"
+fi
+
cd "${ROBOT_CODE}"
export PATH="${PATH}:${ROBOT_CODE}"
while true; do
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index b9ec5c5..439f14f 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -229,7 +229,7 @@
double default_acceleration)
: ProfiledSubsystem<3, 1, ZeroingEstimator>(
::std::move(loop), {{ZeroingEstimator(zeroing_constants)}}),
- profile_(::aos::controls::kLoopFrequency),
+ profile_(this->loop_->plant().coefficients().dt),
range_(range),
default_velocity_(default_velocity),
default_acceleration_(default_acceleration) {
@@ -275,7 +275,7 @@
builder.add_voltage_error(this->X_hat(2, 0));
builder.add_calculated_velocity(
(position() - last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
+ ::aos::time::DurationInSeconds(this->loop_->plant().coefficients().dt));
builder.add_estimator_state(estimator_state);
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 76c5894..2ce45e9 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -138,7 +138,7 @@
// NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
// it to a RT priority of 33.
AOS_PCHECK(
- system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+ system("busybox ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
event_loop->set_name("IMU");
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index b558202..28b1a0b 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -194,10 +194,10 @@
// NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
// it to a RT priority of 33.
PCHECK(
- system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+ system("busybox ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
PCHECK(
- system("ps -ef | grep '\\[spi1\\]' | awk '{print $1}' | xargs chrt -f -p "
+ system("busybox ps -ef | grep '\\[spi1\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
event_loop_->OnRun([this]() { BeginInitialization(); });
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index c62d40a..2bdcc76 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -32,7 +32,7 @@
event_loop_->MakeSender<frc971::sensors::GyroReading>(
"/drivetrain")) {
AOS_PCHECK(
- system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+ system("busybox ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
event_loop->set_name("Gyro");
event_loop_->SetRuntimeRealtimePriority(33);
diff --git a/third_party/flatbuffers/src/idl_gen_js_ts.cpp b/third_party/flatbuffers/src/idl_gen_js_ts.cpp
index be0a205..cfb0cb6 100644
--- a/third_party/flatbuffers/src/idl_gen_js_ts.cpp
+++ b/third_party/flatbuffers/src/idl_gen_js_ts.cpp
@@ -153,10 +153,9 @@
imported_files.emplace(file.first);
}
- code += "export namespace " + file.second.target_namespace + " { \n";
+ //code += "export namespace " + file.second.target_namespace + " { \n";
code += "export import " + file.second.symbol + " = ";
- code += GenFileNamespacePrefix(file.first) + "." +
- file.second.source_namespace + "." + file.second.symbol +
+ code += file.second.symbol +
"; }\n";
}
}
@@ -334,7 +333,7 @@
std::string ns = GetNameSpace(enum_def);
std::string enum_def_name = enum_def.name + (reverse ? "Name" : "");
if (lang_.language == IDLOptions::kTs) {
- if (!ns.empty()) { code += "export namespace " + ns + "{\n"; }
+ //if (!ns.empty()) { code += "export namespace " + ns + "{\n"; }
code += "export enum " + enum_def.name + "{\n";
} else {
if (enum_def.defined_namespace->components.empty()) {
@@ -348,7 +347,7 @@
exports += "this." + enum_def_name + " = " + enum_def_name + ";\n";
}
}
- code += WrapInNameSpace(enum_def) + (reverse ? "Name" : "") + " = {\n";
+ code += enum_def.name + (reverse ? "Name" : "") + " = {\n";
}
for (auto it = enum_def.Vals().begin(); it != enum_def.Vals().end(); ++it) {
auto &ev = **it;
@@ -416,7 +415,7 @@
if (type.base_type == BASE_TYPE_BOOL) { getter = "!!" + getter; }
if (type.enum_def) {
getter = "/** " +
- GenTypeAnnotation(kType, WrapInNameSpace(*type.enum_def), "",
+ GenTypeAnnotation(kType, type.enum_def->name, "",
false) +
" */ (" + getter + ")";
}
@@ -433,15 +432,15 @@
if (value.type.enum_def) {
if (auto val = value.type.enum_def->FindByValue(value.constant)) {
if (lang_.language == IDLOptions::kTs) {
- return GenPrefixedTypeName(WrapInNameSpace(*value.type.enum_def),
+ return GenPrefixedTypeName(value.type.enum_def->name,
value.type.enum_def->file) +
"." + val->name;
} else {
- return WrapInNameSpace(*value.type.enum_def) + "." + val->name;
+ return value.type.enum_def->name + "." + val->name;
}
} else {
return "/** " +
- GenTypeAnnotation(kType, WrapInNameSpace(*value.type.enum_def),
+ GenTypeAnnotation(kType, value.type.enum_def->name,
"", false) +
"} */ (" + value.constant + ")";
}
@@ -473,7 +472,7 @@
if (type.base_type == BASE_TYPE_STRING) {
name = "string|Uint8Array";
} else {
- name = WrapInNameSpace(*type.struct_def);
+ name = type.struct_def->name;
}
return (allowNull) ? (name + "|null") : (name);
}
@@ -485,7 +484,7 @@
case BASE_TYPE_ULONG: return "flatbuffers.Long";
default:
if (IsScalar(type.base_type)) {
- if (type.enum_def) { return WrapInNameSpace(*type.enum_def); }
+ if (type.enum_def) { return type.enum_def->name; }
return "number";
}
return "flatbuffers.Offset";
@@ -675,9 +674,9 @@
if (lang_.language == IDLOptions::kTs) {
object_name = struct_def.name;
GenDocComment(struct_def.doc_comment, code_ptr, "@constructor");
- if (!object_namespace.empty()) {
- code += "export namespace " + object_namespace + "{\n";
- }
+ //if (!object_namespace.empty()) {
+ // code += "export namespace " + object_namespace + "{\n";
+ //}
code += "export class " + struct_def.name;
code += " {\n";
if (lang_.language != IDLOptions::kTs) {
@@ -695,7 +694,7 @@
code += " bb_pos:number = 0;\n";
} else {
bool isStatement = struct_def.defined_namespace->components.empty();
- object_name = WrapInNameSpace(struct_def);
+ object_name = struct_def.name;
GenDocComment(struct_def.doc_comment, code_ptr, "@constructor");
if (isStatement) {
if (parser_.opts.use_goog_js_export_format) {
@@ -851,7 +850,7 @@
else {
switch (field.value.type.base_type) {
case BASE_TYPE_STRUCT: {
- auto type = WrapInNameSpace(*field.value.type.struct_def);
+ auto type = field.value.type.struct_def->name;
GenDocComment(
field.doc_comment, code_ptr,
GenTypeAnnotation(kParam, type + "=", "obj") +
@@ -978,7 +977,7 @@
if (field.value.type.enum_def) {
code += "/** " +
GenTypeAnnotation(
- kType, WrapInNameSpace(*field.value.type.enum_def),
+ kType, field.value.type.enum_def->name,
"", false) +
" */ (" + field.value.constant + ")";
} else {
@@ -1359,9 +1358,9 @@
}
if (lang_.language == IDLOptions::kTs) {
- if (!object_namespace.empty()) {
- code += "}\n";
- }
+ //if (!object_namespace.empty()) {
+ // code += "}\n";
+ //}
code += "}\n";
}
}
diff --git a/y2020/BUILD b/y2020/BUILD
index f91f532..bc22b96 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -99,6 +99,7 @@
"//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2020:constants",
"//y2020/control_loops/drivetrain:drivetrain_base",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index a35798f..f0610a0 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -26,7 +26,6 @@
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 9971;
-const uint16_t kCodingRobotTeamNumber = 7971;
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
@@ -103,12 +102,15 @@
break;
case kCompTeamNumber:
- hood->zeroing_constants.measured_absolute_position = 0.0;
+ hood->zeroing_constants.measured_absolute_position = 0.432541963515072;
- intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position =
+ 1.42977866919024 - Values::kIntakeZero();
- turret->potentiometer_offset = 0.0;
- turret_params->zeroing_constants.measured_absolute_position = 0.0;
+ turret->potentiometer_offset =
+ 5.52519370141247 + 0.00853506822980376 + 0.0109413725126625;
+ turret_params->zeroing_constants.measured_absolute_position =
+ 0.251065633255048;
break;
case kPracticeTeamNumber:
@@ -120,7 +122,7 @@
turret_params->zeroing_constants.measured_absolute_position = 0.0;
break;
- case kCodingRobotTeamNumber:
+ case Values::kCodingRobotTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
intake->zeroing_constants.measured_absolute_position = 0.0;
diff --git a/y2020/constants.h b/y2020/constants.h
index 39f6e36..f0e3564 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -20,6 +20,8 @@
namespace constants {
struct Values {
+ static const uint16_t kCodingRobotTeamNumber = 7971;
+
static const int kZeroingSampleSize = 200;
static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
@@ -47,10 +49,10 @@
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- -0.01, // Back Hard
- 0.65, // Front Hard
- 0.0, // Back Soft
- 0.64 // Front Soft
+ 0.00, // Back Hard
+ 0.64, // Front Hard
+ 0.01, // Back Soft
+ 0.63 // Front Soft
};
}
@@ -69,16 +71,17 @@
kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
}
- // TODO(sabina): update range
static constexpr ::frc971::constants::Range kIntakeRange() {
return ::frc971::constants::Range{
- -1, // Back Hard
- 1, // Front Hard
- -0.95, // Back Soft
- 0.95 // Front Soft
+ -1.05, // Back Hard
+ 1.44, // Front Hard
+ -0.89, // Back Soft
+ 1.26 // Front Soft
};
}
+ static constexpr double kIntakeZero() { return -57 * M_PI / 180.0; }
+
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
intake;
@@ -90,7 +93,7 @@
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kTurretEncoderRatio() {
- return (26.0 / 150.0) * (130.0 / 40.0);
+ return (26.0 / 150.0) * (130.0 / 26.0);
}
static constexpr double kMaxTurretEncoderPulsesPerSecond() {
@@ -103,11 +106,10 @@
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
- // TODO (Kai): Placeholders right now.
- -3.2, // Back Hard
- 3.2, // Front Hard
- -3.14, // Back Soft
- 3.14 // Front Soft
+ -4.6, // Back Hard
+ 4.85, // Front Hard
+ -4.3, // Back Soft
+ 4.7123 // Front Soft
};
}
@@ -140,7 +142,7 @@
// Shooter
static constexpr double kFinisherEncoderCountsPerRevolution() {
- return 4096.0;
+ return 2048.0;
}
static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
@@ -152,7 +154,7 @@
static constexpr double kAcceleratorEncoderCountsPerRevolution() {
- return 4096.0;
+ return 2048.0;
}
static constexpr double kAcceleratorEncoderRatio() {
return (1.2 * 1.2 * 1.2) * (30.0 / 40.0);
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index a9e454a..5551179 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -111,9 +111,9 @@
FlatbufferToTransformationMatrix(
*result.camera_calibration()->fixed_extrinsics());
// Calculate the pose of the camera relative to the robot origin.
- Eigen::Matrix<double, 4, 4> H_camera_robot = fixed_extrinsics;
+ Eigen::Matrix<double, 4, 4> H_robot_camera = fixed_extrinsics;
if (is_turret) {
- H_camera_robot = H_camera_robot *
+ H_robot_camera = H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw(
turret_data.position) *
FlatbufferToTransformationMatrix(
@@ -130,14 +130,14 @@
!vision_result->has_field_to_target()) {
continue;
}
- const Eigen::Matrix<double, 4, 4> H_target_camera =
+ const Eigen::Matrix<double, 4, 4> H_camera_target =
FlatbufferToTransformationMatrix(*vision_result->camera_to_target());
- const Eigen::Matrix<double, 4, 4> H_target_field =
+ const Eigen::Matrix<double, 4, 4> H_field_target =
FlatbufferToTransformationMatrix(*vision_result->field_to_target());
// Back out the robot position that is implied by the current camera
// reading.
- const Pose measured_pose(H_target_field *
- (H_camera_robot * H_target_camera).inverse());
+ const Pose measured_pose(H_field_target *
+ (H_robot_camera * H_camera_target).inverse());
const Eigen::Matrix<double, 3, 1> Z(measured_pose.rel_pos().x(),
measured_pose.rel_pos().y(),
measured_pose.rel_theta());
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 3be86da..acab856 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -228,26 +228,27 @@
drivetrain_plant_.state()(2, 0));
std::unique_ptr<ImageMatchResultT> frame(new ImageMatchResultT());
- for (const auto &H_target_field : TargetLocations()) {
+ for (const auto &H_field_target : TargetLocations()) {
std::unique_ptr<CameraPoseT> camera_target(new CameraPoseT());
camera_target->field_to_target.reset(new TransformationMatrixT());
- camera_target->field_to_target->data = MatrixToVector(H_target_field);
+ camera_target->field_to_target->data = MatrixToVector(H_field_target);
- Eigen::Matrix<double, 4, 4> H_camera_turret =
+ Eigen::Matrix<double, 4, 4> H_turret_camera =
Eigen::Matrix<double, 4, 4>::Identity();
if (is_turreted_) {
- H_camera_turret = frc971::control_loops::TransformationMatrixForYaw(
+ H_turret_camera = frc971::control_loops::TransformationMatrixForYaw(
turret_position_) *
CameraTurretTransformation();
}
// TODO(james): Use non-zero turret angles.
camera_target->camera_to_target.reset(new TransformationMatrixT());
- camera_target->camera_to_target->data = MatrixToVector(
- (robot_pose.AsTransformationMatrix() * TurretRobotTransformation() *
- H_camera_turret).inverse() *
- H_target_field);
+ camera_target->camera_to_target->data =
+ MatrixToVector((robot_pose.AsTransformationMatrix() *
+ TurretRobotTransformation() * H_turret_camera)
+ .inverse() *
+ H_field_target);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -260,17 +261,17 @@
{
frame->camera_calibration->fixed_extrinsics.reset(
new TransformationMatrixT());
- TransformationMatrixT *H_turret_robot =
+ TransformationMatrixT *H_robot_turret =
frame->camera_calibration->fixed_extrinsics.get();
- H_turret_robot->data = MatrixToVector(TurretRobotTransformation());
+ H_robot_turret->data = MatrixToVector(TurretRobotTransformation());
}
if (is_turreted_) {
frame->camera_calibration->turret_extrinsics.reset(
new TransformationMatrixT());
- TransformationMatrixT *H_camera_turret =
+ TransformationMatrixT *H_turret_camera =
frame->camera_calibration->turret_extrinsics.get();
- H_camera_turret->data = MatrixToVector(CameraTurretTransformation());
+ H_turret_camera->data = MatrixToVector(CameraTurretTransformation());
}
camera_delay_queue_.emplace(monotonic_now(), std::move(frame));
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 3cd391c..ee94dd6 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -672,8 +672,8 @@
superstructure_plant_.set_peak_turret_velocity(23.0);
superstructure_plant_.set_peak_turret_acceleration(0.2);
- // Intake needs over 8 seconds to reach the goal
- RunFor(chrono::seconds(9));
+ // Intake needs over 9 seconds to reach the goal
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 06b8e40..a9c0f8c 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -14,6 +14,7 @@
#include "aos/util/log_interval.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
@@ -22,12 +23,24 @@
using aos::input::driver_station::JoystickAxis;
using aos::input::driver_station::POVLocation;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+
namespace y2020 {
namespace input {
namespace joysticks {
namespace superstructure = y2020::control_loops::superstructure;
+// TODO(sabina): fix button locations.
+
+const ButtonLocation kShootFast(4, 1);
+const ButtonLocation kShootSlow(4, 2);
+const ButtonLocation kIntakeExtend(4, 3);
+const ButtonLocation kIntakeIn(4, 4);
+const ButtonLocation kSpit(4, 5);
+
class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
@@ -45,13 +58,72 @@
AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
}
- void HandleTeleop(
- const ::aos::input::driver_station::Data & /*data*/) override {
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
if (!superstructure_status_fetcher_.get()) {
AOS_LOG(ERROR, "Got no superstructure status message.\n");
return;
}
+
+ double hood_pos = constants::Values::kHoodRange().upper;
+ double intake_pos = constants::Values::kIntakeRange().lower;
+ double turret_pos = 0.0;
+ float roller_speed = 0.0f;
+ double accelerator_speed = 0.0;
+ double finisher_speed = 0.0;
+
+ if (data.IsPressed(kShootFast)) {
+ accelerator_speed = 300.0;
+ finisher_speed = 300.0;
+ }
+
+ else if (data.IsPressed(kShootSlow)) {
+ accelerator_speed = 30.0;
+ finisher_speed = 30.0;
+ }
+
+ if (data.IsPressed(kIntakeExtend)) {
+ intake_pos = constants::Values::kIntakeRange().middle();
+ }
+
+ if (data.IsPressed(kIntakeIn)) {
+ roller_speed = 6.0f;
+ } else if (data.IsPressed(kSpit)) {
+ roller_speed = -6.0f;
+ }
+
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
+ {
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), hood_pos);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), intake_pos);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), turret_pos);
+
+ flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
+ superstructure::CreateShooterGoal(*builder.fbb(), accelerator_speed, finisher_speed);
+
+ superstructure::Goal::Builder superstructure_goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_goal_builder.add_hood(hood_offset);
+ superstructure_goal_builder.add_intake(intake_offset);
+ superstructure_goal_builder.add_turret(turret_offset);
+ superstructure_goal_builder.add_roller_voltage(roller_speed);
+ superstructure_goal_builder.add_shooter(shooter_offset);
+
+ if (!builder.Send(superstructure_goal_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
}
private:
diff --git a/y2020/vision/rootfs/README b/y2020/vision/rootfs/README
new file mode 100644
index 0000000..2320774
--- /dev/null
+++ b/y2020/vision/rootfs/README
@@ -0,0 +1,16 @@
+This modifies a stock debian root filesystem to be able to operate as a vision
+pi. It is not trying to be reproducible, but should be good enough for FRC
+purposes.
+
+The default hostname and IP is pi-8971-1, 10.89.71.101.
+ Username pi, password raspberry.
+
+Download 2020-02-13-raspbian-buster-lite.img (or equivalent) and edit
+modify_rootfs.sh to point to it. Run modify_rootfs.sh to build the filesystem
+(you might need to hit return in a spot or two).
+
+After confirming the target device, deploy with
+ dd if=2020-02-13-raspbian-buster-lite.img of=/dev/sdX
+
+From there, log in, sudo to root, and run /root/bin/change_hostname.sh to
+change the hostname to the actual target.
diff --git a/y2020/vision/rootfs/change_hostname.sh b/y2020/vision/rootfs/change_hostname.sh
new file mode 100755
index 0000000..1842504
--- /dev/null
+++ b/y2020/vision/rootfs/change_hostname.sh
@@ -0,0 +1,40 @@
+#!/bin/bash
+
+set -euo pipefail
+
+HOSTNAME="$1"
+
+if [[ ! "${HOSTNAME}" =~ ^pi-[0-9]*-[0-9]$ ]]; then
+ echo "Invalid hostname ${HOSTNAME}, needs to be pi-[team#]-[pi#]"
+ exit 1
+fi
+
+TEAM_NUMBER="$(echo ${HOSTNAME} | sed 's/pi-\(.*\)-.*/\1/')"
+PI_NUMBER="$(echo ${HOSTNAME} | sed 's/pi-.*-\(.*\)/\1/')"
+IP_BASE="$(echo ${TEAM_NUMBER} | sed 's/\(.*\)\(..\)/10.\1.\2/')"
+IP="${IP_BASE}.$(( 100 + ${PI_NUMBER}))"
+
+echo "Changing to team number ${TEAM_NUMBER}, IP ${IP}"
+
+sed -i "s/^static ip_address=.*$/static ip_address=${IP}\/24/" /etc/dhcpcd.conf
+
+sed -i "s/\(127\.0\.1\.1\t\).*$/\1${HOSTNAME}/" /etc/hosts
+
+echo "${HOSTNAME}" > /etc/hostname
+
+if grep /etc/hosts '^10\.[0-9]*\.[0-9]*\.[0-9]*\s*pi-[0-9]*-[0-9] pi[0-9]$' /etc/hosts >/dev/null ;
+then
+ sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi[0-9]\)$/${IP_BASE}\1${TEAM_NUMBER}\2/" /etc/hosts
+else
+ for i in {1..6}; do
+ echo -e "${IP_BASE}.$(( i + 100 ))\tpi-${TEAM_NUMBER}-${i} pi${i}" >> /etc/hosts
+ done
+fi
+
+if grep '^10\.[0-9]*\.[0-9]*\.2\s*roborio$' /etc/hosts >/dev/null;
+then
+ sed -i "s/^10\.[0-9]*\.[0-9]*\(\.2\s*roborio\)$/${IP_BASE}\1/" /etc/hosts
+else
+ echo -e "${IP_BASE}.2\troborio" >> /etc/hosts
+fi
+
diff --git a/y2020/vision/rootfs/dhcpcd.conf b/y2020/vision/rootfs/dhcpcd.conf
new file mode 100644
index 0000000..43a54b7
--- /dev/null
+++ b/y2020/vision/rootfs/dhcpcd.conf
@@ -0,0 +1,59 @@
+# A sample configuration for dhcpcd.
+# See dhcpcd.conf(5) for details.
+
+# Allow users of this group to interact with dhcpcd via the control socket.
+#controlgroup wheel
+
+# Inform the DHCP server of our hostname for DDNS.
+hostname
+
+# Use the hardware address of the interface for the Client ID.
+clientid
+# or
+# Use the same DUID + IAID as set in DHCPv6 for DHCPv4 ClientID as per RFC4361.
+# Some non-RFC compliant DHCP servers do not reply with this set.
+# In this case, comment out duid and enable clientid above.
+#duid
+
+# Persist interface configuration when dhcpcd exits.
+persistent
+
+# Rapid commit support.
+# Safe to enable by default because it requires the equivalent option set
+# on the server to actually work.
+option rapid_commit
+
+# A list of options to request from the DHCP server.
+option domain_name_servers, domain_name, domain_search, host_name
+option classless_static_routes
+# Respect the network MTU. This is applied to DHCP routes.
+option interface_mtu
+
+# Most distributions have NTP support.
+#option ntp_servers
+
+# A ServerID is required by RFC2131.
+require dhcp_server_identifier
+
+# Generate SLAAC address using the Hardware Address of the interface
+#slaac hwaddr
+# OR generate Stable Private IPv6 Addresses based from the DUID
+slaac private
+
+# Example static IP configuration:
+interface eth0
+static ip_address=10.89.71.102/24
+#static ip6_address=fd51:42f8:caae:d92e::ff/64
+#static routers=192.168.0.1
+#static domain_name_servers=192.168.0.1 8.8.8.8 fd51:42f8:caae:d92e::1
+
+# It is possible to fall back to a static IP if DHCP fails:
+# define static profile
+#profile static_eth0
+#static ip_address=192.168.1.23/24
+#static routers=192.168.1.1
+#static domain_name_servers=192.168.1.1
+
+# fallback to static profile on eth0
+#interface eth0
+#fallback static_eth0
diff --git a/y2020/vision/rootfs/modify_rootfs.sh b/y2020/vision/rootfs/modify_rootfs.sh
new file mode 100755
index 0000000..c472caa
--- /dev/null
+++ b/y2020/vision/rootfs/modify_rootfs.sh
@@ -0,0 +1,41 @@
+#!/bin/bash
+
+set -xe
+
+IMAGE="2020-02-13-raspbian-buster-lite.img"
+PARTITION="2020-02-13-raspbian-buster-lite.img.partition"
+HOSTNAME="pi-8971-1"
+
+function target() {
+ HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
+}
+
+function user_pi_target() {
+ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" sudo -h 127.0.0.1 -u pi "$@"
+}
+
+OFFSET="$(fdisk -lu "${IMAGE}" | grep "${IMAGE}2" | awk '{print 512*$2}')"
+mkdir -p "${PARTITION}"
+
+if mount | grep "${PARTITION}" >/dev/null ;
+then
+ echo "Already mounted"
+else
+ echo "Mounting"
+ sudo mount -o loop,offset=${OFFSET} "${IMAGE}" "${PARTITION}"
+fi
+
+sudo cp target_configure.sh "${PARTITION}/tmp/"
+sudo cp dhcpcd.conf "${PARTITION}/tmp/dhcpcd.conf"
+sudo cp change_hostname.sh "${PARTITION}/tmp/change_hostname.sh"
+
+target /bin/mkdir -p /home/pi/.ssh/
+cat ~/.ssh/id_rsa.pub | target tee /home/pi/.ssh/authorized_keys
+
+target /bin/bash /tmp/target_configure.sh
+
+# Run a prompt as root inside the target to poke around and check things.
+target /bin/bash --rcfile /root/.bashrc
+
+sudo umount "${PARTITION}"
+rmdir "${PARTITION}"
diff --git a/y2020/vision/rootfs/target_configure.sh b/y2020/vision/rootfs/target_configure.sh
new file mode 100755
index 0000000..2a072e5
--- /dev/null
+++ b/y2020/vision/rootfs/target_configure.sh
@@ -0,0 +1,51 @@
+#!/bin/bash
+
+set -ex
+
+mkdir -p /root/bin
+
+# Give it a static IP
+cp /tmp/dhcpcd.conf /etc/
+
+# And provide a script to change it.
+cp /tmp/change_hostname.sh /root/bin/
+chmod a+x /root/bin/change_hostname.sh
+
+chown -R pi.pi /home/pi/.ssh
+
+apt-get install -y vim-nox \
+ git \
+ cpufrequtils
+
+echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
+
+# Add a .bashrc and friends for root.
+if [[ ! -e "/root/.dotfiles" ]]; then
+ cd /root/
+ git clone --separate-git-dir=/root/.dotfiles https://github.com/AustinSchuh/.dotfiles.git tmpdotfiles
+ rsync --recursive --verbose --exclude '.git' tmpdotfiles/ /root/
+ rm -r tmpdotfiles
+ git --git-dir=/root/.dotfiles/ --work-tree=/root/ config --local status.showUntrackedFiles no
+ # Run the vundle installer which installs plugins on the first run of vim.
+ vim -c ":qa!"
+fi
+
+# Add a .bashrc and friends for pi.
+if [[ ! -e "/home/pi/.dotfiles" ]]; then
+ cd /home/pi/
+ su -c "git clone --separate-git-dir=/home/pi/.dotfiles https://github.com/AustinSchuh/.dotfiles.git tmpdotfiles" pi
+ su -c "rsync --recursive --verbose --exclude '.git' tmpdotfiles/ /home/pi/" pi
+ su -c "rm -r tmpdotfiles" pi
+ su -c "git --git-dir=/home/pi/.dotfiles/ --work-tree=/home/pi/ config --local status.showUntrackedFiles no" pi
+ # Run the vundle installer which installs plugins on the first run of vim.
+ su -c "vim -c ':qa!'" pi
+fi
+
+# Make SSH work and not complain about pi being the username and pw still.
+rm -f /etc/profile.d/sshpwd.sh
+rm -f /etc/profile.d/wifi-check.sh
+
+systemctl enable ssh.service
+
+# Default us to pi-8971-1
+/root/bin/change_hostname.sh pi-8971-1
diff --git a/y2020/vision/sift/BUILD b/y2020/vision/sift/BUILD
index e2b00f8..02507a9 100644
--- a/y2020/vision/sift/BUILD
+++ b/y2020/vision/sift/BUILD
@@ -1,5 +1,5 @@
load(":fast_gaussian.bzl", "fast_gaussian")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library", "flatbuffer_ts_library")
cc_binary(
name = "fast_gaussian_generator",
@@ -214,6 +214,12 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "sift_ts_fbs",
+ srcs = ["sift.fbs"],
+ visibility = ["//y2020:__subpackages__"],
+)
+
flatbuffer_cc_library(
name = "sift_training_fbs",
srcs = ["sift_training.fbs"],
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index a1083eb..3a8b646 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -25,6 +25,7 @@
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/make_unique.h"
+#include "aos/network/team_number.h"
#include "aos/realtime.h"
#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
@@ -125,7 +126,6 @@
}
// Hood
-
void set_hood_encoder(::std::unique_ptr<frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
hood_encoder_.set_encoder(::std::move(encoder));
@@ -166,20 +166,18 @@
}
// Shooter
-
- void set_flywheel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ void set_finisher_encoder(::std::unique_ptr<frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
- flywheel_encoder_ = ::std::move(encoder);
+ finisher_encoder_ = ::std::move(encoder);
+ }
+ void set_left_accelerator_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ left_accelerator_encoder_ = ::std::move(encoder);
}
- void set_left_kicker_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ void set_right_accelerator_encoder(::std::unique_ptr<frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
- left_kicker_encoder_ = ::std::move(encoder);
- }
-
- void set_right_kicker_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- right_kicker_encoder_ = ::std::move(encoder);
+ right_accelerator_encoder_ = ::std::move(encoder);
}
// Control Panel
@@ -227,7 +225,7 @@
frc971::AbsolutePositionT hood;
CopyPosition(hood_encoder_, &hood,
Values::kHoodEncoderCountsPerRevolution(),
- Values::kHoodEncoderRatio(), false);
+ Values::kHoodEncoderRatio(), true);
flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
frc971::AbsolutePosition::Pack(*builder.fbb(), &hood);
@@ -243,7 +241,7 @@
frc971::PotAndAbsolutePositionT turret;
CopyPosition(turret_encoder_, &turret,
Values::kTurretEncoderCountsPerRevolution(),
- Values::kTurretEncoderRatio(), turret_pot_translate, false,
+ Values::kTurretEncoderRatio(), turret_pot_translate, true,
values.turret.potentiometer_offset);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
@@ -251,16 +249,16 @@
// Shooter
y2020::control_loops::superstructure::ShooterPositionT shooter;
shooter.theta_finisher =
- encoder_translate(flywheel_encoder_->GetRaw(),
+ encoder_translate(finisher_encoder_->GetRaw(),
Values::kFinisherEncoderCountsPerRevolution(),
Values::kFinisherEncoderRatio());
- // TODO; check sign
+
shooter.theta_accelerator_left =
- encoder_translate(left_kicker_encoder_->GetRaw(),
+ encoder_translate(-left_accelerator_encoder_->GetRaw(),
Values::kAcceleratorEncoderCountsPerRevolution(),
Values::kAcceleratorEncoderRatio());
shooter.theta_accelerator_right =
- encoder_translate(right_kicker_encoder_->GetRaw(),
+ encoder_translate(right_accelerator_encoder_->GetRaw(),
Values::kAcceleratorEncoderCountsPerRevolution(),
Values::kAcceleratorEncoderRatio());
flatbuffers::Offset<y2020::control_loops::superstructure::ShooterPosition>
@@ -314,8 +312,9 @@
::frc971::wpilib::AbsoluteEncoder hood_encoder_, intake_joint_encoder_;
- ::std::unique_ptr<::frc::Encoder> flywheel_encoder_, left_kicker_encoder_,
- right_kicker_encoder_, control_panel_encoder_;
+ ::std::unique_ptr<::frc::Encoder> finisher_encoder_,
+ left_accelerator_encoder_, right_accelerator_encoder_,
+ control_panel_encoder_;
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
@@ -361,16 +360,16 @@
washing_machine_control_panel_victor_ = ::std::move(t);
}
- void set_kicker_left_falcon(::std::unique_ptr<::frc::TalonFX> t) {
- kicker_left_falcon_ = ::std::move(t);
+ void set_accelerator_left_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ accelerator_left_falcon_ = ::std::move(t);
}
- void set_kicker_right_falcon(::std::unique_ptr<::frc::TalonFX> t) {
- kicker_right_falcon_ = ::std::move(t);
+ void set_accelerator_right_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ accelerator_right_falcon_ = ::std::move(t);
}
void set_flywheel_falcon(::std::unique_ptr<::frc::TalonFX> t) {
- flywheel_falcon_ = ::std::move(t);
+ finisher_falcon_ = ::std::move(t);
}
void set_climber_falcon(
@@ -383,54 +382,55 @@
private:
void Write(const superstructure::Output &output) override {
- hood_victor_->SetSpeed(std::clamp(output.hood_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ hood_victor_->SetSpeed(
+ std::clamp(output.hood_voltage(), -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
- intake_joint_victor_->SetSpeed(std::clamp(output.intake_joint_voltage(),
- -kMaxBringupPower,
- kMaxBringupPower) /
+ intake_joint_victor_->SetSpeed(std::clamp(-output.intake_joint_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
12.0);
intake_roller_falcon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(output.intake_roller_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
+ std::clamp(-output.intake_roller_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
12.0);
- turret_victor_->SetSpeed(std::clamp(output.turret_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
+ turret_victor_->SetSpeed(std::clamp(-output.turret_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
12.0);
feeder_falcon_->SetSpeed(std::clamp(output.feeder_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
+ -kMaxBringupPower, kMaxBringupPower) /
12.0);
washing_machine_control_panel_victor_->SetSpeed(
- std::clamp(output.washing_machine_spinner_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
+ std::clamp(-output.washing_machine_spinner_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
12.0);
- kicker_left_falcon_->SetSpeed(std::clamp(output.accelerator_left_voltage(),
- -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ accelerator_left_falcon_->SetSpeed(
+ std::clamp(-output.accelerator_left_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
- kicker_right_falcon_->SetSpeed(
+ accelerator_right_falcon_->SetSpeed(
std::clamp(output.accelerator_right_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
+ kMaxBringupPower) /
12.0);
- flywheel_falcon_->SetSpeed(std::clamp(output.finisher_voltage(),
- -kMaxBringupPower,
- kMaxBringupPower) /
+ finisher_falcon_->SetSpeed(std::clamp(output.finisher_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
12.0);
- climber_falcon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(output.climber_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ if (climber_falcon_) {
+ climber_falcon_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ std::clamp(output.climber_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+ }
}
void Stop() override {
@@ -440,16 +440,16 @@
turret_victor_->SetDisabled();
feeder_falcon_->SetDisabled();
washing_machine_control_panel_victor_->SetDisabled();
- kicker_left_falcon_->SetDisabled();
- kicker_right_falcon_->SetDisabled();
- flywheel_falcon_->SetDisabled();
+ accelerator_left_falcon_->SetDisabled();
+ accelerator_right_falcon_->SetDisabled();
+ finisher_falcon_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
turret_victor_, washing_machine_control_panel_victor_;
- ::std::unique_ptr<::frc::TalonFX> feeder_falcon_, kicker_left_falcon_,
- kicker_right_falcon_, flywheel_falcon_;
+ ::std::unique_ptr<::frc::TalonFX> feeder_falcon_, accelerator_left_falcon_,
+ accelerator_right_falcon_, finisher_falcon_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
intake_roller_falcon_, climber_falcon_;
@@ -483,34 +483,45 @@
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
-
// TODO: pin numbers
- sensor_reader.set_hood_encoder(make_encoder(2));
- sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_hood_encoder(
+ make_unique<frc::Encoder>(22, 23, false, frc::Encoder::k4X));
- sensor_reader.set_intake_encoder(make_encoder(3));
- sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(24));
- sensor_reader.set_turret_encoder(make_encoder(4));
- sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(4));
- sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_intake_encoder(make_encoder(5));
+ sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(1));
- sensor_reader.set_flywheel_encoder(make_encoder(5));
- sensor_reader.set_left_kicker_encoder(make_encoder(6));
- sensor_reader.set_right_kicker_encoder(make_encoder(7));
+ sensor_reader.set_turret_encoder(make_encoder(2));
+ sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(0));
+ sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(0));
- sensor_reader.set_control_panel_encoder(make_encoder(8, frc::Encoder::k2X));
+ sensor_reader.set_finisher_encoder(
+ make_unique<frc::Encoder>(3, 2, false, frc::Encoder::k4X));
+ sensor_reader.set_left_accelerator_encoder(make_encoder(4));
+ sensor_reader.set_right_accelerator_encoder(make_encoder(3));
+
+ sensor_reader.set_control_panel_encoder(make_encoder(8, frc::Encoder::k1X));
// Note: If ADIS16470 is plugged in directly to the roboRIO SPI port without
// the Spartan Board, then trigger is on 26, reset 27, and chip select is
// CS0.
- auto imu_trigger = make_unique<frc::DigitalInput>(0);
- auto imu_reset = make_unique<frc::DigitalOutput>(1);
- auto spi = make_unique<frc::SPI>(frc::SPI::Port::kOnboardCS2);
+ frc::SPI::Port spi_port = frc::SPI::Port::kOnboardCS2;
+ std::unique_ptr<frc::DigitalInput> imu_trigger;
+ std::unique_ptr<frc::DigitalOutput> imu_reset;
+ if (::aos::network::GetTeamNumber() ==
+ constants::Values::kCodingRobotTeamNumber) {
+ imu_trigger = make_unique<frc::DigitalInput>(26);
+ imu_reset = make_unique<frc::DigitalOutput>(27);
+ spi_port = frc::SPI::Port::kOnboardCS0;
+ } else {
+ imu_trigger = make_unique<frc::DigitalInput>(9);
+ imu_reset = make_unique<frc::DigitalOutput>(8);
+ }
+ auto spi = make_unique<frc::SPI>(spi_port);
frc971::wpilib::ADIS16470 imu(&sensor_reader_event_loop, spi.get(),
imu_trigger.get(), imu_reset.get());
sensor_reader.set_imu(&imu);
-
AddLoop(&sensor_reader_event_loop);
// Thread 4.
@@ -523,23 +534,23 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
SuperstructureWriter superstructure_writer(&output_event_loop);
- // TODO: check ports
- superstructure_writer.set_hood_victor(make_unique<frc::VictorSP>(2));
+ superstructure_writer.set_hood_victor(make_unique<frc::VictorSP>(8));
superstructure_writer.set_intake_joint_victor(
- make_unique<frc::VictorSP>(3));
+ make_unique<frc::VictorSP>(2));
superstructure_writer.set_intake_roller_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(4));
- superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(5));
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(7));
superstructure_writer.set_feeder_falcon(make_unique<frc::TalonFX>(6));
superstructure_writer.set_washing_machine_control_panel_victor(
- make_unique<frc::VictorSP>(7));
- superstructure_writer.set_kicker_left_falcon(
- make_unique<::frc::TalonFX>(8));
- superstructure_writer.set_kicker_right_falcon(
- make_unique<::frc::TalonFX>(9));
- superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(10));
- superstructure_writer.set_climber_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(11));
+ make_unique<frc::VictorSP>(3));
+ superstructure_writer.set_accelerator_left_falcon(
+ make_unique<::frc::TalonFX>(5));
+ superstructure_writer.set_accelerator_right_falcon(
+ make_unique<::frc::TalonFX>(4));
+ superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(9));
+ // TODO: check port
+ //superstructure_writer.set_climber_falcon(
+ //make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(11));
AddLoop(&output_event_loop);
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 146456f..c0daa07 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -10,6 +10,7 @@
deps = [
"//aos/network/www:proxy",
"//y2020/vision:vision_ts_fbs",
+ "//y2020/vision/sift:sift_ts_fbs",
],
visibility = ["//y2020:__subpackages__"],
)
diff --git a/y2020/www/image_handler.ts b/y2020/www/image_handler.ts
index abaf831..802e448 100644
--- a/y2020/www/image_handler.ts
+++ b/y2020/www/image_handler.ts
@@ -1,22 +1,27 @@
-import {frc971} from 'y2020/vision/vision_generated';
+import {CameraImage} from 'y2020/vision/vision_generated';
export class ImageHandler {
private canvas = document.createElement('canvas');
+ private imageBuffer: Uint8ClampedArray|null = null;
+ private imageTimestamp: flatbuffers.Long|null = null;
+ private result: fr971.vision.ImageMatchResult|null = null;
+ private resultTimestamp: flatbuffers.Long|null = null;
constructor() {
document.body.appendChild(this.canvas);
}
- handleImage(data: Uint8Array) {
+ handleImage(data: Uint8Array): void {
const fbBuffer = new flatbuffers.ByteBuffer(data);
- const image = frc971.vision.CameraImage.getRootAsCameraImage(fbBuffer);
+ const image = CameraImage.getRootAsCameraImage(fbBuffer);
+ this.imageTimestamp = image.monotonicTimestampNs();
const width = image.cols();
const height = image.rows();
if (width === 0 || height === 0) {
return;
}
- const imageBuffer = new Uint8ClampedArray(width * height * 4); // RGBA
+ this.imageBuffer = new Uint8ClampedArray(width * height * 4); // RGBA
// Read four bytes (YUYV) from the data and transform into two pixels of
// RGBA for canvas
@@ -46,16 +51,47 @@
}
}
+ draw();
+ }
+
+ handleImageMetadata(data: Uint8Array): void {
+ const fbBuffer = new flatbuffers.ByteBuffer(data);
+ this.result = frc971.vision.ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
+ this.resultTimestamp = result.imageMonotonicTimestampNs();
+ draw();
+ }
+
+ draw(): void {
+ if (imageTimestamp.low !== resultTimestamp.low ||
+ imageTimestamp.high !== resultTimestamp.high) {
+ return;
+ }
const ctx = this.canvas.getContext('2d');
this.canvas.width = width;
this.canvas.height = height;
const idata = ctx.createImageData(width, height);
- idata.data.set(imageBuffer);
+ idata.data.set(this.imageBuffer);
ctx.putImageData(idata, 0, 0);
+ ctx.beginPath();
+ for (const feature of this.result.getFeatures()) {
+ // Based on OpenCV drawKeypoint.
+ ctx.arc(feature.x, feature.y, feature.size, 0, 2 * Math.PI);
+ ctx.moveTo(feature.x, feature.y);
+ // TODO(alex): check that angle is correct (0?, direction?)
+ const angle = feature.angle * Math.PI / 180;
+ ctx.lineTo(
+ feature.x + feature.radius * cos(angle),
+ feature.y + feature.radius * sin(angle));
+ }
+ ctx.stroke();
}
- getId() {
- return frc971.vision.CameraImage.getFullyQualifiedName();
+ getId(): string {
+ return CameraImage.getFullyQualifiedName();
+ }
+
+ getResultId(): string {
+ return frc971.vision.ImageMatchResult.getFullyQualifiedName();
}
}
diff --git a/y2020/www/main.ts b/y2020/www/main.ts
index 7831713..d414eac 100644
--- a/y2020/www/main.ts
+++ b/y2020/www/main.ts
@@ -9,3 +9,5 @@
const iHandler = new ImageHandler();
conn.addHandler(iHandler.getId(), (data) => iHandler.handleImage(data));
+conn.addHandler(
+ iHandler.getResultId(), (data) => iHandler.handleImageMetadata(data));