Limelight-intake

src/main/deploy/PathWeaver/Paths/2BallAuto1(2).path

@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
 7.701,-7.6316,0.004,0.01,true,false,
-7.935,-5.021599999999999,0.004,0.01,true,false,
+8.076330879361914,-5.086270229312063,0.004,0.01,true,false,

src/main/deploy/PathWeaver/Paths/2BallAuto2(2).path

@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
 5.399,-2.0795999999999992,0.007,-0.003,true,false,
-7.353,-3.7485999999999997,0.007,-0.003,true,false,
+7.621118309072781,-3.3971920079760713,0.007,-0.003,true,false,

src/main/deploy/fenderRPMs.csv

@@ -3,7 +3,7 @@ PSI,RPM
 2.5,2500
 2.75,2500
 3,2500
-3.5,2500
-3.75,2500
-4,2500
+3.5,2600
+3.75,2600
+4,2600
 5,2400

src/main/java/org/team199/robot2022/AutoPath.java

@@ -11,17 +11,19 @@ public class AutoPath {
     public final List<RobotPath> path;
     public final boolean shootAtEnd;
     public final boolean runIntake;
+    public final boolean useLimelight;
     public final ShotPosition startShotPosition, endShotPosition;
 
-    public AutoPath(boolean shootAtStart, List<RobotPath> path, boolean shootAtEnd, boolean runIntake) {
-        this(shootAtStart, path, shootAtEnd, runIntake, ShotPosition.FENDER, ShotPosition.FENDER);
+    public AutoPath(boolean shootAtStart, List<RobotPath> path, boolean shootAtEnd, boolean runIntake, boolean useLimelight) {
+        this(shootAtStart, path, shootAtEnd, runIntake, useLimelight, ShotPosition.FENDER, ShotPosition.FENDER);
     }
 
-    public AutoPath(boolean shootAtStart, List<RobotPath> path, boolean shootAtEnd, boolean runIntake, ShotPosition startShotPosition, ShotPosition endShotPosition) {
+    public AutoPath(boolean shootAtStart, List<RobotPath> path, boolean shootAtEnd, boolean runIntake, boolean useLimelight, ShotPosition startShotPosition, ShotPosition endShotPosition) {
         this.shootAtStart = shootAtStart;
         this.path = path;
         this.shootAtEnd = shootAtEnd;
         this.runIntake = runIntake;
+        this.useLimelight = useLimelight;
         this.startShotPosition = startShotPosition;
         this.endShotPosition = endShotPosition;
     }

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

@@ -149,6 +149,7 @@ public static enum StickDirection {X, Y};
         public static final class LeftJoy {
             public static final int port = 0;
 
+            public static final int slowDriveButton = 1;
             public static final int manualAddPort = 3;
             public static final int manualSubtractPort = 2;
             public static final int shootSoftOnePort = 6;
@@ -162,7 +163,7 @@ public static final class LeftJoy {
 
         public static final class RightJoy {
             public static final int port = 1;
-
+            public static final int autoIntake = 1;
             public static final int shootPort = 2;
             public static final int slowExtendLeftClimberPort = 5;
             public static final int slowRetractLeftClimberPort = 3;

src/main/java/org/team199/robot2022/Robot.java

@@ -75,6 +75,7 @@ public void autonomousInit() {
     robotContainer.dt.resetOdometry();
     robotContainer.getAutonomousCommand().schedule();
     robotContainer.initShooterConfig();
+    robotContainer.setLimelightColor();
   }
 
   /** This function is called periodically during autonomous. */
@@ -90,6 +91,7 @@ public void teleopInit() {
     CommandScheduler.getInstance().cancelAll();
     robotContainer.dt.brake();
     robotContainer.initShooterConfig();
+    robotContainer.setLimelightColor();
   }
 
   /** This function is called periodically during operator control. */
@@ -104,6 +106,8 @@ public void testInit() {
 
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+    robotContainer.setLimelightColor();
+  }
 
 }

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

@@ -5,7 +5,13 @@
 package org.team199.robot2022;
 
 import java.io.IOException;
+import java.util.Arrays;
+import java.util.List;
+import java.util.stream.Collectors;
 
+import org.carlmontrobotics.lib199.Limelight;
+import org.carlmontrobotics.lib199.MotorControllerFactory;
+import org.carlmontrobotics.lib199.path.RobotPath;
 import org.team199.robot2022.commands.Autonomous;
 import org.team199.robot2022.commands.ExtendClimber;
 import org.team199.robot2022.commands.PassiveAutomaticIntake;
@@ -34,16 +40,11 @@
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PerpetualCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
-import org.carlmontrobotics.lib199.MotorControllerFactory;
-import org.carlmontrobotics.lib199.path.RobotPath;
-import java.util.Arrays;
-import java.util.List;
-import java.util.stream.Collectors;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -65,12 +66,14 @@ public class RobotContainer {
   public final Drivetrain dt = new Drivetrain();
   public final PowerDistribution pdp = new PowerDistribution();
   public final Shooter shooter = new Shooter();
+  private final Limelight lime = new Limelight("limelight-intake", 1/90d);
 
   public final IntakeFeeder intakeFeeder;
 
   public final DigitalInput[] autoSelectors;
   public final AutoPath[] autoPaths;
   private final boolean inCompetition = true;
+  private boolean robotWasFieldAligned = true;
 
   private final SendableChooser<AutoPath> autoSelector = new SendableChooser<>();
 
@@ -80,17 +83,21 @@ public class RobotContainer {
   public RobotContainer(Robot robot) {
     MotorControllerFactory.configureCamera();
 
+    lime.config.steeringFactor = .4;
+
+    boolean useLimelightInAuto = true; // CHANGE THIS TO FALSE IF AUTO LIMELIGHT DOESN'T WORK
+
     intakeFeeder = new IntakeFeeder(robot);
     autoPaths = new AutoPath[] {
       null,
-      new AutoPath(true, loadPaths("ShootAndTaxi1"), false, false),
-      new AutoPath(true, loadPaths("ShootAndTaxi2"), false, false),
-      new AutoPath(true, loadPaths("TarmacShootAndTaxi1"), false, false, ShotPosition.TARMAC, ShotPosition.FENDER),
-      new AutoPath(true, loadPaths("TarmacShootAndTaxi2"), false, false, ShotPosition.TARMAC, ShotPosition.FENDER),
-      new AutoPath(true,loadPaths("2BallAuto1(1)", "2BallAuto1(2)"), true, true, ShotPosition.FENDER, ShotPosition.FENDER),
-      new AutoPath(true, loadPaths("2BallAuto2(1)", "2BallAuto2(2)"), true, true, ShotPosition.FENDER, ShotPosition.FENDER),
-      new AutoPath(true, loadPaths("3BallAuto(1)", "3BallAuto(2)", "3BallAuto(3)", "3BallAuto(4)", "3BallAuto(5)"), false, true, ShotPosition.FENDER, ShotPosition.FENDER),
-      new AutoPath(false, loadPaths("RotateInPlace"), false, false, ShotPosition.FENDER, ShotPosition.FENDER)
+      new AutoPath(true, loadPaths("ShootAndTaxi1"), false, false, false),
+      new AutoPath(true, loadPaths("ShootAndTaxi2"), false, false, false),
+      new AutoPath(true, loadPaths("TarmacShootAndTaxi1", "2BallAuto2(2)"), true, false, false, ShotPosition.TARMAC, ShotPosition.FENDER),
+      new AutoPath(true, loadPaths("TarmacShootAndTaxi2", "2BallAuto1(2)"), true, false, false, ShotPosition.TARMAC, ShotPosition.FENDER),
+      new AutoPath(true, loadPaths("2BallAuto1(1)", "2BallAuto1(2)"), true, true, useLimelightInAuto, ShotPosition.FENDER, ShotPosition.FENDER),
+      new AutoPath(true, loadPaths("2BallAuto2(1)", "2BallAuto2(2)"), true, true, useLimelightInAuto, ShotPosition.FENDER, ShotPosition.FENDER),
+      new AutoPath(true, loadPaths("3BallAuto(1)", "3BallAuto(2)", "3BallAuto(3)", "3BallAuto(4)", "3BallAuto(5)"), false, true, false, ShotPosition.FENDER, ShotPosition.FENDER),
+      new AutoPath(false, loadPaths("RotateInPlace"), false, false, false, ShotPosition.FENDER, ShotPosition.FENDER)
     };
 
     autoSelectors = new DigitalInput[Math.min(autoPaths.length, 26)];
@@ -105,8 +112,6 @@ public RobotContainer(Robot robot) {
 
     SmartDashboard.putNumber("Field Offset from North (degrees)", getAutoPath() == null ? 180 : getAutoPath().path.get(0).getRotation2d(0).getDegrees() + 180);
 
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(3);
-
     if (DriverStation.isJoystickConnected(Constants.OI.LeftJoy.port) || inCompetition) {
       configureButtonBindingsLeftJoy();
     } else {
@@ -128,7 +133,9 @@ public RobotContainer(Robot robot) {
     dt.setDefaultCommand(new TeleopDrive(dt,
         () -> inputProcessing(getStickValue(Constants.OI.StickType.LEFT, Constants.OI.StickDirection.Y)),
         () -> inputProcessing(getStickValue(Constants.OI.StickType.LEFT, Constants.OI.StickDirection.X)),
-        () -> inputProcessing(getStickValue(Constants.OI.StickType.RIGHT, Constants.OI.StickDirection.X)), () -> leftJoy.getRawButton(1) || rightJoy.getRawButton(1)));
+        () -> inputProcessing(getStickValue(Constants.OI.StickType.RIGHT, Constants.OI.StickDirection.X)),
+        () -> leftJoy.getRawButton(Constants.OI.LeftJoy.slowDriveButton),
+        () -> rightJoy.getRawButton(Constants.OI.RightJoy.autoIntake), lime));
 
     intakeFeeder.setDefaultCommand(
       new PerpetualCommand(
@@ -165,6 +172,12 @@ private void configureButtonBindingsRightJoy() {
     new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(climber::slowRetractRight)).whenReleased(new InstantCommand(climber::stopRight));
     new JoystickButton(rightJoy, Constants.OI.RightJoy.toggleShooterModePort).whenPressed(new InstantCommand(shooter::toggleDutyCycleMode));
     new JoystickButton(rightJoy, Constants.OI.RightJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override));
+    new POVButton(rightJoy, 90).whenPressed(new InstantCommand(() -> lime.setIdleTurnDirection(Limelight.TurnDirection.CW)));
+    new POVButton(rightJoy, 270).whenPressed(new InstantCommand(() -> lime.setIdleTurnDirection(Limelight.TurnDirection.CCW)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.autoIntake).whenPressed(() -> {
+      RobotContainer.this.robotWasFieldAligned = SmartDashboard.getBoolean("Field Oriented", true);
+      SmartDashboard.putBoolean("Field Oriented", false);
+    }).whenReleased(() -> SmartDashboard.putBoolean("Field Oriented", RobotContainer.this.robotWasFieldAligned));
   }
 
   private void configureButtonBindingsController() {
@@ -175,10 +188,10 @@ private void configureButtonBindingsController() {
     new JoystickButton(controller, Constants.OI.Controller.toggleIntakePort).whenPressed(new InstantCommand(intakeFeeder::toggleIntake, intakeFeeder));
     new JoystickButton(controller, Constants.OI.Controller.extendClimberPort).whenPressed(new ExtendClimber(climber));
     new JoystickButton(controller, Constants.OI.Controller.retractClimberPort).whenPressed(new RetractClimber(climber));
-    new POVButton(controller, 0).whenPressed(new InstantCommand( () ->{shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() + 0.1);}));
-    new POVButton(controller, 180).whenPressed(new InstantCommand( () ->{shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() - 0.1);}));
-    new POVButton(controller, 90).whenPressed(new InstantCommand(() -> {shooter.setMainSpeed(shooter.getTargetSpeed() + 100);}));
-    new POVButton(controller, 270).whenPressed(new InstantCommand(() -> {shooter.setMainSpeed(shooter.getTargetSpeed() - 100);}));
+    new POVButton(controller, 0).whenPressed(new InstantCommand(() -> shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() + 0.1)));
+    new POVButton(controller, 180).whenPressed(new InstantCommand( () -> shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() - 0.1)));
+    new POVButton(controller, 90).whenPressed(new InstantCommand(() -> shooter.setMainSpeed(shooter.getTargetSpeed() + 100)));
+    new POVButton(controller, 270).whenPressed(new InstantCommand(() -> shooter.setMainSpeed(shooter.getTargetSpeed() - 100)));
 
   }
   private List<RobotPath> loadPaths(String... pathNames) {
@@ -196,7 +209,7 @@ public void initShooterConfig() {
    */
   public Command getAutonomousCommand() {
     AutoPath path = getAutoPath();
-    return path == null ? new InstantCommand() : new Autonomous(path, path.shootAtStart, path.shootAtEnd, dt, shooter, intakeFeeder);
+    return path == null ? new InstantCommand() : new Autonomous(path, path.shootAtStart, path.shootAtEnd, dt, shooter, intakeFeeder, lime);
   }
 
   private double getStickValue(Constants.OI.StickType stick, Constants.OI.StickDirection dir) {
@@ -274,4 +287,8 @@ public RobotPath loadPath(String pathName) {
       return null;
     }
   }
+
+  public void setLimelightColor() {
+    NetworkTableInstance.getDefault().getTable(lime.config.ntName).getEntry("pipeline").setNumber(DriverStation.getAlliance().ordinal());
+  }
 }

src/main/java/org/team199/robot2022/commands/Autonomous.java

@@ -8,31 +8,52 @@
 import org.team199.robot2022.subsystems.Shooter;
 import org.team199.robot2022.subsystems.IntakeFeeder.Motor;
 
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+
+import org.carlmontrobotics.lib199.Limelight;
 import org.carlmontrobotics.lib199.path.RobotPath;
 
 public class Autonomous extends SequentialCommandGroup {
 
-    public Autonomous(AutoPath path, boolean shootAtStart, boolean shootAtEnd, Drivetrain drivetrain, Shooter shooter, IntakeFeeder intakeFeeder) {
+    public Autonomous(AutoPath path, boolean shootAtStart, boolean shootAtEnd, Drivetrain drivetrain, Shooter shooter, IntakeFeeder intakeFeeder, Limelight lime) {
         addRequirements(drivetrain, shooter, intakeFeeder);
-        addCommands(new PassiveAutomaticIntake(intakeFeeder));
+        addCommands(new PassiveManualIntake(intakeFeeder));
         if(path.shootAtStart) addCommands(new InstantCommand(() -> shooter.setShotPosition(path.startShotPosition)));
 
         addCommands(
             new InstantCommand(path.path.get(0)::initializeDrivetrainPosition),
+            shootAtStart ? new SequentialCommandGroup( new WaitUntilCommand(shooter::isAtTargetSpeed),  new WaitCommand(1.5), new Shoot(intakeFeeder, shooter)) : new InstantCommand(),
             shootAtStart ? new SequentialCommandGroup( new WaitUntilCommand(shooter::isAtTargetSpeed),  new WaitCommand(1.5), new Shoot(intakeFeeder, shooter)) : new InstantCommand()
         );
         //addCommands(new WaitCommand(4));
         for (int i = 0; i < path.path.size(); i++){
-            addCommands(new ParallelRaceGroup(path.path.get(i).getPathCommand(false, false), new PassiveAutomaticIntake(intakeFeeder).perpetually()));
+            addCommands(new ParallelRaceGroup(path.path.get(i).getPathCommand(false, false), new PassiveManualIntake(intakeFeeder).perpetually()));
             if (i < path.path.size()-1){
-            addCommands(new InstantCommand(path.path.get(i+1)::initializeDrivetrainPosition));
+                if(path.useLimelight && false)
+                    addCommands(
+                        new ParallelRaceGroup(
+                            new TeleopDrive(drivetrain, () -> 0D, () -> 0D, () -> 0D, () -> false, () -> true, lime), // Auto-pickup is sketch
+                            new WaitUntilCommand(() -> intakeFeeder.getNumBalls() == 1), // Wait until we pick something up
+                            new SequentialCommandGroup(
+                                new WaitCommand(5), // If we haven't found anything after 5 seconds, abort
+                                new ConditionalCommand(
+                                    new WaitUntilCommand(() -> intakeFeeder.getNumBalls() == 1), // We're chasing a ball! Wait to pick it up
+                                    new InstantCommand(), // We can't see the ball! Just abort
+                                    () -> NetworkTableInstance.getDefault().getTable(lime.config.ntName).getEntry("tv").getDouble(0) == 1
+                                )
+                            )
+                        )
+                    );
+                else
+                    addCommands(new InstantCommand(path.path.get(i+1)::initializeDrivetrainPosition));
             }
         }
         addCommands(new InstantCommand(() -> {drivetrain.stop();}));

src/main/java/org/team199/robot2022/commands/PassiveManualIntake.java

@@ -23,7 +23,7 @@ public void execute() {
         SmartDashboard.putString("IntakeFeeder Default Type", "Manual");
         // Reset the balls in the cargo as color sensor no longer works and we cannot
         // accurately record the cargo
-        intakeFeeder.clearCargo();
+        // intakeFeeder.clearCargo();
         // Manually intake balls
         if(lastFeed != intakeFeeder.getNumBalls()) isSpinningUp = true;
         lastFeed = intakeFeeder.getNumBalls();

src/main/java/org/team199/robot2022/commands/Shoot.java

@@ -30,7 +30,10 @@ public Shoot(IntakeFeeder intakeFeeder, Shooter shooter) {
         new WaitUntilCommand(shooter::isAtTargetSpeed),
         new FunctionalCommand(
           shooter::disableShooter,
-          () -> {intakeFeeder.invertAndRun(Motor.TOP, false, true);},
+          () -> {
+            intakeFeeder.invertAndRun(Motor.TOP, false, true);
+            intakeFeeder.invertAndRun(Motor.MIDDLE, false, true);
+          },
           interrupted -> {
             if(interrupted) return;
             intakeFeeder.stopRunningFeeder();

src/main/java/org/team199/robot2022/commands/TeleopDrive.java

@@ -9,33 +9,40 @@
 
 import java.util.function.Supplier;
 
+import org.carlmontrobotics.lib199.Limelight;
 import org.team199.robot2022.Constants;
 import org.team199.robot2022.subsystems.Drivetrain;
 
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.drive.Vector2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class TeleopDrive extends CommandBase {
   private static final double kSlowDriveSpeed = 0.25;
   private static final double kSlowDriveRotation = 0.30;
+  private static final double kAlignMultiplier = 1D/3D;
+  private static final double kAlignForward = 0.6;
 
   private final Drivetrain drivetrain;
   private Supplier<Double> fwd;
   private Supplier<Double> str;
   private Supplier<Double> rcw;
   private Supplier<Boolean> slow;
+  private Supplier<Boolean> align;
+  private Limelight lime;
   double currentForward = 0;
   double currentStrafe = 0;
   /**
    * Creates a new TeleopDrive.
    */
-  public TeleopDrive(Drivetrain drivetrain, Supplier<Double> fwd, Supplier<Double> str, Supplier<Double> rcw, Supplier<Boolean> slow) {
+  public TeleopDrive(Drivetrain drivetrain, Supplier<Double> fwd, Supplier<Double> str, Supplier<Double> rcw, Supplier<Boolean> slow, Supplier<Boolean> align, Limelight lime) {
     addRequirements(this.drivetrain = drivetrain);
     this.fwd = fwd;
     this.str = str;
     this.rcw = rcw;
     this.slow = slow;
+    this.align = align;
+    this.lime = lime;
   }
 
   // Called when the command is initially scheduled.
@@ -76,6 +83,13 @@ public void execute() {
    // SmartDashboard.putNumber("Strafe (mps)", currentStrafe);
     double driveMultiplier = slow.get() ? kSlowDriveSpeed : 1;
     double rotationMultiplier = slow.get() ? kSlowDriveRotation : 0.55;
+    if(align.get()) {
+      currentForward = Constants.DriveConstants.maxSpeed * kAlignForward * NetworkTableInstance.getDefault().getTable(lime.config.ntName).getEntry("tv").getDouble(0.0);
+      currentStrafe = 0;
+      driveMultiplier = 1;
+      rotateClockwise = lime.steeringAssist();
+      rotationMultiplier = Constants.DriveConstants.maxRCW * kAlignMultiplier;
+    }
     drivetrain.drive(currentForward * driveMultiplier, currentStrafe * driveMultiplier, rotateClockwise * rotationMultiplier);
   }
 

src/main/java/org/team199/robot2022/subsystems/Drivetrain.java

@@ -26,7 +26,7 @@
 public class Drivetrain extends SubsystemBase implements SwerveDriveInterface {
     // compassOffset is magnetic north relative to the current heading
     private final double compassOffset;
-    private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); // Also try kUSB and kUSB2
+    private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2
 
     private SwerveDriveKinematics kinematics = null;
     private SwerveDriveOdometry odometry = null;