Drive base command impl

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

@@ -8,12 +8,34 @@
 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 globally (i.e. public static).  Do not put anything functional in this class.
+ * 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
- * constants are needed, to reduce verbosity.
+ * <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 {
+	// Ports
+	public static final int LEFT_JOYSTICK_PORT = 0;
+	public static final int RIGHT_JOYSTICK_PORT = 1;
+	public static final int MANIPULATOR_PORT = 2;
+
+	public static final int LEFT_DRIVE_MOTOR_0_CAN_ID = 10;
+	public static final int LEFT_DRIVE_MOTOR_1_CAN_ID = 11;
+	public static final int LEFT_DRIVE_MOTOR_2_CAN_ID = 12;
+
+	public static final int RIGHT_DRIVE_MOTOR_0_CAN_ID = 20;
+	public static final int RIGHT_DRIVE_MOTOR_1_CAN_ID = 21;
+	public static final int RIGHT_DRIVE_MOTOR_2_CAN_ID = 22;
+
+	public static final int LEFT_DRIVE_ENCODER_PORT_A = 0;
+	public static final int LEFT_DRIVE_ENCODER_PORT_B = 1;
+	public static final int RIGHT_DRIVE_ENCODER_PORT_A = 2;
+	public static final int RIGHT_DRIVE_ENCODER_PORT_B = 3;
+
+	public static final int LEFT_SHIFTER_PORT = 4;
+	public static final int RIGHT_SHIFTER_PORT = 3;
 }

src/main/java/frc/robot/Main.java

@@ -10,9 +10,9 @@
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
- * you are doing, do not modify this file except to change the parameter class to the startRobot
- * call.
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to change
+ * the parameter class to the startRobot call.
  */
 public final class Main {
   private Main() {
@@ -21,7 +21,8 @@ private Main() {
   /**
    * Main initialization function. Do not perform any initialization here.
    *
-   * <p>If you change your main robot class, change the parameter type.
+   * <p>
+   * If you change your main robot class, change the parameter type.
    */
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);

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

@@ -12,9 +12,10 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * 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
- * the package after creating this project, you must also update the build.gradle file in the
+ * 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 the package after
+ * creating this project, you must also update the build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
@@ -23,28 +24,34 @@ public class Robot extends TimedRobot {
   private RobotContainer m_robotContainer;
 
   /**
-   * This function is run when the robot is first started up and should be used for any
-   * initialization code.
+   * This function is run when the robot is first started up and should be used
+   * for any initialization code.
    */
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
   }
 
   /**
-   * This function is called every robot packet, no matter the mode. Use this for items like
-   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   * This function is called every robot packet, no matter the mode. Use this for
+   * items like diagnostics that you want ran during disabled, autonomous,
+   * teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled
+    // commands, running already-scheduled commands, removing finished or
+    // interrupted commands,
+    // and running subsystem periodic() methods. This must be called from the
+    // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
@@ -61,7 +68,8 @@ public void disabledPeriodic() {
   }
 
   /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
    */
   @Override
   public void autonomousInit() {

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

@@ -7,51 +7,83 @@
 
 package frc.robot;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.XboxController;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.commands.ArcadeDriveCommand;
+import frc.robot.commands.ToggleShifterCommand;
+import frc.robot.misc2020.EnhancedJoystick;
+import frc.robot.misc2020.Gamepad;
+import frc.robot.subsystems.DriveBase;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
- * 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}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * 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} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
-
-  private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
 
+  EnhancedJoystick leftJoystick;
+  EnhancedJoystick rightJoystick;
+  Gamepad manipulator;
 
+  DriveBase driveBase;
 
   /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   * The container for the robot. Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
-    // Configure the button bindings
+    leftJoystick = new EnhancedJoystick(Constants.LEFT_JOYSTICK_PORT);
+    rightJoystick = new EnhancedJoystick(Constants.RIGHT_JOYSTICK_PORT);
+    manipulator = new Gamepad(Constants.MANIPULATOR_PORT);
+
+    WPI_TalonSRX leftDrive0 = new WPI_TalonSRX(Constants.LEFT_DRIVE_MOTOR_0_CAN_ID);
+    WPI_VictorSPX leftDrive1 = new WPI_VictorSPX(Constants.LEFT_DRIVE_MOTOR_1_CAN_ID);
+    WPI_VictorSPX leftDrive2 = new WPI_VictorSPX(Constants.LEFT_DRIVE_MOTOR_2_CAN_ID);
+
+    WPI_TalonSRX rightDrive0 = new WPI_TalonSRX(Constants.RIGHT_DRIVE_MOTOR_0_CAN_ID);
+    WPI_VictorSPX rightDrive1 = new WPI_VictorSPX(Constants.RIGHT_DRIVE_MOTOR_1_CAN_ID);
+    WPI_VictorSPX rightDrive2 = new WPI_VictorSPX(Constants.RIGHT_DRIVE_MOTOR_2_CAN_ID);
+
+    leftDrive0.setInverted(true);
+    leftDrive2.setInverted(true);
+    rightDrive1.setInverted(true);
+
+    driveBase = new DriveBase(new SpeedControllerGroup(leftDrive0, leftDrive1, leftDrive2),
+        new SpeedControllerGroup(rightDrive0, rightDrive1, rightDrive2), Constants.LEFT_SHIFTER_PORT,
+        Constants.RIGHT_SHIFTER_PORT, Constants.LEFT_DRIVE_ENCODER_PORT_A, Constants.LEFT_DRIVE_ENCODER_PORT_B,
+        Constants.RIGHT_DRIVE_ENCODER_PORT_A, Constants.RIGHT_DRIVE_ENCODER_PORT_B);
+
     configureButtonBindings();
+
+    driveBase.setDefaultCommand(new ArcadeDriveCommand(driveBase, () -> leftJoystick.getX(), () -> -rightJoystick.getY()));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be
+   * created by instantiating a {@link GenericHID} or one of its subclasses
+   * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+   * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    new JoystickButton(leftJoystick, 4).toggleWhenActive(new ToggleShifterCommand(driveBase));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
-    return m_autoCommand;
+    return new InstantCommand();
   }
