Created the basic code for the driver controlled swerve drive

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

@@ -4,16 +4,54 @@
 
 package frc.robot;
 
+
+
 /**
- * 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
+ * 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
  * globally (i.e. public static). Do not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+  public static class Ports {
+    public static int kDriveFrontLeftMove = 1;
+    public static int kDriveFrontLeftTurn = 2;
+    public static int kDriveFrontRightMove = 3;
+    public static int kDriveFrontRightTurn = 4;
+    public static int kDriveBackLeftMove = 5;
+    public static int kDriveBackLeftTurn = 6;
+    public static int kDriveBackRightMove = 7;
+    public static int kDriveBackRightTurn = 8;
+  }
+
+  public static class MotorConstants{
+    public static double movementPerRotation=400;
+  }
+
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
+    public static final int snapButtonNum=1;
+  }
+
+  public static class PIDConstants {
+    public static final double kDrivetrainP = 0.15;
+    public static final double kDrivetrainI = 0.0;
+    public static final double kDrivetrainD = 0.0;
+    public static final double kRobotTurningP=0.15;
+    public static final double kRobotTurningI=0.0;
+    public static final double kRobotTurningD=0.0;
+    public static final double snapMotorP = 0.2;
+    public static final double snapMotorI = 0.2;
+    public static final double snapMotorD = 0.0;
+  }
+  public static class SnapConstants{
+    public static final double snapAngleTolerance=1.0;
+    public static final double snapBearingTolerance=2.0;
   }
 }

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

@@ -4,10 +4,17 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.Talon;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
  * each mode, as described in the TimedRobot documentation. If you change the name of this class or
@@ -19,6 +26,10 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+  private CANSparkMax motor;
+  private long startTime;
+  private long currentTime;
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -81,17 +92,29 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+
+  }
 
   @Override
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+
+    motor = new CANSparkMax(10, MotorType.kBrushless);
+    startTime=System.currentTimeMillis();
   }
 
+
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+    currentTime=System.currentTimeMillis()-startTime;
+    if (currentTime<2000){
+      SmartDashboard.putBoolean("running", true);
+      motor.set(0.1);
+    }
+  }
 
   /** This function is called once when the robot is first started up. */
   @Override

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

@@ -5,13 +5,32 @@
 package frc.robot;
 
 import frc.robot.Constants.OperatorConstants;
+// here is where you put all your commands and subsystems;
 import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
+import frc.robot.commands.Drive;
+import frc.robot.commands.Snap;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Encoders;
+import frc.robot.subsystems.Motors;
+import frc.robot.Constants.Ports;
+import frc.robot.Constants.MotorConstants;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.subsystems.Gyroscope;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.I2C;
+
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -21,15 +40,46 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+  // the video says to define the motors within the motors subsystem, but I will just define it here so I know where all my 
+  // variables are
+
+  // remember to configure the acutal channels
+  private final CANSparkMax frontLeftMove= new CANSparkMax(Ports.kDriveFrontLeftMove,MotorType.kBrushless);
+  private final CANSparkMax frontLeftTurn= new CANSparkMax(Ports.kDriveFrontLeftTurn,MotorType.kBrushless);
+  private final CANSparkMax frontRightMove= new CANSparkMax(Ports.kDriveFrontRightMove,MotorType.kBrushless);
+  private final CANSparkMax frontRightTurn= new CANSparkMax(Ports.kDriveFrontRightTurn,MotorType.kBrushless);
+  private final CANSparkMax backLeftMove= new CANSparkMax(Ports.kDriveBackLeftMove,MotorType.kBrushless);
+  private final CANSparkMax backLeftTurn = new CANSparkMax(Ports.kDriveBackLeftTurn,MotorType.kBrushless);
+  private final CANSparkMax backRightMove= new CANSparkMax(Ports.kDriveBackRightMove,MotorType.kBrushless);
+  private final CANSparkMax backRightTurn= new CANSparkMax(Ports.kDriveBackRightTurn,MotorType.kBrushless);
+  private final AbsoluteEncoder frontLeftMoveEncoder = frontLeftMove.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder frontLeftTurnEncoder = frontLeftTurn.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder frontRightMoveEncoder = frontRightMove.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder frontRightTurnEncoder = frontRightTurn.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder backLeftMoveEncoder = backLeftMove.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder backLeftTurnEncoder = backLeftTurn.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder backRightMoveEncoder = backRightMove.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AbsoluteEncoder backRightTurnEncoder = backRightTurn.getAbsoluteEncoder(Type.kDutyCycle);
+  private final AHRS gyro = new AHRS(I2C.Port.kMXP);
+
+
+  private final Motors motors= new Motors(frontLeftMove, frontLeftTurn, frontRightMove, frontRightTurn, backLeftMove, backLeftTurn, backRightMove, backRightTurn);
+  //motor radius is configured in mm and distance per rotation is still unkown
+  private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, MotorConstants.movementPerRotation);
+  private final Gyroscope gyroscope = new Gyroscope(gyro);
+  // remember to set the joystick port
+
+  private Joystick stick = new Joystick(OperatorConstants.kDriverControllerPort);
+  // this is the button on the handle of the joystick
+  private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButtonNum);
+
 
-  // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
+    defaultCommands();
   }
 
   /**
@@ -42,15 +92,12 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
-
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+
+  }
+  private void defaultCommands(){
+    SmartDashboard.putBoolean("Set default command", false);
+    motors.setDefaultCommand(new Drive(motors,encoders,gyroscope,stick));
   }
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *