Create Drive command (combines limelight drive and teleop drive)

Author: caengineers

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

@@ -26,6 +26,7 @@
 import frc.robot.commands.ToggleClimberRails;
 import frc.robot.commands.ToggleHatchEject;
 import frc.robot.commands.ToggleLight;
+import frc.robot.commands.ToggleLimelight;
 import frc.robot.commands.WobbleDrive;
 import frc.robot.subsystems.Cargo;
 import frc.robot.subsystems.Climber;
@@ -44,6 +45,7 @@ public class OI {
   JoystickButton arcadeOrTankBtn;
   JoystickButton normDriveBtn;
   JoystickButton gradDriveBtn;
+  JoystickButton limelightBtn;
   JoystickButton hatchIntakeBtn, hatchEjectBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
@@ -74,6 +76,8 @@ public class OI {
     normDriveBtn.whenPressed(new NormalDrive());
     gradDriveBtn = new JoystickButton(leftJoy, 5);
     gradDriveBtn.whenPressed(new GradualDrive());
+    limelightBtn = new JoystickButton(rightJoy, 2);   // Figure out the correct button
+    limelightBtn.whenPressed(new ToggleLimelight());
 
     hatchIntakeBtn = new JoystickButton(manipulator, Manip.X);
     hatchIntakeBtn.whenPressed(new IntakeHatch(hp, dt));
@@ -94,7 +98,6 @@ public class OI {
     manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
     manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights));
 
-
     slowClimbBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
     slowClimbBtn.whileHeld(new SlowClimb());
 

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

@@ -13,7 +13,8 @@
 import frc.robot.commands.DrivetrainAnalysis;
 import frc.robot.commands.IncreaseVoltageLinear;
 import frc.robot.commands.IncreaseVoltageStepwise;
-import frc.robot.commands.TeleopDrive;
+import frc.robot.commands.Drive;
+import frc.robot.lib.Limelight;
 import frc.robot.subsystems.Cargo;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
@@ -25,6 +26,7 @@ public class Robot extends TimedRobot {
   private static Drivetrain dt;
   private static HatchPanel hp;
   private static OI oi;
+  private static Limelight lime;
   private static Cargo cargo;
   private static Climber climber;
   private static Lights lights;
@@ -40,6 +42,7 @@ public void robotInit() {
     climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons);
     lights = new Lights(RobotMap.lights);
     oi = new OI(dt, hp, cargo, climber, lights, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
+    lime = new Limelight();
 
     fname1 = "/home/lvuser/drive_char_linear_for.csv";
     fname2 = "/home/lvuser/drive_char_stepwise_for.csv";
@@ -56,7 +59,7 @@ public void robotInit() {
     SmartDashboard.putData("Increase Voltage Stepwise Backward", ivsb);
     SmartDashboard.putData("Drivetrain Characterization Analysis", dca);
 
-    dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy));
+    dt.setDefaultCommand(new Drive(dt, lime, oi.leftJoy, oi.rightJoy));
     SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0);
 
     SmartDashboard.putBoolean("Outreach Mode", false);

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

@@ -1,34 +1,33 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2017-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 frc.robot.lib.Limelight;
+import frc.robot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.subsystems.Drivetrain;
-
-public class TeleopDrive extends Command {
-  Drivetrain dt;
-  Joystick leftJoy, rightJoy;
-
-  double prevSpeed = 0, prevLeft = 0, prevRight = 0;
 
-  double outreachSpeed = 0.3;
-
-  /**
-   * Handles all the teleoperated driving functionality
-   * 
-   * @param dt the Drivetrain object to use, passing it in is useful for testing
-   *           purposes
-   */
-  public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) {
+public class Drive extends Command {
+  private Drivetrain dt;
+  private Joystick leftJoy, rightJoy;
+  private Limelight lime;
+  private Limelight.Mode limelightMode = Limelight.Mode.TARGET;
+  private double adjustment = 0;
+  private double minError = 0.05;   // TODO: Test minimum values
+  private double prevSpeed = 0, prevLeft = 0, prevRight = 0;
+  private double outreachSpeed = 0.3;
+
+  public Drive(Drivetrain dt, Limelight lime, Joystick leftJoy, Joystick rightJoy) {
     requires(dt);
     this.dt = dt;
+    this.lime = lime;
+    this.dt = dt;
     this.leftJoy = leftJoy;
     this.rightJoy = rightJoy;
 
@@ -45,12 +44,22 @@ public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) {
     }
   }
 
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if (SmartDashboard.getBoolean("Arcade Drive", true)) {
-      arcadeDrive();
+    if (SmartDashboard.getBoolean("Using Limelight", false)) {
+        autoAllign();
     } else {
-      tankDrive();
+        if (SmartDashboard.getBoolean("Arcade Drive", true)) {
+            arcadeDrive();
+        } else {
+            tankDrive();
+        }
     }
   }
 
@@ -230,16 +239,49 @@ private void charDrive(double left, double right) {
     dt.disableVoltageCompensation();
   }
 
+  private void autoAllign() {
+    if (limelightMode == Limelight.Mode.DIST) {
+        adjustment = lime.distanceAssist();
+        dt.drive(adjustment, adjustment);
+        if (Math.abs(adjustment) < minError)  {
+          SmartDashboard.putBoolean("Finished Alligning", true);
+        }
+      }
+      else if (limelightMode == Limelight.Mode.STEER) {
+        adjustment = lime.distanceAssist();
+        dt.drive(adjustment, -adjustment);
+        if (Math.abs(adjustment) < minError)  {
+          SmartDashboard.putBoolean("Finished Alligning", true);
+        }
+      } else {
+        double[] params = lime.autoTarget();
+        dt.drive(params[0], params[1]);
+        double maxInput = Math.max(Math.abs(params[0]), Math.abs(params[1]));
+        if (maxInput < minError)  {
+          SmartDashboard.putBoolean("Finished Alligning", true);
+        }
+      }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+      if (!SmartDashboard.getBoolean("Use Limelight", false) || SmartDashboard.getBoolean("Finished Alligning", false)) {
+        SmartDashboard.putBoolean("Use Limelight", false);
+        SmartDashboard.putBoolean("Finished Alligning", false);
+        return true;
+      } else {
+        return false;
+      }
   }
 
+  // Called once after isFinished returns true
   @Override
   protected void end() {
-    dt.stop();
   }
 
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
   @Override
   protected void interrupted() {
     end();