From 42c3d52863434f99be3949bda1e123e56a1f57a8 Mon Sep 17 00:00:00 2001
From: Prateek Machiraju <prateek.machiraju@gmail.com>
Date: Mon, 11 Jan 2021 23:08:22 -0500
Subject: [PATCH] [examples] Sync Java and C++ trajectories in sim example
 (#3081)

This also shifts the trajectory up and to the right so that the robot is
always visible in the Field GUI during traversal. Some drive constants
and trajectory constraints were also synced between the two languages.
---
 .../cpp/RobotContainer.cpp                               | 8 ++++----
 .../statespacedifferentialdrivesimulation/Constants.java | 4 ++--
 .../RobotContainer.java                                  | 9 +++++----
 3 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
index f1a6512bf2..cde0574a2f 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
@@ -69,12 +69,12 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
 
   // An example trajectory to follow.  All units in meters.
   auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      // Start at the origin facing the +X direction
-      frc::Pose2d(),
+      // Start at (1, 2) facing the +X direction
+      frc::Pose2d(1_m, 2_m, 0_deg),
       // Pass through these two interior waypoints, making an 's' curve path
-      {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+      {frc::Translation2d(2_m, 3_m), frc::Translation2d(3_m, 1_m)},
       // End 3 meters straight ahead of where we started, facing forward
-      frc::Pose2d(3_m, 0_m, 0_deg),
+      frc::Pose2d(4_m, 2_m, 0_deg),
       // Pass the config
       config);
 
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
index a0e0d04372..2c8036a122 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
@@ -70,7 +70,7 @@ public final class Constants {
     public static final double kDriveGearing = 8;
 
     // Example value only - as above, this must be tuned for your drive!
-    public static final double kPDriveVel = 0.1;
+    public static final double kPDriveVel = 8.5;
   }
 
   public static final class OIConstants {
@@ -79,7 +79,7 @@ public final class Constants {
 
   public static final class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 3;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 6;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
 
     // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
     public static final double kRamseteB = 2;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
index e5e0266163..3bcc444e85 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -108,12 +109,12 @@ public class RobotContainer {
     // An example trajectory to follow.  All units in meters.
     Trajectory exampleTrajectory =
         TrajectoryGenerator.generateTrajectory(
-            // Start at the origin facing the +X direction
-            new Pose2d(0, 0, new Rotation2d(0)),
+            // Start at (1, 2) facing the +X direction
+            new Pose2d(1, 2, new Rotation2d(0)),
             // Pass through these two interior waypoints, making an 's' curve path
-            List.of(),
+            List.of(new Translation2d(2, 3), new Translation2d(3, 1)),
             // End 3 meters straight ahead of where we started, facing forward
-            new Pose2d(6, 6, new Rotation2d(0)),
+            new Pose2d(4, 2, new Rotation2d(0)),
             // Pass config
             config);