Merge "scouting: Deduplicate action definitions in entry.component.ts"
diff --git a/.bazelrc b/.bazelrc
index a69ccad..3a3c729 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -22,7 +22,6 @@
# Shortcuts for selecting the target platform.
build:k8 --platforms=//tools/platforms:linux_x86
-build:k8_legacy_python --platforms=//tools/platforms:linux_x86_legacy_python --host_platform=//tools/platforms:linux_x86_legacy_python
build:roborio --platforms=//tools/platforms:linux_roborio
build:roborio --platform_suffix=-roborio
build:arm64 --platforms=//tools/platforms:linux_arm64
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.fbs b/frc971/control_loops/drivetrain/drivetrain_config.fbs
index 645eeac..0947063 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_config.fbs
@@ -98,6 +98,15 @@
max_controllable_offset:double = 0.1 (id: 2);
}
+table SplineFollowerConfig {
+ // Q should be a 5x5 positive-definite matrix; it is used as the state cost
+ // of the LQR controller for the spline-following mode.
+ q:frc971.fbs.Matrix (id: 0);
+ // R should be a 2x2 positive-definite matrix; it is used as the input cost
+ // of the LQR controller for the spline-following mode.
+ r:frc971.fbs.Matrix (id: 1);
+}
+
// These constants are all specified by the drivetrain python code, and
// so are separated out for easy codegen.
table DrivetrainLoopConfig {
@@ -135,6 +144,7 @@
is_simulated:bool = false (id: 14);
down_estimator_config:DownEstimatorConfig (id: 15);
line_follow_config:LineFollowConfig (id: 16);
+ spline_follower_config:SplineFollowerConfig (id: 20);
top_button_use:PistolTopButtonUse = kShift (id: 17);
second_button_use:PistolSecondButtonUse = kShiftLow (id: 18);
bottom_button_use:PistolBottomButtonUse = kSlowDown (id: 19);
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 8e0a58d..3f56082 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -50,6 +50,33 @@
}
};
+struct SplineFollowerConfig {
+ // The line-following uses an LQR controller with states of
+ // [longitudinal_position, lateral_positoin, theta, left_velocity,
+ // right_velocity] and inputs of [left_voltage, right_voltage]. These Q and R
+ // matrices are the costs for state and input respectively.
+ Eigen::Matrix<double, 5, 5> Q = Eigen::Matrix<double, 5, 5>(
+ (::Eigen::DiagonalMatrix<double, 5>().diagonal() << ::std::pow(60.0, 2.0),
+ ::std::pow(60.0, 2.0), ::std::pow(40.0, 2.0), ::std::pow(30.0, 2.0),
+ ::std::pow(30.0, 2.0))
+ .finished()
+ .asDiagonal());
+ Eigen::Matrix2d R = Eigen::Matrix2d(
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal() << 5.0, 5.0)
+ .finished()
+ .asDiagonal());
+
+ static SplineFollowerConfig FromFlatbuffer(
+ const fbs::SplineFollowerConfig *fbs) {
+ if (fbs == nullptr) {
+ return {};
+ }
+ return SplineFollowerConfig{
+ .Q = ToEigenOrDie<5, 5>(*CHECK_NOTNULL(fbs->q())),
+ .R = ToEigenOrDie<2, 2>(*CHECK_NOTNULL(fbs->r()))};
+ }
+};
+
template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
@@ -125,6 +152,8 @@
PistolSecondButtonUse second_button_use = PistolSecondButtonUse::kShiftLow;
PistolBottomButtonUse bottom_button_use = PistolBottomButtonUse::kSlowDown;
+ SplineFollowerConfig spline_follower_config{};
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
@@ -229,7 +258,9 @@
.line_follow_config =
LineFollowConfig::FromFlatbuffer(fbs.line_follow_config()),
ASSIGN(top_button_use), ASSIGN(second_button_use),
- ASSIGN(bottom_button_use)
+ ASSIGN(bottom_button_use),
+ .spline_follower_config = SplineFollowerConfig::FromFlatbuffer(
+ fbs.spline_follower_config()),
#undef ASSIGN
};
}
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 163311a..cf9929f 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -770,17 +770,8 @@
// Set up reasonable gain matrices. Current choices of gains are arbitrary
// and just setup to work well enough for the simulation tests.
- // TODO(james): Tune this on a real robot.
- // TODO(james): Pull these out into a config.
- Eigen::Matrix<double, 5, 5> Q;
- Q.setIdentity();
- Q.diagonal() << 30.0, 30.0, 20.0, 15.0, 15.0;
- Q *= 2.0;
- Q = (Q * Q).eval();
-
- Eigen::Matrix<double, 2, 2> R;
- R.setIdentity();
- R *= 5.0;
+ Eigen::Matrix<double, 5, 5> Q = config_->spline_follower_config.Q;
+ Eigen::Matrix<double, 2, 2> R = config_->spline_follower_config.R;
Eigen::Matrix<double, 5, 5> P = Q;
@@ -822,6 +813,7 @@
const Eigen::Matrix<double, 2, 5> K = RBPBinv * APB.transpose();
plan_gains_[i].second = K.cast<float>();
P = AP * A_discrete - APB * K + Q;
+ CHECK_LT(P.norm(), 1e30) << "LQR calculations became unstable.";
}
}
diff --git a/frc971/orin/hardware_monitor.cc b/frc971/orin/hardware_monitor.cc
index f09d0e6..e1c785b 100644
--- a/frc971/orin/hardware_monitor.cc
+++ b/frc971/orin/hardware_monitor.cc
@@ -10,6 +10,7 @@
#include "frc971/orin/hardware_stats_generated.h"
DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+DEFINE_bool(log_voltages, false, "If true, log voltages too.");
namespace frc971::orin {
namespace {
@@ -68,14 +69,19 @@
// Iterate through all thermal zones
std::vector<flatbuffers::Offset<ThermalZone>> thermal_zones;
for (int zone_id = 0; zone_id < 9; zone_id++) {
+ std::optional<std::string> zone_name = ReadFileFirstLine(absl::StrFormat(
+ "/sys/devices/virtual/thermal/thermal_zone%d/type", zone_id));
+ flatbuffers::Offset<flatbuffers::String> name_offset;
+ if (zone_name) {
+ name_offset = builder.fbb()->CreateString(*zone_name);
+ }
+
ThermalZone::Builder thermal_zone_builder =
builder.MakeBuilder<ThermalZone>();
thermal_zone_builder.add_id(zone_id);
- std::optional<std::string> zone_name = ReadFileFirstLine(absl::StrFormat(
- "/sys/devices/virtual/thermal/thermal_zone%d/type", zone_id));
- if (zone_name) {
- thermal_zone_builder.add_name(builder.fbb()->CreateString(*zone_name));
+ if (!name_offset.IsNull()) {
+ thermal_zone_builder.add_name(name_offset);
}
std::optional<std::string> temperature_str =
@@ -93,54 +99,68 @@
std::optional<std::string> fan_speed_str = ReadFileFirstLine(
absl::StrFormat("/sys/class/hwmon/%s/rpm", fan_hwmon_));
- // Iterate through INA3221 electrical reading channels
- std::vector<flatbuffers::Offset<ElectricalReading>> electrical_readings;
- for (int channel = 1; channel <= 3; channel++) {
- ElectricalReading::Builder electrical_reading_builder =
- builder.MakeBuilder<ElectricalReading>();
- electrical_reading_builder.add_channel(channel);
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<ElectricalReading>>>
+ electrical_readings_offset;
+ if (FLAGS_log_voltages) {
+ std::vector<flatbuffers::Offset<ElectricalReading>> electrical_readings;
+ // Iterate through INA3221 electrical reading channels
+ for (int channel = 1; channel <= 3; channel++) {
+ std::optional<std::string> label = ReadFileFirstLine(absl::StrFormat(
+ "/sys/class/hwmon/%s/in%d_label", electrical_hwmon_, channel));
- std::optional<std::string> label = ReadFileFirstLine(absl::StrFormat(
- "/sys/class/hwmon/%s/in%d_label", electrical_hwmon_, channel));
- if (label) {
- electrical_reading_builder.add_label(
- builder.fbb()->CreateString(*label));
+ flatbuffers::Offset<flatbuffers::String> label_offset;
+ if (label) {
+ label_offset = builder.fbb()->CreateString(*label);
+ }
+
+ ElectricalReading::Builder electrical_reading_builder =
+ builder.MakeBuilder<ElectricalReading>();
+ electrical_reading_builder.add_channel(channel);
+
+ if (!label_offset.IsNull()) {
+ electrical_reading_builder.add_label(label_offset);
+ }
+
+ std::optional<std::string> voltage_str =
+ ReadFileFirstLine(absl::StrFormat("/sys/class/hwmon/%s/in%d_input",
+ electrical_hwmon_, channel));
+ uint64_t voltage = 0;
+ if (voltage_str && absl::SimpleAtoi(*voltage_str, &voltage)) {
+ electrical_reading_builder.add_voltage(voltage);
+ }
+
+ std::optional<std::string> current_str = ReadFileFirstLine(
+ absl::StrFormat("/sys/class/hwmon/%s/curr%d_input",
+ electrical_hwmon_, channel));
+ uint64_t current = 0;
+ if (current_str && absl::SimpleAtoi(*current_str, ¤t)) {
+ electrical_reading_builder.add_current(current);
+ }
+
+ uint64_t power = voltage * current / 1000;
+ if (power != 0) {
+ electrical_reading_builder.add_power(power);
+ }
+
+ electrical_readings.emplace_back(electrical_reading_builder.Finish());
}
-
- std::optional<std::string> voltage_str =
- ReadFileFirstLine(absl::StrFormat("/sys/class/hwmon/%s/in%d_input",
- electrical_hwmon_, channel));
- uint64_t voltage = 0;
- if (voltage_str && absl::SimpleAtoi(*voltage_str, &voltage)) {
- electrical_reading_builder.add_voltage(voltage);
- }
-
- std::optional<std::string> current_str =
- ReadFileFirstLine(absl::StrFormat("/sys/class/hwmon/%s/curr%d_input",
- electrical_hwmon_, channel));
- uint64_t current = 0;
- if (current_str && absl::SimpleAtoi(*current_str, ¤t)) {
- electrical_reading_builder.add_current(current);
- }
-
- uint64_t power = voltage * current / 1000;
- if (power != 0) {
- electrical_reading_builder.add_power(power);
- }
-
- electrical_readings.emplace_back(electrical_reading_builder.Finish());
+ electrical_readings_offset =
+ builder.fbb()->CreateVector(electrical_readings);
}
+ auto thermal_zone_offset = builder.fbb()->CreateVector(thermal_zones);
HardwareStats::Builder hardware_stats_builder =
builder.MakeBuilder<HardwareStats>();
- hardware_stats_builder.add_thermal_zones(
- builder.fbb()->CreateVector(thermal_zones));
+ hardware_stats_builder.add_thermal_zones(thermal_zone_offset);
uint64_t fan_speed = 0;
if (fan_speed_str && absl::SimpleAtoi(*fan_speed_str, &fan_speed)) {
hardware_stats_builder.add_fan_speed(fan_speed);
}
- hardware_stats_builder.add_electrical_readings(
- builder.fbb()->CreateVector(electrical_readings));
+ if (!electrical_readings_offset.IsNull()) {
+ hardware_stats_builder.add_electrical_readings(
+ electrical_readings_offset);
+ }
builder.CheckOk(builder.Send(hardware_stats_builder.Finish()));
}
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index 16a53a7..d3ddba7 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -43,7 +43,19 @@
<< "Need a base intrinsics json to use to auto-capture images when the "
"camera moves.";
std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
- IntrinsicsCalibration extractor(&event_loop, hostname, FLAGS_channel,
+
+ std::string camera_name = absl::StrCat(
+ "/", aos::network::ParsePiOrOrin(hostname).value(),
+ std::to_string(aos::network::ParsePiOrOrinNumber(hostname).value()),
+ FLAGS_channel);
+ // THIS IS A HACK FOR 2024, since we call Orin2 "Imu"
+ if (aos::network::ParsePiOrOrin(hostname).value() == "orin" &&
+ aos::network::ParsePiOrOrinNumber(hostname).value() == 2) {
+ LOG(INFO) << "\nHACK for 2024: Renaming orin2 to imu\n";
+ camera_name = absl::StrCat("/imu", FLAGS_channel);
+ }
+
+ IntrinsicsCalibration extractor(&event_loop, hostname, camera_name,
FLAGS_camera_id, FLAGS_base_intrinsics,
FLAGS_display_undistorted,
FLAGS_calibration_folder, exit_handle.get());
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index 59a45ac..5584ed7 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -36,17 +36,27 @@
rvecs_eigen, tvecs_eigen);
}),
image_callback_(
- event_loop,
- absl::StrCat("/", aos::network::ParsePiOrOrin(hostname_).value(),
- std::to_string(cpu_number_.value()), camera_channel_),
+ event_loop, camera_channel_,
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
+ if (exit_collection_) {
+ return;
+ }
charuco_extractor_.HandleImage(rgb_image, eof);
},
kMaxImageAge),
display_undistorted_(display_undistorted),
calibration_folder_(calibration_folder),
- exit_handle_(exit_handle) {
+ exit_handle_(exit_handle),
+ exit_collection_(false) {
+ if (!FLAGS_visualize) {
+ // The only way to exit into the calibration routines is by hitting "q"
+ // while visualization is running. The event_loop doesn't pause enough
+ // to handle ctrl-c exit requests
+ LOG(INFO) << "Setting visualize to true, since currently the intrinsics "
+ "only works this way";
+ FLAGS_visualize = true;
+ }
LOG(INFO) << "Hostname is: " << hostname_ << " and camera channel is "
<< camera_channel_;
@@ -81,7 +91,11 @@
}
int keystroke = cv::waitKey(1);
-
+ if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ LOG(INFO) << "Going to exit";
+ exit_collection_ = true;
+ exit_handle_->Exit();
+ }
// If we haven't got a valid pose estimate, don't use these points
if (!valid) {
LOG(INFO) << "Skipping because pose is not valid";
@@ -161,9 +175,6 @@
<< kDeltaTThreshold;
}
}
-
- } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
- exit_handle_->Exit();
}
}
@@ -175,8 +186,14 @@
std::string_view camera_id, uint16_t team_number,
double reprojection_error) {
flatbuffers::FlatBufferBuilder fbb;
+ // THIS IS A HACK FOR 2024, since we call Orin2 "Imu"
+ std::string cpu_name = absl::StrFormat("%s%d", cpu_type, cpu_number);
+ if (cpu_type == "orin" && cpu_number == 2) {
+ LOG(INFO) << "Renaming orin2 to imu";
+ cpu_name = "imu";
+ }
flatbuffers::Offset<flatbuffers::String> name_offset =
- fbb.CreateString(absl::StrFormat("%s%d", cpu_type, cpu_number));
+ fbb.CreateString(cpu_name.c_str());
flatbuffers::Offset<flatbuffers::String> camera_id_offset =
fbb.CreateString(camera_id);
flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
index 605741f..faa82e9 100644
--- a/frc971/vision/intrinsics_calibration_lib.h
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -77,6 +77,8 @@
const bool display_undistorted_;
const std::string calibration_folder_;
aos::ExitHandle *exit_handle_;
+
+ bool exit_collection_;
};
} // namespace frc971::vision
diff --git a/y2024/BUILD b/y2024/BUILD
index 7bbf64f..7568050 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -152,7 +152,7 @@
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//frc971/wpilib:pdp_values_fbs",
- #y2019 stuff shouldn't be here (e.g. target selector)
+ # y2019 stuff shouldn't be here (e.g. target selector)
"//y2024/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//y2024/control_loops/superstructure:superstructure_goal_fbs",
@@ -277,6 +277,7 @@
"//third_party:phoenix6",
"//third_party:wpilib",
"//y2024/constants:constants_fbs",
+ "//y2024/control_loops/superstructure:led_indicator_lib",
"//y2024/control_loops/superstructure:superstructure_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_output_fbs",
"//y2024/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index 3beaae0..2fe58cf 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -71,7 +71,9 @@
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
- }
+ },
+ "disable_extend": false,
+ "disable_climber": false
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 974fb60..c146417 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -23,7 +23,7 @@
"robot": {
{% set _ = intake_pivot_zero.update(
{
- "measured_absolute_position" : 3.49222521810232
+ "measured_absolute_position" : 3.229
}
) %}
"intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
@@ -43,12 +43,12 @@
}
) %}
"zeroing_constants": {{ catapult_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ 9.41595277209342 }}
+ "potentiometer_offset": {{ 9.41595277209342 - 1.59041961316453 + 0.478015209219659 }}
},
"altitude_constants": {
{% set _ = altitude_zero.update(
{
- "measured_absolute_position" : 0.1877
+ "measured_absolute_position" : 0.2135
}
) %}
"zeroing_constants": {{ altitude_zero | tojson(indent=2)}},
@@ -57,11 +57,11 @@
"turret_constants": {
{% set _ = turret_zero.update(
{
- "measured_absolute_position" : 0.961143535321169
+ "measured_absolute_position" : 0.2077
}
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -6.47164779835404 - 0.0711209027239817 + 1.0576004531907 }}
+ "potentiometer_offset": {{ -6.47164779835404 - 0.0711209027239817 + 1.0576004531907 - 0.343 }}
},
"extend_constants": {
{% set _ = extend_zero.update(
@@ -71,7 +71,9 @@
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
"potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 + 0.3473623911879 - 0.1577}}
- }
+ },
+ "disable_extend": false,
+ "disable_climber": true
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
index ec59033..23cad58 100644
--- a/y2024/constants/9971.json
+++ b/y2024/constants/9971.json
@@ -62,7 +62,9 @@
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
- }
+ },
+ "disable_extend": false,
+ "disable_climber": false
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 65fe20e..52fdb74 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -7,43 +7,50 @@
"distance_from_goal": 0.7,
"shot_params": {
"shot_altitude_angle": 0.85,
- "shot_speed_over_ground": 8.0
+ "shot_speed_over_ground": 16.0
}
},
{
"distance_from_goal": 1.24,
"shot_params": {
"shot_altitude_angle": 0.85,
- "shot_speed_over_ground": 8.0
+ "shot_speed_over_ground": 16.0
}
},
{
"distance_from_goal": 1.904,
"shot_params": {
"shot_altitude_angle": 0.73,
- "shot_speed_over_ground": 8.0
+ "shot_speed_over_ground": 16.0
}
},
// 2.2 -> high.
{
"distance_from_goal": 2.744,
"shot_params": {
- "shot_altitude_angle": 0.62,
- "shot_speed_over_ground": 8.0
+ "shot_altitude_angle": 0.61,
+ "shot_speed_over_ground": 16.0
}
},
{
"distance_from_goal": 3.274,
"shot_params": {
- "shot_altitude_angle": 0.58,
- "shot_speed_over_ground": 8.0
+ "shot_altitude_angle": 0.55,
+ "shot_speed_over_ground": 16.0
}
},
{
"distance_from_goal": 4.00,
"shot_params": {
- "shot_altitude_angle": 0.54,
- "shot_speed_over_ground": 8.0
+ "shot_altitude_angle": 0.515,
+ "shot_speed_over_ground": 16.0
+ }
+ },
+ {
+ "distance_from_goal": 4.68,
+ "shot_params": {
+ "shot_altitude_angle": 0.51,
+ "shot_speed_over_ground": 16.0
}
}
],
@@ -82,7 +89,7 @@
"intake_pivot_stator_current_limit": 100,
"intake_roller_supply_current_limit": 20,
"intake_roller_stator_current_limit": 100,
- "transfer_roller_supply_current_limit": 20,
+ "transfer_roller_supply_current_limit": 40,
"transfer_roller_stator_current_limit": 50,
"drivetrain_supply_current_limit": 50,
"drivetrain_stator_current_limit": 200,
@@ -104,7 +111,7 @@
"retention_roller_supply_current_limit": 10
},
"transfer_roller_voltages": {
- "transfer_in": 9.0,
+ "transfer_in": 11.0,
"transfer_out": -4.0,
"extend_moving": 4.0
},
@@ -248,7 +255,7 @@
},
// TODO(Filip): Update the speaker and amp shooter setpoints
"shooter_speaker_set_point": {
- "turret_position": 0.22,
+ "turret_position": 0.13,
"altitude_position": 0.85,
"shot_velocity": 0.0
},
@@ -269,6 +276,5 @@
"ignore_targets": {
"red": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16],
"blue": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16]
- },
- "disable_extend": false
+ }
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 1554f4c..99d49a4 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -132,6 +132,8 @@
altitude_constants:PotAndAbsEncoderConstants (id: 3);
turret_constants:PotAndAbsEncoderConstants (id: 4);
extend_constants:PotAndAbsEncoderConstants (id: 5);
+ disable_extend:bool (id: 6);
+ disable_climber:bool (id: 7);
}
table ShooterSetPoint {
@@ -197,7 +199,6 @@
altitude_avoid_extend_collision_position: double (id: 28);
autonomous_mode:AutonomousMode (id: 26);
ignore_targets:IgnoreTargets (id: 27);
- disable_extend:bool (id: 29);
}
table Constants {
diff --git a/y2024/constants/test_data/test_team.json b/y2024/constants/test_data/test_team.json
index b717224..37d9de1 100644
--- a/y2024/constants/test_data/test_team.json
+++ b/y2024/constants/test_data/test_team.json
@@ -71,7 +71,9 @@
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
- }
+ },
+ "disable_extend": false,
+ "disable_climber": false
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
index 194b123..1127d6a 100644
--- a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
+++ b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
@@ -10,7 +10,7 @@
"quickturn_wheel_multiplier": 1.2,
"wheel_multiplier": 1.2,
"pistol_grip_shift_enables_line_follow": false,
- "imu_transform":{
+ "imu_transform": {
"rows": 3,
"cols": 3,
"data": [1, 0, 0, 0, 1, 0, 0, 0, 1]
@@ -22,5 +22,21 @@
},
"top_button_use": "kNone",
"second_button_use": "kTurn1",
- "bottom_button_use": "kControlLoopDriving"
+ "bottom_button_use": "kControlLoopDriving",
+ "spline_follower_config": {
+ "q": {
+ "rows": 5,
+ "cols": 5,
+ "data": [3600, 0, 0, 0, 0,
+ 0, 3600, 0, 0, 0,
+ 0, 0, 1600, 0, 0,
+ 0, 0, 0, 16, 0,
+ 0, 0, 0, 0, 16]
+ },
+ "r": {
+ "rows": 2,
+ "cols": 2,
+ "data": [5, 0, 0, 5]
+ }
+ }
}
diff --git a/y2024/control_loops/python/catapult.py b/y2024/control_loops/python/catapult.py
index 406f56e..6f2e9a5 100644
--- a/y2024/control_loops/python/catapult.py
+++ b/y2024/control_loops/python/catapult.py
@@ -25,14 +25,14 @@
return motor
-kCatapultWithGamePiece = angular_system.AngularSystemParams(
+kCatapultWithoutGamePiece = angular_system.AngularSystemParams(
name='Catapult',
# Add the battery series resistance to make it better match.
motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
0.00),
G=(14.0 / 60.0) * (12.0 / 24.0),
- # 208.7328 in^2 lb
- J=0.065 + 0.04,
+ # 135.2928 in^2 lb
+ J=0.06,
q_pos=0.80,
q_vel=15.0,
kalman_q_pos=0.12,
@@ -43,14 +43,32 @@
delayed_u=1,
dt=0.005)
-kCatapultWithoutGamePiece = angular_system.AngularSystemParams(
- name='Catapult',
+kCatapultWithGamePiece = angular_system.AngularSystemParams(
+ name='CatapultWithPiece',
+ # Add the battery series resistance to make it better match.
+ motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
+ 0.00),
+ G=(14.0 / 60.0) * (12.0 / 24.0),
+ # 208.7328 in^2 lb
+ J=0.065 + 0.06,
+ q_pos=0.80,
+ q_vel=15.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=0.7,
+ kalman_r_position=0.05,
+ radius=12 * 0.0254,
+ delayed_u=1,
+ dt=0.005)
+
+kCatapultWithoutGamePieceDecel = angular_system.AngularSystemParams(
+ name='CatapultWithoutPieceDecel',
# Add the battery series resistance to make it better match.
motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
0.00),
G=(14.0 / 60.0) * (12.0 / 24.0),
# 135.2928 in^2 lb
- J=0.06,
+ J=0.04,
q_pos=0.80,
q_vel=15.0,
kalman_q_pos=0.12,
@@ -76,9 +94,10 @@
)
else:
namespaces = ['y2024', 'control_loops', 'superstructure', 'catapult']
- angular_system.WriteAngularSystem(
- [kCatapultWithoutGamePiece, kCatapultWithGamePiece], argv[1:4],
- argv[4:7], namespaces)
+ angular_system.WriteAngularSystem([
+ kCatapultWithoutGamePiece, kCatapultWithGamePiece,
+ kCatapultWithoutGamePieceDecel
+ ], argv[1:4], argv[4:7], namespaces)
if __name__ == '__main__':
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
index 6150caa..946579b 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -78,8 +78,7 @@
hdrs = [
"superstructure.h",
],
- data = [
- ],
+ data = [],
deps = [
":collision_avoidance_lib",
":shooter",
@@ -242,3 +241,31 @@
"//aos/network/www:proxy",
],
)
+
+cc_library(
+ name = "led_indicator_lib",
+ srcs = ["led_indicator.cc"],
+ hdrs = ["led_indicator.h"],
+ data = [
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/queues:gyro_fbs",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ ],
+)
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index a21c09d..bf68527 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -8,9 +8,6 @@
using frc971::control_loops::aiming::ShotMode;
using y2024::control_loops::superstructure::Aimer;
-// When the turret is at 0 the note will be leaving the robot at PI.
-static constexpr double kTurretZeroOffset = M_PI - 0.22;
-
Aimer::Aimer(aos::EventLoop *event_loop,
const y2024::Constants *robot_constants)
: event_loop_(event_loop),
@@ -76,7 +73,7 @@
robot_constants_->common()->turret()->range()),
interpolation_table_.Get(current_goal_.target_distance)
.shot_speed_over_ground,
- /*wrap_mode=*/0.15, kTurretZeroOffset},
+ /*wrap_mode=*/0.15, M_PI - kTurretZeroOffset},
RobotState{
robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 9bec187..97e319d 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -17,6 +17,9 @@
class Aimer {
public:
+ // When the turret is at 0 the note will be leaving the robot at PI.
+ static constexpr double kTurretZeroOffset = 0.13;
+
Aimer(aos::EventLoop *event_loop, const Constants *robot_constants);
void Update(
diff --git a/y2024/control_loops/superstructure/led_indicator.cc b/y2024/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..3927548
--- /dev/null
+++ b/y2024/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,165 @@
+#include "y2024/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+namespace chrono = std::chrono;
+
+namespace y2024::control_loops::superstructure {
+
+LedIndicator::LedIndicator(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ drivetrain_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ server_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+ "/roborio/aos")),
+ localizer_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ gyro_reading_fetcher_(
+ event_loop_->MakeFetcher<frc971::sensors::GyroReading>(
+ "/drivetrain")),
+ drivetrain_status_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {
+ led::CANdleConfiguration config;
+ config.statusLedOffWhenActive = true;
+ config.disableWhenLOS = false;
+ config.brightnessScalar = 1.0;
+ candle_.ConfigAllSettings(config, 0);
+
+ event_loop_->AddPhasedLoop([this](int) { DecideColor(); },
+ chrono::milliseconds(20));
+ event_loop_->OnRun(
+ [this]() { startup_time_ = event_loop_->monotonic_now(); });
+}
+
+// This method will be called once per scheduler run
+void LedIndicator::DisplayLed(uint8_t r, uint8_t g, uint8_t b) {
+ candle_.SetLEDs(static_cast<int>(r), static_cast<int>(g),
+ static_cast<int>(b));
+}
+
+bool DisconnectedIMUPiServer(
+ const aos::message_bridge::ServerStatistics &server_statistics) {
+ for (const auto *node_status : *server_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool DisconnectedIMUPiClient(
+ const aos::message_bridge::ClientStatistics &client_statistics) {
+ for (const auto *node_status : *client_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool OrinsDisconnected(
+ const frc971::controls::LocalizerOutput &localizer_output) {
+ if (!localizer_output.all_pis_connected()) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_fetcher_.Fetch();
+ drivetrain_status_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
+ gyro_reading_fetcher_.Fetch();
+ localizer_output_fetcher_.Fetch();
+
+ if (localizer_output_fetcher_.get()) {
+ if (localizer_output_fetcher_->image_accepted_count() !=
+ last_accepted_count_) {
+ last_accepted_count_ = localizer_output_fetcher_->image_accepted_count();
+ last_accepted_time_ = event_loop_->monotonic_now();
+ }
+ }
+
+ // Estopped: Red
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // If the imu gyro readings are not being sent/updated recently. Only do this
+ // after we've been on for a bit.
+ if (event_loop_->context().monotonic_event_time >
+ startup_time_ + chrono::seconds(5) &&
+ (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time +
+ frc971::controls::kLoopFrequency * 10 <
+ event_loop_->monotonic_now() ||
+ !gyro_reading_fetcher_->has_velocity())) {
+ // Flash red/white
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(255, 255, 255);
+ }
+ return;
+ }
+
+ if (localizer_output_fetcher_.get() == nullptr ||
+ server_statistics_fetcher_.get() == nullptr ||
+ client_statistics_fetcher_.get() == nullptr ||
+ OrinsDisconnected(*localizer_output_fetcher_) ||
+ DisconnectedIMUPiServer(*server_statistics_fetcher_) ||
+ DisconnectedIMUPiClient(*client_statistics_fetcher_)) {
+ // Flash red/green
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+
+ return;
+ }
+
+ // Not zeroed: Yellow
+ if ((superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) ||
+ (drivetrain_status_fetcher_.get() &&
+ !drivetrain_status_fetcher_->filters_ready())) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+
+ // Want to know when we have a note.
+ //
+ if (superstructure_status_fetcher_.get()) {
+ // Check if there is a target that is in sight
+ if (event_loop_->monotonic_now() <
+ last_accepted_time_ + chrono::milliseconds(100)) {
+ if (superstructure_status_fetcher_.get() != nullptr &&
+ superstructure_status_fetcher_->shooter()->auto_aiming()) {
+ DisplayLed(0, 0, 255);
+ return;
+ } else {
+ DisplayLed(0, 255, 0);
+ return;
+ }
+ }
+ }
+ DisplayLed(0, 0, 0);
+}
+
+} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/led_indicator.h b/y2024/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..a7d891a
--- /dev/null
+++ b/y2024/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,96 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "ctre/phoenix/led/CANdle.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024::control_loops::superstructure {
+
+class FlashCounter {
+ public:
+ FlashCounter(size_t flash_iterations) : flash_iterations_(flash_iterations) {}
+
+ bool Flash() {
+ if (counter_ % flash_iterations_ == 0) {
+ flash_ = !flash_;
+ }
+ counter_++;
+ return flash_;
+ }
+
+ private:
+ size_t flash_iterations_;
+ size_t counter_ = 0;
+ bool flash_ = false;
+};
+
+class LedIndicator {
+ public:
+ LedIndicator(aos::EventLoop *event_loop);
+
+ // Colors in order of priority:
+ //
+ // Red: estopped
+ // Flash red/white: imu disconnected
+ // Flash red/green: any orin disconnected
+ // Pink: not zeroed
+ //
+ // State machine:
+ // INTAKING: Flash Orange/Off
+ // LOADED: Yellow
+ // MOVING: Flash Yellow/Off
+ // LOADING_CATAPULT: Flash Purple/Off
+ // READY: Green
+ // FIRING: Purple
+ //
+ // HAS A TARGET: Blue
+ // VISION: Flash Blue/Off
+
+ void DecideColor();
+
+ private:
+ static constexpr size_t kFlashIterations = 5;
+
+ void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
+
+ ctre::phoenix::led::CANdle candle_{8, "rio"};
+
+ aos::EventLoop *event_loop_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+ aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+
+ size_t last_accepted_count_ = 0;
+ aos::monotonic_clock::time_point last_accepted_time_ =
+ aos::monotonic_clock::min_time;
+
+ aos::monotonic_clock::time_point startup_time_ =
+ aos::monotonic_clock::min_time;
+
+ FlashCounter flash_counter_{kFlashIterations};
+};
+
+} // namespace y2024::control_loops::superstructure
+
+#endif // Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 79da980..7002657 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -186,10 +186,10 @@
{.intake_pivot_position = intake_pivot_position,
.turret_position = turret_.estimated_position(),
.extend_position =
- ((!robot_constants_->common()->disable_extend()) ? extend_position
- : 0.0)},
+ ((!robot_constants_->robot()->disable_extend()) ? extend_position
+ : 0.0)},
turret_goal->unsafe_goal(),
- ((!robot_constants_->common()->disable_extend()) ? extend_goal : 0.0));
+ ((!robot_constants_->robot()->disable_extend()) ? extend_goal : 0.0));
if (!CatapultRetracted()) {
altitude_.set_min_position(
@@ -248,7 +248,7 @@
//
// accel = v^2 / (2 * x)
catapult_.mutable_profile()->set_maximum_velocity(
- catapult::kFreeSpeed * catapult::kOutputRatio * 4.0 / 12.0);
+ catapult::kFreeSpeed * catapult::kOutputRatio * 5.5 / 12.0);
if (disabled) {
state_ = CatapultState::RETRACTING;
@@ -272,6 +272,7 @@
if (subsystems_in_range && shooter_goal != nullptr && fire &&
catapult_close && piece_loaded) {
state_ = CatapultState::FIRING;
+ max_catapult_goal_velocity_ = catapult_.goal(1);
} else {
catapult_.set_controller_index(0);
catapult_.mutable_profile()->set_maximum_acceleration(
@@ -294,10 +295,17 @@
robot_constants_->common()
->current_limits()
->shooting_retention_roller_stator_current_limit();
- catapult_.set_controller_index(1);
+ max_catapult_goal_velocity_ =
+ std::max(max_catapult_goal_velocity_, catapult_.goal(1));
+
+ if (max_catapult_goal_velocity_ > catapult_.goal(1) + 0.1) {
+ catapult_.set_controller_index(2);
+ } else {
+ catapult_.set_controller_index(1);
+ }
catapult_.mutable_profile()->set_maximum_acceleration(400.0);
- catapult_.mutable_profile()->set_maximum_deceleration(500.0);
- catapult_.set_unprofiled_goal(2.0, 0.0);
+ catapult_.mutable_profile()->set_maximum_deceleration(1000.0);
+ catapult_.set_unprofiled_goal(2.45, 0.0);
if (CatapultClose()) {
state_ = CatapultState::RETRACTING;
++shot_count_;
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 5363288..e873629 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -138,6 +138,10 @@
CatapultSubsystem catapult_;
+ // Max speed we have seen during this shot. This is used to figure out when
+ // we start decelerating and switch controllers.
+ double max_catapult_goal_velocity_ = 0.0;
+
PotAndAbsoluteEncoderSubsystem turret_;
PotAndAbsoluteEncoderSubsystem altitude_;
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 5727546..4ab5966 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -162,7 +162,7 @@
// considered ready to accept note from the transfer rollers. If disable
// extend is triggered, this will autoatically be false.
const bool extend_at_retracted =
- (!robot_constants_->common()->disable_extend() &&
+ (!robot_constants_->robot()->disable_extend() &&
PositionNear(extend_.position(), extend_set_points->retracted(),
kExtendThreshold));
@@ -532,7 +532,7 @@
const bool collided = collision_avoidance_.IsCollided({
.intake_pivot_position = intake_pivot_.estimated_position(),
.turret_position = shooter_.turret().estimated_position(),
- .extend_position = ((!robot_constants_->common()->disable_extend())
+ .extend_position = ((!robot_constants_->robot()->disable_extend())
? extend_.estimated_position()
: 0.0),
});
@@ -642,10 +642,15 @@
status->fbb());
// Zero out extend voltage if "disable_extend" is true
- if (robot_constants_->common()->disable_extend()) {
+ if (robot_constants_->robot()->disable_extend()) {
output_struct.extend_voltage = 0.0;
}
+ // Zero out climber voltage if "disable_climber" is true
+ if (robot_constants_->robot()->disable_climber()) {
+ output_struct.climber_voltage = 0.0;
+ }
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 9dad4ec..52c9a6c 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1391,11 +1391,11 @@
EXPECT_NEAR(
-M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_NEAR(-M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position() -
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_EQ(
@@ -1437,11 +1437,11 @@
EXPECT_NEAR(
M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_NEAR(M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position() +
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index 4a8d414..293e57e 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -51,6 +51,7 @@
const ButtonLocation kCatapultLoad(2, 1);
const ButtonLocation kAmp(2, 4);
const ButtonLocation kFire(2, 8);
+const ButtonLocation kDriverFire(1, 1);
const ButtonLocation kTrap(2, 6);
const ButtonLocation kAutoAim(1, 8);
const ButtonLocation kAimSpeaker(2, 11);
@@ -159,7 +160,8 @@
->shooter_speaker_set_point()
->turret_position());
}
- superstructure_goal_builder->set_fire(data.IsPressed(kFire));
+ superstructure_goal_builder->set_fire(data.IsPressed(kFire) ||
+ data.IsPressed(kDriverFire));
if (data.IsPressed(kRetractClimber)) {
superstructure_goal_builder->set_climber_goal(
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index b8e982e..daef22c 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -11,7 +11,7 @@
DEFINE_double(max_pose_error, 1e-5,
"Throw out target poses with a higher pose error than this");
-DEFINE_double(max_distortion, 0.1, "");
+DEFINE_double(max_distortion, 1000.0, "");
DEFINE_double(
max_pose_error_ratio, 0.4,
"Throw out target poses with a higher pose error ratio than this");
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index 4904554..bd4fe76 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -167,3 +167,20 @@
"@org_tuxfamily_eigen//:eigen",
],
)
+
+cc_binary(
+ name = "image_replay",
+ srcs = [
+ "image_replay.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024:__subpackages__"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
diff --git a/y2024/vision/image_replay.cc b/y2024/vision/image_replay.cc
new file mode 100644
index 0000000..f03bcf1
--- /dev/null
+++ b/y2024/vision/image_replay.cc
@@ -0,0 +1,47 @@
+#include "gflags/gflags.h"
+#include "opencv2/imgproc.hpp"
+#include <opencv2/highgui.hpp>
+
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_string(node, "orin1", "The node to view the log from");
+DEFINE_string(channel, "/camera0", "The channel to view the log from");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *node = factory.GetNodeEventLoopFactory(FLAGS_node);
+
+ std::unique_ptr<aos::EventLoop> image_loop = node->MakeEventLoop("image");
+ image_loop->MakeWatcher(
+ "/" + FLAGS_node + "/" + FLAGS_channel,
+ [](const frc971::vision::CameraImage &msg) {
+ cv::Mat color_image(cv::Size(msg.cols(), msg.rows()), CV_8UC2,
+ (void *)msg.data()->data());
+
+ cv::Mat bgr(color_image.size(), CV_8UC3);
+ cv::cvtColor(color_image, bgr, cv::COLOR_YUV2BGR_YUYV);
+
+ cv::imshow("Replay", bgr);
+ cv::waitKey(1);
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 6b2acc4..301ab79 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -53,6 +53,7 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024/constants.h"
#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/led_indicator.h"
#include "y2024/control_loops/superstructure/superstructure_can_position_static.h"
#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
@@ -493,7 +494,7 @@
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
std::shared_ptr<TalonFX> extend =
- (robot_constants->common()->disable_extend())
+ (robot_constants->robot()->disable_extend())
? nullptr
: std::make_shared<TalonFX>(
12, false, "Drivetrain Bus", &canivore_signal_registry,
@@ -714,7 +715,7 @@
can_superstructure_writer.add_talonfx("catapult_two", catapult_two);
can_superstructure_writer.add_talonfx("turret", turret);
can_superstructure_writer.add_talonfx("climber", climber);
- if (!robot_constants->common()->disable_extend()) {
+ if (!robot_constants->robot()->disable_extend()) {
can_superstructure_writer.add_talonfx("extend", extend);
}
can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
@@ -731,6 +732,14 @@
AddLoop(&can_output_event_loop);
+ // Thread 6
+ // Setup led_indicator
+ ::aos::ShmEventLoop led_indicator_event_loop(&config.message());
+ led_indicator_event_loop.set_name("LedIndicator");
+ control_loops::superstructure::LedIndicator led_indicator(
+ &led_indicator_event_loop);
+ AddLoop(&led_indicator_event_loop);
+
RunLoops();
}
};
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index ed43e10..2680856 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -163,7 +163,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "imu",
"logger": "NOT_LOGGED",
- "frequency": 52,
+ "frequency": 100,
"num_senders": 2,
"max_size": 200
},