Merge "Climber Messages"
diff --git a/frc971/analysis/plot.py b/frc971/analysis/plot.py
index 1eba716..ecb23b3 100644
--- a/frc971/analysis/plot.py
+++ b/frc971/analysis/plot.py
@@ -28,36 +28,124 @@
for channel in self.config.channel:
if channel.alias in aliases:
raise ValueError("Duplicate alias " + channel.alias)
- aliases.add(channel.alias)
if not self.reader.subscribe(channel.name, channel.type):
- raise ValueError("No such channel with name " + channel.name +
- " and type " + channel.type)
+ print("Warning: No such channel with name " + channel.name +
+ " and type " + channel.type)
+ continue
+ aliases.add(channel.alias)
self.reader.process()
for channel in self.config.channel:
self.data[channel.alias] = []
+ if channel.alias not in aliases:
+ print("Unable to plot channel alias " + channel.alias)
+ continue
for message in self.reader.get_data_for_channel(
channel.name, channel.type):
valid_json = message[2].replace('nan', '"nan"')
- parsed_json = json.loads(valid_json)
- self.data[channel.alias].append((message[0], message[1],
- parsed_json))
+ valid_json = valid_json.replace(' inf', ' "inf"')
+ valid_json = valid_json.replace('-inf', '"-inf"')
+ try:
+ parsed_json = json.loads(valid_json)
+ except json.decoder.JSONDecodeError as ex:
+ print("JSON Decode failed:")
+ print(valid_json)
+ raise ex
+ self.data[channel.alias].append(
+ (message[0], message[1], parsed_json))
self.calculate_signals()
+ def calculate_down_estimator_signals(self):
+ if 'DrivetrainStatus' in self.data:
+ # Calculates a rolling mean of the acceleration output from
+ # the down estimator.
+ entries = []
+ buffer_len = 100
+ last_100_accels = np.zeros((buffer_len, 3))
+ accels_next_row = 0
+ for entry in self.data['DrivetrainStatus']:
+ msg = entry[2]
+ new_msg = {}
+ if 'down_estimator' not in msg:
+ continue
+ down_estimator = msg['down_estimator']
+ new_msg['down_estimator'] = {}
+ accel_x = 'accel_x'
+ accel_y = 'accel_y'
+ accel_z = 'accel_z'
+ if (accel_x in down_estimator and accel_y in down_estimator
+ and accel_x in down_estimator):
+ last_100_accels[accels_next_row, :] = [
+ down_estimator[accel_x], down_estimator[accel_y],
+ down_estimator[accel_z]
+ ]
+
+ accels_next_row += 1
+ accels_next_row = accels_next_row % buffer_len
+ mean_accels = np.mean(last_100_accels, axis=0)
+ new_msg['down_estimator'][
+ 'accel_x_rolling_mean'] = mean_accels[0]
+ new_msg['down_estimator'][
+ 'accel_y_rolling_mean'] = mean_accels[1]
+ new_msg['down_estimator'][
+ 'accel_z_rolling_mean'] = mean_accels[2]
+ entries.append((entry[0], entry[1], new_msg))
+ if 'CalcDrivetrainStatus' in self.data:
+ raise RuntimeError(
+ 'CalcDrivetrainStatus is already a member of data.')
+ self.data['CalcDrivetrainStatus'] = entries
+
def calculate_imu_signals(self):
if 'IMU' in self.data:
entries = []
+ # Calculates a rolling mean of the raw output from the IMU.
+ buffer_len = 1000
+ last_1000_accels = np.zeros((buffer_len, 3))
+ accels_next_row = 0
+ last_1000_gyros = np.zeros((buffer_len, 3))
+ gyros_next_row = 0
for entry in self.data['IMU']:
accel_x = 'accelerometer_x'
accel_y = 'accelerometer_y'
accel_z = 'accelerometer_z'
+ gyro_x = 'gyro_x'
+ gyro_y = 'gyro_y'
+ gyro_z = 'gyro_z'
msg = entry[2]
new_msg = {}
if accel_x in msg and accel_y in msg and accel_x in msg:
- total_acceleration = np.sqrt(
- msg[accel_x]**2 + msg[accel_y]**2 + msg[accel_z]**2)
+ last_1000_accels[accels_next_row, :] = [
+ msg[accel_x], msg[accel_y], msg[accel_z]
+ ]
+ total_acceleration = np.linalg.norm(
+ last_1000_accels[accels_next_row, :])
new_msg['total_acceleration'] = total_acceleration
+
+ accels_next_row += 1
+ accels_next_row = accels_next_row % buffer_len
+ std_accels = np.std(last_1000_accels, axis=0)
+ new_msg['accel_x_rolling_std'] = std_accels[0]
+ new_msg['accel_y_rolling_std'] = std_accels[1]
+ new_msg['accel_z_rolling_std'] = std_accels[2]
+ mean_accels = np.mean(last_1000_accels, axis=0)
+ new_msg['accel_x_rolling_mean'] = mean_accels[0]
+ new_msg['accel_y_rolling_mean'] = mean_accels[1]
+ new_msg['accel_z_rolling_mean'] = mean_accels[2]
+ if gyro_x in msg and gyro_y in msg and gyro_z in msg:
+ last_1000_gyros[gyros_next_row, :] = [
+ msg[gyro_x], msg[gyro_y], msg[gyro_z]
+ ]
+ gyros_next_row += 1
+ gyros_next_row = gyros_next_row % buffer_len
+ std_gyros = np.std(last_1000_gyros, axis=0)
+ new_msg['gyro_x_rolling_std'] = std_gyros[0]
+ new_msg['gyro_y_rolling_std'] = std_gyros[1]
+ new_msg['gyro_z_rolling_std'] = std_gyros[2]
+ mean_gyros = np.mean(last_1000_gyros, axis=0)
+ new_msg['gyro_x_rolling_mean'] = mean_gyros[0]
+ new_msg['gyro_y_rolling_mean'] = mean_gyros[1]
+ new_msg['gyro_z_rolling_mean'] = mean_gyros[2]
timestamp = 'monotonic_timestamp_ns'
if timestamp in msg:
timestamp_sec = msg[timestamp] * 1e-9
@@ -80,6 +168,7 @@
an overall magnitude for the accelerometer readings, which is helpful
to understanding how some internal filters work."""
self.calculate_imu_signals()
+ self.calculate_down_estimator_signals()
def extract_field(self, message: dict, field: str):
"""Extracts a field with the given name from the message.
@@ -139,15 +228,20 @@
fig = plt.figure()
num_subplots = len(figure_config.axes)
for ii in range(num_subplots):
- axes = fig.add_subplot(
- num_subplots, 1, ii + 1, sharex=shared_axis)
- shared_axis = shared_axis or axes
axes_config = figure_config.axes[ii]
+ share_axis = axes_config.share_x_axis
+ axes = fig.add_subplot(
+ num_subplots,
+ 1,
+ ii + 1,
+ sharex=shared_axis if share_axis else None)
+ if share_axis and shared_axis is None:
+ shared_axis = axes
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)")
+ legend = axes.legend(framealpha=0.5, loc='upper right')
+ axes.set_xlabel(axes_config.xlabel)
axes.grid(True)
if axes_config.HasField("ylabel"):
axes.set_ylabel(axes_config.ylabel)
@@ -156,21 +250,18 @@
def main(argv):
parser = argparse.ArgumentParser(
description="Plot data from an aos logfile.")
- parser.add_argument(
- "--logfile",
- type=str,
- required=True,
- help="Path to the logfile to parse.")
- parser.add_argument(
- "--config",
- type=str,
- required=True,
- help="Name of the plot config to use.")
- parser.add_argument(
- "--config_dir",
- type=str,
- default="frc971/analysis/plot_configs",
- help="Directory to look for plot configs in.")
+ parser.add_argument("--logfile",
+ type=str,
+ required=True,
+ help="Path to the logfile to parse.")
+ parser.add_argument("--config",
+ type=str,
+ required=True,
+ help="Name of the plot config to use.")
+ parser.add_argument("--config_dir",
+ type=str,
+ default="frc971/analysis/plot_configs",
+ help="Directory to look for plot configs in.")
args = parser.parse_args(argv[1:])
if not os.path.isdir(args.config_dir):
diff --git a/frc971/analysis/plot_config.proto b/frc971/analysis/plot_config.proto
index c4c61c2..cd01f73 100644
--- a/frc971/analysis/plot_config.proto
+++ b/frc971/analysis/plot_config.proto
@@ -45,6 +45,9 @@
message Axes {
repeated Line line = 1;
optional string ylabel = 2;
+ optional string xlabel = 3 [default = "Monotonic Time (sec)"];
+ // Whether to share an x-axis with all the other axes.
+ optional bool share_x_axis = 4 [default = true];
}
// Message representing a single pyplot figure.
diff --git a/frc971/analysis/plot_configs/down_estimator.pb b/frc971/analysis/plot_configs/down_estimator.pb
new file mode 100644
index 0000000..48cf2ef
--- /dev/null
+++ b/frc971/analysis/plot_configs/down_estimator.pb
@@ -0,0 +1,187 @@
+channel {
+ name: "/drivetrain"
+ type: "frc971.control_loops.drivetrain.Status"
+ alias: "DrivetrainStatus"
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.yaw"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.lateral_pitch"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.longitudinal_pitch"
+ }
+ }
+ ylabel: "rad"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.quaternion_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.quaternion_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.quaternion_z"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.quaternion_w"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.accel_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.accel_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.accel_z"
+ }
+ }
+ ylabel: "m/s/s"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.velocity_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.velocity_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.velocity_z"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.position_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.position_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.position_z"
+ }
+ }
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "zeroing.gyro_x_average"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "zeroing.gyro_y_average"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "zeroing.gyro_z_average"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.expected_accel_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.expected_accel_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.expected_accel_z"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.gravity_magnitude"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "CalcDrivetrainStatus"
+ field: "down_estimator.accel_x_rolling_mean"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcDrivetrainStatus"
+ field: "down_estimator.accel_y_rolling_mean"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcDrivetrainStatus"
+ field: "down_estimator.accel_z_rolling_mean"
+ }
+ }
+ }
+}
diff --git a/frc971/analysis/plot_configs/drivetrain.pb b/frc971/analysis/plot_configs/drivetrain.pb
index 0ac4fff..0335da8 100644
--- a/frc971/analysis/plot_configs/drivetrain.pb
+++ b/frc971/analysis/plot_configs/drivetrain.pb
@@ -294,4 +294,170 @@
}
}
}
+ axes {
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.velocity_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.velocity_y"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.position_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "down_estimator.position_y"
+ }
+ }
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_x_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_y_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_z_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "g"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_x_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_y_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_z_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "g"
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_x_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_y_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_z_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "rad / sec"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_x_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_y_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_z_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "rad / sec"
+ }
}
diff --git a/frc971/analysis/plot_configs/gyro.pb b/frc971/analysis/plot_configs/gyro.pb
index 760752c..790fc55 100644
--- a/frc971/analysis/plot_configs/gyro.pb
+++ b/frc971/analysis/plot_configs/gyro.pb
@@ -82,3 +82,157 @@
ylabel: "g"
}
}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_x_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_y_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_z_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "g"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_x_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_y_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "accel_z_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "g"
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_x_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_y_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_z_rolling_mean"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "rad / sec"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_x_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_y_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ line {
+ y_signal {
+ channel: "CalcIMU"
+ field: "gyro_z_rolling_std"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "rad / sec"
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "temperature"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "C"
+ }
+}
diff --git a/frc971/analysis/plot_configs/gyro_temp.pb b/frc971/analysis/plot_configs/gyro_temp.pb
new file mode 100644
index 0000000..02c8c3b
--- /dev/null
+++ b/frc971/analysis/plot_configs/gyro_temp.pb
@@ -0,0 +1,21 @@
+channel {
+ name: "/drivetrain"
+ type: "frc971.IMUValues"
+ alias: "IMU"
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "IMU"
+ field: "temperature"
+ }
+ x_signal {
+ channel: "CalcIMU"
+ field: "monotonic_timestamp_sec"
+ }
+ }
+ ylabel: "C"
+ }
+}
diff --git a/frc971/analysis/plot_configs/localizer.pb b/frc971/analysis/plot_configs/localizer.pb
new file mode 100644
index 0000000..f81ec89
--- /dev/null
+++ b/frc971/analysis/plot_configs/localizer.pb
@@ -0,0 +1,259 @@
+channel {
+ name: "/drivetrain"
+ type: "frc971.control_loops.drivetrain.Status"
+ alias: "DrivetrainStatus"
+}
+channel {
+ name: "/drivetrain/truth"
+ type: "frc971.control_loops.drivetrain.Status"
+ alias: "DrivetrainTruthStatus"
+}
+channel {
+ name: "/drivetrain"
+ type: "frc971.control_loops.drivetrain.Position"
+ alias: "DrivetrainPosition"
+}
+channel {
+ name: "/drivetrain"
+ type: "frc971.control_loops.drivetrain.Output"
+ alias: "DrivetrainOutput"
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainTruthStatus"
+ field: "y"
+ }
+ x_signal {
+ channel: "DrivetrainTruthStatus"
+ field: "x"
+ }
+ }
+ share_x_axis: false
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "y"
+ }
+ x_signal {
+ channel: "DrivetrainStatus"
+ field: "x"
+ }
+ }
+ share_x_axis: false
+ xlabel: "x (m)"
+ ylabel: "y (m)"
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.yaw"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainTruthStatus"
+ field: "theta"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.theta"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.lateral_pitch"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.longitudinal_pitch"
+ }
+ }
+ ylabel: "rad"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.position_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.position_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainTruthStatus"
+ field: "x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainTruthStatus"
+ field: "y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.y"
+ }
+ }
+ ylabel: "m"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.velocity_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.velocity_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.longitudinal_velocity_offset"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.lateral_velocity"
+ }
+ }
+ ylabel: "m/s"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.accel_x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.accel_y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "down_estimator.accel_z"
+ }
+ }
+ ylabel: "m/s/s"
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.left_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.right_velocity"
+ }
+ }
+ ylabel: "m/s"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainPosition"
+ field: "left_encoder"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainPosition"
+ field: "right_encoder"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.left_encoder"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.right_encoder"
+ }
+ }
+ ylabel: "m"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.angular_error"
+ }
+ }
+ ylabel: "rad / sec"
+ }
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainOutput"
+ field: "left_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainOutput"
+ field: "right_voltage"
+ }
+ }
+ ylabel: "V"
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.left_voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "localizer.right_voltage_error"
+ }
+ }
+ ylabel: "V"
+ }
+}
diff --git a/y2020/BUILD b/y2020/BUILD
index c32cd10..9f7795e 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -34,6 +34,7 @@
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/hood:hood_plants",
+ "//y2020/control_loops/superstructure/intake:intake_plants",
"@com_google_absl//absl/base",
],
)
diff --git a/y2020/constants.cc b/y2020/constants.cc
index a16b2f1..aee797f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -12,6 +12,7 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "y2020/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2020/control_loops/superstructure/hood/integral_hood_plant.h"
@@ -48,6 +49,26 @@
hood->zeroing_constants.allowable_encoder_error = 0.9;
hood->zeroing_constants.middle_position = Values::kHoodRange().middle();
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
+ &r->intake;
+
+ // Intake constants.
+ intake->zeroing_voltage = 3.0;
+ intake->operating_voltage = 12.0;
+ intake->zeroing_profile_params = {0.5, 3.0};
+ intake->default_profile_params = {6.0, 30.0};
+ intake->range = Values::kIntakeRange();
+ intake->make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ intake->zeroing_constants.moving_buffer_size = 20;
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+ intake->zeroing_constants.middle_position = Values::kIntakeRange().middle();
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -55,14 +76,17 @@
case kCompTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
break;
case kPracticeTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
break;
case kCodingRobotTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
break;
default:
diff --git a/y2020/constants.h b/y2020/constants.h
index 47b8543..69e7f28 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
+#include "y2020/control_loops/superstructure/intake/intake_plant.h"
namespace y2020 {
namespace constants {
@@ -33,7 +34,7 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- //TODO(sabina): Update constants
+ // TODO(sabina): Update constants
static constexpr double kHoodEncoderRatio() { return 1.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
@@ -52,17 +53,42 @@
};
}
+ // Intake
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kIntakeEncoderRatio() { return (16.0 / 32.0); }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() / (2.0 * M_PI) *
+ 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
+ };
+ }
+
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
hood;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ intake;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
// returns a reference to it.
const Values &GetValues();
-// Creates Values instances for each team number it is called with and returns
-// them.
+// Creates Values instances for each team number it is called with and
+// returns them.
const Values &GetValuesForTeam(uint16_t team_number);
} // namespace constants
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 88d574d..b9bbcf0 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -100,5 +100,6 @@
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020/control_loops/superstructure/hood:hood_plants",
+ "//y2020/control_loops/superstructure/intake:intake_plants",
],
)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 91a7586..cfa6d94 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -13,7 +13,8 @@
const ::std::string &name)
: aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- hood_(constants::GetValues().hood) {
+ hood_(constants::GetValues().hood),
+ intake_joint_(constants::GetValues().intake) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -24,6 +25,7 @@
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
hood_.Reset();
+ intake_joint_.Reset();
}
OutputT output_struct;
@@ -34,16 +36,25 @@
output != nullptr ? &(output_struct.hood_voltage) : nullptr,
status->fbb());
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
+ intake_joint_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake_joint(),
+ output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
+ status->fbb());
+
bool zeroed;
bool estopped;
- const AbsoluteEncoderProfiledJointStatus *hood_status =
- GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
- zeroed = hood_status->zeroed();
- estopped = hood_status->estopped();
+ {
+ AbsoluteEncoderProfiledJointStatus *hood_status =
+ GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
- if (output != nullptr) {
- output->Send(Output::Pack(*output->fbb(), &output_struct));
+ AbsoluteEncoderProfiledJointStatus *intake_status =
+ GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
+
+ zeroed = hood_status->zeroed() && intake_status->zeroed();
+ estopped = hood_status->estopped() || intake_status->estopped();
}
Status::Builder status_builder = status->MakeBuilder<Status>();
@@ -52,8 +63,18 @@
status_builder.add_estopped(estopped);
status_builder.add_hood(hood_status_offset);
+ status_builder.add_intake(intake_status_offset);
status->Send(status_builder.Finish());
+
+ if (output != nullptr) {
+ if (unsafe_goal) {
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
+ } else {
+ output_struct.intake_roller_voltage = 0.0;
+ }
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
+ }
}
} // namespace superstructure
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index dbc1ccd..680b447 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -25,6 +25,7 @@
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
const AbsoluteEncoderSubsystem &hood() const { return hood_; }
+ const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
@@ -33,6 +34,7 @@
private:
AbsoluteEncoderSubsystem hood_;
+ AbsoluteEncoderSubsystem intake_joint_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 468e0c9..39fb975 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -10,6 +10,7 @@
#include "gtest/gtest.h"
#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
+#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/superstructure.h"
namespace y2020 {
@@ -47,8 +48,12 @@
hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
hood_encoder_(constants::GetValues()
- .hood.zeroing_constants.one_revolution_distance) {
+ .hood.zeroing_constants.one_revolution_distance),
+ intake_plant_(new CappedTestPlant(intake::MakeIntakePlant())),
+ intake_encoder_(constants::GetValues()
+ .intake.zeroing_constants.one_revolution_distance) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
+ InitializeIntakePosition(constants::Values::kIntakeRange().upper);
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
@@ -72,6 +77,16 @@
.hood.zeroing_constants.measured_absolute_position);
}
+ void InitializeIntakePosition(double start_pos) {
+ intake_plant_->mutable_X(0, 0) = start_pos;
+ intake_plant_->mutable_X(1, 0) = 0.0;
+
+ intake_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .intake.zeroing_constants.measured_absolute_position);
+ }
+
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
::aos::Sender<Position>::Builder builder =
@@ -82,9 +97,15 @@
flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
hood_encoder_.GetSensorValues(&hood_builder);
+ frc971::AbsolutePosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_offset =
+ intake_encoder_.GetSensorValues(&intake_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_hood(hood_offset);
+ position_builder.add_intake_joint(intake_offset);
builder.Send(position_builder.Finish());
}
@@ -92,9 +113,13 @@
double hood_position() const { return hood_plant_->X(0, 0); }
double hood_velocity() const { return hood_plant_->X(1, 0); }
+ double intake_position() const { return intake_plant_->X(0, 0); }
+ double intake_velocity() const { return intake_plant_->X(1, 0); }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
const double last_hood_velocity = hood_velocity();
+ const double last_intake_velocity = intake_velocity();
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -109,28 +134,56 @@
EXPECT_NEAR(superstructure_output_fetcher_->hood_voltage(), 0.0,
voltage_check_hood);
+ const double voltage_check_intake =
+ (static_cast<AbsoluteEncoderSubsystem::State>(
+ superstructure_status_fetcher_->intake()->state()) ==
+ AbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().intake.operating_voltage
+ : constants::GetValues().intake.zeroing_voltage;
+
+ EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage(), 0.0,
+ voltage_check_intake);
+
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_output_fetcher_->hood_voltage() +
hood_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
+ intake_plant_->voltage_offset();
+
hood_plant_->Update(hood_U);
+ intake_plant_->Update(intake_U);
const double position_hood = hood_plant_->Y(0, 0);
+ const double position_intake = intake_plant_->Y(0, 0);
hood_encoder_.MoveTo(position_hood);
+ intake_encoder_.MoveTo(position_intake);
EXPECT_GE(position_hood, constants::Values::kHoodRange().lower_hard);
EXPECT_LE(position_hood, constants::Values::kHoodRange().upper_hard);
+ EXPECT_GE(position_intake, constants::Values::kIntakeRange().lower_hard);
+ EXPECT_LE(position_intake, constants::Values::kIntakeRange().upper_hard);
+
const double loop_time = ::aos::time::DurationInSeconds(dt_);
const double hood_acceleration =
(hood_velocity() - last_hood_velocity) / loop_time;
+ const double intake_acceleration =
+ (intake_velocity() - last_intake_velocity) / loop_time;
+
EXPECT_GE(peak_hood_acceleration_, hood_acceleration);
EXPECT_LE(-peak_hood_acceleration_, hood_acceleration);
EXPECT_GE(peak_hood_velocity_, hood_velocity());
EXPECT_LE(-peak_hood_velocity_, hood_velocity());
+
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_intake_velocity_, intake_velocity());
+ EXPECT_LE(-peak_intake_velocity_, intake_velocity());
}
void set_peak_hood_acceleration(double value) {
@@ -138,6 +191,11 @@
}
void set_peak_hood_velocity(double value) { peak_hood_velocity_ = value; }
+ void set_peak_intake_acceleration(double value) {
+ peak_intake_acceleration_ = value;
+ }
+ void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -152,11 +210,16 @@
::std::unique_ptr<CappedTestPlant> hood_plant_;
PositionSensorSimulator hood_encoder_;
+ ::std::unique_ptr<CappedTestPlant> intake_plant_;
+ PositionSensorSimulator intake_encoder_;
+
// The acceleration limits to check for while moving.
double peak_hood_acceleration_ = 1e10;
+ double peak_intake_acceleration_ = 1e10;
// The velocity limits to check for while moving.
double peak_hood_velocity_ = 1e10;
+ double peak_intake_velocity_ = 1e10;
};
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
@@ -189,11 +252,14 @@
EXPECT_NEAR(superstructure_goal_fetcher_->hood()->unsafe_goal(),
superstructure_status_fetcher_->hood()->position(), 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
}
void CheckIfZeroed() {
- superstructure_status_fetcher_.Fetch();
- ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
}
void WaitUntilZeroed() {
@@ -230,7 +296,10 @@
// Tests that the superstructure does nothing when the goal is to remain still.
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
- superstructure_plant_.InitializeHoodPosition(0.77);
+ superstructure_plant_.InitializeHoodPosition(
+ constants::Values::kHoodRange().middle());
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().middle());
WaitUntilZeroed();
@@ -239,11 +308,16 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.77);
+ *builder.fbb(), constants::Values::kHoodRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().middle());
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -259,6 +333,7 @@
// Set a reasonable goal.
superstructure_plant_.InitializeHoodPosition(0.7);
+ superstructure_plant_.InitializeIntakePosition(0.7);
WaitUntilZeroed();
{
@@ -269,9 +344,15 @@
*builder.fbb(), 0.2,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.2,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -283,8 +364,6 @@
}
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
-//
-// We are going to disable collision detection to make this easier to implement.
TEST_F(SuperstructureTest, SaturationTest) {
SetEnabled(true);
// Zero it before we move.
@@ -296,9 +375,14 @@
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kHoodRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -315,16 +399,26 @@
*builder.fbb(), constants::Values::kHoodRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_hood_velocity(23.0);
superstructure_plant_.set_peak_hood_acceleration(0.2);
- RunFor(chrono::seconds(8));
+ superstructure_plant_.set_peak_intake_velocity(23.0);
+ superstructure_plant_.set_peak_intake_acceleration(0.2);
+
+ // Intake needs over 8 seconds to reach the goal
+ RunFor(chrono::seconds(9));
VerifyNearGoal();
}
@@ -335,6 +429,9 @@
RunFor(chrono::seconds(2));
EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.hood().state());
+
+ EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake_joint().state());
}
// Tests that running disabled works
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
index 25be42d..63f91c7 100644
--- a/y2020/control_loops/superstructure/superstructure_position.fbs
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -13,7 +13,7 @@
}
table Position {
- // Zero is at the horizontal, positive towards the front (meters on the lead screw).
+ // Zero is at the horizontal, positive towards the front (radians).
hood:frc971.AbsolutePosition;
// Position of the intake. 0 when four-bar is vertical, positive extended.
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index de4dfb7..b8e84c0 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -57,6 +57,21 @@
const std::vector<cv::KeyPoint> &keypoints,
const cv::Mat &descriptors);
+ // Returns the 3D location for the specified training feature.
+ cv::Point3f Training3dPoint(int training_image_index, int feature_index) {
+ const sift::KeypointFieldLocation *const location =
+ training_data_->images()
+ ->Get(training_image_index)
+ ->features()
+ ->Get(feature_index)
+ ->field_location();
+ return cv::Point3f(location->x(), location->y(), location->z());
+ }
+
+ int number_training_images() const {
+ return training_data_->images()->size();
+ }
+
aos::EventLoop *const event_loop_;
const sift::TrainingData *const training_data_;
V4L2Reader *const reader_;
@@ -144,29 +159,27 @@
const std::vector<std::vector<cv::DMatch>> &matches) {
// First, we need to pull out all the matches for each image. Might as well
// build up the Match tables at the same time.
- std::vector<std::vector<flatbuffers::Offset<sift::Match>>> per_image_matches;
+ std::vector<std::vector<sift::Match>> per_image_matches(
+ number_training_images());
for (const std::vector<cv::DMatch> &image_matches : matches) {
for (const cv::DMatch &image_match : image_matches) {
- sift::Match::Builder match_builder(*fbb);
- match_builder.add_query_feature(image_match.queryIdx);
- match_builder.add_train_feature(image_match.trainIdx);
- if (per_image_matches.size() <= static_cast<size_t>(image_match.imgIdx)) {
- per_image_matches.resize(image_match.imgIdx + 1);
- }
- per_image_matches[image_match.imgIdx].emplace_back(
- match_builder.Finish());
+ CHECK_LT(image_match.imgIdx, number_training_images());
+ per_image_matches[image_match.imgIdx].emplace_back();
+ sift::Match *const match = &per_image_matches[image_match.imgIdx].back();
+ match->mutate_query_feature(image_match.queryIdx);
+ match->mutate_train_feature(image_match.trainIdx);
+ match->mutate_distance(image_match.distance);
}
}
// Then, we need to build up each ImageMatch table.
std::vector<flatbuffers::Offset<sift::ImageMatch>> image_match_tables;
for (size_t i = 0; i < per_image_matches.size(); ++i) {
- const std::vector<flatbuffers::Offset<sift::Match>> &this_image_matches =
- per_image_matches[i];
+ const std::vector<sift::Match> &this_image_matches = per_image_matches[i];
if (this_image_matches.empty()) {
continue;
}
- const auto vector_offset = fbb->CreateVector(this_image_matches);
+ const auto vector_offset = fbb->CreateVectorOfStructs(this_image_matches);
sift::ImageMatch::Builder image_builder(*fbb);
image_builder.add_train_image(i);
image_builder.add_matches(vector_offset);
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 5a1384e..77d0dc6 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -1,5 +1,12 @@
namespace frc971.vision.sift;
+// Represents the location of a keypoint in field coordinates.
+struct KeypointFieldLocation {
+ x:float;
+ y:float;
+ z:float;
+}
+
// Represents a single feature extracted from an image.
table Feature {
// Contains the descriptor data.
@@ -28,14 +35,20 @@
// Which octave this keypoint is from.
octave:int;
+
+ // Where this feature's keypoint is on the field. This will only be filled out
+ // for training features, not ones extracted from query images.
+ field_location:KeypointFieldLocation;
}
// Represents a single match between a training image and a query image.
-table Match {
+struct Match {
// The index of the feature for the query image.
query_feature:int;
// The index of the feature for the training image.
train_feature:int;
+ // How "good" the match is.
+ distance:float;
}
// Represents all the matches between a single training image and a query
@@ -51,11 +64,49 @@
data:[double];
}
-// Contains the information the EKF wants from an image.
+// Calibration information for a given camera on a given robot.
+table CameraCalibration {
+ // The name of the camera node which this calibration data applies to.
+ node_name:string;
+ // The team number of the robot which this calibration data applies to.
+ team_number:int;
+
+ // Intrinsics for the camera.
+ //
+ // This is the standard OpenCV intrinsics matrix in row major order (3x3).
+ intrinsics:[float];
+
+ // Fixed extrinsics for the camera. This transforms from camera coordinates to
+ // robot coordinates. For example: multiplying (0, 0, 0, 1) by this results in
+ // the position of the camera aperature in robot coordinates.
+ fixed_extrinsics:TransformationMatrix;
+
+ // Extrinsics for a camera on a turret. This will only be filled out for
+ // applicable cameras. For turret-mounted cameras, fixed_extrinsics defines
+ // a position for the center of rotation of the turret, and this field defines
+ // a position for the camera on the turret.
+ //
+ // The combination of the two transformations is underdefined, so nothing can
+ // distinguish between the two parts of the final extrinsics for a given
+ // turret position.
+ //
+ // To get the final extrinsics for a camera using this transformation,
+ // multiply (in order):
+ // fixed_extrinsics
+ // rotation around the Z axis by the turret angle
+ // turret_extrinsics
+ turret_extrinsics:TransformationMatrix;
+}
+
+// Contains the information the EKF wants from an image matched against a single
+// training image.
//
// This is represented as a transformation to a target in field coordinates.
table CameraPose {
// Transformation matrix from the target to the camera's origin.
+ // (0, 0, 0) is the aperture of the camera (we pretend it's an ideal pinhole
+ // camera). Positive Z is out of the camera. Positive X and Y are right
+ // handed, but which way they face depends on the camera extrinsics.
camera_to_target:TransformationMatrix;
// Field coordinates of the target, represented as a transformation matrix
@@ -74,6 +125,7 @@
table ImageMatchResult {
// The matches from this image to each of the training images which matched.
+ // Each member is against the same captured image.
image_matches:[ImageMatch];
// The transformations for this image for each of the training images which
// matched.
@@ -85,6 +137,9 @@
// Timestamp when the frame was captured.
image_monotonic_timestamp_ns:long;
+
+ // Information about the camera which took this image.
+ camera_calibration:CameraCalibration;
}
root_type ImageMatchResult;
diff --git a/y2020/vision/sift/sift_training.fbs b/y2020/vision/sift/sift_training.fbs
index 2af0233..5e82e9f 100644
--- a/y2020/vision/sift/sift_training.fbs
+++ b/y2020/vision/sift/sift_training.fbs
@@ -18,6 +18,9 @@
// Represents the information used to match incoming images against.
table TrainingData {
images:[TrainingImage];
+
+ // Calibration information for all the cameras we know about.
+ camera_calibrations:[CameraCalibration];
}
root_type TrainingData;