Create an auto mode for tuning shooter velocities
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Iea8f0119832460b500cdfca786d6053e0202d11b
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index 4894b16..d3b055c 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -57,3 +57,26 @@
gen_reflections = 1,
target_compatible_with = ["@platforms//os:linux"],
)
+
+flatbuffer_cc_library(
+ name = "shooter_tuning_params_fbs",
+ srcs = [
+ "shooter_tuning_params.fbs",
+ ],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+cc_binary(
+ name = "shooter_tuning_params_setter",
+ srcs = ["shooter_tuning_params_setter.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":shooter_tuning_params_fbs",
+ ":shooter_tuning_readings_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/shooter_tuning_params.fbs b/y2020/control_loops/superstructure/shooter/shooter_tuning_params.fbs
new file mode 100644
index 0000000..7fe2dc7
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter_tuning_params.fbs
@@ -0,0 +1,26 @@
+namespace y2020.control_loops.superstructure.shooter;
+
+// Parameters for tuning a flywheel's velocity.
+// Using this for the parameters of the accelerators and the finisher.
+table FlywheelTuningParams {
+ // Shoot balls at
+ // velocity_initial, velocity_initial + velocity_increment, ...,
+ // up to velocity_final, randomizing the order of these velocities.
+ velocity_initial:double (id: 0);
+ velocity_final:double (id: 1);
+ velocity_increment:double (id: 2);
+}
+
+// Parameters for the auto mode that tunes the shooter
+table TuningParams {
+ // Parameters for the flywheels
+ accelerator:FlywheelTuningParams (id: 0);
+ finisher:FlywheelTuningParams (id: 1);
+
+ // Number of balls to shooter at each iteration in
+ // the sweep of all possible accelerator and finisher
+ // velocities.
+ balls_per_iteration:int (id: 2);
+}
+
+root_type TuningParams;
\ No newline at end of file
diff --git a/y2020/control_loops/superstructure/shooter/shooter_tuning_params_setter.cc b/y2020/control_loops/superstructure/shooter/shooter_tuning_params_setter.cc
new file mode 100644
index 0000000..3c6158e
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter_tuning_params_setter.cc
@@ -0,0 +1,58 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_params_generated.h"
+
+DEFINE_double(velocity_initial_finisher, 300.0, "Initial finisher velocity");
+DEFINE_double(velocity_final_finisher, 500.0, "Final finisher velocity");
+DEFINE_double(velocity_finisher_increment, 25.0, "Finisher velocity increment");
+
+DEFINE_double(velocity_initial_accelerator, 180.0,
+ "Initial accelerator velocity");
+DEFINE_double(velocity_final_accelerator, 250.0, "Final accelerator velocity");
+DEFINE_double(velocity_accelerator_increment, 20.0,
+ "Accelerator velocity increment");
+
+DEFINE_int32(balls_per_iteration, 10,
+ "Balls to shoot per iteration in the velocity sweep");
+
+namespace shooter = y2020::control_loops::superstructure::shooter;
+
+flatbuffers::Offset<shooter::FlywheelTuningParams> BuildFlywheelTuningParams(
+ aos::Sender<shooter::TuningParams>::Builder &builder,
+ double velocity_initial, double velocity_final, double velocity_increment) {
+ auto flywheel_tuning_params_builder =
+ builder.MakeBuilder<shooter::FlywheelTuningParams>();
+ flywheel_tuning_params_builder.add_velocity_initial(velocity_initial);
+ flywheel_tuning_params_builder.add_velocity_final(velocity_final);
+ flywheel_tuning_params_builder.add_velocity_increment(velocity_increment);
+ return flywheel_tuning_params_builder.Finish();
+}
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ auto sender = event_loop.MakeSender<shooter::TuningParams>("/superstructure");
+ auto builder = sender.MakeBuilder();
+
+ auto finisher_params = BuildFlywheelTuningParams(
+ builder, FLAGS_velocity_initial_finisher, FLAGS_velocity_final_finisher,
+ FLAGS_velocity_finisher_increment);
+ auto accelerator_params = BuildFlywheelTuningParams(
+ builder, FLAGS_velocity_initial_accelerator,
+ FLAGS_velocity_final_accelerator, FLAGS_velocity_accelerator_increment);
+
+ auto tuning_params_builder = builder.MakeBuilder<shooter::TuningParams>();
+ tuning_params_builder.add_finisher(finisher_params);
+ tuning_params_builder.add_accelerator(accelerator_params);
+ tuning_params_builder.add_balls_per_iteration(FLAGS_balls_per_iteration);
+ CHECK(builder.Send(tuning_params_builder.Finish()));
+
+ return 0;
+}