Add Servo for Limelight/toggle button

src/main/deploy/PathWeaver/Paths/CBall1.path

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+3.39,-2.4,1.0,0.0,true,false,
+5.0,-0.7,1.0,0.0,true,false,

src/main/deploy/PathWeaver/Paths/LBall1.path

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+3.39,-3.4,1.0,0.0,true,false,
+5.0,-0.7,1.0,0.0,true,false,

src/main/deploy/PathWeaver/Paths/RBall1.path

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+3.39,-1.4,1.0,0.0,true,false,
+5.0,-0.7,1.0,0.0,true,false,

src/main/java/org/team199/robot2021/Constants.java

@@ -106,8 +106,13 @@ public static final class DriveConstants {
         // TODO: Find experimental value for current draw
         public static final double intakeCurrentDraw = 8.0;
 
+        // TODO: change these to current for 2021 robot
         public static final double cameraHeight = Units.inchesToMeters(40.0);
         public static final double cameraMountingAngleDeg = 29.5;
+
+        // TODO: Find distances between intake and limelight
+        public static final double xLimeDistIntake = 0.0;
+        public static final double zLimeDistIntake = 0.0;
     }
 
     public static final class DrivePorts {
@@ -153,6 +158,7 @@ public static final class DrivePorts {
 
         public static final int kAutoPathSwitch1Port = 0;
         public static final int kAutoPathSwitch2Port = 1;
+        public static final int kLimelightServoPort = 0; //TODO: set proper channel
     }
 
     /**
@@ -235,6 +241,8 @@ public static final class Controller {
             // intake/feeder
             public static final int kIntakeButton = X;
             public static final int kOuttakeButton = Y;
+            // limelight toggle
+            public static final int kLimelightToggleButton = LT; //Limelight Toggle button set to Left Button
             //public static final int kRegurgitateButton = B;
         }
     }

src/main/java/org/team199/robot2021/RobotContainer.java

@@ -28,11 +28,14 @@
 
 import org.team199.lib.RobotPath;
 import org.team199.robot2021.Constants.OI;
+import org.team199.robot2021.commands.BallSearchAndPickup;
 import org.team199.robot2021.commands.GalacticSearchCommand;
 import org.team199.robot2021.commands.TeleopDrive;
 import org.team199.robot2021.commands.ToggleIntake;
 import org.team199.robot2021.subsystems.Drivetrain;
+import org.team199.robot2021.subsystems.Feeder;
 import org.team199.robot2021.subsystems.Intake;
+import org.team199.robot2021.subsystems.Shooter;
 
 import frc.robot.lib.Limelight;
 
@@ -46,9 +49,9 @@
 public class RobotContainer {
     final Drivetrain drivetrain = new Drivetrain();
     private final Limelight lime = new Limelight();
-    //private final Shooter shooter = new Shooter(lime);
+    private final Shooter shooter = new Shooter(lime);
     private final Intake intake = new Intake();
-    //private final Feeder feeder = new Feeder();
+    private final Feeder feeder = new Feeder();
     private final Joystick leftJoy = new Joystick(Constants.OI.LeftJoy.port);
     private final Joystick rightJoy = new Joystick(Constants.OI.RightJoy.port);
     private final Joystick controller = new Joystick(Constants.OI.Controller.port);
@@ -192,8 +195,12 @@ private void configureButtonBindingsController() {
         new JoystickButton(controller, Constants.OI.Controller.A).whenPressed(new InstantCommand(() -> {drivetrain.resetHeading();}));
         new JoystickButton(controller, Constants.OI.Controller.B).whenPressed(new InstantCommand(() -> { SmartDashboard.putBoolean("Field Oriented", !SmartDashboard.getBoolean("Field Oriented", true)); }));
         // Intake toggle button
-        new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
-
+        // new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
+        new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new BallSearchAndPickup(drivetrain, intake, feeder, shooter, lime));
+        new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {
+            intake.setLimelightSearching(!intake.isLimelightSearching()); 
+
+        }));
         // Power cell regurgitate button
         //new JoystickButton(controller, Constants.OI.Controller.kRegurgitateButton).whileHeld(new Regurgitate(intake, feeder));
 

src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java

@@ -23,8 +23,9 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh
         TimeOfFlight shooterDistanceSensor = feeder.getShooterDistanceSensor();
         ShooterHorizontalAim aim = new ShooterHorizontalAim(drivetrain, lime);
         AutoShoot shoot = new AutoShoot(feeder, shooter, shooterDistanceSensor, 3);
+        BallSearchAndPickup search = new BallSearchAndPickup(drivetrain, intake, feeder, shooter, lime);
 
-        /*
+
         addCommands(
             aim,
             new InstantCommand(() -> { 
@@ -36,7 +37,7 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh
                 intake.intake();
                 intake.doTheFlop();
             }, intake),
-            path.getPathCommand(),
+            path.getPathCommand(true, true),
             new InstantCommand(() -> {
                 intake.retract();
                 intake.stop();
@@ -45,8 +46,9 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh
             new InstantCommand(() -> { 
                 SmartDashboard.putNumber("Shooter.kTargetSpeed", linearInterpol.calculate(drivetrain.getOdometry().getPoseMeters().getTranslation().getDistance(target))); 
             }),
-            shoot
+            shoot, 
+            search
         );
-        */
+
     }
 }

