Commented out non-drivetrain motors

still doesn't work

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2021.1.2"
+    id "edu.wpi.first.GradleRIO" version "2021.3.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
@@ -38,7 +38,7 @@ deploy {
 }
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.

src/main/java/frc/robot/RobotContainer.java

@@ -86,9 +86,12 @@ public Command getAutonomousCommand() {
     config.setKinematics(drivetrain.getKinematics());
 
     Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
-      Arrays.asList(new Pose2d(), new Pose2d(1.0, 0, new Rotation2d())),
+      Arrays.asList(new Pose2d(),
+      new Pose2d(0, 1.0, new Rotation2d(0)), 
+      new Pose2d(0, 1.0, new Rotation2d(90)),
+      new Pose2d(1.0, 1.0, new Rotation2d(90))),
       config);
-
+ 
     RamseteCommand command = new RamseteCommand(
       trajectory,
       drivetrain::getPose,

src/main/java/frc/robot/commands/ArcadeDrive.java

@@ -31,7 +31,6 @@ public void initialize() {
   @Override
   public void execute() {
     dt.driveWithJoysticks(RobotContainer.controller);
-    System.out.print("Drive"); 
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Roll.java

@@ -31,7 +31,6 @@ public void initialize() {
   @Override
   public void execute() {
     succRoller.intake(RobotContainer.controller);
-    System.out.print("Execute"); 
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/Arm.java

@@ -15,7 +15,7 @@
 import frc.robot.Constants;
 
 public class Arm extends SubsystemBase {
-  TalonSRX armMotor = new TalonSRX(Constants.MOTOR_ARM_ID);
+  // TalonSRX armMotor = new TalonSRX(Constants.MOTOR_ARM_ID);
   /**
    * Creates a new Arm.
    */
@@ -24,7 +24,7 @@ public Arm() {
   }
 
   public void turn(XboxController controller) {
-    armMotor.set(ControlMode.PercentOutput, controller.getRawAxis(Constants.JOYSTICK_ARM_CONTROL_ID));
+    // armMotor.set(ControlMode.PercentOutput, controller.getRawAxis(Constants.JOYSTICK_ARM_CONTROL_ID));
   }
 
   @Override

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -77,28 +77,29 @@ public class Drivetrain extends SubsystemBase {
   DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(getHeading());
 
   // Feedforward controller for Ramsete
-  SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.4, 1, 0.4);
+  SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.137, 3.24, 0.592);
 
   // how robot turn wheel
-  PIDController leftPIDController = new PIDController(0.1,0.0,0.05);
-  PIDController rightPIDController = new PIDController(0.1,0.0,0.05); 
+  PIDController leftPIDController = new PIDController(2.21,0.0,0.0);
+  PIDController rightPIDController = new PIDController(2.21,0.0,0.0); 
 
   Pose2d pose;
 
   DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
 
   //Create drivetrain simulation
-  static final double KvLinear = 0; 
-  static final double KaLinear = 0;
-  static final double KvAngular = 0; 
-  static final double KaAngular = 0; 
+  static final double KvLinear = 3.24; 
+  static final double KaLinear = 0.592;
+  static final double KvAngular = 1.5; 
+  static final double KaAngular = 0.3; 
+  static final double trackWidthMeters = 0.51;
 
   private DifferentialDrivetrainSim m_driveSim = new DifferentialDrivetrainSim(
     LinearSystemId.identifyDrivetrainSystem(KvLinear, KaLinear, KvAngular, KaAngular), 
     DCMotor.getNEO(2), 
-    0.025,
-    10.75, 
-    Units.inchesToMeters(3),  
+    10.75,
+    0.509, 
+    Units.inchesToMeters(3),
 
     // The standard deviations for measurement noise:
     // x and y:          0.001 m
@@ -132,13 +133,13 @@ public void simulationPeriodic(){
 
     // subsystem in a separate thread or have changed the nominal timestep
     // of TimedRobot, this value needs to match it.
-    m_driveSim.update(0.02); 
+    m_driveSim.update(0.07); 
 
     //Update sensors
-    leftPos.set(m_driveSim.getLeftPositionMeters()); 
-    leftVel.set(m_driveSim.getLeftVelocityMetersPerSecond());
-    rightPos.set(m_driveSim.getRightPositionMeters()); 
-    rightVel.set(m_driveSim.getRightVelocityMetersPerSecond());
+    simEncoderLeft.getDouble("Position").set(m_driveSim.getLeftPositionMeters()); 
+    simEncoderLeft.getDouble("Velocity").set(m_driveSim.getLeftVelocityMetersPerSecond());
+    simEncoderRight.getDouble("Position").set(m_driveSim.getRightPositionMeters()); 
+    simEncoderRight.getDouble("Velocity").set(m_driveSim.getRightVelocityMetersPerSecond());
     //Update gyro
     simAngle.set(-m_driveSim.getHeading().getDegrees());
   }

src/main/java/frc/robot/subsystems/Roller.java

@@ -14,7 +14,7 @@
 
 
 public class Roller extends SubsystemBase {
-  VictorSPX rollerMotor = new VictorSPX(Constants.MOTOR_ROLLER_ID);
+  // VictorSPX rollerMotor = new VictorSPX(Constants.MOTOR_ROLLER_ID);
   /**
    * Creates a new Roller.
    */
@@ -37,7 +37,7 @@ public void intake(XboxController controller) {
     */
     double intake = controller.getRawAxis(Constants.JOYSTICK_INTAKE_ID);
     double output = controller.getRawAxis(Constants.JOYSTICK_OUTPUT_ID);
-    rollerMotor.set(ControlMode.PercentOutput, intake - output);
+    // rollerMotor.set(ControlMode.PercentOutput, intake - output);
   }
 
   @Override

src/main/java/frc/robot/subsystems/Winch.java

@@ -7,7 +7,7 @@
 
 package frc.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+// import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -16,14 +16,14 @@ public class Winch extends SubsystemBase {
   /**
    * Creates a new Winch.
    */
-  TalonSRX winchMotor = new TalonSRX(Constants.MOTOR_CLIMB_WINCH_ID);
+  // TalonSRX winchMotor = new TalonSRX(Constants.MOTOR_CLIMB_WINCH_ID);
 
   public void winchUp() {
-    winchMotor.set(ControlMode.PercentOutput, 1.0);
+    // winchMotor.set(ControlMode.PercentOutput, 1.0);
   }
 
   public void winchDown() {
-    winchMotor.set(ControlMode.PercentOutput, -1.0);
+    // winchMotor.set(ControlMode.PercentOutput, -1.0);
   }
 
   @Override