Test build of the amp shot button

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,10 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

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

@@ -13,7 +13,58 @@
  * 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 distancePerRotation=10.0;
+    public static double movementPerRotation=10.0;
+  }
+
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
+    public static final int snapButton=1;
+    public static final int AmpShotButton=2;
+  }
+
+  public static class PIDConstants {
+    public static final double kDrivetrainP = 0.2;
+    public static final double kDrivetrainI = 0.0;
+    public static final double kDrivetrainD = 0.0;
+    public static final double kRobotTurnP = 0.2;
+    public static final double kRobotTurnI = 0.0;
+    public static final double kRobotTurnD = 0.0;
+    public static final double kRobotMoveP = 0.2;
+    public static final double kRobotMoveI = 0.0;
+    public static final double kRobotMoveD = 0.0;
+  }
+  public static class AprilTags{
+    //this stores the settings for the april tag detector config and the families to be detected by the detector
+    //remember to actually configure these
+    public static final int numThreads=1;
+    public static final float quadDecimate=2;
+    public static final float quadSigma=0;
+    //6.5 inches to mm
+    public static final double normalTagLength=165.1;
+    //the number of pixels the border of the april tag will have at 1m. 
+    public static final double pixelsAtMeter=400;
+    //the X coordinate of the center when the april tag is aligned with the camera. 
+    public static final double centerXCoordinate=300;
+    //https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html
+    public static final String family="36h11";
+  }
+  public static class AmpAprilTag{
+    //remember to actually fill this in with the correct april tags
+    public static final int ampNum=0;
+    //the optimal distance at which the note can be inserted into the amp in m
+    public static final double optimalDistance=0.1;
   }
 }

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

@@ -4,7 +4,9 @@
 
 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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -18,6 +20,11 @@ public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
+  private Talon FrontLeftMove;
+  private Talon FrontRightMove;
+  private Talon BackLeftMove;
+  private Talon BackRightMove;
+  private Encoder frontLeftMove;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -87,11 +94,28 @@ public void teleopPeriodic() {}
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+    FrontLeftMove= new Talon(0);
+    FrontRightMove= new Talon(0);
+    BackLeftMove= new Talon(0);
+    BackRightMove= new Talon(0);
+    frontLeftMove = new Encoder(0, 0);
+    FrontLeftMove.set(0.1);
+    FrontRightMove.set(0.1);
+    BackLeftMove.set(0.1);
+    BackRightMove.set(0.1);
   }
 
+
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+    if (frontLeftMove.get()==100){
+      FrontLeftMove.set(0);
+      FrontRightMove.set(0);
+      BackLeftMove.set(0);
+      BackRightMove.set(0);
+    }
+  }
 
   /** This function is called once when the robot is first started up. */
   @Override

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

@@ -4,13 +4,37 @@
 
 package frc.robot;
 
-import frc.robot.Constants.OperatorConstants;
+import frc.robot.commands.AlignWithAprilTag;
+// 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.commands.AlignWithAprilTag;
+import frc.robot.commands.MoveToAmp;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Encoders;
+import frc.robot.subsystems.Motors;
+import frc.robot.subsystems.Gyroscope;
+import frc.robot.subsystems.TagDetector;
+import frc.robot.Constants.Ports;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.Constants.MotorConstants;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagDetector.Config;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+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
@@ -21,15 +45,52 @@
 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 AprilTagDetector detector= new AprilTagDetector();
+  private final AprilTagDetector.Config configuration= new Config();
+
+
+  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);
+  private final TagDetector tagDetector= new TagDetector(detector, configuration);
+
+  // remember to set the joystick port
 
-  // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+  private Joystick stick = new Joystick(OperatorConstants.kDriverControllerPort);
+  // this is the button on the handle of the joystick
+  private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButton);
+  private JoystickButton ampShotButton= new JoystickButton(stick, OperatorConstants.AmpShotButton);
+
+  //remember to add the actual shoot into amp command in here. 
+  private final SequentialCommandGroup autoAmpShot= new SequentialCommandGroup(new AlignWithAprilTag(encoders, motors, tagDetector), new MoveToAmp(encoders, motors, tagDetector));
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
+    defaultCommands();
   }
 
   /**
@@ -42,15 +103,13 @@ 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());
+    snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick));
+    ampShotButton.whileTrue(autoAmpShot);
+
+  }
+  private void defaultCommands(){
+    motors.setDefaultCommand(new Drive(motors,encoders,gyroscope,stick));
   }
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *