Create branch and add basic limelight functionality

Author: alexander-mcdowell

Robot2019/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.3.2"
+    id "edu.wpi.first.GradleRIO" version "2019.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"

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

@@ -11,18 +11,20 @@
 import edu.wpi.cscore.VideoSink;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import frc.robot.commands.ToggleClimberRails;
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
-import frc.robot.commands.ToggleHatchEject;
+import frc.robot.commands.GradualDrive;
 import frc.robot.commands.IntakeCargo;
-import frc.robot.commands.ToggleHatchIntake;
+import frc.robot.commands.IntakeHatch;
 import frc.robot.commands.ManualClimb;
 import frc.robot.commands.NormalDrive;
+import frc.robot.commands.SlowClimb;
 import frc.robot.commands.ResetWobble;
 import frc.robot.commands.SetArcadeOrTank;
 import frc.robot.commands.SlowDrive;
 import frc.robot.commands.ToggleCamera;
+import frc.robot.commands.ToggleClimberRails;
+import frc.robot.commands.ToggleHatchEject;
 import frc.robot.commands.ToggleLight;
 import frc.robot.commands.WobbleDrive;
 import frc.robot.subsystems.Cargo;
@@ -41,11 +43,13 @@ public class OI {
   JoystickButton leftSlowBtn, rightSlowBtn;
   JoystickButton arcadeOrTankBtn;
   JoystickButton normDriveBtn;
+  JoystickButton gradDriveBtn;
   JoystickButton hatchIntakeBtn, hatchEjectBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
   JoystickButton autoClimbBtn;
   JoystickButton manualClimbBtn;
+  JoystickButton slowClimbBtn;
   JoystickButton toggleCameraBtn;
   JoystickButton wobbleDriveBtn;
   JoystickButton cycleLightBtn;
@@ -67,26 +71,32 @@ public class OI {
     arcadeOrTankBtn = new JoystickButton(leftJoy, 4);
     arcadeOrTankBtn.whenPressed(new SetArcadeOrTank());
     normDriveBtn = new JoystickButton(leftJoy, 3);
-    normDriveBtn.whileHeld(new NormalDrive());
+    normDriveBtn.whenPressed(new NormalDrive());
+    gradDriveBtn = new JoystickButton(leftJoy, 5);
+    gradDriveBtn.whenPressed(new GradualDrive());
 
     hatchIntakeBtn = new JoystickButton(manipulator, Manip.X);
-    hatchIntakeBtn.whenPressed(new ToggleHatchIntake(hp));
+    hatchIntakeBtn.whenPressed(new IntakeHatch(hp, dt));
     hatchEjectBtn = new JoystickButton(manipulator, Manip.Y);
     hatchEjectBtn.whenPressed(new ToggleHatchEject(hp));
 
     cargoIntakeBtn = new JoystickButton(manipulator, Manip.A); // TODO: set ports to correct values
-    cargoIntakeBtn.whenPressed(new IntakeCargo(cargo));
+    cargoIntakeBtn.whenPressed(new IntakeCargo(cargo, lights));
     cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values
     cargoEjectBtn.whenPressed(new EjectCargo(cargo));
 
-    climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
+    climberRailBtn = new JoystickButton(manipulator, Manip.RB_rShoulder);
     climberRailBtn.whenPressed(new ToggleClimberRails(climber));
 
-    autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
-    autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy));
+    autoClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger);
+    autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy, lights));
+
+    manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
+    manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights));
+
 
-    manualClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger);
-    manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator));
+    slowClimbBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
+    slowClimbBtn.whileHeld(new SlowClimb());
 
     toggleCameraBtn = new JoystickButton(leftJoy, 2);
     toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer));

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

@@ -19,6 +19,7 @@
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.HatchPanel;
 import frc.robot.subsystems.Lights;