-}
+}

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

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveBase;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ArcadeDriveCommand extends CommandBase {
+    private final DriveBase driveBase;
+    private final DoubleSupplier leftPowerSupplier;
+    private final DoubleSupplier rightPowerSupplier;
+    private final BooleanSupplier squareInputs;
+
+    public ArcadeDriveCommand(DriveBase driveBase, DoubleSupplier leftPowerSupplier, DoubleSupplier rightPowerSupplier,
+            BooleanSupplier squareInputs) {
+        this.driveBase = driveBase;
+        this.leftPowerSupplier = leftPowerSupplier;
+        this.rightPowerSupplier = rightPowerSupplier;
+        this.squareInputs = squareInputs;
+        addRequirements(driveBase);
+    }
+
+    public ArcadeDriveCommand(DriveBase driveBase, DoubleSupplier leftPowerSupplier,
+            DoubleSupplier rightPowerSupplier) {
+        this(driveBase, leftPowerSupplier, rightPowerSupplier, () -> false);
+    }
+
+    @Override
+    public void execute() {
+        driveBase.arcadeDrive(leftPowerSupplier.getAsDouble(), rightPowerSupplier.getAsDouble(), squareInputs.getAsBoolean());
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        driveBase.stopMotor();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveBase;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class CurvatureDriveCommand extends CommandBase {
+    private final DriveBase driveBase;
+    private final DoubleSupplier leftPowerSupplier;
+    private final DoubleSupplier rightPowerSupplier;
+    private final BooleanSupplier squareInputs;
+
+    public CurvatureDriveCommand(DriveBase driveBase, DoubleSupplier leftPowerSupplier,
+            DoubleSupplier rightPowerSupplier, BooleanSupplier squareInputs) {
+        this.driveBase = driveBase;
+        this.leftPowerSupplier = leftPowerSupplier;
+        this.rightPowerSupplier = rightPowerSupplier;
+        this.squareInputs = squareInputs;
+        addRequirements(driveBase);
+    }
+
+    public CurvatureDriveCommand(DriveBase driveBase, DoubleSupplier leftPowerSupplier,
+            DoubleSupplier rightPowerSupplier) {
+        this(driveBase, leftPowerSupplier, rightPowerSupplier, () -> false);
+    }
+
+    @Override
+    public void execute() {
+        driveBase.curvatureDrive(leftPowerSupplier.getAsDouble(), rightPowerSupplier.getAsDouble(), squareInputs.getAsBoolean());
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        driveBase.stopMotor();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -14,7 +14,6 @@
  * An example command that uses an example subsystem.
  */
 public class ExampleCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
   private final ExampleSubsystem m_subsystem;
 
   /**

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

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveBase;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class SetHighGearCommand extends InstantCommand {
+    public SetHighGearCommand(DriveBase driveBase, boolean highGear) {
+        super(() -> driveBase.setHighGear(highGear), driveBase);
+    }
+
+    public SetHighGearCommand(DriveBase driveBase) {
+        this(driveBase, true);
+    }
+}