Add wpilib_interface code for shooter tuning
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I8a89e0793ecfc61ccd7c9f7018d901053f190eb6
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/y2020/BUILD b/y2020/BUILD
index 19debdc..49e3c7b 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -116,6 +116,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
],
@@ -230,6 +231,7 @@
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2020/control_loops/drivetrain:localizer_debug_fbs",
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index 0694319..4894b16 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -1,5 +1,7 @@
package(default_visibility = ["//visibility:public"])
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
cc_library(
name = "shooter_plants",
target_compatible_with = ["@platforms//os:linux"],
@@ -46,3 +48,12 @@
"//y2020/control_loops/superstructure/finisher:finisher_plants",
],
)
+
+flatbuffer_cc_library(
+ name = "shooter_tuning_readings_fbs",
+ srcs = [
+ "shooter_tuning_readings.fbs",
+ ],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2020/control_loops/superstructure/shooter/shooter_tuning_readings.fbs b/y2020/control_loops/superstructure/shooter/shooter_tuning_readings.fbs
new file mode 100644
index 0000000..ca10302
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter_tuning_readings.fbs
@@ -0,0 +1,14 @@
+namespace y2020.control_loops.superstructure.shooter;
+
+// Contains the readings used to tune the accelerator
+// and finisher velocities to minimize variation in ball velocity.
+// We have two sensors, and find the time that the ball takes to
+// pass between the two to find its velocity.
+// This will be sent each time a ball is detected.
+table TuningReadings {
+ // The velocity (m/s) of the last ball passing between the two
+ // sensors
+ velocity_ball:double (id: 0);
+}
+
+root_type TuningReadings;
\ No newline at end of file
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index b7cf7e1..c79ba88 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -48,10 +48,16 @@
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
+#include "gflags/gflags.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_readings_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
+DEFINE_bool(shooter_tuning, true,
+ "If true, reads from ball beambreak sensors and sends shooter "
+ "tuning readings");
+
using ::aos::monotonic_clock;
using ::y2020::constants::Values;
namespace superstructure = ::y2020::control_loops::superstructure;
@@ -119,7 +125,10 @@
drivetrain_position_sender_(
event_loop
->MakeSender<::frc971::control_loops::drivetrain::Position>(
- "/drivetrain")) {
+ "/drivetrain")),
+ shooter_tuning_readings_sender_(
+ event_loop->MakeSender<superstructure::shooter::TuningReadings>(
+ "/superstructure")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -194,6 +203,20 @@
void set_imu(frc971::wpilib::ADIS16470 *imu) { imu_ = imu; }
+ void set_ball_beambreak_inputs(::std::unique_ptr<frc::DigitalInput> sensor1,
+ ::std::unique_ptr<frc::DigitalInput> sensor2) {
+ ball_beambreak_inputs_[0] = ::std::move(sensor1);
+ ball_beambreak_inputs_[1] = ::std::move(sensor2);
+ ball_beambreak_reader_.set_input_one(sensor1.get());
+ ball_beambreak_reader_.set_input_two(sensor2.get());
+ }
+
+ void Start() override {
+ if (FLAGS_shooter_tuning) {
+ AddToDMA(&ball_beambreak_reader_);
+ }
+ }
+
void RunIteration() override {
CHECK_NOTNULL(imu_)->DoReads();
@@ -292,6 +315,21 @@
builder.Send(auto_mode_builder.Finish());
}
+
+ if (FLAGS_shooter_tuning &&
+ ball_beambreak_reader_.pulses_detected() > balls_detected_) {
+ balls_detected_ = ball_beambreak_reader_.pulses_detected();
+
+ // Distance between beambreak sensors, in meters.
+ constexpr double kDistanceBetweenBeambreaks = 0.4813;
+
+ auto builder = shooter_tuning_readings_sender_.MakeBuilder();
+ auto shooter_tuning_readings_builder =
+ builder.MakeBuilder<superstructure::shooter::TuningReadings>();
+ shooter_tuning_readings_builder.add_velocity_ball(
+ kDistanceBetweenBeambreaks / ball_beambreak_reader_.last_width());
+ builder.Send(shooter_tuning_readings_builder.Finish());
+ }
}
private:
@@ -299,6 +337,8 @@
::aos::Sender<superstructure::Position> superstructure_position_sender_;
::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
+ ::aos::Sender<superstructure::shooter::TuningReadings>
+ shooter_tuning_readings_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer turret_encoder_;
@@ -311,6 +351,14 @@
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
frc971::wpilib::ADIS16470 *imu_ = nullptr;
+
+ // Used to interface with the two beam break sensors that the ball for tuning
+ // shooter parameters has to pass through.
+ // We will time how long it takes to pass between the two sensors to get its
+ // velocity.
+ std::array<std::unique_ptr<frc::DigitalInput>, 2> ball_beambreak_inputs_;
+ frc971::wpilib::DMAPulseSeparationReader ball_beambreak_reader_;
+ int balls_detected_ = 0;
};
class SuperstructureWriter
@@ -518,6 +566,11 @@
sensor_reader.set_left_accelerator_encoder(make_encoder(4));
sensor_reader.set_right_accelerator_encoder(make_encoder(3));
+ if (FLAGS_shooter_tuning) {
+ sensor_reader.set_ball_beambreak_inputs(
+ make_unique<frc::DigitalInput>(6), make_unique<frc::DigitalInput>(7));
+ }
+
// Note: If ADIS16470 is plugged in directly to the roboRIO SPI port without
// the Spartan Board, then trigger is on 26, reset 27, and chip select is
// CS0.
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index db5da14..143a5fe 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -190,6 +190,14 @@
},
{
"name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.shooter.TuningReadings",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 64
+ },
+ {
+ "name": "/superstructure",
"type": "y2020.joysticks.Setpoint",
"source_node": "roborio",
"num_senders": 2