Added the necessary code for paths

.DataLogTool/datalogtool.json

@@ -0,0 +1,5 @@
+{
+  "download": {
+    "serverTeam": "2626"
+  }
+}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

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
@@ -55,4 +57,34 @@ public static class DIGITAL {
     public final int INTAKE_LIMIT_SWITCH_OUT = 6;
     public final int INTAKE_LIMIT_SWITCH_IN = 7;
   }
+
+  /*
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
+   */
+  public static class DriveConstants {
+    public static final double ksVolts = 0.22;
+    public static final double kvVoltSecondsPerMeter = 1.98;
+    public static final double kaVoltSecondsSquaredPerMeter = 0.2;
+
+    public static final double kPDriveVel = 8.5;
+
+    public static final double kTrackwidthMeters = 0.58;
+    public static final DifferentialDriveKinematics kDriveKinematics =
+        new DifferentialDriveKinematics(kTrackwidthMeters);
+
+    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 1;
+
+    public static final double kRamseteB = 2;
+    public static final double kRamseteZeta = 0.7;
+  }
 }

src/main/java/frc/robot/Robot.java

@@ -4,11 +4,17 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryUtil;
 import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import java.io.IOException;
+import java.nio.file.Path;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -23,6 +29,9 @@ public class Robot extends TimedRobot {
 
   public Compressor compressor;
 
+  String trajectoryJSON = "paths/YourPath.wpilib.json";
+  Trajectory trajectory = new Trajectory();
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -33,6 +42,13 @@ public void robotInit() {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
     compressor = new Compressor(1, PneumaticsModuleType.CTREPCM);
+
+    try {
+      Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
+      trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
+    } catch (IOException ex) {
+      DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace());
+    }
   }
 
   /**

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

@@ -16,7 +16,6 @@
 public class SetShooterAngleCommand extends PIDCommand {
   /** Creates a new SetShooterAngleCommand. */
   private AngleShooter angleShooter;
-
   Limelight limelight;
 
   public SetShooterAngleCommand(AngleShooter angleShooter, Limelight limelight) {

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

@@ -4,11 +4,10 @@
 
 package frc.robot.subsystems;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.util.ReplanningConfig;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
@@ -17,7 +16,6 @@
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -41,15 +39,13 @@ public class Drivetrain extends SubsystemBase {
   public boolean isTankDrive;
   public static final ADIS16470_IMU gyro = new ADIS16470_IMU();
   private MecanumDrive m_robotDrive;
-
-  /** Creates a new TankDrivetrain. */
-  public Drivetrain() {
-    // ADIS16470_IMU gyro = new ADIS16470_IMU();
   private DifferentialDriveOdometry odometry;
   private DifferentialDriveKinematics kinematics;
 
   /** Creates a new TankDrivetrain. */
   public Drivetrain() {
+    // ADIS16470_IMU gyro = new ADIS16470_IMU();
+
     piston =
         new DoubleSolenoid(1, PneumaticsModuleType.CTREPCM, pcm.PISTON_FORWARD, pcm.PISTON_REVERSE);
 
@@ -70,32 +66,21 @@ public Drivetrain() {
     arriereGaucheEncoder = arrieregauche.getEncoder();
     m_robotDrive.setSafetyEnabled(false);
 
-    // TODO: add where we start on the field?
+    avantDroitEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000);
+    arriereDroitEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000);
+    avantGaucheEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000);
+    arriereGaucheEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000);
+
+    avantDroitEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100);
+    arriereDroitEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100);
+    avantGaucheEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100);
+    arriereGaucheEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100);
+
     odometry =
         new DifferentialDriveOdometry(
             getRotation2d(), avantGaucheEncoder.getPosition(), avantDroitEncoder.getPosition());
 
     kinematics = new DifferentialDriveKinematics(58.6);
-
-    // à faire
-    AutoBuilder.configureRamsete(
-        this::getPose2d,
-        this::resetPose2d,
-        this::getChassisSpeeds,
-        this::drivePathplanner,
-        new ReplanningConfig(),
-        () -> {
-          // Boolean supplier that controls when the path will be mirrored for the red alliance
-          // This will flip the path being followed to the red side of the field.
-          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-
-          var alliance = DriverStation.getAlliance();
-          if (alliance.isPresent()) {
-            return alliance.get() == DriverStation.Alliance.Red;
-          }
-          return false;
-        },
-        this);
   }
 
   public Pose2d getPose2d() {
@@ -112,8 +97,12 @@ public ChassisSpeeds getChassisSpeeds() {
   }
 
   public DifferentialDriveWheelSpeeds getWheelSpeed() {
-    return new DifferentialDriveWheelSpeeds(
-        arriereGaucheEncoder.getVelocity() / 60, arriereDroitEncoder.getVelocity() / 60);
+    double leftEncoder =
+        (arriereGaucheEncoder.getVelocity() + avantGaucheEncoder.getVelocity()) / 2;
+    double rightEncoder = (arriereDroitEncoder.getVelocity() + avantDroitEncoder.getVelocity()) / 2;
+
+    // TODO: check the division
+    return new DifferentialDriveWheelSpeeds(leftEncoder, rightEncoder);
   }
 
   public double getGyroAngle() {
@@ -216,6 +205,13 @@ public void driveTank(double joystickDroit, double joystickGauche) {
     arrieredroit.set(joystickDroit);
   }
 
+  public void tankDriveVolts(double leftVolts, double rightVolts) {
+    avantdroit.setVoltage(rightVolts);
+    arrieredroit.setVoltage(rightVolts);
+    avantgauche.setVoltage(leftVolts);
+    arrieregauche.setVoltage(leftVolts);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler

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

@@ -24,7 +24,7 @@ public class Intake extends SubsystemBase {
 
   DigitalInput intakeLimitOut = new DigitalInput(degitalInput.INTAKE_LIMIT_SWITCH_OUT);
   DigitalInput intakeLimitIn = new DigitalInput(degitalInput.INTAKE_LIMIT_SWITCH_IN);
-  
+
   public Intake() {
     intakeDroit = new CANSparkMax(deviceNumber.DeviceNumberIntakeDroit, MotorType.kBrushless);
     intakeGauche = new CANSparkMax(deviceNumber.DeviceNumberIntakeGauche, MotorType.kBrushless);
@@ -57,7 +57,7 @@ public boolean getIntakeLimitOut() {
   public boolean getIntakeLimitIn() {
     return intakeLimitIn.get();
   }
-  
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

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

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.util.ControlMode.*;
 
-
 public class Limelight extends SubsystemBase {
   /** Creates a new Limelight. */
   private NetworkTable networkTable;