Attempted auton :(

.Glass/glass-window.json

@@ -15,7 +15,6 @@
       "ypos": "32"
     }
   },
-<<<<<<< HEAD
   "Table": {
     "0x542B5671,2": {
       "Column 0  Width": "319",
@@ -32,37 +31,17 @@
     "###/SmartDashboard/angle dude": {
       "Collapsed": "0",
       "Pos": "45,142",
-=======
-  "Window": {
-    "###/SmartDashboard/Reset Gyro": {
-      "Collapsed": "0",
-      "Pos": "60,60",
-      "Size": "79,71"
-    },
-    "###/SmartDashboard/angle dude": {
-      "Collapsed": "0",
-      "Pos": "50,144",
->>>>>>> 2ba30152a311f0b115492cc616d1b7c406896819
       "Size": "79,71"
     },
     "###/SmartDashboard/drive to distance": {
       "Collapsed": "0",
-<<<<<<< HEAD
       "Pos": "31,60",
-=======
-      "Pos": "85,55",
->>>>>>> 2ba30152a311f0b115492cc616d1b7c406896819
       "Size": "79,71"
     },
     "###NetworkTables": {
       "Collapsed": "0",
-<<<<<<< HEAD
       "Pos": "128,51",
       "Size": "877,629"
-=======
-      "Pos": "116,19",
-      "Size": "878,629"
->>>>>>> 2ba30152a311f0b115492cc616d1b7c406896819
     },
     "###NetworkTables Settings": {
       "Collapsed": "0",

.Glass/glass.json

@@ -1,11 +1,5 @@
 {
   "NetworkTables": {
-<<<<<<< HEAD
-=======
-    "Retained Values": {
-      "open": false
-    },
->>>>>>> 2ba30152a311f0b115492cc616d1b7c406896819
     "transitory": {
       "FMSInfo": {
         "open": true
@@ -39,14 +33,11 @@
       "/SmartDashboard/drive to distance": "Command"
     },
     "windows": {
-<<<<<<< HEAD
       "/SmartDashboard/Reset Gyro": {
         "window": {
           "visible": true
         }
       },
-=======
->>>>>>> 2ba30152a311f0b115492cc616d1b7c406896819
       "/SmartDashboard/angle dude": {
         "window": {
           "visible": true

.SysId/sysid-window.json

@@ -13,7 +13,7 @@
   "Window": {
     "###Analyzer": {
       "Collapsed": "0",
-      "Pos": "320,25",
+      "Pos": "327,-15",
       "Size": "360,550"
     },
     "###Generator": {
@@ -23,8 +23,8 @@
     },
     "###Logger": {
       "Collapsed": "0",
-      "Pos": "3,323",
-      "Size": "390,385"
+      "Pos": "5,274",
+      "Size": "390,440"
     },
     "###Program Log": {
       "Collapsed": "0",
@@ -45,6 +45,11 @@
       "Collapsed": "0",
       "Pos": "685,25",
       "Size": "590,690"
+    },
+    "Warning": {
+      "Collapsed": "0",
+      "Pos": "596,12",
+      "Size": "89,695"
     }
   }
 }

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -32,4 +34,23 @@ public static final class CAN {
     public static final int BL = 4;
     public static final int BR = 11;
   }
+
+  public static final class SysID{
+    public static final double ks = -0.31008;
+    public static final double kv = 4.9366;
+    public static final double ka = 7.5013;
+    public static final double kp = 0.018;
+
+    public static final double MaxSpeed = 3;
+    public static final double MaxAcceleration = 1;
+
+    public static final double trackwidth = .9372;
+    public static final DifferentialDriveKinematics kDriveKinematics =
+    new DifferentialDriveKinematics(trackwidth);
+
+    public static final double kRamseteB = 2;
+    public static final double kRamseteZeta = 0.7;
+
+
+  }
 }

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

@@ -4,14 +4,25 @@
 
 package frc.robot;
 
+import frc.robot.Constants.SysID;
 //import frc.robot.commands.TurnToAngle;
 import frc.robot.subsystems.Drivetrain;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.Logger;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+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.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -86,7 +97,43 @@ public void updateLogger(){
 }
 
    public Command getAutonomousCommand() {
-    // An example command will be run in autonomous
-    return null;
+//     var autoVoltageConstraint =
+//       new DifferentialDriveVoltageConstraint(
+//         new SimpleMotorFeedforward(SysID.ks, SysID.kv,SysID.ka), SysID.kDriveKinematics, 10);
+
+//         TrajectoryConfig config =
+//         new TrajectoryConfig(
+//                 SysID.MaxSpeed,
+//                 SysID.MaxAcceleration)
+//             // Add kinematics to ensure max speed is actually obeyed
+//             .setKinematics(SysID.kDriveKinematics)
+//             // Apply the voltage constraint
+//             .addConstraint(autoVoltageConstraint);
+
+//     Trajectory tr = 
+//     TrajectoryGenerator.generateTrajectory(new Pose2d(0,0,new Rotation2d(0)),null , new Pose2d(0,1,new Rotation2d(0)), null);
+//     RamseteCommand ramseteCommand =
+//     new RamseteCommand(
+//         tr,
+//         drivetrain::getPose,
+//         new RamseteController(SysID.kRamseteB, SysID.kRamseteZeta),
+//         new SimpleMotorFeedforward(
+//             SysID.ks,
+//             SysID.kv,
+//             SysID.ka),
+//         SysID.kDriveKinematics,
+//         drivetrain::getWheelSpeeds,
+//         new PIDController(SysID.kp, 0, 0),
+//         new PIDController(SysID.kp, 0, 0),
+//        (double left, double right) -> drivetrain.tankDriveVolts
+//       ,
+//         drivetrain);
+
+// // Reset odometry to the starting pose of the trajectory.
+// drivetrain.resetOdometry(tr.getInitialPose());
+
+// Run path following command, then stop at the end.
+return null;
+
   }
 }

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

@@ -15,19 +15,32 @@
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+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.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.NetworkTableValue;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc.robot.Constants;
+import frc.robot.Constants.SysID;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Config;
 import io.github.oblarg.oblog.annotations.Log;
@@ -59,6 +72,8 @@ public class Drivetrain extends SubsystemBase implements Loggable {
   private final PIDController ddcontrol = new PIDController(.1, 0, 0.01);
   private final PIDController anglecontrol = new PIDController(0.018, 0, 0.0027);
 
+  private final DifferentialDriveOdometry  odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), FLEncoder.getPosition(), FREncoder.getPosition());
+
   // Create double for logging the yaw of the robot
   @Log private double pitch = 0;
   @Log private double roll = 0;
@@ -91,6 +106,7 @@ public class Drivetrain extends SubsystemBase implements Loggable {
   //  anglecontrol.setSetpoint(0);
   // }
 
+
 
   /** Creates a new ExampleSubsystem. */
   public Drivetrain() {
@@ -113,6 +129,9 @@ public Drivetrain() {
     motorFL.burnFlash();
     motorBR.burnFlash();
     motorBL.burnFlash();
+
+
+
   }
 
 
@@ -130,6 +149,8 @@ public void periodic() {
     BRENCValue = BREncoder.getPosition();
 
     tx = limelightTable.getEntry("tx").getValue().getDouble();
+
+    odometry.update(gyro.getRotation2d(), FLEncoder.getPosition(), FREncoder.getPosition());
   }
 
   @Override
@@ -148,6 +169,14 @@ public void drive(double speed, double turn) {
     robotDrive.feed();
   }
 
+  public Pose2d getPose2d(){
+    return odometry.getPoseMeters();
+  }
+
+  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+    return new DifferentialDriveWheelSpeeds(FLEncoder.getVelocity(), FREncoder.getVelocity());
+  }
+
   // Limelight Functions Start
 
   @Log
@@ -198,4 +227,13 @@ public PIDCommand getDriveEncDistanceCommandFL(double setpoint){
     //c.getController().setTolerance(5);
     return c;
   }
+  public void tankDriveVolts(double left, double right){
+    motorFL.setVoltage(left);
+    motorFR.setVoltage(right);
+  }
+
+
+  // public Command getAuto(){
+
+  // }
 }