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/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