Finished drivetrain and intake

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

@@ -22,14 +22,14 @@ public final class Constants {
      */
     public static final class Motors {
         public static final class DriveTrainMotors {
-            public static final int FRMOTOR = 0;
-            public static final int FLMOTOR = 1;
-            public static final int BRMOTOR = 2;
-            public static final int BLMOTOR = 3;
+            public static final int FRMOTOR = 5;
+            public static final int FLMOTOR = 6;
+            public static final int BRMOTOR = 4;
+            public static final int BLMOTOR = 7;
         }
         public static final class IntakeMotors {
-            public static final int LMOTOR = 4;
-            public static final int RMOTOR = 5;
+            public static final int LMOTOR = 1;
+            public static final int RMOTOR = 2;
         }
     }
 

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

@@ -1,13 +1,16 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Robot.RobotRunType;
 import frc.robot.commands.Drive;
+import frc.robot.commands.IntakeCommand;
+import frc.robot.commands.OutTakeCommand;
+import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.drive.Drivetrain;
 import frc.robot.subsystems.drive.DrivetrainIO;
 import frc.robot.subsystems.drive.DrivetrainReal;
@@ -20,12 +23,13 @@
  */
 public class RobotContainer {
     /* Controllers */
-    XboxController driver = new XboxController(0);
+    CommandXboxController driver = new CommandXboxController(0);
     // Initialize AutoChooser Sendable
     private final SendableChooser<String> autoChooser = new SendableChooser<>();
 
     /* Subsystems */
     private Drivetrain driveTrain;
+    private Intake intake;
 
 
     /**
@@ -45,6 +49,7 @@ public RobotContainer(RobotRunType runtimeType) {
                 driveTrain = new Drivetrain(new DrivetrainIO() {});
         }
         // Configure the button bindings
+        driveTrain.setDefaultCommand(new Drive(driveTrain, driver));
         configureButtonBindings();
     }
 
@@ -58,8 +63,9 @@ public RobotContainer(RobotRunType runtimeType) {
     ;
 
     private void configureButtonBindings() {
+        driver.rightTrigger().whileTrue(new OutTakeCommand(intake, driver));
+        driver.leftTrigger().whileTrue(new IntakeCommand(intake, driver));
 
-        driveTrain.setDefaultCommand(new Drive(driveTrain, driver));
 
 
     }

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

@@ -1,22 +1,22 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.drive.Drivetrain;
 
 /**
  * Drive.
  */
 
 public class Drive extends Command {
-    private XboxController controller;
+    private CommandXboxController controller;
     private Drivetrain drive;
 
     /**
      * Drive again.
      */
 
-    public Drive(Drivetrain drive, XboxController controller) {
+    public Drive(Drivetrain drive, CommandXboxController controller) {
         this.controller = controller;
         this.drive = drive;
         addRequirements(drive);
@@ -26,7 +26,7 @@ public Drive(Drivetrain drive, XboxController controller) {
     public void execute() {
         double leftY = (Math.abs(controller.getLeftY()) < .05) ? 0 : controller.getLeftY();
         double rightY = (Math.abs(controller.getRightY()) < .05) ? 0 : controller.getRightY();
-        this.drive.setPower(leftY, rightY);
+        drive.setPower(leftY, rightY);
 
     }
 }

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

@@ -0,0 +1,23 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.Intake.Intake;
+
+public class IntakeCommand extends Command {
+    private CommandXboxController controller;
+    private Intake intake;
+
+    public IntakeCommand(Intake intake, CommandXboxController controller) {
+        this.controller = controller;
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void execute() {
+        intake.setIntakeVoltage(1);
+        controller.leftTrigger().whileTrue(new InstantCommand(() -> intake.setIntakeVoltage(-1)));
+    }
+}

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

@@ -0,0 +1,24 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.Intake.Intake;
+
+public class OutTakeCommand extends Command {
+    private CommandXboxController controller;
+    private Intake intake;
+
+    public OutTakeCommand(Intake intake, CommandXboxController controller) {
+        this.controller = controller;
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void execute() {
+        intake.setIntakeVoltage(-1);
+        controller.rightTrigger().whileTrue(new InstantCommand(() -> intake.setIntakeVoltage(1)));
+        controller.leftTrigger().whileTrue(new InstantCommand(() -> intake.setIntakeVoltage(-1)));
+    }
+}

src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java

@@ -22,7 +22,6 @@ public DrivetrainReal() {
         FRMotor.addFollower(BRMotor);
 
         FRMotor.setInverted(true);
-        BRMotor.setInverted(true);
     }
 
 
@@ -35,8 +34,8 @@ public void updateInputs(DrivetrainIOInputs inputs) {
      * Drive Voltage
      */
     public void setDriveVoltage(double lvolts, double rvolts) {
-        FRMotor.setVoltage(lvolts);
-        FRMotor.setVoltage(rvolts);
+        FLMotor.set(lvolts);
+        FRMotor.set(rvolts);
     }
 
 }

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

@@ -1,5 +1,22 @@
-package frc.robot.subsystems.intake;
+package frc.robot.subsystems.Intake;
 
-public class intake {
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Intake extends SubsystemBase {
+    private IntakeIO io;
+    private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+    public Intake(IntakeIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    }
+
+    public void setIntakeVoltage(int power) {
+        io.setIntakeVoltage(power);
+    }
 
 }

src/main/java/frc/robot/subsystems/intake/intakeIO.java

@@ -1,18 +1,19 @@
-package frc.robot.subsystems.intake;
+package frc.robot.subsystems.Intake;
 
 import org.littletonrobotics.junction.AutoLog;
 
-public interface intakeIO {
+public interface IntakeIO {
 
     @AutoLog
-    public static class intakeIOInputs {
+    public static class IntakeIOInputs {
         public double rightMotor;
         public double leftMotor;
     }
 
     /** Updates the set of loggable inputs. */
+    public default void updateInputs(IntakeIOInputs inputs) {}
 
-    public default void intakePower(double lvolts, double rvolts) {}
+    public default void setIntakeVoltage(double power) {}
 
 }
 

src/main/java/frc/robot/subsystems/intake/intakeReal.java

@@ -1,20 +1,20 @@
-package frc.robot.subsystems.intake;
+package frc.robot.subsystems.Intake;
 
 import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
 
-public class intakeReal implements intakeIO {
+public class intakeReal implements IntakeIO {
     VictorSP leftIntake = new VictorSP(frc.robot.Constants.Motors.IntakeMotors.LMOTOR);
     VictorSP rightIntake = new VictorSP(frc.robot.Constants.Motors.IntakeMotors.RMOTOR);
 
-    public intakeReal() {
+
+    public void setIntakeVoltage(double volts) {
         rightIntake.setInverted(true);
+        leftIntake.set(volts);
+        rightIntake.set(volts);
     }
 
-
-    public void intakeVoltage(double lvolts, double rvolts) {
-        leftIntake.set(lvolts);
-        rightIntake.set(rvolts);
-    }
+    @Override
+    public void updateInputs(IntakeIOInputs inputs) {};
 
 }