Merge changes I266cdfce,Ia9866068
* changes:
Add led_indicator to wpilib_interface
Add led_indicator/candle to y2024
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/y2024/constants/common.json b/y2024/constants/common.json
index 99947ef..b387f69 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
}
}
],
@@ -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
},
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/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/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 88bf390..7002657 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -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_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/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/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
},