Add basic TeleopDrive functionalities

Robot2019/src/main/java/frc/robot/OI.java

@@ -7,36 +7,28 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import frc.robot.commands.SlowDrive;
+
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the commands and command groups that allow control of the robot.
  */
 public class OI {
-  //// CREATING BUTTONS
-  // One type of button is a joystick button which is any button on a
-  //// joystick.
-  // You create one by telling it which joystick it's on and which button
-  // number it is.
-  // Joystick stick = new Joystick(port);
-  // Button button = new JoystickButton(stick, buttonNumber);
-
-  // There are a few additional built in buttons you can use. Additionally,
-  // by subclassing Button you can create custom triggers and bind those to
-  // commands the same as any other Button.
-
-  //// TRIGGERING COMMANDS WITH BUTTONS
-  // Once you have a button, it's trivial to bind it to a button in one of
-  // three ways:
+  Joystick leftJoy;
+  Joystick rightJoy;
 
-  // Start the command when the button is pressed and let it run the command
-  // until it is finished as determined by it's isFinished method.
-  // button.whenPressed(new ExampleCommand());
+  JoystickButton leftSlowButton;
+  JoystickButton rightSlowButton;
 
-  // Run the command while the button is being held down and interrupt it once
-  // the button is released.
-  // button.whileHeld(new ExampleCommand());
+  OI() {
+    leftJoy = new Joystick(0); // TODO: set ports to correct values
+    rightJoy = new Joystick(1); // TODO: set ports to correct values
 
-  // Start the command when the button is released and let it run the command
-  // until it is finished as determined by it's isFinished method.
-  // button.whenReleased(new ExampleCommand());
+    leftSlowButton = new JoystickButton(leftJoy, 1);
+    leftSlowButton.whileHeld(new SlowDrive(SlowDrive.Side.LEFT));
+    rightSlowButton = new JoystickButton(rightJoy, 1);
+    rightSlowButton.whileHeld(new SlowDrive(SlowDrive.Side.RIGHT));
+  }
 }

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

@@ -15,32 +15,21 @@
 import frc.robot.commands.TeleopDrive;
 import frc.robot.subsystems.Drivetrain;
 
-/**
- * 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 {
   private static Drivetrain dt;
-  private static OI m_oi;
+  private static OI oi;
 
-  Command m_autonomousCommand;
-  SendableChooser<Command> m_chooser = new SendableChooser<>();
+  Command autonomousCommand;
+  SendableChooser<Command> chooser = new SendableChooser<>();
 
-  /**
-   * This function is run when the robot is first started up and should be used
-   * for any initialization code.
-   */
   @Override
   public void robotInit() {
-    dt = new Drivetrain(RobotMap.left1, RobotMap.left2, RobotMap.left3, RobotMap.right1, RobotMap.right2,
-        RobotMap.right3);
-    m_oi = new OI();
-    m_chooser.setDefaultOption("Default Auto", new TeleopDrive(dt));
-    // chooser.addOption("My Auto", new MyAutoCommand());
-    SmartDashboard.putData("Auto mode", m_chooser);
+    oi = new OI();
+    dt = new Drivetrain(RobotMap.leftMaster, RobotMap.leftSlave1, RobotMap.leftSlave2, RobotMap.rightMaster,
+        RobotMap.rightSlave1, RobotMap.rightSlave2, oi.leftJoy, oi.rightJoy, RobotMap.leftEnc, RobotMap.rightEnc,
+        RobotMap.gyro);
+    chooser.setDefaultOption("Default Auto", new TeleopDrive(dt));
+    SmartDashboard.putData("Auto Mode", chooser);
   }
 
   /**
@@ -65,38 +54,15 @@ public void disabledPeriodic() {
     Scheduler.getInstance().run();
   }
 
-  /**
-   * This autonomous (along with the chooser code above) shows how to select
-   * between different autonomous modes using the dashboard. The sendable chooser
-   * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
-   * remove all of the chooser code and uncomment the getString code to get the
-   * auto name from the text box below the Gyro
-   *
-   * <p>
-   * You can add additional auto modes by adding additional commands to the
-   * chooser code above (like the commented example) or additional comparisons to
-   * the switch structure below with additional strings & commands.
-   */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_chooser.getSelected();
-
-    /*
-     * String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
-     * switch(autoSelected) { case "My Auto": autonomousCommand = new
-     * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
-     * ExampleCommand(); break; }
-     */
+    autonomousCommand = chooser.getSelected();
 
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.start();
+    if (autonomousCommand != null) {
+      autonomousCommand.start();
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
   @Override
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
@@ -108,22 +74,16 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
   @Override
   public void testPeriodic() {
   }

Robot2019/src/main/java/frc/robot/RobotMap.java

@@ -1,14 +1,20 @@
 /*----------------------------------------------------------------------------*/
 /* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be apccompanied by the FIRST BSD license file in the root directory of */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
 package frc.robot;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.SPI;
 
 /**
  * The RobotMap is a mapping from the ports sensors and actuators are wired into
@@ -17,30 +23,60 @@
  * floating around.
  */
 public class RobotMap {
-  // For example to map the left and right motors, you could define the
-  // following variables to use with your drivetrain subsystem.
-  // public static int leftMotor = 1;
-  // public static int rightMotor = 2;
-
-  // If you are using multiple modules, make sure to define both the port
-  // number and the module. For example you with a rangefinder:
-  // public static int rangefinderPort = 1;
-  // public static int rangefinderModule = 1;
-  static WPI_TalonSRX left1, right1;
-  static WPI_VictorSPX left2, left3, right2, right3;
+  static WPI_TalonSRX leftMaster, rightMaster;
+  static WPI_VictorSPX leftSlave1, leftSlave2, rightSlave1, rightSlave2;
+  static Encoder leftEnc, rightEnc;
+  static String driveMode;
+  static AHRS gyro;
 
   static {
-    // TODO: Put ports
-    left1 = new WPI_TalonSRX(-1);
-    left2 = new WPI_VictorSPX(-1);
-    left2.follow(left1);
-    left3 = new WPI_VictorSPX(-1);
-    left3.follow(left1);
-    right1 = new WPI_TalonSRX(-1);
-    right2 = new WPI_VictorSPX(-1);
-    right2.follow(right1);
-    right3 = new WPI_VictorSPX(-1);
-    right3.follow(right1);
+    // Initialize motors on the left side of the drivetrain.
+    leftMaster = createConfiguredTalon(0); // TODO: set ports to correct values
+    leftSlave1 = createConfiguredVictor(1); // TODO: set ports to correct values
+    leftSlave2 = createConfiguredVictor(2); // TODO: set ports to correct values
+
+    // Initialize motors on the right side of the drivetrain.
+    rightMaster = createConfiguredTalon(3); // TODO: set ports to correct values
+    rightSlave1 = createConfiguredVictor(4); // TODO: set ports to correct values
+    rightSlave2 = createConfiguredVictor(5); // TODO: set ports to correct values
+
+    leftEnc = new Encoder(new DigitalInput(0), new DigitalInput(1)); // TODO: set ports to correct values
+    rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3)); // TODO: set ports to correct values
+
+    gyro = new AHRS(SPI.Port.kMXP);
   }
 
