diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0420217..01f10e6 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -51,16 +51,16 @@ public final class Constants {
     }
 
     public static final class AutoConstants {
-      public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0);
-      public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0);
-      public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0);
-      public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0);
+      public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+      public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+      public static final Gains THETA_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+      public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
 
-      public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value
-      public static final double PATH_MAX_ACC = 3; // TODO: find the actual value
+      public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
+      public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
 
-      public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value
-      public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value
+      // public static final double kPX_CONTROLLER = 0.8; //TODO: tune
+      // public static final double kPY_CONTROLLER = 0.8; //TODO: tune
     }
 
     public static final class Conversions {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 08f209f..37158ad 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import java.util.ArrayList;
 import java.util.List;
 
 import edu.wpi.first.math.controller.PIDController;
@@ -17,6 +18,7 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.Trajectory.State;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -105,20 +107,32 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
+
         //Create Trajectory Settings
         TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, 
             SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
-        //Generate Trajactory
-        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), 
-            List.of(new Translation2d(1, 0), new Translation2d(1, -1)), 
-            new Pose2d(0, 0, Rotation2d.fromDegrees(180)), 
+
+        // generate trajectory
+        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+            
+            new Pose2d(0, 0, new Rotation2d(0)),
+            List.of(
+                    new Translation2d(0, 1)),
+            new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
+            
             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);
+
         //Defining PID Controller for tracking trajectory
-        PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0);
-        PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0);
+        PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0);
+        PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0);
         ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP,
-        AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0);
+        AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS);
         thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
         //Command to follow trajectory
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 6cf7ba8..5790496 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -77,13 +77,10 @@ public class SwerveDrive extends SubsystemBase {
       // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
 
-      if (rightStick.getNorm() < 0.1) {
-        rot = 0;
-      }
-
       // Convert field-relative speeds to robot-relative speeds.
       chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
 
+    } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
@@ -178,7 +175,10 @@ public class SwerveDrive extends SubsystemBase {
     updateOdometry();
 
     // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()});
-
+    SmartDashboard.putNumber("Odo X", getOdometry().getX());
+    SmartDashboard.putNumber("Odo Y", getOdometry().getY());
+    SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees());
+    
     SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
   }