Merge "Swap naming order of transformation matrices"
diff --git a/aos/BUILD b/aos/BUILD
index 2bd49c1..12d25b2 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -1,5 +1,5 @@
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",
@@ -285,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/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_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/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.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/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 90129dd..f0610a0 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -102,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:
diff --git a/y2020/constants.h b/y2020/constants.h
index 7fbb704..f0e3564 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -49,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
};
}
@@ -71,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;
@@ -92,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() {
@@ -105,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
};
}
@@ -142,7 +142,7 @@
// Shooter
static constexpr double kFinisherEncoderCountsPerRevolution() {
- return 4096.0;
+ return 2048.0;
}
static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
@@ -154,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/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 b336fd7..3a8b646 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -126,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));
@@ -167,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
@@ -228,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);
@@ -244,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);
@@ -252,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>
@@ -315,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_;
@@ -362,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(
@@ -384,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 {
@@ -441,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_;
@@ -484,23 +483,25 @@
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
@@ -514,14 +515,13 @@
imu_reset = make_unique<frc::DigitalOutput>(27);
spi_port = frc::SPI::Port::kOnboardCS0;
} else {
- imu_trigger = make_unique<frc::DigitalInput>(0);
- imu_reset = make_unique<frc::DigitalOutput>(1);
+ 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.
@@ -534,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));