Merge changes Ie0e8c64c,Icd321c1a
* changes:
Add tests that currents have sane responses
Add initial velocity MPC
diff --git a/frc971/can_logger/BUILD b/frc971/can_logger/BUILD
index f72f085..b784540 100644
--- a/frc971/can_logger/BUILD
+++ b/frc971/can_logger/BUILD
@@ -11,6 +11,7 @@
"//aos:init",
"//aos/events:shm_event_loop",
"//aos/time",
+ "@com_google_absl//absl/flags:flag",
"@com_google_absl//absl/log",
"@com_google_absl//absl/log:check",
],
diff --git a/frc971/can_logger/can_logger_main.cc b/frc971/can_logger/can_logger_main.cc
index b8f3f3f..8a2ca2a 100644
--- a/frc971/can_logger/can_logger_main.cc
+++ b/frc971/can_logger/can_logger_main.cc
@@ -1,7 +1,11 @@
+#include "absl/flags/flag.h"
+
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/can_logger/can_logger.h"
+ABSL_FLAG(std::string, interface_name, "can0", "Can interface to use");
+
int main(int argc, char **argv) {
::aos::InitGoogle(&argc, &argv);
@@ -10,7 +14,8 @@
::aos::ShmEventLoop event_loop(&config.message());
- frc971::can_logger::CanLogger can_logger(&event_loop, "/can", "can0");
+ frc971::can_logger::CanLogger can_logger(&event_loop, "/can",
+ absl::GetFlag(FLAGS_interface_name));
event_loop.Run();
diff --git a/frc971/wpilib/swerve/BUILD b/frc971/wpilib/swerve/BUILD
index 0b2cc1c..b060520 100644
--- a/frc971/wpilib/swerve/BUILD
+++ b/frc971/wpilib/swerve/BUILD
@@ -1,3 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
package(default_visibility = ["//visibility:public"])
cc_library(
@@ -27,7 +29,14 @@
"swerve_module.h",
],
deps = [
+ ":swerve_constants_fbs",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:talonfx",
],
)
+
+static_flatbuffer(
+ name = "swerve_constants_fbs",
+ srcs = ["swerve_constants.fbs"],
+ visibility = ["//visibility:public"],
+)
diff --git a/frc971/wpilib/swerve/swerve_constants.fbs b/frc971/wpilib/swerve/swerve_constants.fbs
new file mode 100644
index 0000000..4d93512
--- /dev/null
+++ b/frc971/wpilib/swerve/swerve_constants.fbs
@@ -0,0 +1,14 @@
+namespace frc971.wpilib.swerve;
+
+// Captures the robot-design-specific constants for a swerve module.
+// I.e., gear ratios and the such but not zeroing constants that vary by
+// instance of the design.
+table SwervePositionConstants {
+ // Scales between the relative and absolute encoders on the rotation
+ // module and the actual rotation of the module, in radians.
+ // module_rotation_in_radians = *_encoder_scale * encoder_reading
+ // The relative encoder reading will generally be the actual count of the
+ // encoder and the absolute encoder will generally vary between 0 and 1.
+ relative_encoder_scale:double (id: 0);
+ absolute_encoder_scale:double (id: 1);
+}
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index 1a16dce..7d91797 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -5,6 +5,7 @@
#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_position_static.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/swerve/swerve_constants_static.h"
#include "frc971/wpilib/talonfx.h"
namespace frc971::wpilib::swerve {
@@ -51,11 +52,14 @@
// Populates the Position message with the mag encoder values.
void PopulatePosition(
- frc971::control_loops::swerve::SwerveModulePositionStatic *fbs) {
+ frc971::control_loops::swerve::SwerveModulePositionStatic *fbs,
+ const SwervePositionConstants *constants) {
auto rotation_position = fbs->add_rotation_position();
- rotation_position->set_encoder(rotation_encoder.ReadRelativeEncoder());
+ rotation_position->set_encoder(rotation_encoder.ReadRelativeEncoder() *
+ constants->relative_encoder_scale());
rotation_position->set_absolute_encoder(
- rotation_encoder.ReadAbsoluteEncoder());
+ rotation_encoder.ReadAbsoluteEncoder() *
+ constants->absolute_encoder_scale());
}
// Populates a CAN-position message with the CAN-based devices (currently,
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index acc300d..2570141 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -17,6 +17,7 @@
data = [
":aos_config",
":swerve_publisher_output_json",
+ "//y2024_swerve/constants:constants.json",
"@ctre_phoenix6_api_cpp_athena//:shared_libraries",
"@ctre_phoenix6_tools_athena//:shared_libraries",
"@ctre_phoenix_api_cpp_athena//:shared_libraries",
@@ -156,6 +157,7 @@
":constants",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_can_position_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
@@ -164,6 +166,7 @@
"//frc971/wpilib:talonfx",
"//frc971/wpilib:wpilib_robot_base",
"//frc971/wpilib/swerve:swerve_drivetrain_writer",
+ "//y2024_swerve/constants:constants_fbs",
],
)
@@ -192,12 +195,16 @@
src = "y2024_swerve_roborio.json",
flatbuffers = [
"//frc971:can_configuration_fbs",
+ "//frc971/queues:gyro_fbs",
"//aos/network:remote_message_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_goal_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_status_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
+ "//y2024_swerve/constants:constants_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_can_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/can_logger:can_logging_fbs",
diff --git a/y2024_swerve/constants/BUILD b/y2024_swerve/constants/BUILD
index 416b10c..6040e5a 100644
--- a/y2024_swerve/constants/BUILD
+++ b/y2024_swerve/constants/BUILD
@@ -56,6 +56,7 @@
"//frc971/control_loops/drivetrain:drivetrain_config_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
+ "//frc971/wpilib/swerve:swerve_constants_fbs",
"//frc971/zeroing:constants_fbs",
],
)
diff --git a/y2024_swerve/constants/common.json b/y2024_swerve/constants/common.json
index 48a4afa..393b7b9 100644
--- a/y2024_swerve/constants/common.json
+++ b/y2024_swerve/constants/common.json
@@ -1,3 +1,7 @@
"common": {
-
-}
\ No newline at end of file
+ "swerve_positions_constants": {
+ {% set pi = 3.141592653589793 %}
+ "relative_encoder_scale": {{ 2.0 * pi / 4096 }},
+ "absolute_encoder_scale": {{ 2.0 * pi }}
+ }
+}
diff --git a/y2024_swerve/constants/constants.fbs b/y2024_swerve/constants/constants.fbs
index a621f3b..88bddf1 100644
--- a/y2024_swerve/constants/constants.fbs
+++ b/y2024_swerve/constants/constants.fbs
@@ -1,5 +1,6 @@
include "frc971/zeroing/constants.fbs";
include "frc971/vision/calibration.fbs";
+include "frc971/wpilib/swerve/swerve_constants.fbs";
namespace y2024_swerve;
@@ -8,7 +9,7 @@
}
table Common {
-
+ swerve_positions_constants:frc971.wpilib.swerve.SwervePositionConstants (id: 0);
}
table RobotConstants {
diff --git a/y2024_swerve/wpilib_interface.cc b/y2024_swerve/wpilib_interface.cc
index e6863b9..3d370d0 100644
--- a/y2024_swerve/wpilib_interface.cc
+++ b/y2024_swerve/wpilib_interface.cc
@@ -4,6 +4,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_can_position_static.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_position_static.h"
@@ -13,6 +14,7 @@
#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024_swerve/constants.h"
+#include "y2024_swerve/constants/constants_generated.h"
ABSL_FLAG(bool, ctre_diag_server, false,
"If true, enable the diagnostics server for interacting with "
@@ -45,9 +47,11 @@
public:
SensorReader(aos::ShmEventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
+ const Constants *robot_constants,
frc971::wpilib::swerve::SwerveModules modules)
: ::frc971::wpilib::SensorReader(event_loop),
values_(values),
+ robot_constants_(robot_constants),
drivetrain_position_sender_(
event_loop
->MakeSender<frc971::control_loops::swerve::PositionStatic>(
@@ -61,10 +65,17 @@
{
auto builder = drivetrain_position_sender_.MakeStaticBuilder();
- modules_.front_left->PopulatePosition(builder->add_front_left());
- modules_.front_right->PopulatePosition(builder->add_front_right());
- modules_.back_left->PopulatePosition(builder->add_back_left());
- modules_.back_right->PopulatePosition(builder->add_back_right());
+ auto swerve_position_constants =
+ robot_constants_->common()->swerve_positions_constants();
+
+ modules_.front_left->PopulatePosition(builder->add_front_left(),
+ swerve_position_constants);
+ modules_.front_right->PopulatePosition(builder->add_front_right(),
+ swerve_position_constants);
+ modules_.back_left->PopulatePosition(builder->add_back_left(),
+ swerve_position_constants);
+ modules_.back_right->PopulatePosition(builder->add_back_right(),
+ swerve_position_constants);
builder.CheckOk(builder.Send());
}
@@ -102,6 +113,8 @@
private:
std::shared_ptr<const constants::Values> values_;
+ const Constants *robot_constants_;
+
aos::Sender<frc971::control_loops::swerve::PositionStatic>
drivetrain_position_sender_;
@@ -121,6 +134,18 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024_swerve::Constants>(
+ &config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+
+ const Constants *robot_constants = &constants_fetcher.constants();
+
+ AddLoop(&constant_fetcher_event_loop);
+
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
std::vector<std::shared_ptr<TalonFX>> falcons;
@@ -213,7 +238,8 @@
// Thread 3
aos::ShmEventLoop sensor_reader_event_loop(&config.message());
sensor_reader_event_loop.set_name("SensorReader");
- SensorReader sensor_reader(&sensor_reader_event_loop, values, modules);
+ SensorReader sensor_reader(&sensor_reader_event_loop, values,
+ robot_constants, modules);
sensor_reader.set_front_left_encoder(
make_encoder(3), std::make_unique<frc::DigitalInput>(3));
diff --git a/y2024_swerve/y2024_swerve_imu.json b/y2024_swerve/y2024_swerve_imu.json
index c87faf5..ad1f837 100644
--- a/y2024_swerve/y2024_swerve_imu.json
+++ b/y2024_swerve/y2024_swerve_imu.json
@@ -41,8 +41,8 @@
"type": "aos.timing.Report",
"source_node": "imu",
"frequency": 50,
- "num_senders": 20,
- "max_size": 8552
+ "num_senders": 30,
+ "max_size": 16384
},
{
"name": "/imu/aos",
@@ -57,7 +57,7 @@
"source_node": "imu",
"frequency": 50,
"num_senders": 20,
- "max_size": 4608
+ "max_size": 6912
},
{
"name": "/imu/aos",
@@ -98,6 +98,7 @@
"roborio"
],
"max_size": 400,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
"destination_nodes": [
{
"name": "orin1",
@@ -220,7 +221,7 @@
"max_size": 200
},
{
- "name": "/imu/can/cana",
+ "name": "/imu/can",
"type": "frc971.can_logger.CanFrame",
"source_node": "imu",
"frequency": 9000,
@@ -326,6 +327,7 @@
"name": "imu_can_logger",
"executable_name": "can_logger",
"args": [
+ "--interface_name=cana",
"--priority=59",
"--affinity=5"
],
@@ -337,7 +339,7 @@
"name": "can_translator",
"executable_name": "can_translator",
"args": [
- "--channel=/can/cana"
+ "--channel=/can"
],
"nodes": [
"imu"
@@ -551,6 +553,7 @@
"hostname": "orin2",
"hostnames": [
"orin-971-2",
+ "orin-6971-2",
"orin-7971-2",
"orin-8971-2",
"orin-9971-2"
diff --git a/y2024_swerve/y2024_swerve_orin1.json b/y2024_swerve/y2024_swerve_orin1.json
index 64c015f..6615a03 100644
--- a/y2024_swerve/y2024_swerve_orin1.json
+++ b/y2024_swerve/y2024_swerve_orin1.json
@@ -27,7 +27,7 @@
"source_node": "orin1",
"frequency": 50,
"num_senders": 20,
- "max_size": 4096
+ "max_size": 6144
},
{
"name": "/orin1/aos",
@@ -389,25 +389,6 @@
"orin1"
]
},
- {
- "name": "image_streamer",
- "executable_name": "image_streamer",
- "args": [
- "--device=/dev/uvcvideo",
- "--height=480",
- "--width=640",
- "--nopublish_images",
- "--exposure=0",
- "--framerate=30",
- "--streaming_port=1181",
- "--bitrate=2000000",
- "--data_dir=/home/pi/bin/image_streamer_www"
- ],
- "user": "pi",
- "nodes": [
- "orin1"
- ]
- }
],
"maps": [
{
@@ -453,6 +434,7 @@
"hostname": "orin1",
"hostnames": [
"orin-971-1",
+ "orin-6971-1",
"orin-7971-1",
"orin-8971-1",
"orin-9971-1"
diff --git a/y2024_swerve/y2024_swerve_roborio.json b/y2024_swerve/y2024_swerve_roborio.json
index e5e40dd..705370d 100644
--- a/y2024_swerve/y2024_swerve_roborio.json
+++ b/y2024_swerve/y2024_swerve_roborio.json
@@ -26,10 +26,17 @@
"max_size": 8192
},
{
+ "name": "/roborio/drivetrain",
+ "type": "frc971.sensors.GyroReading",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2
+ },
+ {
"name": "/roborio/aos",
"type": "aos.RobotState",
"source_node": "roborio",
- "frequency": 50,
+ "frequency": 250,
"num_senders": 20,
"max_size": 192
},
@@ -47,7 +54,7 @@
"source_node": "roborio",
"frequency": 50,
"num_senders": 20,
- "max_size": 3000
+ "max_size": 8192
},
{
"name": "/roborio/aos",
@@ -135,6 +142,19 @@
},
{
"name": "/roborio/drivetrain",
+ "type": "frc971.control_loops.swerve.Goal",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/drivetrain",
+ "type": "frc971.control_loops.swerve.Status",
+ "source_node": "roborio",
+ "max_size": 2056,
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/drivetrain",
"type": "frc971.control_loops.swerve.Position",
"source_node": "roborio",
"frequency": 250,
@@ -270,6 +290,14 @@
"type": "frc971.CANConfiguration",
"source_node": "roborio",
"frequency": 2
+ },
+ {
+ "name": "/roborio/constants",
+ "type": "y2024_swerve.Constants",
+ "source_node": "roborio",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 65536
}
],
"applications": [
@@ -293,6 +321,14 @@
]
},
{
+ "name": "constants_sender_roborio",
+ "executable_name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
"name": "roborio_web_proxy",
"executable_name": "web_proxy_main",
"args": [
@@ -338,8 +374,15 @@
]
},
{
+ "name": "swerve_control_loops",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
"name": "can_logger",
"executable_name": "can_logger",
+ "autostart": false,
"nodes": [
"roborio"
]
@@ -381,6 +424,15 @@
"rename": {
"name": "/roborio/drivetrain"
}
+ },
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/constants"
+ }
}
],
"nodes": [