+  private static WPI_TalonSRX createConfiguredTalon(int port) {
+    WPI_TalonSRX tsrx = new WPI_TalonSRX(port);
+
+    // Put all configurations for the talon motor controllers in here.
+    // All values are from last year's code.
+    tsrx.configNominalOutputForward(0, 10);
+    tsrx.configNominalOutputReverse(0, 10);
+    tsrx.configPeakOutputForward(1, 10);
+    tsrx.configPeakOutputReverse(-1, 10);
+    tsrx.configPeakCurrentLimit(0, 0);
+    tsrx.configPeakCurrentDuration(0, 0);
+    // 40 Amps is the amp limit of a CIM. lThe PDP has 40 amp circuit breakers,
+    tsrx.configContinuousCurrentLimit(40, 0);
+    tsrx.enableCurrentLimit(true);
+    tsrx.configNeutralDeadband(0.001, 10);
+    tsrx.setNeutralMode(NeutralMode.Brake);
+
+    return tsrx;
+  }
+
+  private static WPI_VictorSPX createConfiguredVictor(int port) {
+    WPI_VictorSPX vspx = new WPI_VictorSPX(port);
+
+    // Put all configurations for the victor motor controllers in here.
+    vspx.configNominalOutputForward(0, 10);
+    vspx.configNominalOutputReverse(0, 10);
+    vspx.configPeakOutputForward(1, 10);
+    vspx.configPeakOutputReverse(-1, 10);
+    vspx.configNeutralDeadband(0.001, 10);
+    vspx.setNeutralMode(NeutralMode.Brake);
+
+    return vspx;
+  }
 }

Robot2019/src/main/java/frc/robot/commands/SlowDrive.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SlowDrive extends Command {
+  public enum Side {
+    LEFT, RIGHT
+  }
+
+  String sdValue;
+
+  /**
+   * Reduces the joystick input values while this command is running
+   * 
+   * @param side the joystick to reduce
+   */
+  public SlowDrive(Side side) {
+    sdValue = (side == Side.LEFT ? "Slow Left" : "Slow Right");
+  }
+
+  @Override
+  protected void initialize() {
+    SmartDashboard.putBoolean(sdValue, true);
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  protected void end() {
+    SmartDashboard.putBoolean(sdValue, false);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}