Merge pull request #182 from AusTINCANsProgrammingTeam/DriverTesting2

Prepare for monday driver testing

Author: zbrotherton

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

@@ -27,7 +27,7 @@ public AimModes previous() {
     },
     EJECT(2500.0, 0.0),
     LAUNCH(0.0, 0.0),
-    TARMAC(2600.0, 0.0),
+    TARMAC(2550.0, 0.0),
     TEST {
       @Override
       public AimModes next() {
@@ -105,7 +105,8 @@ public AimModes previous() {
   public static final int driveRightFrontIndex = 2;
   public static final int driveRightRearIndex = 3;
 
-  public static final int driveBaseCurrentLimit = 60;
+  public static final int driveBaseCurrentLimit = 50;
+  public static final double driveBaseTurnRate = 0.85;
 
   // drive base pid values
   // TODO: need to tune for real robot

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

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -21,6 +23,7 @@ public class Robot extends TimedRobot {
   private SendableChooser<Constants.Auton> chooser = new SendableChooser<>();
   private RobotContainer robotContainer;
   private TabContainer tabContainer;
+  public UsbCamera usbCamera;
 
   // This function is run when the robot is first started up and should be used
   // for any
@@ -32,6 +35,9 @@ public void robotInit() {
     // autonomous chooser on the dashboard.
     // TODO: Put commands here
 
+    usbCamera = CameraServer.startAutomaticCapture();
+    usbCamera.setResolution(320, 240);
+
     robotContainer = new RobotContainer();
 
     chooser.setDefaultOption("Taxi", Constants.Auton.TAXI); // default is taxi mode

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

@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.AutonModes;
 import frc.robot.commands.CDSBallManagementCommand;
@@ -135,10 +134,8 @@ private void initSubsystems() {
 
     limelightSubsystem = new LimelightSubsystem();
 
-    if (axisCount1 > 0 && buttonCount1 > 0) {
-      climbSubsystem = new ClimbSubsystem(operatorJoystick);
-      climbCommand = new ClimbCommand(climbSubsystem);
-    }
+    climbSubsystem = new ClimbSubsystem(operatorJoystick);
+    climbCommand = new ClimbCommand(climbSubsystem);
   }
 
   private void initCommands() {
@@ -160,7 +157,8 @@ private void initCommands() {
         ballManagementCommand = new CDSBallManagementCommand(cdsSubsystem, intakeSubsystem);
         cdsSubsystem.setDefaultCommand(ballManagementCommand);
       } else {
-        combinedIntakeCDS = new CombinedIntakeCDSForwardCommand(intakeSubsystem, cdsSubsystem);
+        combinedIntakeCDS =
+            new CombinedIntakeCDSForwardCommand(intakeSubsystem, cdsSubsystem, shooterSubsystem);
       }
     }
 
@@ -221,38 +219,13 @@ private void configureButtonBindings() {
               shooterSubsystem));
     }
 
-    if (axisCount1 == 0 && buttonCount1 == 0) {
-
-      // Shooter
-      if (shooterSubsystem != null && shooterHeldAuto != null) {
-        buttons[Constants.backButton].whenPressed(shooterHeldAuto);
-        buttons[Constants.LJoystickButton].whenPressed(
-            new InstantCommand(shooterSubsystem::cycleAimModeNext, shooterSubsystem));
-        buttons[Constants.RJoystickButton].whenPressed(
-            new InstantCommand(shooterSubsystem::cycleAimModePrevious, shooterSubsystem));
-      }
-
-      // Limelight
-      if (limelightAlign != null) {
-        buttons[Constants.startButton].whenPressed(limelightAlign);
-      }
-
-      // ClimbSubysystem has no binding because there are not enuf axises
-      if (climbSubsystem != null) {}
-
-      System.out.printf("Using Testing One-controller button mappings");
-    } else {
-
-      if (climbSubsystem != null) {
-        buttons2[Constants.startButton].whenPressed(ClimbEnabling);
-      }
-
-      if (outtakeCommand != null && intakeForwardCommand != null) {
-        buttons2[Constants.RTriggerButton].whileHeld(intakeForwardCommand);
-        buttons2[Constants.RBumper].whileHeld(outtakeCommand);
-      }
+    if (climbSubsystem != null) {
+      buttons2[Constants.startButton].whenPressed(ClimbEnabling);
+    }
 
-      System.out.printf("Using Competition Two-controller button mappings");
+    if (outtakeCommand != null && intakeForwardCommand != null) {
+      buttons2[Constants.RTriggerButton].whileHeld(intakeForwardCommand);
+      buttons2[Constants.RBumper].whileHeld(outtakeCommand);
     }
   }
 

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

@@ -188,7 +188,7 @@ private void initializeCommandGroups() {
           new ParallelDeadlineGroup(
               twoBallRamseteCommands[0].andThen(
                   () -> driveBaseSubsystem.stopDriveMotors()), // travel to get ball
-              new CombinedIntakeCDSForwardCommand(intakeSubsystem, cdsSubsystem));
+              new CombinedIntakeCDSForwardCommand(intakeSubsystem, cdsSubsystem, shooterSubsystem));
 
       twoBallCommand =
           new SequentialCommandGroup(
@@ -203,7 +203,7 @@ private void initializeCommandGroups() {
           new ParallelDeadlineGroup(
               threeBallRamseteCommands[0].andThen(
                   () -> driveBaseSubsystem.stopDriveMotors()), // travel to get the two balls
-              new CombinedIntakeCDSForwardCommand(intakeSubsystem, cdsSubsystem));
+              new CombinedIntakeCDSForwardCommand(intakeSubsystem, cdsSubsystem, shooterSubsystem));
 
       threeBallCommand =
           new SequentialCommandGroup(

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

@@ -28,7 +28,7 @@ public CDSForwardCommand(CDSSubsystem CDSSubsystem, ShooterSubsystem shooterSubs
   public void initialize() {
     mCDSSubsystem.CDSWheelToggle(false);
     mCDSSubsystem.CDSBeltToggle(false);
-    mShooterSubsystem.runCargo(Constants.stopperWheelSpeed);
+    mShooterSubsystem.runCargo(Constants.Shooter.cargoReverse);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -28,9 +28,9 @@ public void initialize() {
     m_subsystem.resetTargetedHeight();
 
     if (m_subsystem.getclimbingenable() == true) {
-      m_drivesubsystem.setArcadedrivespeed(40);
+      m_drivesubsystem.setDriveBaseSpeed(0.40);
     } else {
-      m_drivesubsystem.setArcadedrivespeed(100);
+      m_drivesubsystem.setDriveBaseSpeed(1);
     }
   }
 

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

@@ -5,22 +5,30 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
 import frc.robot.subsystems.CDSSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 
 public class CombinedIntakeCDSForwardCommand extends CommandBase {
   /** Creates a new OuttakeCommand. */
   private final CDSSubsystem CDSSubsystem;
 
+  private final ShooterSubsystem shooterSubsystem;
+
   private final IntakeSubsystem intakeSubsystem;
 
   public CombinedIntakeCDSForwardCommand(
-      IntakeSubsystem mIntakeSubsystem, CDSSubsystem mCDSSubsystem) {
+      IntakeSubsystem mIntakeSubsystem,
+      CDSSubsystem mCDSSubsystem,
+      ShooterSubsystem mShooterSubsystem) {
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(mShooterSubsystem);
     addRequirements(mIntakeSubsystem);
     addRequirements(mCDSSubsystem);
     intakeSubsystem = mIntakeSubsystem;
     CDSSubsystem = mCDSSubsystem;
+    shooterSubsystem = mShooterSubsystem;
   }
 
   // Called when the command is initially scheduled.
@@ -29,6 +37,7 @@ public void initialize() {
     CDSSubsystem.CDSBeltToggle(false);
     CDSSubsystem.CDSWheelToggle(false);
     intakeSubsystem.toggleIntake(false);
+    shooterSubsystem.runCargo(Constants.Shooter.cargoReverse);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -40,6 +49,7 @@ public void execute() {}
   public void end(boolean interrupted) {
     CDSSubsystem.stopCDS();
     intakeSubsystem.stopIntake();
+    shooterSubsystem.runCargo(0.0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/common/hardware/MotorController.java

@@ -26,7 +26,6 @@ public MotorController(String name, int deviceID) {
     // Create default values for Spark Max motor controller
     setSmartCurrentLimit(Constants.defaultCurrentLimit); // default current limit is 40A
     setIdleMode(CANSparkMax.IdleMode.kCoast); // default mode is Coast
-    setOpenLoopRampRate(Constants.openLoopRampRate); // default open loop rate
 
     mPIDController = getPIDController();
     mEncoder = getEncoder();

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

@@ -12,7 +12,6 @@
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.common.hardware.MotorController;
@@ -224,7 +223,7 @@ public void enableClimb() {
   }
 
   public void periodic() {
-
+    /*
     SmartDashboard.putNumber(
         "Climb motor 1 Applied Output", m_climbMotorControllerOne.getAppliedOutput());
     SmartDashboard.putNumber(
@@ -271,6 +270,7 @@ public void periodic() {
       m_climbMotorControllerOne.getPIDCtrl().setI(sbClimbOneI.getDouble(0));
       m_climbMotorControllerOne.getPIDCtrl().setD(sbClimbOneD.getDouble(0));
     }
+    */
   }
 
   public boolean getLimitSwitchVal() {

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

@@ -13,7 +13,6 @@
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.ADIS16448_IMU;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
@@ -34,10 +33,8 @@
 import frc.robot.common.hardware.MotorController;
 
 public class DriveBaseSubsystem extends SubsystemBase {
+  private double driveBaseSpeed;
   private ShuffleboardTab driverTab = Shuffleboard.getTab("Driver Teleop tab");
-  private NetworkTableEntry teleopSpeed =
-      driverTab.add("Speed percentage", 100).withPosition(0, 1).getEntry();
-
   private final Joystick m_driverJoystick;
   private final MotorController[] m_motorControllers;
   private final DifferentialDrive m_differentialDrive;
@@ -64,7 +61,7 @@ public class DriveBaseSubsystem extends SubsystemBase {
   private boolean isReverse = false;
 
   public DriveBaseSubsystem(Joystick joystick, boolean usingExternal) {
-
+    driveBaseSpeed = 1;
     m_driverJoystick = joystick;
 
     m_motorControllers = new MotorController[4];
@@ -106,6 +103,24 @@ public DriveBaseSubsystem(Joystick joystick, boolean usingExternal) {
     m_motorControllers[Constants.driveLeftFrontIndex].setInverted(true);
     m_motorControllers[Constants.driveLeftRearIndex].setInverted(true);
 
+    m_motorControllers[Constants.driveLeftRearIndex].setOpenLoopRampRate(
+        Constants.openLoopRampRate);
+    m_motorControllers[Constants.driveLeftFrontIndex].setOpenLoopRampRate(
+        Constants.openLoopRampRate);
+    m_motorControllers[Constants.driveRightRearIndex].setOpenLoopRampRate(
+        Constants.openLoopRampRate);
+    m_motorControllers[Constants.driveRightFrontIndex].setOpenLoopRampRate(
+        Constants.openLoopRampRate);
+
+    m_motorControllers[Constants.driveLeftRearIndex].setSmartCurrentLimit(
+        Constants.driveBaseCurrentLimit);
+    m_motorControllers[Constants.driveLeftFrontIndex].setSmartCurrentLimit(
+        Constants.driveBaseCurrentLimit);
+    m_motorControllers[Constants.driveRightRearIndex].setSmartCurrentLimit(
+        Constants.driveBaseCurrentLimit);
+    m_motorControllers[Constants.driveRightFrontIndex].setSmartCurrentLimit(
+        Constants.driveBaseCurrentLimit);
+
     // Forces rear motors of each side to follow the first
     m_motorControllers[Constants.driveLeftRearIndex].follow(
         m_motorControllers[Constants.driveLeftFrontIndex]);
@@ -217,24 +232,24 @@ public void periodic() {
 
   }
 
-  public void setArcadedrivespeed(double input) {
-    teleopSpeed.setDouble(input);
+  public void setDriveBaseSpeed(double driveBaseSpeed) {
+    this.driveBaseSpeed = driveBaseSpeed;
   }
 
   // Normal Arcade Drive
   public void arcadeDrive() {
     // Note: -0.85 to accomodate comfort of driver (sensitivity)
     m_differentialDrive.arcadeDrive(
-        m_driverJoystick.getRawAxis(Constants.leftJoystickY) * -(teleopSpeed.getDouble(100) / 100),
-        m_driverJoystick.getRawAxis(Constants.rightJoystickX),
+        m_driverJoystick.getRawAxis(Constants.leftJoystickY) * -driveBaseSpeed,
+        m_driverJoystick.getRawAxis(Constants.rightJoystickX) * Constants.driveBaseTurnRate,
         true);
     // joystick has y-axis flipped so up is negative why down is positive
   }
 
   // Arcade Drive where you can only move forwards and backwards for testing
   public void arcadeDrive(double rotation) {
     m_differentialDrive.arcadeDrive(
-        -0.85 * m_driverJoystick.getRawAxis(Constants.leftJoystickY), rotation);
+        m_driverJoystick.getRawAxis(Constants.leftJoystickY) * -driveBaseSpeed, rotation);
   }
 
   // TODO: Make a command to switch modes (extra)