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/BUILD b/y2020/BUILD
index 49e3c7b..dfe09f9 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -231,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_params_fbs",
"//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
diff --git a/y2020/actors/BUILD b/y2020/actors/BUILD
index 4ec7b08..3555531 100644
--- a/y2020/actors/BUILD
+++ b/y2020/actors/BUILD
@@ -77,3 +77,22 @@
"//frc971/autonomous:auto_fbs",
],
)
+
+cc_binary(
+ name = "shooter_tuning_action",
+ srcs = [
+ "shooter_tuning_actor.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:shm_event_loop",
+ "//aos:init",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_params_fbs",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ ],
+)
diff --git a/y2020/actors/shooter_tuning_actor.cc b/y2020/actors/shooter_tuning_actor.cc
new file mode 100644
index 0000000..03d169c
--- /dev/null
+++ b/y2020/actors/shooter_tuning_actor.cc
@@ -0,0 +1,169 @@
+#include <algorithm>
+#include <cstdlib>
+#include <ctime>
+#include <fstream>
+#include <sstream>
+#include <utility>
+
+#include "aos/init.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "glog/logging.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_params_generated.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_readings_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+
+namespace y2020::actors {
+
+namespace superstructure = y2020::control_loops::superstructure;
+
+// Actor for tuning the shooter flywheel velocities.
+// We found that the shooter is most precise when the ball velocity varies the
+// least. To figure out which finisher and accelerator velocities cause the ball
+// velocity to vary the least, this does a sweep of a range of those velocities
+// in randomized order, and shoots balls at each set of velocities. It also
+// fetches the ball velocity at each iteration, which we will use to calculate
+// each iteration's deviation in ball velocity and figure out which flywheel
+// velocities are the best with the csv of velocities that this outputs.
+class ShooterTuningActor : public frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit ShooterTuningActor(aos::EventLoop *event_loop);
+
+ bool RunAction(
+ const frc971::autonomous::AutonomousActionParams *params) override;
+
+ private:
+ void SendSuperstructureGoal();
+ void WaitAndWriteBallData();
+
+ aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+ aos::Fetcher<superstructure::shooter::TuningReadings>
+ tuning_readings_fetcher_;
+ const superstructure::shooter::TuningParams *tuning_params_;
+
+ // Vector of (velocity_accelerator, velocity finisher) containing all velocity
+ // values in the sweep in randomized order.
+ std::vector<std::pair<double, double>> velocities_;
+
+ bool shooting_ = false;
+ double velocity_finisher_ = 0.0;
+ double velocity_accelerator_ = 0.0;
+
+ std::ofstream fout_;
+};
+
+ShooterTuningActor::ShooterTuningActor(aos::EventLoop *event_loop)
+ : frc971::autonomous::BaseAutonomousActor(
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<superstructure::Goal>("/superstructure")),
+ tuning_readings_fetcher_(
+ event_loop->MakeFetcher<superstructure::shooter::TuningReadings>(
+ "/superstructure")) {
+ auto tuning_params_fetcher =
+ event_loop->MakeFetcher<superstructure::shooter::TuningParams>(
+ "/superstructure");
+ CHECK(tuning_params_fetcher.Fetch());
+ tuning_params_ = tuning_params_fetcher.get();
+ const auto finisher = tuning_params_->finisher();
+ const auto accelerator = tuning_params_->accelerator();
+
+ // Add the velocities for the sweep
+ for (velocity_finisher_ = finisher->velocity_initial();
+ velocity_finisher_ <= finisher->velocity_final();
+ velocity_finisher_ += finisher->velocity_increment()) {
+ for (velocity_accelerator_ = accelerator->velocity_initial();
+ velocity_accelerator_ <= accelerator->velocity_final();
+ velocity_accelerator_ += accelerator->velocity_increment()) {
+ for (int i = 0; i < tuning_params_->balls_per_iteration(); i++) {
+ velocities_.emplace_back(velocity_accelerator_, velocity_finisher_);
+ }
+ }
+ }
+ // Randomize the ordering of the velocities
+ std::srand(std::time(nullptr));
+ std::random_shuffle(velocities_.begin(), velocities_.end());
+
+ fout_.open("shooter_tuning_data.csv", std::ios_base::app);
+}
+
+bool ShooterTuningActor::RunAction(
+ const frc971::autonomous::AutonomousActionParams * /*params*/) {
+ LOG(INFO) << "velocity_accelerator,velocity_finisher,velocity_ball";
+
+ shooting_ = true;
+ for (const auto &velocity_pair : velocities_) {
+ velocity_accelerator_ = velocity_pair.first;
+ velocity_finisher_ = velocity_pair.second;
+ SendSuperstructureGoal();
+ WaitAndWriteBallData();
+ }
+
+ shooting_ = false;
+ velocity_finisher_ = 0.0;
+ velocity_accelerator_ = 0.0;
+ SendSuperstructureGoal();
+ return true;
+}
+
+void ShooterTuningActor::WaitAndWriteBallData() {
+ aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ frc971::controls::kLoopFrequency / 2);
+ bool ball_detected = false;
+
+ while (!ball_detected) {
+ phased_loop.SleepUntilNext();
+
+ if (tuning_readings_fetcher_.FetchNext()) {
+ ball_detected = true;
+ std::stringstream output;
+ output << velocity_accelerator_ << ',' << velocity_finisher_ << ','
+ << tuning_readings_fetcher_->velocity_ball() << std::endl;
+ const auto output_str = output.str();
+ LOG(INFO) << output_str;
+ fout_ << output_str;
+ }
+ }
+}
+
+void ShooterTuningActor::SendSuperstructureGoal() {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ auto shooter_builder = builder.MakeBuilder<superstructure::ShooterGoal>();
+ shooter_builder.add_velocity_finisher(velocity_finisher_);
+ shooter_builder.add_velocity_accelerator(velocity_accelerator_);
+ auto shooter_offset = shooter_builder.Finish();
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_shooter(shooter_offset);
+ superstructure_builder.add_shooting(shooting_);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+}
+
+} // namespace y2020::actors
+
+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());
+ y2020::actors::ShooterTuningActor actor(&event_loop);
+
+ event_loop.OnRun([&]() {
+ actor.RunAction(nullptr);
+ LOG(INFO) << "Finished shooter tuning action";
+ event_loop.Exit();
+ });
+ event_loop.Run();
+
+ return 0;
+}
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;
+}
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 143a5fe..67916b6 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -198,6 +198,14 @@
},
{
"name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.shooter.TuningParams",
+ "source_node": "roborio",
+ "frequency": 5,
+ "num_senders": 2,
+ "max_size": 256
+ },
+ {
+ "name": "/superstructure",
"type": "y2020.joysticks.Setpoint",
"source_node": "roborio",
"num_senders": 2