+import edu.wpi.first.wpilibj.Timer;
 
 public class Robot extends TimedRobot {
   private static Drivetrain dt;
@@ -28,6 +29,7 @@ public class Robot extends TimedRobot {
   private static Climber climber;
   private static Lights lights;
   private static String fname1, fname2, fname3, fname4;
+  private static Timer timey;
 
   @Override
   public void robotInit() {
@@ -56,6 +58,9 @@ public void robotInit() {
 
     dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy));
     SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0);
+
+    SmartDashboard.putBoolean("Outreach Mode", false);
+    timey = new Timer();
   }
 
   /**
@@ -83,15 +88,22 @@ public void disabledPeriodic() {
 
   @Override
   public void autonomousInit() {
+    timey.reset();
+    timey.start();
+    hp.grab();
   }
 
   @Override
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
+
+    SmartDashboard.putNumber("Time left", (int) (15 - timey.get() + 1));
   }
 
   @Override
   public void teleopInit() {
+    timey.reset();
+    timey.start();
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
@@ -101,6 +113,8 @@ public void teleopInit() {
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+
+    SmartDashboard.putNumber("Time left", (int) (135 - timey.get() + 1));
   }
 
   @Override

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

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.VictorSP;
 
@@ -41,7 +42,7 @@ public class RobotMap {
   static VictorSP cargoRoller;
   static Encoder leftEnc, rightEnc;
   static String driveMode;
-  static VictorSP lights;
+  static Relay lights;
   static AHRS ahrs;
   static PowerDistributionPanel pdp;
   static UsbCamera driveCamera, hatchCamera;
@@ -77,7 +78,7 @@ public class RobotMap {
     rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3));
 
     // Initialize lights
-    lights = new VictorSP(3); // TODO: Set this to correct port
+    lights = new Relay(0); // TODO: Set this to correct port
 
     ahrs = new AHRS(SPI.Port.kMXP);
     pdp = new PowerDistributionPanel();
@@ -120,7 +121,7 @@ private static WPI_TalonSRX createConfiguredTalon(int port) {
     catchError(tsrx.configPeakCurrentLimit(0, 0));
     catchError(tsrx.configPeakCurrentDuration(0, 0));
     // 40 Amps is the amp limit of a CIM. lThe PDP has 40 amp circuit breakers,
-    catchError(tsrx.configContinuousCurrentLimit(40, 0));
+    catchError(tsrx.configContinuousCurrentLimit(30, 0));
     tsrx.enableCurrentLimit(true);
     catchError(tsrx.configNeutralDeadband(0.001, 10));
     tsrx.setNeutralMode(NeutralMode.Brake);

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

@@ -8,13 +8,16 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.Lights;
 import frc.robot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.Joystick;
 
 public class Climb extends Command {
   private Climber climber;
   private Drivetrain dt;
+  private Lights lights;
   private Joystick dtJoy;
   private State state;
 
@@ -26,52 +29,57 @@ public class Climb extends Command {
   private final double retractGoal = 15; // This only needs to be off the ground. Climb is 19 inches.
   private final double offGroundHeight = 10;
 
-  public Climb(Climber climber, Drivetrain dt, Joystick joy) {
+  public Climb(Climber climber, Drivetrain dt, Joystick joy, Lights lights) {
     // Use requires() here to declare subsystem dependencies
     this.climber = climber;
     this.dt = dt;
+    this.lights = lights;
     dtJoy = joy;
     requires(climber);
     requires(dt);
+    requires(lights);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
     state = State.CLIMBING;
     climber.resetEncoder();
+    lights.setLights(Lights.LightState.HATCH);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    switch (state) {
-    case CLIMBING:
-      dt.drive(backDrive, backDrive);
-      climber.runClimber(climbUp);
-      if (!climber.needToClimb()) {
-        state = State.LEVELING;
+    if (!SmartDashboard.getBoolean("Outreach Mode", false)) {
+      switch (state) {
+      case CLIMBING:
+        dt.drive(backDrive, backDrive);
+        climber.runClimber(climbUp);
+        if (!climber.needToClimb()) {
+          state = State.LEVELING;
+        }
+        if (robotOnPlatform())
+          state = State.RETRACTING;
+        break;
+      case LEVELING:
+        dt.drive(backDrive, backDrive);
+        climber.runClimber(climbDown);
+        if (!climber.canDrop()) {
+          state = State.CLIMBING;
+        }
+        if (robotOnPlatform())
+          state = State.RETRACTING;
+        break;
+      case RETRACTING:
+        climber.runClimber(retract);
+        if (climber.getEncDistance() <= retractGoal) {
+          state = State.FINISHED;
+        }
+        break;
+      case FINISHED:
+        end();
       }
-      if (robotOnPlatform())
-        state = State.RETRACTING;
-      break;
-    case LEVELING:
-      dt.drive(backDrive, backDrive);
-      climber.runClimber(climbDown);
-      if (!climber.canDrop()) {
-        state = State.CLIMBING;
-      }
-      if (robotOnPlatform())
-        state = State.RETRACTING;
-      break;
-    case RETRACTING:
-      climber.runClimber(retract);
-      if (climber.getEncDistance() <= retractGoal) {
-        state = State.FINISHED;
-      }
-      break;
-    case FINISHED:
-      end();
     }
   }
 

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

@@ -11,7 +11,6 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Cargo;
-
 public class EjectCargo extends Command {
 
   final double ejectDuration = 0.5; // seconds

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

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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.InstantCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Add your docs here.
+ */
+public class GradualDrive extends InstantCommand {
+  /**
+   * Add your docs here.
+   */
+  public GradualDrive() {
+    super();
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+  }
+
+  // Called once when the command executes
+  @Override
+  protected void initialize() {
+    SmartDashboard.putBoolean("Gradual Drive", !SmartDashboard.getBoolean("Gradual Drive", true));
+  }
+
+}

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

@@ -11,22 +11,27 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Cargo;
+import frc.robot.subsystems.Lights;
 
 public class IntakeCargo extends Command {
   Timer timer;
   Cargo cargo;
+  Lights lights;
   boolean overdraw;
 
-  public IntakeCargo(Cargo cargo) {
+  public IntakeCargo(Cargo cargo, Lights lights) {
     requires(cargo);
+    requires(lights);
     this.cargo = cargo;
+    this.lights = lights;
     timer = new Timer();
     overdraw = false;
   }
 
   @Override
   protected void initialize() {
     timer.reset();
+    lights.setLights(Lights.LightState.CARGO);
   }
 
   @Override

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

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandGroup;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.HatchPanel;
+
+public class IntakeHatch extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public IntakeHatch(HatchPanel hp, Drivetrain dt) {
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+    addSequential(new ToggleHatchIntake(hp));
+    addSequential(new WaitTime(dt, 0.25));
+    addSequential(new MoveBack(dt, 0.25));
+  }
+}