Do some initial y2020 drivetrain tuning
1) Makes the drivetrain replay work in a multinode world.
2) Tweaks localizer and spline constants.
3) Initializes the localizer at the start of auto.
4) Runs a simple s-spline in auto, just for kicks.
Change-Id: I1cf990ee8d0c0811fa1e8de7b62209eb0260fa29
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/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/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(