src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team199.robot2021.commands;
+
+import frc.robot.lib.Limelight;
+import org.team199.robot2021.subsystems.Drivetrain;
+import org.team199.robot2021.subsystems.Feeder;
+import org.team199.robot2021.subsystems.Intake;
+import org.team199.robot2021.subsystems.Shooter;
+import org.team199.robot2021.Constants;
+import org.team199.robot2021.commands.RotateAngle;
+import org.team199.robot2021.commands.DriveDistance;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class BallSearchAndPickup extends SequentialCommandGroup {
+  private Drivetrain drivetrain;
+  private Intake intake;
+  private Feeder feeder;
+  private Shooter shooter;
+  private Limelight lime;
+
+  /** Creates a new BallSearchAndPickup. */
+  public BallSearchAndPickup(Drivetrain drivetrain, Intake intake, Feeder feeder, Shooter shooter, 
+  Limelight lime) {
+    this.lime = lime;
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain, intake, feeder, shooter);
+    RotateAngle check = new RotateAngle(drivetrain, Math.PI/5);
+
+    for (int i = 0; i < 10; i++) {
+      addCommands(check);
+      if (SmartDashboard.getNumber("Found Vision Target", 0) != 0) {
+        break;
+      }
+    }
+
+    if (SmartDashboard.getNumber("Found Vision Target", 0) != 0) {
+      // Use addRequirements() here to declare subsystem dependencies.
+      //Search for Balls
+      double[] distComponents = lime.determineObjectDist(Constants.DriveConstants.cameraHeight, 0.0, Constants.DriveConstants.cameraMountingAngleDeg);
+      double xOffset = Constants.DriveConstants.xLimeDistIntake;
+      double zOffset = Constants.DriveConstants.zLimeDistIntake;
+      //(Make sure to subtract the horizontal dist. b/t limelight and center of intake from the left right dist)
+      distComponents[0] = distComponents[0] - xOffset;
+      distComponents[1] = distComponents[1] - zOffset;
+      //Find angle and distance from balls using trigonometry and getObjectDistance method in Limelight class (0 is forward, 1 is strafe)
+      double angle = Math.atan2(distComponents[0], distComponents[1]);
+      double distance = Math.hypot(distComponents[0], distComponents[1]);
+      RotateAngle rotate = new RotateAngle(drivetrain, angle);
+      DriveDistance drive = new DriveDistance(drivetrain, distance);
+      //Rotate Robot along angle till it's facing ball
+        //Create new command to do this where isfinished checks if the goal angle is the same as the current angle
+      //Run the intak
+      addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive, new InstantCommand(() -> intake.stop()), new InstantCommand(() -> intake.retract()));
+    }
+  }
+
+  // Called when the command is initially scheduled.
+
+}

