Storage Logic with Rotate Turret Command

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

@@ -41,7 +41,7 @@ public final class Constants {
 
     // Turret Constants
     public static final int TURRET_ENCODER_LIMIT = 85; // degree of freedom, if not met seeking will be required
-
+    public static final double TURRT_ROTATION_SPEED = 0.3; // percentage of output for turret
     public static final int TURRET_PID_SLOT = 0;
 
     public static final double P_TURRET = 0.1;

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

@@ -96,6 +96,7 @@ public void teleopInit() {
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {
+    CommandScheduler.getInstance().run();
   }
 
   @Override

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

@@ -4,25 +4,34 @@
 
 package frc.robot;
 
+import javax.naming.directory.DirContext;
 import javax.swing.plaf.basic.BasicDirectoryModel;
 
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.ArcadeDrive;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.commands.ExampleCommand;
+import frc.robot.commands.IntakeBall;
+import frc.robot.commands.RotateTurret;
 import frc.robot.commands.auto.Barrel;
 import frc.robot.commands.auto.Bounce;
 import frc.robot.commands.auto.Slalom;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Turret;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.*;
 
 /**
@@ -40,20 +49,23 @@ public class RobotContainer {
 
   // Subsystems
   private final DriveTrain driveTrain = new DriveTrain();
-  private final Turret turret = new Turret();
-  private final Shooter shooter = new Shooter();
+  //private final Turret turret = new Turret();
+  //private final Shooter shooter = new Shooter();
   private final Limelight limelight = new Limelight();
+  private final Intake intake = new Intake();
+
+  private final Field2d field = new Field2d();
 
   // Xbox Controller
   XboxController controller = new XboxController(0);
 
   // Commands
-
   private final ArcadeDrive arcadeDrive = new ArcadeDrive(driveTrain, () -> controller.getRawAxis(1),
       () -> controller.getRawAxis(0), () -> controller.getRawButtonPressed(5),
       () -> controller.getRawButtonReleased(5), () -> controller.getRawButtonPressed(6),
       () -> controller.getRawButtonReleased(6));    
 
+
   private SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 
   /**
@@ -66,7 +78,14 @@ public RobotContainer() {
     autoChooser.addOption("Bounce Paths", new Bounce(driveTrain, driveTrain.DRIVE_KINEMATICS));
     autoChooser.addOption("Barrel Racing Path", new Barrel(driveTrain, driveTrain.DRIVE_KINEMATICS));
     autoChooser.addOption("Slalom Path", new Slalom(driveTrain, driveTrain.DRIVE_KINEMATICS));
+
+    // SmartDashboard
     SmartDashboard.putData("Auton Task", autoChooser);
+    SmartDashboard.putData("Field", field);
+    SmartDashboard.putNumber("RightPosition", driveTrain.getRightMasterEncoderValue());
+    SmartDashboard.putNumber("LeftPosition", driveTrain.getLeftMasterEncoderValue());
+
+    // System.out.println(controller.getPort());
 
     driveTrain.setDefaultCommand(arcadeDrive);
 
@@ -80,6 +99,11 @@ public RobotContainer() {
    * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+
+    //RotateTurret Biniding
+    JoystickButton xButton = new JoystickButton(controller, 3);
+    xButton.whenHeld(new RotateTurret());
+
 
   }
 

src/main/java/frc/robot/Util.java

@@ -0,0 +1,10 @@
+package frc.robot;
+
+public class Util {
+
+    //absoluteDifference
+    public static boolean eps(double a, double b, double eps) {
+        return Math.abs(a - b) < eps;
+    }
+
+}

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

@@ -0,0 +1,37 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Intake;
+
+public class IntakeBall extends CommandBase {
+
+    private Intake intake;
+
+    public IntakeBall(Intake intake) {
+        intake = new Intake();
+        addRequirements(intake);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {}
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        intake.spinInBar();
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        intake.stop();
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+}

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

@@ -0,0 +1,47 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.Util;
+import frc.robot.subsystems.Limelight;
+import frc.robot.subsystems.Turret;
+
+public class RotateTurret extends CommandBase {
+
+    private Turret turret;
+    private Limelight lime;
+
+    public RotateTurret() {
+        addRequirements(turret, lime);
+    }
+
+    @Override
+    public void initialize(){
+        lime.turnOn();
+    }
+    @Override
+    public void execute() {
+        /*
+        if (lime.canSeeTarget() && lime.getHorizontalAngle() > 1.5) {
+            turret.set(lime.getHorizontalAngle() > 0 ? 0.5 : -0.5);
+        }*/
+        if(lime.canSeeTarget()){
+            double eps = lime.getHorizontalAngle();
+            if(eps != 0 && !Util.eps(eps, 0, 1.5)){
+                turret.set(Constants.TURRT_ROTATION_SPEED);
+            }else{
+                turret.stop();
+            }
+        }else{
+            turret.stop();
+        }
+
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        turret.stop();
+        lime.turnOff();
+    }
+
+}

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

@@ -0,0 +1,41 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Limelight;
+import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Storage;
+
+public class ShootStream extends CommandBase {
+
+    private Shooter shooter;
+    private Storage storage;
+    private Limelight lime;
+
+    public ShootStream(Shooter shooter, Storage storage, Limelight lime){
+        this.shooter = shooter;
+        this.storage = storage;
+        this.lime = lime;
+        addRequirements(shooter, storage, lime);
+    }    
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute(){
+        double velocity = shooter.calculateVelocity(lime.calculateDistance());
+        shooter.set(velocity);
+        if(shooter.atSetpoint()){
+            storage.spinBoth();
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        storage.stopBoth();
+        shooter.stop();
+    }
+
+}

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

@@ -0,0 +1,64 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Storage;
+
+public class StorageManager extends CommandBase {
+
+    private Storage storage;
+
+    public StorageManager(Storage storage) {
+        this.storage = storage;
+        addRequirements(storage);
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute() {
+
+        if(storage.getSensor(0).get()) {
+            if(storage.getBallCount() == 0){
+                storage.spinLowerMotor();
+                if(storage.getSensor(1).get()){
+                    storage.stopLowerMotor();
+                }
+                storage.incBallCount();
+            }
+
+            if(storage.getBallCount() == 2 && storage.getSensor(1).get() && storage.getSensor(2).get()){
+                storage.spinBoth();
+                if(storage.getSensor(3).get() && storage.getSensor(4).get()){
+                    storage.stopBoth();
+                }
+            }
+
+            if(storage.getBallCount() >= 2 && storage.getSensor(3).get() && storage.getSensor(4).get()){
+                int i = 0;
+                if(storage.getSensor(i).get()){
+                    storage.spinLowerMotor();
+                    if(storage.getSensor(i++).get()){
+                        storage.stopLowerMotor();
+                    }
+                }
+                storage.incBallCount();
+            }
+        }
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        storage.stopBoth();
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+}

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

@@ -19,9 +19,6 @@
 import com.revrobotics.EncoderType;
 import frc.robot.Constants;
 
-import java.io.Console;
-import java.util.function.BooleanSupplier;
-
 public class DriveTrain extends SubsystemBase {
 
     // DriveBase is divided into 2 sections, a right and left section.

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

@@ -22,4 +22,8 @@ public void spinInBar(){
     public void spinOutBar(){
         intakeBar.set(ControlMode.PercentOutput, Constants.INTAKE_SPEED_OUT);
     }
+
+    public void stop(){
+        intakeBar.set(ControlMode.PercentOutput, 0);
+    }
 }

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

@@ -14,12 +14,14 @@ public class Limelight extends SubsystemBase {
     private NetworkTableEntry horizontalAngleOffSet;
     private NetworkTableEntry verticalAngleOffSet;
     private NetworkTableEntry ta;
+    private NetworkTableEntry ledMode;
 
     private boolean target;
     private double x;
     private double y;
     private double area;
 
+
     public Limelight() {
 
         networkTable = NetworkTableInstance.getDefault().getTable("Limelight");
@@ -30,7 +32,8 @@ public Limelight() {
         verticalAngleOffSet = networkTable.getEntry("ty"); // Vertical Offset From Crosshair To Target (-20.5 degrees to
                                                            // 20.5 degrees)
         ta = networkTable.getEntry("ta"); // Target Area (0% of image to 100% of image
-
+        ledMode = networkTable.getEntry("ledMode");
+
     }
 
     /**
@@ -70,13 +73,23 @@ public double calculateDistance() {
                 / Math.tan(Math.toRadians(y) + Math.toRadians(Constants.MOUNTED_ANGLE));
     }
 
+    public void turnOn() {
+        ledMode.setDouble(3);
+    }
+
+    public void turnOff() {
+        ledMode.setDouble(1);
+    }
+
     public void periodic() {
 
         target = targetEntry.getBoolean(false);
         x = horizontalAngleOffSet.getDouble(0.0);
         y = verticalAngleOffSet.getDouble(0.0);
         area = ta.getDouble(0.0);
 
+        SmartDashboard.putNumber("Horizontal Error", x);
+
     }
 
 }