Add setpoint setter
Mechanical really likes being able to tune shots easily... This is
going to live for a bit.
Change-Id: I1c6ba3ffab2f50647af6fbbcbc64cfd5a913a452
diff --git a/y2020/BUILD b/y2020/BUILD
index 4aa851f..bfa6ce2 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -4,6 +4,9 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
+ binaries = [
+ ":setpoint_setter",
+ ],
data = [
":config.json",
],
@@ -114,6 +117,7 @@
":joystick_reader.cc",
],
deps = [
+ ":setpoint_fbs",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -134,6 +138,7 @@
name = "config",
src = "y2020.json",
flatbuffers = [
+ ":setpoint_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
@@ -173,3 +178,23 @@
"//y2020/www:main_bundle",
],
)
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+flatbuffer_cc_library(
+ name = "setpoint_fbs",
+ srcs = [
+ "setpoint.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+cc_binary(
+ name = "setpoint_setter",
+ srcs = ["setpoint_setter.cc"],
+ deps = [
+ ":setpoint_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 026a50b..2049e83 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -13,10 +13,11 @@
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/setpoint_generated.h"
using aos::input::driver_station::ButtonLocation;
using aos::input::driver_station::ControlBit;
@@ -57,8 +58,9 @@
superstructure_goal_sender_(
event_loop->MakeSender<superstructure::Goal>("/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<superstructure::Status>(
- "/superstructure")) {}
+ event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+ setpoint_fetcher_(event_loop->MakeFetcher<y2020::joysticks::Setpoint>(
+ "/superstructure")) {}
void AutoEnded() override {
AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
@@ -71,6 +73,8 @@
return;
}
+ setpoint_fetcher_.Fetch();
+
double hood_pos = constants::Values::kHoodRange().middle();
double intake_pos = -0.89;
double turret_pos = 0.0;
@@ -84,15 +88,26 @@
}
if (data.IsPressed(kHood)) {
- hood_pos = 0.05;
+ hood_pos = 0.45;
+ } else {
+ if (setpoint_fetcher_.get()) {
+ hood_pos = setpoint_fetcher_->hood();
+ } else {
+ hood_pos = 0.58;
+ }
}
if (data.IsPressed(kShootFast)) {
- accelerator_speed = 500.0;
- finisher_speed = 500.0;
+ if (setpoint_fetcher_.get()) {
+ accelerator_speed = setpoint_fetcher_->accelerator();
+ finisher_speed = setpoint_fetcher_->finisher();
+ } else {
+ accelerator_speed = 250.0;
+ finisher_speed = 500.0;
+ }
} else if (data.IsPressed(kShootSlow)) {
- accelerator_speed = 30.0;
- finisher_speed = 30.0;
+ accelerator_speed = 180.0;
+ finisher_speed = 375.0;
}
if (data.IsPressed(kIntakeExtend)) {
@@ -162,6 +177,8 @@
::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+
+ ::aos::Fetcher<y2020::joysticks::Setpoint> setpoint_fetcher_;
};
} // namespace joysticks
diff --git a/y2020/setpoint.fbs b/y2020/setpoint.fbs
new file mode 100644
index 0000000..5f1af72
--- /dev/null
+++ b/y2020/setpoint.fbs
@@ -0,0 +1,11 @@
+namespace y2020.joysticks;
+
+table Setpoint {
+ accelerator:double;
+
+ finisher:double;
+
+ hood:double;
+}
+
+root_type Setpoint;
diff --git a/y2020/setpoint_setter.cc b/y2020/setpoint_setter.cc
new file mode 100644
index 0000000..1cbc518
--- /dev/null
+++ b/y2020/setpoint_setter.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "gflags/gflags.h"
+#include "y2020/setpoint_generated.h"
+
+DEFINE_double(accelerator, 250.0, "Accelerator speed");
+DEFINE_double(finisher, 500.0, "Finsher speed");
+DEFINE_double(hood, 0.45, "Hood setpoint");
+
+int main(int argc, char ** argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::InitNRT();
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ ::aos::Sender<y2020::joysticks::Setpoint> setpoint_sender =
+ event_loop.MakeSender<y2020::joysticks::Setpoint>("/superstructure");
+
+ aos::Sender<y2020::joysticks::Setpoint>::Builder builder =
+ setpoint_sender.MakeBuilder();
+
+ y2020::joysticks::Setpoint::Builder setpoint_builder =
+ builder.MakeBuilder<y2020::joysticks::Setpoint>();
+
+ setpoint_builder.add_accelerator(FLAGS_accelerator);
+ setpoint_builder.add_finisher(FLAGS_finisher);
+ setpoint_builder.add_hood(FLAGS_hood);
+ builder.Send(setpoint_builder.Finish());
+
+ aos::Cleanup();
+}
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 3d966b8..5a0e1b7 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -248,6 +248,12 @@
"num_senders": 2
},
{
+ "name": "/superstructure",
+ "type": "y2020.joysticks.Setpoint",
+ "source_node": "roborio",
+ "num_senders": 2
+ },
+ {
"name": "/drivetrain",
"type": "frc971.IMUValues",
"source_node": "roborio",