src/main/java/org/team199/robot2021/commands/DriveDistance.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team199.robot2021.commands;
+
+import org.team199.robot2021.subsystems.Drivetrain;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveDistance extends CommandBase {
+  private Drivetrain drivetrain;
+  private double distance;
+  private Translation2d initDist;
+  private double hasDriven;
+  /** Creates a new DriveDistance. */
+  public DriveDistance(Drivetrain drivetrain, double distance) {
+    this.drivetrain = drivetrain;
+    this.distance = distance;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drivetrain.drive(1, 0, 0);
+    //Sets initial Distance
+    initDist = drivetrain.getOdometry().getPoseMeters().getTranslation();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //Keep Going forward (Set speed of wheels or whatever that is)
+    drivetrain.drive(1,0,0);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    hasDriven = drivetrain.getOdometry().getPoseMeters().getTranslation().getDistance(initDist);
+    return (hasDriven >= distance);
+  }
+}

src/main/java/org/team199/robot2021/commands/RotateAngle.java

@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team199.robot2021.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.team199.robot2021.subsystems.Drivetrain;
+
+public class RotateAngle extends CommandBase {
+  private Drivetrain drivetrain;
+  private double rotateAngle;
+  private double currentAngle;
+  private double goalAngle;
+  /** Creates a new RotateAngle. */
+  public RotateAngle(Drivetrain drivetrain, double rotateAngle) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.drivetrain = drivetrain;
+    this.rotateAngle = rotateAngle;
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    //TODO: determine speed angle rotates
+    //Rotates based on if rotateAngle is positive or negative
+    //TODO: define goalAngle as the final angle based on odometry values
+    //TODO: ensure getHeading returns an angle
+    goalAngle = drivetrain.getHeading() *Math.PI/180 + rotateAngle;
+    if (Math.abs(rotateAngle) != rotateAngle) {
+      drivetrain.drive(0, 0, -0.1);
+    } else {
+      drivetrain.drive(0, 0, 0.1);
+    }
+    //TODO: use getHeading to find the angle we want the drivetrain to rotate. 
+    //TODO: Change goalAngle to turnAngle, with goalAngle being the angle we want the drivetrian to rotate
+    }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    currentAngle = drivetrain.getHeading()*Math.PI/180;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.drive(0, 0 , 0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    // TODO: Determine margin of error
+    return (Math.abs(goalAngle - currentAngle) < 5);
+  }
+}

src/main/java/org/team199/robot2021/subsystems/Intake.java

@@ -11,17 +11,24 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.lib.MotorControllerFactory;
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
 
 public class Intake extends SubsystemBase {
     // TODO: find good values and then set to final
     private static double kIntakeSpeed = .6;
     private static double kSlowSpeed = -0.1;
+    private static double kLimeBottomAngleDegs = 0; //TODO: set correct angle
+    private static double kLimeTopAngleDegs = 0; //TODO: set correct angle
 
     private final CANSparkMax rollerMotor = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kIntakeRoller);
+    private final Servo limelightServo = new Servo(Constants.DrivePorts.kLimelightServoPort); 
     private final DoubleSolenoid intakePistons1 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[2], Constants.DrivePorts.kIntakePistons[3]);
     private final DoubleSolenoid intakePistons2 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[0], Constants.DrivePorts.kIntakePistons[1]);
 
     private boolean deployed = false;
+    private boolean isLimelightSearching = false;
 
     /**
      * Vectored intake that rolls balls through the bumper gap and into feeder.
@@ -77,5 +84,21 @@ public void doTheFlop() {
     public boolean isDeployed() {
         return deployed;
     }
+    public boolean isLimelightSearching() {
+        return isLimelightSearching;
+    }
+    public void setLimelightSearching(boolean status) {
+        isLimelightSearching = status;
+        if (isLimelightSearching) {
+            limelightServo.setAngle(kLimeBottomAngleDegs); // TODO: set correct angle
+            NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(1);
+        } else {
+            limelightServo.setAngle(kLimeTopAngleDegs); // TODO: set correct angle
+            NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0);
+
+        }
+
+    }
+
 
 }