Merge "Add halide dependency"
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index bdee4f2..8824181 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -766,6 +766,7 @@
ReserveEvents();
+ aos::SetCurrentThreadName(name_.substr(0, 16));
// Now, all the callbacks are setup. Lock everything into memory and go RT.
if (priority_ != 0) {
::aos::InitRT();
@@ -847,6 +848,11 @@
priority_ = priority;
}
+void ShmEventLoop::set_name(const std::string_view name) {
+ name_ = std::string(name);
+ UpdateTimingReport();
+}
+
pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
} // namespace aos
diff --git a/aos/events/shm_event_loop.h b/aos/events/shm_event_loop.h
index bb4ad77..c888e57 100644
--- a/aos/events/shm_event_loop.h
+++ b/aos/events/shm_event_loop.h
@@ -62,10 +62,7 @@
void SetRuntimeRealtimePriority(int priority) override;
- void set_name(const std::string_view name) override {
- name_ = std::string(name);
- UpdateTimingReport();
- }
+ void set_name(const std::string_view name) override;
const std::string_view name() const override { return name_; }
const Node *node() const override { return node_; }
diff --git a/aos/init.cc b/aos/init.cc
index e506d86..957899c7 100644
--- a/aos/init.cc
+++ b/aos/init.cc
@@ -62,6 +62,7 @@
InitStart();
aos_core_create_shared_mem(false, for_realtime && ShouldBeRealtime());
logging::RegisterQueueImplementation();
+ ExpandStackSize();
AOS_LOG(INFO, "%s initialized non-realtime\n", program_invocation_short_name);
}
diff --git a/aos/realtime.cc b/aos/realtime.cc
index 6a80550..9f7e810 100644
--- a/aos/realtime.cc
+++ b/aos/realtime.cc
@@ -33,15 +33,31 @@
namespace {
-void SetSoftRLimit(int resource, rlim64_t soft, bool set_for_root) {
+enum class SetLimitForRoot {
+ kYes,
+ kNo
+};
+
+enum class AllowSoftLimitDecrease {
+ kYes,
+ kNo
+};
+
+void SetSoftRLimit(
+ int resource, rlim64_t soft, SetLimitForRoot set_for_root,
+ AllowSoftLimitDecrease allow_decrease = AllowSoftLimitDecrease::kYes) {
bool am_root = getuid() == 0;
- if (set_for_root || !am_root) {
+ if (set_for_root == SetLimitForRoot::kYes || !am_root) {
struct rlimit64 rlim;
PCHECK(getrlimit64(resource, &rlim) == 0)
<< ": " << program_invocation_short_name << "-init: getrlimit64("
<< resource << ") failed";
- rlim.rlim_cur = soft;
+ if (allow_decrease == AllowSoftLimitDecrease::kYes) {
+ rlim.rlim_cur = soft;
+ } else {
+ rlim.rlim_cur = std::max(rlim.rlim_cur, soft);
+ }
rlim.rlim_max = ::std::max(rlim.rlim_max, soft);
PCHECK(setrlimit64(resource, &rlim) == 0)
@@ -55,7 +71,7 @@
void LockAllMemory() {
// Allow locking as much as we want into RAM.
- SetSoftRLimit(RLIMIT_MEMLOCK, RLIM_INFINITY, false);
+ SetSoftRLimit(RLIMIT_MEMLOCK, RLIM_INFINITY, SetLimitForRoot::kNo);
WriteCoreDumps();
PCHECK(mlockall(MCL_CURRENT | MCL_FUTURE) == 0)
@@ -87,10 +103,10 @@
LockAllMemory();
// Only let rt processes run for 3 seconds straight.
- SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
+ SetSoftRLimit(RLIMIT_RTTIME, 3000000, SetLimitForRoot::kYes);
// Allow rt processes up to priority 40.
- SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
+ SetSoftRLimit(RLIMIT_RTPRIO, 40, SetLimitForRoot::kNo);
}
void UnsetCurrentThreadRealtimePriority() {
@@ -100,10 +116,11 @@
<< ": sched_setscheduler(0, SCHED_OTHER, 0) failed";
}
-void SetCurrentThreadName(const ::std::string &name) {
+void SetCurrentThreadName(const std::string_view name) {
CHECK_LE(name.size(), 16u) << ": thread name '" << name << "' too long";
VLOG(1) << "This thread is changing to '" << name << "'";
- PCHECK(prctl(PR_SET_NAME, name.c_str()) == 0);
+ std::string string_name(name);
+ PCHECK(prctl(PR_SET_NAME, string_name.c_str()) == 0);
if (&logging::internal::ReloadThreadName != nullptr) {
logging::internal::ReloadThreadName();
}
@@ -111,7 +128,7 @@
void SetCurrentThreadRealtimePriority(int priority) {
// Make sure we will only be allowed to run for 3 seconds straight.
- SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
+ SetSoftRLimit(RLIMIT_RTTIME, 3000000, SetLimitForRoot::kYes);
struct sched_param param;
param.sched_priority = priority;
@@ -121,7 +138,12 @@
void WriteCoreDumps() {
// Do create core files of unlimited size.
- SetSoftRLimit(RLIMIT_CORE, RLIM_INFINITY, true);
+ SetSoftRLimit(RLIMIT_CORE, RLIM_INFINITY, SetLimitForRoot::kYes);
+}
+
+void ExpandStackSize() {
+ SetSoftRLimit(RLIMIT_STACK, 1000000, SetLimitForRoot::kYes,
+ AllowSoftLimitDecrease::kNo);
}
} // namespace aos
diff --git a/aos/realtime.h b/aos/realtime.h
index a6b4223..20df979 100644
--- a/aos/realtime.h
+++ b/aos/realtime.h
@@ -1,7 +1,7 @@
#ifndef AOS_REALTIME_H_
#define AOS_REALTIME_H_
-#include <string>
+#include <string_view>
namespace aos {
@@ -16,7 +16,7 @@
// Sets the name of the current thread.
// This will displayed by `top -H`, dump_rtprio, and show up in logs.
// name can have a maximum of 16 characters.
-void SetCurrentThreadName(const ::std::string &name);
+void SetCurrentThreadName(const std::string_view name);
// Sets the current thread's realtime priority.
void SetCurrentThreadRealtimePriority(int priority);
@@ -28,6 +28,8 @@
void LockAllMemory();
+void ExpandStackSize();
+
} // namespace aos
#endif // AOS_REALTIME_H_
diff --git a/frc971/analysis/plot.py b/frc971/analysis/plot.py
index d325bd7..1eba716 100644
--- a/frc971/analysis/plot.py
+++ b/frc971/analysis/plot.py
@@ -8,7 +8,7 @@
import sys
from frc971.analysis.py_log_reader import LogReader
-from frc971.analysis.plot_config_pb2 import PlotConfig, Signal
+from frc971.analysis.plot_config_pb2 import PlotConfig, Signal, Line
from google.protobuf import text_format
import numpy as np
@@ -58,6 +58,10 @@
total_acceleration = np.sqrt(
msg[accel_x]**2 + msg[accel_y]**2 + msg[accel_z]**2)
new_msg['total_acceleration'] = total_acceleration
+ timestamp = 'monotonic_timestamp_ns'
+ if timestamp in msg:
+ timestamp_sec = msg[timestamp] * 1e-9
+ new_msg['monotonic_timestamp_sec'] = timestamp_sec
entries.append((entry[0], entry[1], new_msg))
if 'CalcIMU' in self.data:
raise RuntimeError('CalcIMU is already a member of data.')
@@ -77,28 +81,57 @@
to understanding how some internal filters work."""
self.calculate_imu_signals()
- def plot_signal(self, axes: matplotlib.axes.Axes, signal: Signal):
- if not signal.channel in self.data:
- raise ValueError("No channel alias " + signal.channel)
- field_path = signal.field.split('.')
- monotonic_time = []
- signal_data = []
- for entry in self.data[signal.channel]:
- monotonic_time.append(entry[0] * 1e-9)
- value = entry[2]
- for name in field_path:
- # If the value wasn't populated in a given message, fill in
- # NaN rather than crashing.
- if name in value:
- value = value[name]
- else:
- value = float("nan")
- break
- # Catch NaNs and convert them to floats.
- value = float(value)
- signal_data.append(value)
- label_name = signal.channel + "." + signal.field
- axes.plot(monotonic_time, signal_data, marker='o', label=label_name)
+ def extract_field(self, message: dict, field: str):
+ """Extracts a field with the given name from the message.
+
+ message will be a dictionary with field names as the keys and then
+ values, lists, or more dictionaries as the values. field is the full
+ path to the field to extract, with periods separating sub-messages."""
+ field_path = field.split('.')
+ value = message
+ for name in field_path:
+ # If the value wasn't populated in a given message, fill in
+ # NaN rather than crashing.
+ if name in value:
+ value = value[name]
+ else:
+ return None
+ # Catch NaNs and convert them to floats.
+ return float(value)
+
+ def plot_line(self, axes: matplotlib.axes.Axes, line: Line):
+ if not line.HasField('y_signal'):
+ raise ValueError("No y_channel specified for line.")
+ y_signal = line.y_signal
+ if not y_signal.channel in self.data:
+ raise ValueError("No channel alias " + y_signal.channel)
+ x_signal = line.x_signal if line.HasField('x_signal') else None
+ if x_signal is not None and not x_signal.channel in self.data:
+ raise ValueError("No channel alias " + x_signal.channel)
+ y_messages = self.data[y_signal.channel]
+ x_messages = self.data[
+ x_signal.channel] if x_signal is not None else None
+ if x_messages is not None and len(x_messages) != len(y_messages):
+ raise ValueError(
+ "X and Y signal lengths don't match. X channel is " +
+ x_signal.channel + " Y channel is " + y_signal.channel)
+ x_data = []
+ y_data = []
+ for ii in range(len(y_messages)):
+ y_entry = y_messages[ii]
+ if x_signal is None:
+ x_data.append(y_entry[0] * 1e-9)
+ else:
+ x_entry = x_messages[ii]
+ x_data.append(self.extract_field(x_entry[2], x_signal.field))
+ y_data.append(self.extract_field(y_entry[2], y_signal.field))
+ if x_data[-1] is None and y_data[-1] is not None:
+ raise ValueError(
+ "Only one of the x and y signals is present. X " +
+ x_signal.channel + "." + x_signal.field + " Y " +
+ y_signal.channel + "." + y_signal.field)
+ label_name = y_signal.channel + "." + y_signal.field
+ axes.plot(x_data, y_data, marker='o', label=label_name)
def plot(self):
shared_axis = None
@@ -110,8 +143,8 @@
num_subplots, 1, ii + 1, sharex=shared_axis)
shared_axis = shared_axis or axes
axes_config = figure_config.axes[ii]
- for signal in axes_config.signal:
- self.plot_signal(axes, signal)
+ for line in axes_config.line:
+ self.plot_line(axes, line)
# Make the legend transparent so we can see behind it.
legend = axes.legend(framealpha=0.5)
axes.set_xlabel("Monotonic Time (sec)")
diff --git a/frc971/analysis/plot_config.proto b/frc971/analysis/plot_config.proto
index 9aaea89..c4c61c2 100644
--- a/frc971/analysis/plot_config.proto
+++ b/frc971/analysis/plot_config.proto
@@ -24,10 +24,26 @@
optional string field = 2;
}
+// A single line to plot.
+message Line {
+ // The signal to plot on the y-axis.
+ optional Signal y_signal = 1;
+ // If set, we will use this signal for the x-axis of the plot. By default, we
+ // will use the monotonic sent time of the message. This is helpful for both
+ // plotting against non-time based signals (e.g., plotting x/y robot position)
+ // as well as plotting against times other than the message sent time (e.g.,
+ // for the IMU where the sample capture time is separate from the actual
+ // sent time.
+ // Note that the x and y signals should have exactly the same number of
+ // entries--otherwise, we need to write logic to handle resampling one signal
+ // to a different rate.
+ optional Signal x_signal = 2;
+}
+
// Message representing a single pyplot Axes, with specifications for exactly
// which signals to show in the supplied subplot.
message Axes {
- repeated Signal signal = 1;
+ repeated Line line = 1;
optional string ylabel = 2;
}
diff --git a/frc971/analysis/plot_configs/drivetrain.pb b/frc971/analysis/plot_configs/drivetrain.pb
index 2b15220..0ac4fff 100644
--- a/frc971/analysis/plot_configs/drivetrain.pb
+++ b/frc971/analysis/plot_configs/drivetrain.pb
@@ -31,39 +31,53 @@
figure {
axes {
- signal {
- channel: "JoystickState"
- field: "test_mode"
+ line {
+ y_signal {
+ channel: "JoystickState"
+ field: "test_mode"
+ }
}
- signal {
- channel: "JoystickState"
- field: "autonomous"
+ line {
+ y_signal {
+ channel: "JoystickState"
+ field: "autonomous"
+ }
}
- signal {
- channel: "RobotState"
- field: "browned_out"
+ line {
+ y_signal {
+ channel: "RobotState"
+ field: "browned_out"
+ }
}
- signal {
- channel: "JoystickState"
- field: "enabled"
+ line {
+ y_signal {
+ channel: "JoystickState"
+ field: "enabled"
+ }
}
ylabel: "[bool]"
}
axes {
- signal {
- channel: "RobotState"
- field: "voltage_battery"
+ line {
+ y_signal {
+ channel: "RobotState"
+ field: "voltage_battery"
+ }
}
ylabel: "[V]"
}
axes {
- signal {
- channel: "Status"
- field: "line_follow_logging.frozen"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "line_follow_logging.frozen"
+ }
}
- signal {
- channel: "Goal"
- field: "quickturn"
+ line {
+ y_signal {
+ channel: "Goal"
+ field: "quickturn"
+ }
}
ylabel: "[bool]"
}
@@ -71,59 +85,83 @@
figure {
axes {
- signal {
- channel: "Status"
- field: "poly_drive_logging.ff_left_voltage"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "poly_drive_logging.ff_left_voltage"
+ }
}
- signal {
- channel: "Status"
- field: "poly_drive_logging.ff_right_voltage"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "poly_drive_logging.ff_right_voltage"
+ }
}
- signal {
- channel: "Output"
- field: "left_voltage"
+ line {
+ y_signal {
+ channel: "Output"
+ field: "left_voltage"
+ }
}
- signal {
- channel: "Output"
- field: "right_voltage"
+ line {
+ y_signal {
+ channel: "Output"
+ field: "right_voltage"
+ }
}
ylabel: "[V]"
}
axes {
- signal {
- channel: "Status"
- field: "theta"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "theta"
+ }
}
ylabel: "[rad]"
}
axes {
- signal {
- channel: "Status"
- field: "robot_speed"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "robot_speed"
+ }
}
- signal {
- channel: "Status"
- field: "trajectory_logging.left_velocity"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "trajectory_logging.left_velocity"
+ }
}
- signal {
- channel: "Status"
- field: "poly_drive_logging.goal_left_velocity"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "poly_drive_logging.goal_left_velocity"
+ }
}
- signal {
- channel: "Status"
- field: "estimated_left_velocity"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "estimated_left_velocity"
+ }
}
- signal {
- channel: "Status"
- field: "trajectory_logging.right_velocity"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "trajectory_logging.right_velocity"
+ }
}
- signal {
- channel: "Status"
- field: "poly_drive_logging.goal_right_velocity"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "poly_drive_logging.goal_right_velocity"
+ }
}
- signal {
- channel: "Status"
- field: "estimated_right_velocity"
+ line {
+ y_signal {
+ channel: "Status"
+ field: "estimated_right_velocity"
+ }
}
ylabel: "[m/s]"
}
@@ -131,33 +169,129 @@
figure {
axes {
- signal {
- channel: "IMU"
- field: "gyro_x"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "gyro_x"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "gyro_y"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "gyro_y"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "gyro_z"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "gyro_z"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
ylabel: "rad / sec"
}
axes {
- signal {
- channel: "IMU"
- field: "accelerometer_x"
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "total_acceleration"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "accelerometer_y"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "accelerometer_x"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "accelerometer_z"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "accelerometer_y"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "accelerometer_z"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
ylabel: "g"
}
}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.yaw"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.lateral_pitch"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.longitudinal_pitch"
+ }
+ }
+ ylabel: "rad"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.quaternion_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.quaternion_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.quaternion_z"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.quaternion_w"
+ }
+ }
+ }
+}
diff --git a/frc971/analysis/plot_configs/gyro.pb b/frc971/analysis/plot_configs/gyro.pb
index f246835..760752c 100644
--- a/frc971/analysis/plot_configs/gyro.pb
+++ b/frc971/analysis/plot_configs/gyro.pb
@@ -6,36 +6,78 @@
figure {
axes {
- signal {
- channel: "IMU"
- field: "gyro_x"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "gyro_x"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "gyro_y"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "gyro_y"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "gyro_z"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "gyro_z"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
ylabel: "rad / sec"
}
axes {
- signal {
- channel: "CalcIMU"
- field: "total_acceleration"
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "total_acceleration"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "accelerometer_x"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "accelerometer_x"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "accelerometer_y"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "accelerometer_y"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
- signal {
- channel: "IMU"
- field: "accelerometer_z"
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "accelerometer_z"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
}
ylabel: "g"
}
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 6f46965..a120c0c 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -374,6 +374,7 @@
":drivetrain_position_fbs",
":drivetrain_status_fbs",
":gear",
+ ":improved_down_estimator",
":line_follow_drivetrain",
":localizer",
":localizer_fbs",
@@ -385,6 +386,7 @@
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro_fbs",
"//frc971/wpilib:imu_fbs",
+ "//frc971/zeroing:imu_zeroer",
],
)
@@ -405,6 +407,7 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:imu_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
],
@@ -436,6 +439,7 @@
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:imu_fbs",
] + cpu_select({
"amd64": [
"//third_party/matplotlib-cpp",
@@ -652,7 +656,11 @@
"improved_down_estimator.h",
],
deps = [
+ ":drivetrain_status_fbs",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:runge_kutta",
+ "@//aos/time",
"@com_github_google_glog//:glog",
"@org_tuxfamily_eigen//:eigen",
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 51c6f03..094a947 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -47,14 +47,17 @@
dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
dt_line_follow_(dt_config_, localizer->target_selector()),
- down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
left_high_requested_(dt_config_.default_high_gear),
right_high_requested_(dt_config_.default_high_gear) {
::aos::controls::HPolytope<0>::Init();
- down_U_.setZero();
event_loop->SetRuntimeRealtimePriority(30);
+ event_loop->OnRun([this]() {
+ // On the first fetch, make sure that we are caught all the way up to the
+ // present.
+ imu_values_fetcher_.Fetch();
+ });
}
int DrivetrainLoop::ControllerIndexFromGears() {
@@ -98,7 +101,6 @@
if (!has_been_enabled_ && output) {
has_been_enabled_ = true;
- down_estimator_.mutable_X_hat(1, 0) = 0.0;
}
// TODO(austin): Put gear detection logic here.
@@ -142,17 +144,22 @@
gear_logging_offset = gear_logging_builder.Finish();
}
- const bool is_latest_imu_values = imu_values_fetcher_.Fetch();
- if (is_latest_imu_values) {
- const double rate = -imu_values_fetcher_->gyro_y();
- const double accel_squared =
- ::std::pow(imu_values_fetcher_->accelerometer_x(), 2.0) +
- ::std::pow(imu_values_fetcher_->accelerometer_y(), 2.0) +
- ::std::pow(imu_values_fetcher_->accelerometer_z(), 2.0);
- const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x(),
- imu_values_fetcher_->accelerometer_z()) +
- 0.008;
+ while (imu_values_fetcher_.FetchNext()) {
+ imu_zeroer_.ProcessMeasurement(*imu_values_fetcher_);
+ last_gyro_time_ = monotonic_now;
+ aos::monotonic_clock::time_point reading_time(std::chrono::nanoseconds(
+ imu_values_fetcher_->monotonic_timestamp_ns()));
+ if (last_imu_update_ == aos::monotonic_clock::min_time) {
+ last_imu_update_ = reading_time;
+ }
+ down_estimator_.Predict(imu_zeroer_.ZeroedGyro(), imu_zeroer_.ZeroedAccel(),
+ reading_time - last_imu_update_);
+ last_imu_update_ = reading_time;
+ }
+ bool got_imu_reading = false;
+ if (imu_values_fetcher_.get() != nullptr) {
+ got_imu_reading = true;
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
last_accel_ = -imu_values_fetcher_->accelerometer_x();
@@ -164,51 +171,29 @@
last_accel_ = -imu_values_fetcher_->accelerometer_y();
break;
}
-
- if (accel_squared > 1.03 || accel_squared < 0.97) {
- AOS_LOG(DEBUG, "New IMU value, rejecting reading\n");
- } else {
- // -y is our gyro.
- // z accel is down
- // x accel is the front of the robot pointed down.
- Eigen::Matrix<double, 1, 1> Y;
- Y(0, 0) = angle;
- down_estimator_.Correct(Y);
- }
-
- AOS_LOG(DEBUG,
- "New IMU value, rate is %f, angle %f, fused %f, bias "
- "%f\n",
- rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
- down_U_(0, 0) = rate;
}
- down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
// TODO(austin): Signal the current gear to both loops.
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = imu_values_fetcher_->gyro_x();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = imu_zeroer_.ZeroedGyro().x();
}
break;
case GyroType::IMU_Y_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = imu_values_fetcher_->gyro_y();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = imu_zeroer_.ZeroedGyro().y();
}
break;
case GyroType::IMU_Z_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = imu_values_fetcher_->gyro_z();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = imu_zeroer_.ZeroedGyro().z();
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = -imu_values_fetcher_->gyro_z();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = -imu_zeroer_.ZeroedGyro().z();
}
break;
case GyroType::SPARTAN_GYRO:
@@ -327,6 +312,9 @@
const flatbuffers::Offset<PolyDriveLogging> poly_drive_logging_offset =
dt_openloop_.PopulateStatus(status->fbb());
+ const flatbuffers::Offset<DownEstimatorState> down_estimator_state_offset =
+ down_estimator_.PopulateStatus(status->fbb());
+
flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
dt_line_follow_.PopulateStatus(status);
flatbuffers::Offset<TrajectoryLogging> trajectory_logging_offset =
@@ -360,12 +348,12 @@
builder.add_y(localizer_->y());
builder.add_theta(::aos::math::NormalizeAngle(localizer_->theta()));
- builder.add_ground_angle(down_estimator_.X_hat(0) + dt_config_.down_offset);
builder.add_cim_logging(cim_logging_offset);
builder.add_poly_drive_logging(poly_drive_logging_offset);
builder.add_gear_logging(gear_logging_offset);
builder.add_line_follow_logging(line_follow_logging_offset);
builder.add_trajectory_logging(trajectory_logging_offset);
+ builder.add_down_estimator(down_estimator_state_offset);
status->Send(builder.Finish());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9f6a34c..f7924c5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
@@ -21,6 +22,7 @@
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/imu_generated.h"
+#include "frc971/zeroing/imu_zeroer.h"
namespace frc971 {
namespace control_loops {
@@ -57,6 +59,11 @@
::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
+
+ zeroing::ImuZeroer imu_zeroer_;
+ DrivetrainUkf down_estimator_;
+ aos::monotonic_clock::time_point last_imu_update_ =
+ aos::monotonic_clock::min_time;
LocalizerInterface *localizer_;
StateFeedbackLoop<7, 2, 4> kf_;
@@ -67,9 +74,6 @@
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
- StateFeedbackLoop<2, 1, 1> down_estimator_;
- Eigen::Matrix<double, 1, 1> down_U_;
-
// Current gears for each drive side.
Gear left_gear_;
Gear right_gear_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index a289fed..e29fed9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -59,7 +59,7 @@
}
virtual ~DrivetrainTest() {}
- void TearDown() { drivetrain_plant_.MaybePlot(); }
+ void TearDown() override { drivetrain_plant_.MaybePlot(); }
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
@@ -104,6 +104,31 @@
->is_executed());
}
+ void VerifyDownEstimator() {
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+ // TODO(james): Handle Euler angle singularities...
+ const double down_estimator_yaw =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->down_estimator())->yaw();
+ const double localizer_yaw =
+ drivetrain_status_fetcher_->theta();
+ EXPECT_LT(
+ std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
+ 1e-5);
+ const double true_yaw = (drivetrain_plant_.GetRightPosition() -
+ drivetrain_plant_.GetLeftPosition()) /
+ (dt_config_.robot_radius * 2.0);
+ EXPECT_LT(std::abs(aos::math::DiffAngle(down_estimator_yaw, true_yaw)),
+ 1e-4);
+ // We don't currently simulate any pitch or roll, so we shouldn't be
+ // reporting any.
+ EXPECT_NEAR(
+ 0, drivetrain_status_fetcher_->down_estimator()->longitudinal_pitch(),
+ 1e-10);
+ EXPECT_NEAR(0,
+ drivetrain_status_fetcher_->down_estimator()->lateral_pitch(),
+ 1e-10);
+ }
+
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
@@ -127,6 +152,8 @@
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
+ // Run for enough time to let the gyro zero.
+ RunFor(std::chrono::seconds(100));
SetEnabled(true);
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
@@ -138,6 +165,7 @@
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
+ VerifyDownEstimator();
}
// Tests that the drivetrain converges on a goal when under the effect of a
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 1956abd..0e49552 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -99,6 +99,27 @@
rel_theta:float;
}
+table DownEstimatorState {
+ quaternion_x:double;
+ quaternion_y:double;
+ quaternion_z:double;
+ quaternion_w:double;
+
+ // Side-to-side and forwards/backwards pitch numbers. Note that we do this
+ // instead of standard roll/pitch/yaw euler angles because it was a pain to
+ // try and numerically stable roll/pitch/yaw numbers, and Eigen's interface
+ // doesn't resolve the redundancies quite how we'd like.
+ // Lateral pitch is the side-to-side pitch of the robot; longitudinal pitch is
+ // the forwards to backwards pitch of the robot; longitudinal_pitch
+ // corresponds with the traditional usage of "pitch".
+ // All angles in radians.
+ lateral_pitch:float;
+ longitudinal_pitch:float;
+ // Current yaw angle (heading) of the robot, as estimated solely by the down
+ // estimator.
+ yaw:float;
+}
+
table Status {
// Estimated speed of the center of the robot in m/s (positive forwards).
robot_speed:double;
@@ -138,7 +159,8 @@
// True if the output voltage was capped last cycle.
output_was_capped:bool;
- // The angle of the robot relative to the ground.
+ // The pitch of the robot relative to the ground--only includes
+ // forwards/backwards rotation.
ground_angle:double;
// Information about shifting logic and curent gear, for logging purposes
@@ -150,6 +172,8 @@
line_follow_logging:LineFollowLogging;
poly_drive_logging:PolyDriveLogging;
+
+ down_estimator:DownEstimatorState;
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 8ebbb1b..56e664b 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -51,8 +51,8 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- IMUType::IMU_X,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ IMUType::IMU_FLIPPED_X,
::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
@@ -101,9 +101,8 @@
drivetrain_status_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- gyro_reading_sender_(
- event_loop->MakeSender<::frc971::sensors::GyroReading>(
- "/drivetrain")),
+ imu_sender_(
+ event_loop->MakeSender<::frc971::IMUValues>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -168,15 +167,24 @@
}
{
- auto builder = gyro_reading_sender_.MakeBuilder();
- frc971::sensors::GyroReading::Builder gyro_builder =
- builder.MakeBuilder<frc971::sensors::GyroReading>();
- gyro_builder.add_angle((right_encoder - left_encoder) /
- (dt_config_.robot_radius * 2.0));
- gyro_builder.add_velocity(
+ auto builder = imu_sender_.MakeBuilder();
+ frc971::IMUValues::Builder imu_builder =
+ builder.MakeBuilder<frc971::IMUValues>();
+ imu_builder.add_gyro_x(0.0);
+ imu_builder.add_gyro_y(0.0);
+ imu_builder.add_gyro_z(
(drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
(dt_config_.robot_radius * 2.0));
- builder.Send(gyro_builder.Finish());
+ // Acceleration due to gravity, in m/s/s.
+ constexpr double kG = 9.807;
+ imu_builder.add_accelerometer_x(last_acceleration_.x() / kG);
+ imu_builder.add_accelerometer_y(0.0);
+ imu_builder.add_accelerometer_z(1.0);
+ imu_builder.add_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ event_loop_->monotonic_now().time_since_epoch())
+ .count());
+ builder.Send(imu_builder.Finish());
}
}
@@ -216,13 +224,18 @@
U(1, 0) += drivetrain_plant_.right_voltage_offset();
drivetrain_plant_.Update(U);
double dt_float = ::aos::time::DurationInSeconds(dt_config_.dt);
- state_ = RungeKuttaU(
+ const auto dynamics =
[this](const ::Eigen::Matrix<double, 5, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) {
return ContinuousDynamics(velocity_drivetrain_->plant(),
dt_config_.Tlr_to_la(), X, U);
- },
- state_, U, dt_float);
+ };
+ state_ = RungeKuttaU(dynamics, state_, U, dt_float);
+ const Eigen::Matrix<double, 5, 1> Xdot = dynamics(state_, U);
+ // TODO(james): Account for centripetal accelerations.
+ // TODO(james): Allow inputting arbitrary calibrations, e.g., for testing
+ // situations where the IMU is not perfectly flat in the CG of the robot.
+ last_acceleration_ << (Xdot(3, 0) + Xdot(4, 0)) / 2.0, 0.0, 0.0;
}
void DrivetrainSimulation::MaybePlot() {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 2284f63..a906ae2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -9,7 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/imu_generated.h"
namespace frc971 {
namespace control_loops {
@@ -89,7 +89,7 @@
drivetrain_output_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
+ ::aos::Sender<::frc971::IMUValues> imu_sender_;
DrivetrainConfig<double> dt_config_;
@@ -106,6 +106,9 @@
Eigen::Matrix<double, 2, 1> last_U_;
+ // Last robot acceleration, in m/s/s.
+ Eigen::Vector3d last_acceleration_;
+
bool left_gear_high_ = false;
bool right_gear_high_ = false;
bool first_ = true;
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 40dc8e3..8b2ff4c 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -139,16 +139,17 @@
// States are X_hat_bar (position estimate) and P (Covariance)
void QuaternionUkf::Predict(const Eigen::Matrix<double, 3, 1> &U,
- const Eigen::Matrix<double, 3, 1> &measurement) {
+ const Eigen::Matrix<double, 3, 1> &measurement,
+ const aos::monotonic_clock::duration dt) {
// Compute the sigma points.
// Our system is pretty linear. The traditional way of dealing with process
// noise is to augment your state vector with the mean of the process noise,
// and augment your covariance matrix with the covariance of your process
- // noise. Sigma points are then computed. These points are then propegated
+ // noise. Sigma points are then computed. These points are then propagated
// through the model. This ends up effectively meaning that perturbations
- // from the unaugmented state with covariance P are propegated through the
+ // from the unaugmented state with covariance P are propagated through the
// model, and points which are at the mean but with perturbations to simulated
- // process noise are propegated through the system. The covariance is then
+ // process noise are propagated through the system. The covariance is then
// calculated from this set of points, and works out to have a covariance of
// essentially P + Q.
//
@@ -163,12 +164,12 @@
const Eigen::Matrix<double, 4, 3 * 2 + 1> X =
GenerateSigmaPoints(X_hat_, P_ + Q_);
- // Now, compute Y, the sigma points which have been propegated forwards by the
+ // Now, compute Y, the sigma points which have been propagated forwards by the
// model.
Eigen::Matrix<double, 4, 3 * 2 + 1> Y;
for (int i = 0; i < Y.cols(); ++i) {
// Y = Transformed sigma points
- Y.col(i) = A(X.col(i), U);
+ Y.col(i) = A(X.col(i), U, dt);
}
// We now have the sigma points after the model update.
@@ -182,14 +183,17 @@
// If the only obvious acceleration is that due to gravity, then accept the
// measurement.
- constexpr double kUseAccelThreshold = 0.1;
- if (std::abs(measurement.squaredNorm() - 1.0) < kUseAccelThreshold) {
+ // TODO(james): Calibrate this on a real robot. This may require some sort of
+ // calibration routine.
+ constexpr double kUseAccelThreshold = 0.02;
+ if (std::abs(measurement.squaredNorm() - 1.0) > kUseAccelThreshold) {
P_ = P_prior;
return;
}
// TODO(austin): Maybe re-calculate the sigma points here before transforming
- // them? Otherwise we can't cleanly decouple the model and measurement updates.
+ // them? Otherwise we can't cleanly decouple the model and measurement
+ // updates.
// Apply the measurement transform to all the sigma points to get a
// representation of the distribution of the measurement.
@@ -282,6 +286,36 @@
return X;
}
+flatbuffers::Offset<DownEstimatorState> DrivetrainUkf::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ DownEstimatorState::Builder builder(*fbb);
+ builder.add_quaternion_x(X_hat().x());
+ builder.add_quaternion_y(X_hat().y());
+ builder.add_quaternion_z(X_hat().z());
+ builder.add_quaternion_w(X_hat().w());
+
+ {
+ // Note that this algorithm is not very numerically stable near pitches of
+ // +/- pi / 2.
+ const Eigen::Vector3d robot_x_in_global_frame =
+ X_hat() * Eigen::Vector3d::UnitX();
+ builder.add_yaw(
+ std::atan2(robot_x_in_global_frame.y(), robot_x_in_global_frame.x()));
+ const double xy_norm = robot_x_in_global_frame.block<2, 1>(0, 0).norm();
+ builder.add_longitudinal_pitch(
+ std::atan2(-robot_x_in_global_frame.z(), xy_norm));
+ }
+ {
+ const Eigen::Vector3d robot_y_in_global_frame =
+ X_hat() * Eigen::Vector3d::UnitY();
+ const double xy_norm = robot_y_in_global_frame.block<2, 1>(0, 0).norm();
+ builder.add_lateral_pitch(
+ std::atan2(robot_y_in_global_frame.z(), xy_norm));
+ }
+
+ return builder.Finish();
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 2b47f9e..aa45e4e 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -4,6 +4,9 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
@@ -94,6 +97,14 @@
// position flat on the ground with a heading of zero (which, in our normal
// field coordinates, means pointed straight away from our driver's station
// wall).
+ // The X axis is pointed straight out from the driver's station, the Z axis
+ // straight up and the Y axis straight to the left (i.e., a right-handed
+ // coordinate system).
+ // The quaternion itself represents the transformation from the body frame to
+ // global frame. E.g., for the gravity vector, the acceleration due to gravity
+ // in the global frame is equal to X_hat_ * gravity_in_robot_frame. Note that
+ // this convention does seem to be the inverse of that used in the paper
+ // referenced on Quaternion UKFs.
constexpr static int kNumStates = 4;
// Inputs to the system--we use the (x, y, z) gyro measurements as the inputs
// to the system.
@@ -119,20 +130,26 @@
}
// Handles updating the state of the UKF, given the gyro and accelerometer
- // measurements.
+ // measurements. Given the design of the filter, U is the x/y/z gyro
+ // measurements and measurement is the accelerometer x/y/z measurements.
+ // dt is the length of the current timestep.
+ // U specifically corresponds with the U in the paper, which corresponds with
+ // the input to the system used by the filter.
void Predict(const Eigen::Matrix<double, kNumInputs, 1> &U,
- const Eigen::Matrix<double, 3, 1> &measurement);
+ const Eigen::Matrix<double, kNumMeasurements, 1> &measurement,
+ const aos::monotonic_clock::duration dt);
// Returns the updated state for X after one time step, given the current
// state and gyro measurements.
- virtual Eigen::Matrix<double, 4, 1> A(
- const Eigen::Matrix<double, 4, 1> &X,
- const Eigen::Matrix<double, kNumInputs, 1> &U) const = 0;
+ virtual Eigen::Matrix<double, kNumStates, 1> A(
+ const Eigen::Matrix<double, kNumStates, 1> &X,
+ const Eigen::Matrix<double, kNumInputs, 1> &U,
+ const aos::monotonic_clock::duration dt) const = 0;
// Returns the current expected accelerometer measurements given the current
// state.
- virtual Eigen::Matrix<double, 3, 1> H(
- const Eigen::Matrix<double, 4, 1> &X) const = 0;
+ virtual Eigen::Matrix<double, kNumMeasurements, 1> H(
+ const Eigen::Matrix<double, kNumStates, 1> &X) const = 0;
// Returns the current estimate of the robot's orientation. Note that this
// filter does not have anything other than the gyro with which to estimate
@@ -140,7 +157,7 @@
// filters.
const Eigen::Quaternion<double> &X_hat() const { return X_hat_; }
- Eigen::Matrix<double, 3, 1> Z_hat() const { return Z_hat_; };
+ Eigen::Matrix<double, kNumMeasurements, 1> Z_hat() const { return Z_hat_; };
private:
// Measurement Noise (Uncertainty)
@@ -155,13 +172,11 @@
Eigen::Quaternion<double> X_hat_;
// Current expected accelerometer measurement.
- Eigen::Matrix<double, 3, 1> Z_hat_;
+ Eigen::Matrix<double, kNumMeasurements, 1> Z_hat_;
};
class DrivetrainUkf : public QuaternionUkf {
public:
- constexpr static double kDt = 0.00505;
-
// UKF for http://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf
// Reference in case the link is dead:
// Kraft, Edgar. "A quaternion-based unscented Kalman filter for orientation
@@ -195,21 +210,30 @@
// Moves the robot by the provided rotation vector (U).
Eigen::Matrix<double, kNumStates, 1> A(
const Eigen::Matrix<double, kNumStates, 1> &X,
- const Eigen::Matrix<double, kNumInputs, 1> &U) const override {
+ const Eigen::Matrix<double, kNumInputs, 1> &U,
+ const aos::monotonic_clock::duration dt) const override {
return RungeKutta(
- std::bind(&QuaternionDerivative, U, std::placeholders::_1), X, kDt);
+ std::bind(&QuaternionDerivative, U, std::placeholders::_1), X,
+ aos::time::DurationInSeconds(dt));
}
// Returns the expected accelerometer measurement (which is just going to be
// 1g downwards).
Eigen::Matrix<double, kNumMeasurements, 1> H(
const Eigen::Matrix<double, kNumStates, 1> &X) const override {
- // TODO(austin): Figure out how to compute what the sensors *should* read.
+ // Assume that we expect to see a reading of (0, 0, 1) when flat on the
+ // ground.
+ // TODO(james): Figure out a calibration routine for managing the fact that
+ // the accelerometer will not be perfectly oriented within the robot (or
+ // determine that calibration routines would be unnecessary).
Eigen::Quaternion<double> Xquat(X);
Eigen::Matrix<double, 3, 1> gprime =
- Xquat * Eigen::Matrix<double, 3, 1>(0.0, 0.0, -1.0);
+ Xquat.conjugate() * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0);
return gprime;
}
+
+ flatbuffers::Offset<DownEstimatorState> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index db7eeee..26b27e9 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -12,6 +12,19 @@
namespace control_loops {
namespace testing {
+namespace {
+// Check if two quaternions are logically equal, to within some reasonable
+// tolerance. This is needed because a single rotation can be represented by two
+// quaternions.
+bool QuaternionEqual(const Eigen::Quaterniond &a, const Eigen::Quaterniond &b,
+ double tolerance) {
+ // If a == b, then a.inverse() * b will be the identity. The identity
+ // quaternion is the only time where the vector portion of the quaternion is
+ // zero.
+ return (a.inverse() * b).vec().norm() <= tolerance;
+}
+} // namespace
+
// Do a known transformation to see if quaternion integration is working
// correctly.
TEST(RungeKuttaTest, QuaternionIntegral) {
@@ -31,7 +44,7 @@
VLOG(1) << "ux is " << ux;
VLOG(1) << "qux is " << qux;
- // Start by rotating around the X world vector for pi/2
+ // Start by rotating around the X body vector for pi/2
Eigen::Quaternion<double> integral1(
RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative, ux,
std::placeholders::_1),
@@ -39,7 +52,7 @@
VLOG(1) << "integral1 * uz => " << integral1 * uz;
- // Then rotate around the Y world vector for pi/2
+ // Then rotate around the Y body vector for pi/2
Eigen::Quaternion<double> integral2(
RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative, uy,
std::placeholders::_1),
@@ -47,12 +60,14 @@
VLOG(1) << "integral2 * uz => " << integral2 * uz;
- // Then rotate around the X world vector for -pi/2
+ // Then rotate around the X body vector for -pi/2
Eigen::Quaternion<double> integral3(
RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative,
-ux, std::placeholders::_1),
integral2.normalized().coeffs(), 0.5 * M_PI));
+ integral1.normalize();
+ integral2.normalize();
integral3.normalize();
VLOG(1) << "Integral is w: " << integral1.w() << " vec: " << integral1.vec()
@@ -62,15 +77,129 @@
<< " norm " << integral3.norm();
VLOG(1) << "ux => " << integral3 * ux;
+ EXPECT_NEAR(0.0, (ux - integral1 * ux).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (uz - integral1 * uy).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (-uy - integral1 * uz).norm(), 5e-2);
+
+ EXPECT_NEAR(0.0, (uy - integral2 * ux).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (uz - integral2 * uy).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (ux - integral2 * uz).norm(), 5e-2);
+
EXPECT_NEAR(0.0, (uy - integral3 * ux).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (-ux - integral3 * uy).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (uz - integral3 * uz).norm(), 5e-2);
}
-TEST(RungeKuttaTest, Ukf) {
+TEST(RungeKuttaTest, UkfConstantRotation) {
drivetrain::DrivetrainUkf dtukf;
- Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+ const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+ EXPECT_EQ(0.0,
+ (Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
+ .norm());
Eigen::Matrix<double, 3, 1> measurement;
measurement.setZero();
- dtukf.Predict(ux, measurement);
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(ux * M_PI_2, measurement, std::chrono::milliseconds(5));
+ }
+ const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, ux));
+ EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
+ << "Expected: " << expected.coeffs()
+ << " Got: " << dtukf.X_hat().coeffs();
+ EXPECT_NEAR(0.0,
+ (Eigen::Vector3d(0.0, 1.0, 0.0) - dtukf.H(dtukf.X_hat().coeffs()))
+ .norm(), 1e-10);
+}
+
+// Tests that the euler angles in the status message are correct.
+TEST(RungeKuttaTest, UkfEulerStatus) {
+ drivetrain::DrivetrainUkf dtukf;
+ const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+ const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
+ const Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
+ // First, rotate 3 radians in the yaw axis, then 0.5 radians in the pitch
+ // axis, and then 0.1 radians about the roll axis.
+ constexpr double kYaw = 3.0;
+ constexpr double kPitch = 0.5;
+ constexpr double kRoll = 0.1;
+ Eigen::Matrix<double, 3, 1> measurement;
+ measurement.setZero();
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(uz * kYaw, measurement, std::chrono::milliseconds(5));
+ }
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(uy * kPitch, measurement, std::chrono::milliseconds(5));
+ }
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(ux * kRoll, measurement, std::chrono::milliseconds(5));
+ }
+ const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(kYaw, uz) *
+ Eigen::AngleAxis<double>(kPitch, uy) *
+ Eigen::AngleAxis<double>(kRoll, ux));
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ fbb.Finish(dtukf.PopulateStatus(&fbb));
+
+ aos::FlatbufferDetachedBuffer<drivetrain::DownEstimatorState> state(
+ fbb.Release());
+ EXPECT_EQ(kPitch, state.message().longitudinal_pitch());
+ EXPECT_EQ(kYaw, state.message().yaw());
+ // The longitudinal pitch is not actually the same number as the roll, so we
+ // don't check it here.
+
+ EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
+ << "Expected: " << expected.coeffs()
+ << " Got: " << dtukf.X_hat().coeffs();
+}
+
+
+// Tests that if the gyro indicates no movement but that the accelerometer shows
+// that we are slightly rotated, that we eventually adjust our estimate to be
+// correct.
+TEST(RungeKuttaTest, UkfAccelCorrectsBias) {
+ drivetrain::DrivetrainUkf dtukf;
+ const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+ Eigen::Matrix<double, 3, 1> measurement;
+ // Supply the accelerometer with a slightly off reading to ensure that we
+ // don't require exactly 1g to work.
+ measurement << 0.01, 0.99, 0.0;
+ EXPECT_TRUE(
+ QuaternionEqual(Eigen::Quaterniond::Identity(), dtukf.X_hat(), 0.0))
+ << "X_hat: " << dtukf.X_hat().coeffs();
+ EXPECT_EQ(0.0,
+ (Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
+ .norm());
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict({0.0, 0.0, 0.0}, measurement, std::chrono::milliseconds(5));
+ }
+ const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, ux));
+ EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
+ << "Expected: " << expected.coeffs()
+ << " Got: " << dtukf.X_hat().coeffs();
+}
+
+// Tests that if the accelerometer is reading values with a magnitude that isn't ~1g,
+// that we are slightly rotated, that we eventually adjust our estimate to be
+// correct.
+TEST(RungeKuttaTest, UkfIgnoreBadAccel) {
+ drivetrain::DrivetrainUkf dtukf;
+ const Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
+ Eigen::Matrix<double, 3, 1> measurement;
+ // Set up a scenario where, if we naively took the accelerometer readings, we
+ // would think that we were rotated. But the gyro readings indicate that we
+ // are only rotating about the Z (yaw) axis.
+ measurement << 0.3, 1.0, 0.0;
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict({0.0, 0.0, 1.0}, measurement, std::chrono::milliseconds(5));
+ }
+ const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(1.0, uz));
+ EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 1e-1))
+ << "Expected: " << expected.coeffs()
+ << " Got: " << dtukf.X_hat().coeffs();
+ EXPECT_NEAR(
+ 0.0,
+ (Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs())).norm(),
+ 1e-10)
+ << dtukf.H(dtukf.X_hat().coeffs());
}
// Tests that small perturbations around a couple quaternions averaged out
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index b7cce1e..b558202 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -159,7 +159,7 @@
constexpr double kGyroLsbRadianSecond =
1.0 / 10.0 * (2.0 * M_PI / 360.0) /* degrees -> radians */;
// G/LSB for the accelerometers (for the full 32-bit value).
-constexpr double kAccelerometerLsbG = 1.0 / 54'428'800.0;
+constexpr double kAccelerometerLsbG = 1.0 / 52'428'800.0;
// C/LSB for the temperature.
constexpr double kTemperatureLsbDegree = 0.1;
@@ -196,6 +196,9 @@
PCHECK(
system("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 "
+ "33") == 0);
event_loop_->OnRun([this]() { BeginInitialization(); });
}
diff --git a/frc971/wpilib/ahal/BUILD b/frc971/wpilib/ahal/BUILD
index 3f06ff8..44341b7 100644
--- a/frc971/wpilib/ahal/BUILD
+++ b/frc971/wpilib/ahal/BUILD
@@ -18,6 +18,7 @@
visibility = ["//third_party:__pkg__"],
deps = [
"//aos:make_unique",
+ "//aos:realtime",
"//aos/logging",
"//third_party:wpilib_hal",
],
diff --git a/frc971/wpilib/ahal/RobotBase.h b/frc971/wpilib/ahal/RobotBase.h
index 24e2898..cf1ed4c 100644
--- a/frc971/wpilib/ahal/RobotBase.h
+++ b/frc971/wpilib/ahal/RobotBase.h
@@ -11,6 +11,7 @@
#include <iostream>
#include <thread>
+#include "aos/realtime.h"
#include "hal/HAL.h"
#include "frc971/wpilib/ahal/Base.h"
@@ -34,11 +35,15 @@
#else
#define START_ROBOT_CLASS(_ClassName_) \
int main(int argc, char *argv[]) { \
- ::aos::InitGoogle(&argc, &argv); \
+ aos::InitGoogle(&argc, &argv); \
+ /* HAL_Initialize spawns several threads, including the CAN drivers. */ \
+ /* Go to realtime so that the child threads are RT. */ \
+ aos::SetCurrentThreadRealtimePriority(10); \
if (!HAL_Initialize(500, 0)) { \
std::cerr << "FATAL ERROR: HAL could not be initialized" << std::endl; \
return -1; \
} \
+ aos::UnsetCurrentThreadRealtimePriority(); \
HAL_Report(HALUsageReporting::kResourceType_Language, \
HALUsageReporting::kLanguage_CPlusPlus); \
static _ClassName_ robot; \
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 2fec111..c614cfe 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -14,16 +14,16 @@
using aos::Joystick;
-JoystickSender::JoystickSender(::aos::EventLoop *event_loop)
+JoystickSender::JoystickSender(::aos::ShmEventLoop *event_loop)
: event_loop_(event_loop),
joystick_state_sender_(
event_loop_->MakeSender<::aos::JoystickState>("/aos")),
team_id_(::aos::network::GetTeamNumber()) {
event_loop_->SetRuntimeRealtimePriority(29);
+ event_loop->set_name("joystick_sender");
event_loop_->OnRun([this]() {
frc::DriverStation *const ds = &frc::DriverStation::GetInstance();
- ::aos::SetCurrentThreadName("DSReader");
// TODO(Brian): Fix the potential deadlock when stopping here (condition
// variable / mutex needs to get exposed all the way out or something).
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
index e2609e8..6622d85 100644
--- a/frc971/wpilib/joystick_sender.h
+++ b/frc971/wpilib/joystick_sender.h
@@ -3,7 +3,7 @@
#include <atomic>
-#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/robot_state/joystick_state_generated.h"
namespace frc971 {
@@ -11,10 +11,10 @@
class JoystickSender {
public:
- JoystickSender(::aos::EventLoop *event_loop);
+ JoystickSender(::aos::ShmEventLoop *event_loop);
private:
- ::aos::EventLoop *event_loop_;
+ ::aos::ShmEventLoop *event_loop_;
::aos::Sender<::aos::JoystickState> joystick_state_sender_;
const uint16_t team_id_;
};
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index e95c231..2662934 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -10,8 +10,12 @@
// able to do so.
class ImuZeroer {
public:
- // Average 5 seconds of data (assuming 2kHz sampling rate).
- static constexpr size_t kSamplesToAverage = 10000.0;
+ // Average 0.5 seconds of data (assuming 2kHz sampling rate).
+ // TODO(james): Make the gyro zero in a constant amount of time, rather than a
+ // constant number of samples...
+ // TODO(james): Run average and GetRange calculations over every sample on
+ // every timestep, to provide consistent timing.
+ static constexpr size_t kSamplesToAverage = 1000.0;
ImuZeroer();
bool Zeroed() const;
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 9919e2f..9ec52da 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -89,16 +89,16 @@
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
// Gyro should be zeroed to {1, 2, 3}.
- ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-10);
- ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-10);
- ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-8);
+ ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-8);
+ ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-8);
// If we get another measurement offset by {1, 1, 1} we should read the result
// as {1, 1, 1}.
zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-10);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-10);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-8);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-8);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-8);
ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 50f2f8a..d1b5e78 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -74,7 +74,7 @@
::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- "/superstructure")),
+ "/vision")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
"/superstructure")),
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 24dd6c2..cc3daac 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -27,8 +27,6 @@
namespace {
DrivetrainConfig<double> GetTest2019DrivetrainConfig() {
DrivetrainConfig<double> config = GetDrivetrainConfig();
- config.gyro_type =
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO;
return config;
}
}; // namespace
@@ -220,7 +218,7 @@
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(1e-10);
+ VerifyEstimatorAccurate(1e-7);
}
// Bad camera updates (NaNs) should have no effect.
@@ -241,7 +239,7 @@
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(1e-10);
+ VerifyEstimatorAccurate(1e-7);
}
// Tests that camera udpates with a perfect models results in no errors.
diff --git a/y2020/BUILD b/y2020/BUILD
index b044f53..53ccb8e 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -72,7 +72,6 @@
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
- "//third_party:phoenix",
"//third_party:wpilib",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index a363775..4989390 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -10,7 +10,6 @@
#include <mutex>
#include <thread>
-#include "ctre/phoenix/CANifier.h"
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Counter.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
@@ -31,7 +30,6 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16470.h"
@@ -241,6 +239,7 @@
// Thread 4.
::aos::ShmEventLoop output_event_loop(&config.message());
+ output_event_loop.set_name("output_writer");
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);