fix broken merge stuff & use diff drive in cmds

--Gabe

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

@@ -17,19 +17,17 @@
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.Autos;
 import frc.robot.commands.Balance;
-import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.autotasks.ExampleAutoTask;
 import frc.robot.subsystems.Drive;
-import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.TaskScheduler;
 import frc.robot.commands.CloseClawCone;
 import frc.robot.commands.CloseClawCube;
 import frc.robot.commands.DeployPlunger;
-import frc.robot.commands.DriveFieldOriented;
+import frc.robot.commands.DriveTank;
 import frc.robot.commands.DriveForwards;
 import frc.robot.commands.DriveReverse;
-import frc.robot.commands.DriveRobotOriented;
+import frc.robot.commands.DriveArcade;
 import frc.robot.commands.ExtendArm;
 import frc.robot.commands.ManualArm;
 import frc.robot.commands.RetractArm;
@@ -56,7 +54,12 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  private final Drive driveSubsystem = new Drive();
+  /* Messy, ugly commands and subsystems section
+   * TODO: organize later
+   */
+  /* Autotasks are mostly commented out in here for now because I don't care that they exist */
+  private final Limelight limelight = new Limelight();
+  private final Drive driveSubsystem = new Drive(limelight);
   /* BIG CHUNGUS ARM CODE */
   private final I2CManager I2CManager = new I2CManager();
   private final ArmBase armBase = new ArmBase();
@@ -75,31 +78,18 @@ public class RobotContainer {
   private final DriveForwards driveForwards = new DriveForwards(driveSubsystem);
   private final DriveReverse driveReverse = new DriveReverse(driveSubsystem);
   private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera);
-
   private final Balance balance = new Balance(driveSubsystem);
-  private final DriveRobotOriented driveRO = new DriveRobotOriented(driveSubsystem);
-  private final DriveFieldOriented driveFO = new DriveFieldOriented(driveSubsystem);
-
+  private final DriveArcade driveRO = new DriveArcade(driveSubsystem);
+  private final DriveTank driveFO = new DriveTank(driveSubsystem);
   private final Command openClaw = armClaw.manualOpenClawCommand();
   private final Command closeClaw = armClaw.manualCloseClawCommand();
   private final CloseClawCone closeClawCone = new CloseClawCone(armClaw);
   private final CloseClawCube closeClawCube = new CloseClawCube(armClaw);
-
   private final ExtendArm extend = new ExtendArm(armExtender);
   private final RetractArm retract = new RetractArm(armExtender);
-  /* SUBSYSTEMS */
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
-  private final Limelight limelight = new Limelight();
-  private final Drive driveSubsystem = new Drive(limelight);
   private final TaskScheduler taskScheduler = new TaskScheduler();
-  private final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
-
-  /* COMMANDS */
-  private final Balance balance = new Balance(driveSubsystem);
-  private final ExampleCommand exampleCommand = new ExampleCommand(exampleSubsystem);
-
   /* AUTO TASKS */
-  private final ExampleAutoTask exampleAutoTask = new ExampleAutoTask(exampleCommand);
+  //private final ExampleAutoTask exampleAutoTask = new ExampleAutoTask(exampleCommand);
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
   public static CommandXboxController controller =
@@ -231,7 +221,7 @@ private void configureBindings() {
   }
 
   private void verifyAutoTasks() {
-    exampleAutoTask.verify();
+    //exampleAutoTask.verify();
   }
 
   /**

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

@@ -55,6 +55,7 @@ public void execute() {
       double rollAngleRadians = rollAngleDegrees * (Math.PI / 180.0);
       yAxisRate = Math.sin(rollAngleRadians) * -1;
     }
+    /* TODO: quinn fix this */
     drive.moveMecanumRobot((-xAxisRate * Constants.BalanceConstants.SPEED_MULTIPLIER), (-yAxisRate * Constants.BalanceConstants.SPEED_MULTIPLIER), 0);
   }
 

src/main/java/frc/robot/commands/DriveRobotOriented.java → src/main/java/frc/robot/commands/DriveArcade.java

@@ -5,15 +5,14 @@
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.Drive;
 
-public class DriveRobotOriented extends CommandBase {
+public class DriveArcade extends CommandBase {
 
   /* Variables */
   private final Drive drivetrain;
-  private double mecanumX;
-  private double mecanumY;
-  private double mecanumZ;
+  private double x;
+  private double z;
 
-  public DriveRobotOriented(Drive driveSubsystem) {
+  public DriveArcade(Drive driveSubsystem) {
     drivetrain = driveSubsystem;
     addRequirements(driveSubsystem);
   }
@@ -26,11 +25,10 @@ public void initialize() {
   @Override
   public void execute() {
     /* Gets the left and right axes of the robot and uses that to move */
-    mecanumX = RobotContainer.getScaledControllerLeftXAxis();
-    mecanumY = RobotContainer.getScaledControllerLeftYAxis();
-    mecanumZ = RobotContainer.getScaledControllerRightXAxis();
+    x = RobotContainer.getScaledControllerLeftYAxis();
+    z = RobotContainer.getScaledControllerRightXAxis();
     /*mecanumZ = RobotContainer.getScaledRightXAxis();*/
-    drivetrain.moveMecanumRobot(mecanumY, mecanumX, mecanumZ);
+    drivetrain.moveArcade(x, z);
   }
 
   @Override

src/main/java/frc/robot/commands/DriveFieldOriented.java → src/main/java/frc/robot/commands/DriveTank.java

@@ -5,15 +5,14 @@
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.Drive;
 
-public class DriveFieldOriented extends CommandBase {
+public class DriveTank extends CommandBase {
 
   /* Variables */
   private final Drive drivetrain;
-  private double mecanumX;
-  private double mecanumY;
-  private double mecanumZ;
+  private double left;
+  private double right;
 
-  public DriveFieldOriented(Drive driveSubsystem) {
+  public DriveTank(Drive driveSubsystem) {
     drivetrain = driveSubsystem;
     addRequirements(driveSubsystem);
   }
@@ -26,10 +25,9 @@ public void initialize() {
   @Override
   public void execute() {
     /* Gets the left and right axes of the robot and uses that to move */
-    mecanumX = RobotContainer.getScaledControllerLeftXAxis();
-    mecanumY = RobotContainer.getScaledControllerLeftYAxis();
-    mecanumZ = RobotContainer.getScaledControllerRightXAxis();
-    drivetrain.moveMecanumField(mecanumY, mecanumX, mecanumZ);
+    left = RobotContainer.getScaledControllerLeftYAxis();
+    right = RobotContainer.getScaledControllerRightYAxis();
+    drivetrain.moveTank(left, right);
   }
 
   @Override

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

@@ -22,7 +22,7 @@ public StartLeavingCommunity(Drive driveSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    drivetrain.moveMecanumRobot(0.5, 0, 0);
+    drivetrain.moveArcade(0.5, 0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.