Merge "Upping buffer size on detailed results to 1000000"
diff --git a/frc971/analysis/plot_configs/localizer.pb b/frc971/analysis/plot_configs/localizer.pb
index 644b516..930d63d 100644
--- a/frc971/analysis/plot_configs/localizer.pb
+++ b/frc971/analysis/plot_configs/localizer.pb
@@ -185,6 +185,18 @@
line {
y_signal {
channel: "DrivetrainStatus"
+ field: "trajectory_logging.left_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "trajectory_logging.right_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
field: "localizer.left_velocity"
}
}
diff --git a/frc971/constants.h b/frc971/constants.h
index debfb55..7c2121d 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -97,9 +97,9 @@
double lower;
double upper;
- double middle() const { return (lower_hard + upper_hard) / 2.0; }
+ constexpr double middle() const { return (lower_hard + upper_hard) / 2.0; }
- double range() const { return upper_hard - lower_hard; }
+ constexpr double range() const { return upper_hard - lower_hard; }
};
} // namespace constants
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index d2c0be3..cb50165 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -146,7 +146,7 @@
static constexpr int kNOutputs = 3;
// Time constant to use for estimating how the longitudinal/lateral velocity
// offsets decay, in seconds.
- static constexpr double kVelocityOffsetTimeConstant = 10.0;
+ static constexpr double kVelocityOffsetTimeConstant = 1.0;
static constexpr double kLateralVelocityTimeConstant = 1.0;
// Inputs are [left_volts, right_volts]
typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index b28334c..051e71a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -138,7 +138,7 @@
(::Eigen::DiagonalMatrix<double, 5>().diagonal()
<< 1.0 / ::std::pow(0.12, 2),
1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
- 1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
+ 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
.finished()
.asDiagonal();
const ::Eigen::DiagonalMatrix<double, 2> R =
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 20672cf..ab8afd0 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -42,6 +42,8 @@
// position of (10, -5, 0) and a yaw of pi / 2, that suggests the robot is
// facing straight to the left from the driver's perspective and is placed 10m
// from the driver's station wall and 5m to the right of the center of the wall.
+// For 2020, we move the origin to be the center of the field and make positive
+// x always point towards the red alliance driver stations.
//
// Furthermore, Poses can be chained such that a Pose can be placed relative to
// another Pose; the other Pose can dynamically update, thus allowing us to,
@@ -134,7 +136,7 @@
// If new_base == nullptr, provides a Pose referenced to the global frame.
// Note that the lifetime of new_base should be greater than the lifetime of
// the returned object (unless new_base == nullptr).
- TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+ [[nodiscard]] TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
// Convert this pose to the heading/distance/skew numbers that we
// traditionally use for EKF corrections.
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 7b45215..e0b4878 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -11,18 +11,31 @@
args = arg_parser.parse_args()
SCREEN_SIZE = args.size
-WIDTH_OF_FIELD_IN_METERS = 8.258302
-
WIDTH_OF_ROBOT = 0.65
LENGTH_OF_ROBOT = 0.8
-ROBOT_SIDE_TO_BALL_CENTER = 0.15 #Placeholder value
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 # Placeholder value
BALL_RADIUS = 0.165
-ROBOT_SIDE_TO_HATCH_PANEL = 0.1 #Placeholder value
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 # Placeholder value
HATCH_PANEL_WIDTH = 0.4826
-def pxToM(p):
+FIELD = 2020
+
+if FIELD == 2019:
+ WIDTH_OF_FIELD_IN_METERS = 8.258302 # Half Field
+elif FIELD == 2020:
+ WIDTH_OF_FIELD_IN_METERS = 15.98295 # Full Field
+ LENGTH_OF_FIELD_IN_METERS = 8.21055 # Full Field
+
+def pxToM(p, length = False):
+ if(length):
+ return p * LENGTH_OF_FIELD_IN_METERS / (SCREEN_SIZE/2)
return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
-def mToPx(m):
+def mToPx(m, length = False):
+ if(length):
+ return (m*(SCREEN_SIZE/2)/LENGTH_OF_FIELD_IN_METERS)
return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
+
+def inToM(i):
+ return (i*0.0254)
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index 89979f0..a2ddf5d 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -3,7 +3,6 @@
from constants import *
import numpy as np
-
def set_color(cr, color, a=1):
if color.a == 1.0:
cr.set_source_rgba(color.r, color.g, color.b, a)
@@ -37,6 +36,11 @@
cr.stroke()
set_color(cr, palette["WHITE"])
+def draw_circle(cr, x, y, radius, color=palette["RED"]):
+ set_color(cr, color)
+ cr.arc(x, y, radius, 0, 2*np.pi)
+ cr.fill()
+ cr.stroke()
def draw_control_points(cr, points, width=10, radius=4, color=palette["BLUE"]):
for i in range(0, len(points)):
@@ -46,12 +50,131 @@
cr.fill()
set_color(cr, palette["WHITE"])
-
def display_text(cr, text, widtha, heighta, widthb, heightb):
cr.scale(widtha, -heighta)
cr.show_text(text)
cr.scale(widthb, -heightb)
+def markers(cr):
+ SHOW_MARKERS = False
+ if SHOW_MARKERS:
+ # Shield Generator Reference
+ color = palette["BLUE"]
+ yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+ # Top Left
+ draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True), 10, color)
+ # Top Right
+ draw_circle(cr, mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True), 10, color)
+ # Bottom Right
+ draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True), 10, color)
+ # Bottom Left
+ draw_circle(cr, mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True), 10, color)
+
+ # Trench Run Reference
+ color = palette["GREEN"]
+ # Bottom Trench Run
+ # Bottom Right
+ draw_circle(cr, mToPx(inToM(206.625)), yorigin, 10, color)
+ # Top Right
+ draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True), 10, color)
+ # Top Left
+ draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True), 10, color)
+ # Bottom Left
+ draw_circle(cr, mToPx(inToM(422.625)), yorigin, 10, color)
+
+ # Top Trench Run
+ # Top Right
+ draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(323.25), True), 10, color)
+ # Bottom Right
+ draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True), 10, color)
+ # Top Left
+ draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True), 10, color)
+ # Bottom Left
+ draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(323.25), True), 10, color)
+ cr.stroke()
+
+def draw_init_lines(cr):
+ yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+ set_color(cr, palette["RED"])
+ cr.move_to(mToPx(inToM(120)), yorigin)
+ cr.line_to(mToPx(inToM(120)), yorigin + mToPx(8.21055, True))
+
+ cr.move_to(mToPx(inToM(505.25)), yorigin)
+ cr.line_to(mToPx(inToM(505.25)), yorigin + mToPx(8.21055, True))
+
+ cr.stroke()
+
+def draw_trench_run(cr):
+ set_color(cr, palette["GREEN"])
+ yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+ cr.move_to(mToPx(inToM(206.625)), yorigin)
+ cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True))
+
+ cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True))
+ cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True))
+
+ cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True))
+ cr.line_to(mToPx(inToM(422.625)), yorigin)
+
+ cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(323.25), True))
+ cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True))
+
+ cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True))
+ cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True))
+
+ cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True))
+ cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(323.25), True))
+
+ cr.stroke()
+
+def draw_shield_generator(cr):
+ set_color(cr, palette["BLUE"])
+ yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+
+ cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True))
+ cr.line_to(mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True))
+
+ cr.move_to(mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True))
+ cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True))
+
+ cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True))
+ cr.line_to(mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True))
+
+ cr.move_to(mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True))
+ cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True))
+
+ cr.stroke()
+
+def draw_control_panel(cr): # Base plates are not included
+ set_color(cr, palette["LIGHT_GREY"])
+ yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+ # Bottom Control Panel
+ # Top Line
+ cr.move_to(mToPx(inToM(225.624)), yorigin + mToPx(inToM(55.5), True))
+ cr.line_to(mToPx(inToM(285.624)), yorigin + mToPx(inToM(55.5), True))
+
+ # Left Line
+ cr.move_to(mToPx(inToM(225.624)), yorigin + mToPx(inToM(55.5), True))
+ cr.line_to(mToPx(inToM(225.624)), yorigin)
+
+ # Right Line
+ cr.move_to(mToPx(inToM(285.624)), yorigin + mToPx(inToM(55.5), True))
+ cr.line_to(mToPx(inToM(285.624)), yorigin)
+
+ # Top Control Panel
+ # Bottom Line
+ cr.move_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(281.26), True))
+ cr.line_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(281.26), True))
+
+ # Right Line
+ cr.move_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(281.26), True))
+ cr.line_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(323.25), True))
+
+ # Left Line
+ cr.move_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(281.26), True))
+ cr.line_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(323.25), True))
+
+ cr.stroke()
def draw_HAB(cr):
# BASE Constants
@@ -307,7 +430,6 @@
mToPx(1.41605))
cr.stroke()
-
def draw_points(cr, p, size):
for i in range(0, len(p)):
draw_px_cross(cr, p[i][0], p[i][1], size,
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index d04089b..ddc38ac 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -93,9 +93,17 @@
return self.all_controls[self.get_index_of_nearest_point()]
def draw_field_elements(self, cr):
- draw_HAB(cr)
- draw_rockets(cr)
- draw_cargo_ship(cr)
+ if FIELD == 2019:
+ draw_HAB(cr)
+ draw_rockets(cr)
+ draw_cargo_ship(cr)
+ elif FIELD == 2020:
+ set_color(cr, palette["BLACK"])
+ markers(cr)
+ draw_shield_generator(cr)
+ draw_trench_run(cr)
+ draw_init_lines(cr)
+ draw_control_panel(cr)
def draw_robot_at_point(self, cr, i, p, spline):
p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
@@ -206,16 +214,16 @@
self.extents_y_max - self.extents_y_min)
cr.fill()
- #Drawing the switch and scale in the field
cr.move_to(0, 50)
cr.show_text('Press "e" to export')
cr.show_text('Press "i" to import')
- set_color(cr, palette["WHITE"])
- cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
- cr.fill()
set_color(cr, palette["BLACK"])
- cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
+ if FIELD == 2020:
+ cr.rectangle(0, mToPx(-7.991475), SCREEN_SIZE, SCREEN_SIZE/2)
+ else:
+ cr.rectangle(0, mToPx(-7.991475), SCREEN_SIZE, SCREEN_SIZE)
+ print(mToPx(-7.991475))
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
self.draw_field_elements(cr)
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/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 86eed14..3498a3e 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -59,16 +59,16 @@
{longitudinal_constraint_offset, lateral_constraint_offset,
voltage_constraint_offset});
- const float startx = 0.4;
- const float starty = 3.4;
+ const float startx = 0.0;
+ const float starty = 0.05;
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
- 0.6f + startx, 0.4f + startx,
- 0.4f + startx, 1.0f + startx});
+ builder->fbb()->CreateVector<float>({0.0f + startx, 0.8f + startx,
+ 0.8f + startx, 1.2f + startx,
+ 1.2f + startx, 2.0f + startx});
flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
- starty - 0.3f, starty - 0.7f,
- starty - 1.0f, starty - 1.0f});
+ starty - 0.1f, starty - 0.2f,
+ starty - 0.3f, starty - 0.3f});
frc971::MultiSpline::Builder multispline_builder =
builder->MakeBuilder<frc971::MultiSpline>();
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index d015c65..e87e064 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -8,6 +8,7 @@
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/actors/auto_splines.h"
namespace y2020 {
namespace actors {
@@ -19,17 +20,43 @@
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")) {}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
+
+ {
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ // TODO(james): Set starting position based on the alliance.
+ localizer_control_builder.add_x(0.0);
+ localizer_control_builder.add_y(0.0);
+ localizer_control_builder.add_theta(0.0);
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ if (!builder.Send(localizer_control_builder.Finish())) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
}
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+ SplineHandle spline1 =
+ PlanSpline(AutonomousSplines::BasicSSpline, SplineDirection::kForward);
+
+ if (!spline1.WaitForPlan()) return true;
+ spline1.Start();
+
+ if (!spline1.WaitForSplineDistanceRemaining(0.02)) return true;
+
AOS_LOG(INFO, "Params are %d\n", params->mode());
return true;
}
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 3bfa610..d86feab 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -6,6 +6,7 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
namespace y2020 {
namespace actors {
@@ -19,6 +20,9 @@
private:
void Reset();
+
+ ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
};
} // namespace actors
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index af6a841..faf2cef 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -41,15 +41,19 @@
"frc971.control_loops.drivetrain.Output");
reader.Register();
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "roborio");
+ }
+
aos::logger::DetachedBufferWriter file_writer(FLAGS_output_file);
std::unique_ptr<aos::EventLoop> log_writer_event_loop =
- reader.event_loop_factory()->MakeEventLoop("log_writer");
+ reader.event_loop_factory()->MakeEventLoop("log_writer", node);
log_writer_event_loop->SkipTimingReport();
- CHECK(nullptr == log_writer_event_loop->node());
aos::logger::Logger writer(&file_writer, log_writer_event_loop.get());
std::unique_ptr<aos::EventLoop> drivetrain_event_loop =
- reader.event_loop_factory()->MakeEventLoop("drivetrain");
+ reader.event_loop_factory()->MakeEventLoop("drivetrain", node);
drivetrain_event_loop->SkipTimingReport();
y2020::control_loops::drivetrain::Localizer localizer(
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index d81b6b5..4534404 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -62,8 +62,11 @@
":superstructure_status_fbs",
"//aos/controls:control_loop",
"//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020:constants",
"//y2020/control_loops/superstructure/shooter",
+ "//y2020/control_loops/superstructure/turret:aiming",
],
)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 5dfd2d1..1fd5610 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,11 @@
hood_(constants::GetValues().hood),
intake_joint_(constants::GetValues().intake),
turret_(constants::GetValues().turret.subsystem_params),
- shooter_() {
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -34,6 +38,22 @@
const aos::monotonic_clock::time_point position_timestamp =
event_loop()->context().monotonic_event_time;
+ if (drivetrain_status_fetcher_.Fetch()) {
+ aos::Alliance alliance = aos::Alliance::kInvalid;
+ if (joystick_state_fetcher_.Fetch()) {
+ alliance = joystick_state_fetcher_->alliance();
+ }
+ const turret::Aimer::WrapMode mode =
+ (unsafe_goal != nullptr && unsafe_goal->shooting())
+ ? turret::Aimer::WrapMode::kAvoidWrapping
+ : turret::Aimer::WrapMode::kAvoidEdges;
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode,
+ turret::Aimer::ShotMode::kShootOnTheFly);
+ }
+
+ const flatbuffers::Offset<AimerStatus> aimer_status_offset =
+ aimer_.PopulateStatus(status->fbb());
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -73,10 +93,14 @@
output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
status->fbb());
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *turret_goal = unsafe_goal != nullptr ? (unsafe_goal->turret_tracking()
+ ? aimer_.TurretGoal()
+ : unsafe_goal->turret())
+ : nullptr;
flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
turret_status_offset = turret_.Iterate(
- unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
- position->turret(),
+ turret_goal, position->turret(),
output != nullptr ? &(output_struct.turret_voltage) : nullptr,
status->fbb());
@@ -134,6 +158,7 @@
status_builder.add_intake(intake_status_offset);
status_builder.add_turret(turret_status_offset);
status_builder.add_shooter(shooter_status_offset);
+ status_builder.add_aimer(aimer_status_offset);
status->Send(status_builder.Finish());
@@ -144,7 +169,7 @@
if (shooter_.ready() &&
unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
unsafe_goal->shooter()->velocity_finisher() > 10.0) {
- output_struct.feeder_voltage = 9.0;
+ output_struct.feeder_voltage = 12.0;
} else {
output_struct.feeder_voltage = 0.0;
}
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 1526610..1a9d401 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -3,6 +3,8 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
@@ -10,6 +12,7 @@
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
#include "y2020/control_loops/superstructure/climber.h"
+#include "y2020/control_loops/superstructure/turret/aiming.h"
namespace y2020 {
namespace control_loops {
@@ -53,6 +56,11 @@
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
shooter::Shooter shooter_;
+ turret::Aimer aimer_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
Climber climber_;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 6e4e08b..b461ec6 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -36,6 +36,7 @@
CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef ::frc971::control_loops::drivetrain::Status DrivetrainStatus;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
@@ -72,7 +73,6 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
-
hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
hood_encoder_(constants::GetValues()
.hood.zeroing_constants.one_revolution_distance),
@@ -398,6 +398,10 @@
test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+ joystick_state_sender_(
+ test_event_loop_->MakeSender<aos::JoystickState>("/aos")),
superstructure_event_loop_(MakeEventLoop("superstructure", roborio_)),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
@@ -506,6 +510,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+ ::aos::Sender<aos::JoystickState> joystick_state_sender_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -827,6 +833,79 @@
VerifyNearGoal();
}
+class SuperstructureAllianceTest
+ : public SuperstructureTest,
+ public ::testing::WithParamInterface<aos::Alliance> {};
+
+// Tests that the turret switches to auto-aiming when we set turret_tracking to
+// true.
+TEST_P(SuperstructureAllianceTest, TurretAutoAim) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+
+ WaitUntilZeroed();
+
+ constexpr double kShotAngle = 1.0;
+ {
+ auto builder = joystick_state_sender_.MakeBuilder();
+
+ aos::JoystickState::Builder joystick_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_builder.add_alliance(GetParam());
+
+ ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_turret_tracking(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 1m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle));
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle));
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ superstructure_status_fetcher_.Fetch();
+ EXPECT_FLOAT_EQ(kShotAngle,
+ superstructure_status_fetcher_->turret()->position());
+ EXPECT_FLOAT_EQ(kShotAngle,
+ superstructure_status_fetcher_->aimer()->turret_position());
+ EXPECT_FLOAT_EQ(0,
+ superstructure_status_fetcher_->aimer()->turret_velocity());
+}
+
+INSTANTIATE_TEST_CASE_P(ShootAnyAlliance, SuperstructureAllianceTest,
+ ::testing::Values(aos::Alliance::kRed,
+ aos::Alliance::kBlue,
+ aos::Alliance::kInvalid));
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 0b12d33..81293d3 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -29,6 +29,15 @@
accelerator_right:FlywheelControllerStatus;
}
+table AimerStatus {
+ // The current goal angle for the turret auto-tracking, in radians.
+ turret_position:double;
+ // The current goal velocity for the turret, in radians / sec.
+ turret_velocity:double;
+ // Whether we are currently aiming for the inner port.
+ aiming_for_inner_port:bool;
+}
+
table Status {
// All subsystems know their location.
zeroed:bool;
@@ -46,6 +55,9 @@
// Status of the control_panel
control_panel:frc971.control_loops.RelativeEncoderProfiledJointStatus;
+
+ // Status of the vision auto-tracking.
+ aimer:AimerStatus;
}
root_type Status;
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 894d418..010c6fd 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -30,3 +30,28 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "aiming",
+ srcs = ["aiming.cc"],
+ hdrs = ["aiming.h"],
+ deps = [
+ "//aos:flatbuffers",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2020:constants",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_test(
+ name = "aiming_test",
+ srcs = ["aiming_test.cc"],
+ deps = [
+ ":aiming",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
new file mode 100644
index 0000000..360fd1a
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -0,0 +1,252 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+using frc971::control_loops::Pose;
+
+// Shooting-on-the-fly concept:
+// The current way that we manage shooting-on-the fly endeavors to be reasonably
+// simple, until we get a chance to see how the actual dynamics play out.
+// Essentially, we assume that the robot's velocity will represent a constant
+// offset to the ball's velocity over the entire trajectory to the goal and
+// then offset the target that we are pointing at based on that.
+// Let us assume that, if the robot shoots while not moving, regardless of shot
+// distance, the ball's average speed-over-ground to the target will be a
+// constant s_shot (this implies that if the robot is driving straight towards
+// the target, the actual ball speed-over-ground will be greater than s_shot).
+// We will define things in the robot's coordinate frame. We will be shooting
+// at a target that is at position (target_x, target_y) in the robot frame. The
+// robot is travelling at (v_robot_x, v_robot_y). In order to shoot the ball,
+// we need to generate some virtual target (virtual_x, virtual_y) that we will
+// shoot at as if we were standing still. The total time-of-flight to that
+// target will be t_shot = norm2(virtual_x, virtual_y) / s_shot.
+// we will have virtual_x + v_robot_x * t_shot = target_x, and the same
+// for y. This gives us three equations and three unknowns (virtual_x,
+// virtual_y, and t_shot), and given appropriate assumptions, can be solved
+// analytically. However, doing so is obnoxious and given appropriate functions
+// for t_shot may not be feasible. As such, instead of actually solving the
+// equation analytically, we will use an iterative solution where we maintain
+// a current virtual target estimate. We start with this estimate as if the
+// robot is stationary. We then use this estimate to calculate t_shot, and
+// calculate the next value for the virtual target.
+
+namespace {
+// The overall length and width of the field, in meters.
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+// Height of the center of the port(s) above the ground, in meters.
+constexpr double kPortHeight = 2.494;
+
+// Maximum shot angle at which we will attempt to make the shot into the inner
+// port, in radians. Zero would imply that we could only shoot if we were
+// exactly perpendicular to the target. Larger numbers allow us to aim at the
+// inner port more aggressively, at the risk of being more likely to miss the
+// outer port entirely.
+constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
+
+// Distance (in meters) from the edge of the field to the port.
+constexpr double kEdgeOfFieldToPort = 2.404;
+
+// The amount (in meters) that the inner port is set back from the outer port.
+constexpr double kInnerPortBackset = 0.743;
+
+// Average speed-over-ground of the ball on its way to the target. Our current
+// model assumes constant ball velocity regardless of shot distance.
+// TODO(james): Is this an appropriate model? For the outer port it should be
+// good enough that it doesn't really matter, but for the inner port it may be
+// more appropriate to do something more dynamic--however, it is not yet clear
+// how we would best estimate speed-over-ground given a hood angle + shooter
+// speed. Assuming a constant average speed over the course of the trajectory
+// should be reasonable, since all we are trying to do here is calculate an
+// overall time-of-flight (we don't actually care about the ball speed itself).
+constexpr double kBallSpeedOverGround = 15.0; // m/s
+
+// Minimum distance that we must be from the inner port in order to attempt the
+// shot--this is to account for the fact that if we are too close to the target,
+// then we won't have a clear shot on the inner port.
+constexpr double kMinimumInnerPortShotDistance = 4.0;
+
+// Amount of buffer, in radians, to leave to help avoid wrapping. I.e., any time
+// that we are in kAvoidEdges mode, we will keep ourselves at least
+// kAntiWrapBuffer radians away from the hardstops.
+constexpr double kAntiWrapBuffer = 0.2;
+
+// If the turret is at zero, then it will be at this angle relative to pointed
+// straight forwards on the robot.
+constexpr double kTurretZeroOffset = M_PI;
+
+constexpr double kTurretRange = constants::Values::kTurretRange().range();
+static_assert((kTurretRange - 2.0 * kAntiWrapBuffer) > 2.0 * M_PI,
+ "kAntiWrap buffer should be small enough that we still have 360 "
+ "degrees of range.");
+
+Pose ReverseSideOfField(Pose target) {
+ *target.mutable_pos() *= -1;
+ target.set_theta(aos::math::NormalizeAngle(target.rel_theta() + M_PI));
+ return target;
+}
+
+flatbuffers::DetachedBuffer MakePrefilledGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ Aimer::Goal::Builder builder(fbb);
+ builder.add_unsafe_goal(0);
+ builder.add_goal_velocity(0);
+ builder.add_ignore_profile(true);
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+}
+
+// This implements the iteration in the described shooting-on-the-fly algorithm.
+// robot_pose: Current robot pose.
+// robot_velocity: Current robot velocity, in the absolute field frame.
+// target_pose: Absolute goal Pose.
+// current_virtual_pose: Current estimate of where we want to shoot at.
+Pose IterateVirtualGoal(const Pose &robot_pose,
+ const Eigen::Vector3d &robot_velocity,
+ const Pose &target_pose,
+ const Pose ¤t_virtual_pose) {
+ const double air_time =
+ current_virtual_pose.Rebase(&robot_pose).xy_norm() / kBallSpeedOverGround;
+ const Eigen::Vector3d virtual_target =
+ target_pose.abs_pos() - air_time * robot_velocity;
+ return Pose(virtual_target, target_pose.abs_theta());
+}
+} // namespace
+
+Pose InnerPortPose(aos::Alliance alliance) {
+ const Pose target({kFieldLength / 2 + kInnerPortBackset,
+ -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+ 0.0);
+ if (alliance == aos::Alliance::kRed) {
+ return ReverseSideOfField(target);
+ }
+ return target;
+}
+
+Pose OuterPortPose(aos::Alliance alliance) {
+ Pose target(
+ {kFieldLength / 2, -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+ 0.0);
+ if (alliance == aos::Alliance::kRed) {
+ return ReverseSideOfField(target);
+ }
+ return target;
+}
+
+Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+
+void Aimer::Update(const Status *status, aos::Alliance alliance,
+ WrapMode wrap_mode, ShotMode shot_mode) {
+ const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
+ const Pose inner_port = InnerPortPose(alliance);
+ const Pose outer_port = OuterPortPose(alliance);
+ const Pose robot_pose_from_inner_port = robot_pose.Rebase(&inner_port);
+
+ // TODO(james): This code should probably just be in the localizer and have
+ // xdot/ydot get populated in the status message directly... that way we don't
+ // keep duplicating this math.
+ // Also, this doesn't currently take into account the lateral velocity of the
+ // robot. All of this would be helped by just doing this work in the Localizer
+ // itself.
+ const Eigen::Vector2d linear_angular =
+ drivetrain::GetDrivetrainConfig().Tlr_to_la() *
+ Eigen::Vector2d(status->localizer()->left_velocity(),
+ status->localizer()->right_velocity());
+ const double xdot = linear_angular(0) * std::cos(status->theta());
+ const double ydot = linear_angular(0) * std::sin(status->theta());
+
+ const double inner_port_angle = robot_pose_from_inner_port.heading();
+ const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
+ aiming_for_inner_port_ =
+ (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
+ (inner_port_distance > kMinimumInnerPortShotDistance);
+
+ // This code manages compensating the goal turret heading for the robot's
+ // current velocity, to allow for shooting on-the-fly.
+ // This works by solving for the correct turret angle numerically, since while
+ // we technically could do it analytically, doing so would both make it hard
+ // to make small changes (since it would force us to redo the math) and be
+ // error-prone since it'd be easy to make typos or other minor math errors.
+ Pose virtual_goal;
+ {
+ const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
+ virtual_goal = goal;
+ if (shot_mode == ShotMode::kShootOnTheFly) {
+ for (int ii = 0; ii < 3; ++ii) {
+ virtual_goal =
+ IterateVirtualGoal(robot_pose, {xdot, ydot, 0}, goal, virtual_goal);
+ }
+ VLOG(1) << "Shooting-on-the-fly target position: "
+ << virtual_goal.abs_pos().transpose();
+ }
+ virtual_goal = virtual_goal.Rebase(&robot_pose);
+ }
+
+ const double heading_to_goal = virtual_goal.heading();
+ CHECK(status->has_localizer());
+ distance_ = virtual_goal.xy_norm();
+
+ // The following code all works to calculate what the rate of turn of the
+ // turret should be. The code only accounts for the rate of turn if we are
+ // aiming at a static target, which should be close enough to correct that it
+ // doesn't matter that it fails to account for the
+ // shooting-on-the-fly compensation.
+ const double rel_x = virtual_goal.rel_pos().x();
+ const double rel_y = virtual_goal.rel_pos().y();
+ const double squared_norm = rel_x * rel_x + rel_y * rel_y;
+
+ // If squared_norm gets to be too close to zero, just zero out the relevant
+ // term to prevent NaNs. Note that this doesn't address the chattering that
+ // would likely occur if we were to get excessively close to the target.
+ // Note that x and y terms are swapped relative to what you would normally see
+ // in the derivative of atan because xdot and ydot are the derivatives of
+ // robot_pos and we are working with the atan of (target_pos - robot_pos).
+ const double atan_diff = (squared_norm < 1e-3)
+ ? 0.0
+ : (rel_y * xdot - rel_x * ydot) / squared_norm;
+ // heading = atan2(relative_y, relative_x) - robot_theta
+ // dheading / dt = (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
+ const double dheading_dt = atan_diff - linear_angular(1);
+
+ double range = kTurretRange;
+ if (wrap_mode == WrapMode::kAvoidEdges) {
+ range -= 2.0 * kAntiWrapBuffer;
+ }
+ // Calculate a goal turret heading such that it is within +/- pi of the
+ // current position (i.e., a goal that would minimize the amount the turret
+ // would have to travel).
+ // We then check if this goal would bring us out of range of the valid angles,
+ // and if it would, we reset to be within +/- pi of zero.
+ double turret_heading =
+ goal_.message().unsafe_goal() +
+ aos::math::NormalizeAngle(heading_to_goal - kTurretZeroOffset -
+ goal_.message().unsafe_goal());
+ if (std::abs(turret_heading - constants::Values::kTurretRange().middle()) >
+ range / 2.0) {
+ turret_heading = aos::math::NormalizeAngle(turret_heading);
+ }
+
+ goal_.mutable_message()->mutate_unsafe_goal(turret_heading);
+ goal_.mutable_message()->mutate_goal_velocity(dheading_dt);
+}
+
+flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ AimerStatus::Builder builder(*fbb);
+ builder.add_turret_position(goal_.message().unsafe_goal());
+ builder.add_turret_velocity(goal_.message().goal_velocity());
+ builder.add_aiming_for_inner_port(aiming_for_inner_port_);
+ return builder.Finish();
+}
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
new file mode 100644
index 0000000..3b3071e
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -0,0 +1,74 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+
+#include "aos/flatbuffers.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+// Returns the port that we want to score on given our current alliance. The yaw
+// of the port will be such that the positive x axis points out the back of the
+// target.
+frc971::control_loops::Pose InnerPortPose(aos::Alliance alliance);
+frc971::control_loops::Pose OuterPortPose(aos::Alliance alliance);
+
+// This class manages taking in drivetrain status messages and generating turret
+// goals so that it gets aimed at the goal.
+class Aimer {
+ public:
+ typedef frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ Goal;
+ typedef frc971::control_loops::drivetrain::Status Status;
+ // Mode to run the aimer in, to control how we manage wrapping the turret
+ // angle.
+ enum class WrapMode {
+ // Keep the turret as far away from the edges of the range of motion as
+ // reasonable, to minimize the odds that we will hit the hardstops once we
+ // start shooting.
+ kAvoidEdges,
+ // Do everything reasonable to avoid having to wrap the shooter--set this
+ // while shooting so that we don't randomly spin the shooter 360 while
+ // shooting.
+ kAvoidWrapping,
+ };
+
+ // Control modes for managing how we manage shooting on the fly.
+ enum class ShotMode {
+ // Don't do any shooting-on-the-fly compensation--just point straight at the
+ // target. Primarily used in tests.
+ kStatic,
+ // Do do shooting-on-the-fly compensation.
+ kShootOnTheFly,
+ };
+
+ Aimer();
+
+ void Update(const Status *status, aos::Alliance alliance, WrapMode wrap_mode,
+ ShotMode shot_mode);
+
+ const Goal *TurretGoal() const { return &goal_.message(); }
+
+ // Returns the distance to the goal, in meters.
+ double DistanceToGoal() const { return distance_; }
+
+ flatbuffers::Offset<AimerStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+ aos::FlatbufferDetachedBuffer<Goal> goal_;
+ bool aiming_for_inner_port_ = false;
+ double distance_ = 0.0;
+};
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+#endif // y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
new file mode 100644
index 0000000..cb95b56
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -0,0 +1,249 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "frc971/control_loops/pose.h"
+#include "gtest/gtest.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+namespace testing {
+
+using frc971::control_loops::Pose;
+
+class AimerTest : public ::testing::Test {
+ public:
+ typedef Aimer::Goal Goal;
+ typedef Aimer::Status Status;
+ struct StatusData {
+ double x;
+ double y;
+ double theta;
+ double linear;
+ double angular;
+ };
+ aos::FlatbufferDetachedBuffer<Status> MakeStatus(const StatusData &data) {
+ flatbuffers::FlatBufferBuilder fbb;
+ frc971::control_loops::drivetrain::LocalizerState::Builder state_builder(
+ fbb);
+ state_builder.add_left_velocity(
+ data.linear -
+ data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+ state_builder.add_right_velocity(
+ data.linear +
+ data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+ const auto state_offset = state_builder.Finish();
+ Status::Builder builder(fbb);
+ builder.add_x(data.x);
+ builder.add_y(data.y);
+ builder.add_theta(data.theta);
+ builder.add_localizer(state_offset);
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+ }
+
+ const Goal *Update(
+ const StatusData &data, aos::Alliance alliance = aos::Alliance::kBlue,
+ Aimer::WrapMode wrap_mode = Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode shot_mode = Aimer::ShotMode::kShootOnTheFly) {
+ const auto buffer = MakeStatus(data);
+ aimer_.Update(&buffer.message(), alliance, wrap_mode, shot_mode);
+ const Goal *goal = aimer_.TurretGoal();
+ EXPECT_TRUE(goal->ignore_profile());
+ return goal;
+ }
+
+ protected:
+ Aimer aimer_;
+};
+
+TEST_F(AimerTest, StandingStill) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 1.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(-1.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = -1.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Test that we handle the case that where we are right on top of the target.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(0.0, aimer_.DistanceToGoal());
+}
+
+TEST_F(AimerTest, SpinningRobot) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 1.0});
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+}
+
+// Tests that when we drive straight away from the target we don't have to spin
+// the turret.
+TEST_F(AimerTest, DrivingAwayFromTarget) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal distance comes out in an easy to calculate number.
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, try with shooting-on-the-fly enabled--because we are driving straight
+ // towards the target, only the goal distance should be impacted.
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Tests that when we drive perpendicular to the target, we do have to spin.
+TEST_F(AimerTest, DrivingLateralToTarget) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal_velocity comes out in an easy to calculate number.
+ const Goal *goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
+ EXPECT_EQ(M_PI_2, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, test with shooting-on-the-fly enabled, The goal numbers should all be
+ // slightly offset due to the robot velocity.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ // Confirm that the turret heading goal is less then -pi / 2, but not by too
+ // much.
+ EXPECT_GT(M_PI_2 - 0.001, goal->unsafe_goal());
+ EXPECT_LT(M_PI_2 - 0.1, goal->unsafe_goal());
+ // Similarly, the turret velocity goal should be a bit greater than -1.0,
+ // since the turret is no longer at exactly a right angle.
+ EXPECT_LT(-1.0, goal->goal_velocity());
+ EXPECT_GT(-0.95, goal->goal_velocity());
+ // And the distance to the goal should be a bit greater than 1.0.
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Confirms that we will indeed shoot at the inner port when we have a good shot
+// angle on it.
+TEST_F(AimerTest, InnerPort) {
+ const Pose target = InnerPortPose(aos::Alliance::kRed);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0},
+ aos::Alliance::kRed);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that when we move the turret heading so that it would be entirely
+// out of the normal range of motion that we send a valid (in-range) goal.
+TEST_F(AimerTest, WrapWhenOutOfRange) {
+ // Start ourselves needing a turret angle of M_PI.
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ StatusData status{.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0};
+ const Goal *goal = Update(status);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Move the robot a small amount--we should go past pi and not wrap yet.
+ status.theta = -0.1;
+ goal = Update(status);
+ EXPECT_FLOAT_EQ(0.1, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Move the robot so that, if we had no hard-stops, we would go past it.
+ status.theta = -2.0;
+ goal = Update(status);
+ EXPECT_FLOAT_EQ(2.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that the avoid edges turret mode doesn't let us get all the way to
+// the turret hard-stops but that the avoid wrapping mode does.
+TEST_F(AimerTest, WrappingModes) {
+ // Start ourselves needing a turret angle of M_PI.
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ StatusData status{.x = target.abs_pos().x() - 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0};
+ const Goal *goal =
+ Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ constexpr double kUpperLimit = constants::Values::kTurretRange().upper;
+ // Move the robot to the upper limit with AvoidWrapping set--we should be at
+ // the upper limit and not wrapped.
+ status.theta = goal->unsafe_goal() - kUpperLimit;
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+ EXPECT_FLOAT_EQ(kUpperLimit, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Enter kAvoidEdges mode--we should wrap around.
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges);
+ // confirm that this test is actually testing something...
+ ASSERT_NE(aos::math::NormalizeAngle(kUpperLimit), kUpperLimit);
+ EXPECT_FLOAT_EQ(aos::math::NormalizeAngle(kUpperLimit), goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+} // namespace testing
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
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/vision/rootfs/target_configure.sh b/y2020/vision/rootfs/target_configure.sh
index 032764a..193f8b5 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/y2020/vision/rootfs/target_configure.sh
@@ -37,7 +37,20 @@
libopencv-videoio3.2 \
libopencv-videostab3.2 \
libopencv-viz3.2 \
- python3-opencv
+ python3-opencv \
+ gstreamer1.0-plugins-bad \
+ gstreamer1.0-plugins-base \
+ gstreamer1.0-plugins-good \
+ gstreamer1.0-plugins-ugly \
+ gstreamer1.0-x \
+ gstreamer1.0-nice \
+ gstreamer1.0-gl \
+ libgstreamer-plugins-bad1.0-0 \
+ libgstreamer-plugins-base1.0-0 \
+ libgstreamer1.0-0 \
+ libgstreamer-gl1.0-0 \
+ libnice10 \
+ libnice-dev
echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 5975807..b40ac87 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",