From 43cfe18f5411e24b8a73303a463d9ad94106c937 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 9 Feb 2023 19:34:59 -0700
Subject: [PATCH] Update RobotContainer.java

---
 .../java/frc4388/robot/RobotContainer.java    | 28 +++++++++++--------
 1 file changed, 16 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 429f3f3..4058926 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -117,27 +117,31 @@ public class RobotContainer {
             SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
 
         // generate trajectory
-        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
             
-        //     new Pose2d(0, 0, new Rotation2d(0)),
-        //     List.of(
-        //             new Translation2d(0, 1)),
-        //     new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
+            new Pose2d(0, 0, new Rotation2d(0)),
+            List.of(
+                    new Translation2d(0, 1)),
+            new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
             
-        //     trajectoryConfig);
+            trajectoryConfig);
 
         ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
         simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
         simplePath.add(new Pose2d(0, -1, new Rotation2d(0)));
 
-        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
+        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
+        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+        //     new Pose2d(0, 0, new Rotation2d(0),
+        //     List.of(new Translation2d(0, 1)),
+        //     new Pose2d(0, 2, new Rotation2d(0)),
+        //     trajectoryConfig
+        // );
+
+
+        // ArrayList<double[]> states = new ArrayList<double[]>();
 
-        ArrayList<double[]> states = new ArrayList<double[]>();
 
-        for(double i = 0; i <=5.0; i += 0.1) {
-            Trajectory.State state = trajectory.sample(i);
-            states.add(new double[] {state.velocityMetersPerSecond, state.curvatureRadPerMeter});
-        }
 
         //Defining PID Controller for tracking trajectory
         PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0);