commit 47a0ab34302242df7956ef576fcac95c8c5f46fc
author milind upadhyay <milind.upadhyay@gmail.com> Wed Nov 25 19:34:41 2020 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Jan 23 14:36:06 2021 -0800
tree 3cdc2510761264f618f927380f7287cc1979cada
parent 5229c7e1273f1508dfd40c626c510a7889b627de

Add basic autonomous program.

The robot is in autonomous mode in the beginning of the match. This code makes it simply drive forward in autonomous mode.
It takes a command line argument for either executing spline code or driving forward.
Removed c++ 17 initialzation with curly braces because the roborio compiler is not compatible with it.

Change-Id: Ia80fb5ce8bba219cbbf4fc46e99ffdbe36cd0d93

y2020/actors/autonomous_actor.cc

diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 0b2b9c6..9b526dd 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -11,6 +11,8 @@
 #include "y2020/control_loops/drivetrain/drivetrain_base.h"
 #include "y2020/actors/auto_splines.h"
 
+DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
+
 namespace y2020 {
 namespace actors {
 
@@ -26,7 +28,9 @@
           ::frc971::control_loops::drivetrain::LocalizerControl>(
           "/drivetrain")),
       joystick_state_fetcher_(
-          event_loop->MakeFetcher<aos::JoystickState>("/aos")) {}
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+  set_max_drivetrain_voltage(2.0);
+}
 
 void AutonomousActor::Reset() {
   InitializeEncoders();
@@ -62,25 +66,45 @@
 }
 
 bool AutonomousActor::RunAction(
-    const ::frc971::autonomous::AutonomousActionParams *params) {
+     const ::frc971::autonomous::AutonomousActionParams *params) {
   Reset();
+  AOS_LOG(INFO, "Params are %d\n", params->mode());
   if (alliance_ == aos::Alliance::kInvalid) {
     AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
     return false;
   }
+  if (FLAGS_spline_auto) {
+    SplineAuto();
+  } else {
+    return DriveFwd();
+  }
+  return true;
+}
 
+void AutonomousActor::SplineAuto() {
   SplineHandle spline1 = PlanSpline(std::bind(AutonomousSplines::BasicSSpline,
                                               std::placeholders::_1, alliance_),
                                     SplineDirection::kForward);
 
-  if (!spline1.WaitForPlan()) return true;
+  if (!spline1.WaitForPlan()) return;
   spline1.Start();
 
-  if (!spline1.WaitForSplineDistanceRemaining(0.02)) return true;
-
-  AOS_LOG(INFO, "Params are %d\n", params->mode());
-  return true;
+  if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
 }
 
+ProfileParametersT MakeProfileParametersT(const float max_velocity,
+                                          const float max_acceleration) {
+  ProfileParametersT params;
+  params.max_velocity = max_velocity;
+  params.max_acceleration = max_acceleration;
+  return params;
+}
+
+bool AutonomousActor::DriveFwd() {
+  const ProfileParametersT kDrive = MakeProfileParametersT(0.1f, 0.5f);
+  const ProfileParametersT kTurn = MakeProfileParametersT(5.0f, 15.0f);
+  StartDrive(0.5, 0.0, kDrive, kTurn);
+  return WaitForDriveDone();
+}
 }  // namespace actors
 }  // namespace y2020

y2020/actors/autonomous_actor.h

diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 747fa8d..4ad04d2 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -20,6 +20,8 @@
 
  private:
   void Reset();
+  void SplineAuto();
+  bool DriveFwd();
 
   ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
       localizer_control_sender_;