Vision and intaking

README.md

@@ -16,14 +16,18 @@ For the purposes of the code and documentation:
 #### Subsystems 
 Located in /subsystems
 *   `Drivetrain.java`
-*   `Intake.java` - Controls the intake AND the handoff to the indexer
+*   `Intake.java` - Controls the intake AND the indexer
 *   `Shooter.java` - Indexer and shooter, NOT including aiming- that's from the command
 *   `Spinner.java` - Mechanism and sensors for manipulating the control panel
 *   `Climber.java`
 
 #### Commands
 Located in /commands
 *   `UserDrive.java` - Drive with user input
+*   `IntakeCommand.java` - Intake based on user input and index based on sensor input
+*   `VisionCommand.java` - Stitch together the other vision commands into a single SequentialCommandGroup
+  * `VisionPosition.java` - Turn to the appropriate angle based on vision
+  * `VisionShoot.java` - Launch balls and manage shooter velocity
 
 #### Utils
 Located in /utils

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

@@ -20,21 +20,23 @@
 public final class Constants {
 
     // CAN ids
-    public static final int K_DRIVE_LEFT_FRONT_ID   = 01;
-    public static final int K_DRIVE_LEFT_MIDDLE_ID  = 02;
-    public static final int K_DRIVE_LEFT_BACK_ID    = 03;
-
-    public static final int K_DRIVE_RIGHT_FRONT_ID  = 04;
-    public static final int K_DRIVE_RIGHT_MIDDLE_ID = 05;
-    public static final int K_DRIVE_RIGHT_BACK_ID   = 06;
-
-    public static final int K_SHOOTER_FlYWHEEL_ID   = 07;
-
+    public static final int K_DRIVE_LEFT_FRONT_ID   = 01;//why
+    public static final int K_DRIVE_LEFT_MIDDLE_ID  = 02;//comment
+    public static final int K_DRIVE_LEFT_BACK_ID    = 03;//like
+                                                         //this
+    public static final int K_DRIVE_RIGHT_FRONT_ID  = 04;      //when
+    public static final int K_DRIVE_RIGHT_MIDDLE_ID = 05;     //you
+    public static final int K_DRIVE_RIGHT_BACK_ID   = 06;    //can
+                                                            //comment
+    public static final int K_SHOOTER_FlYWHEEL_ID   = 07;  //like
+                                                          //this
     // PWM Ports
     public static final int K_SHOOTER_KICKER_SPARK  = 0;
+    public static final int K_INTAKE_GRABBER_SPARK  = 1;
+    public static final int K_INTAKE_INDEXER_SPARK  = 2;
 
     // DIO Ports
-    public static final int K_DIST_SWITCH = 0;
+    public static final int K_INTAKE_INDEX_SWITCH   = 0;
 
     // Analog Ports
     public static final int K_DIST_SENSOR = 0;

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

@@ -14,8 +14,10 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.JoystickFlywheel;
 import frc.robot.subsystems.DriveTrain;
+import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
 import frc.robot.utils.Controller;
 import frc.robot.utils.Encoder;
@@ -31,15 +33,17 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
+  private final Command autoCommand;
+
   private final DriveTrain driveTrain;
   public final Shooter shooter;
-  private final Command autoCommand;
+  public final Intake intake;  
 
   // user controlls
   private UserAnalog driveRight;
   private UserAnalog driveLeft;
   private UserAnalog flywheelSpeed;
-
+  private UserAnalog intakeSpeed;
   // inputs
 
   /**
@@ -49,10 +53,11 @@ public RobotContainer() {
     // Configure the button bindings
     // driveTrain = new DriveTrain();
     shooter = new Shooter();
+    driveTrain = null;
+    intake = new Intake();
     configureButtonBindings();
     configureCommands();
     autoCommand = null;
-    driveTrain = null;
   }
 
   /**
@@ -68,6 +73,7 @@ private void configureButtonBindings() {
     // UserAnalog driveLeft = Controller.simpleAxis(Controller.PRIMARY,
     // Controller.AXIS_LY);
     flywheelSpeed = Controller.simpleAxis(Controller.PRIMARY, Controller.AXIS_LY);
+    intakeSpeed = UserAnalog.fromDigital(Controller.simpleButton(Controller.PRIMARY, Controller.BUTTON_RBUMPER),1,0);
   }
 
   private void configureCommands() {
@@ -78,6 +84,9 @@ private void configureCommands() {
     PIDTunerCommand tuneShooter = new PIDTunerCommand(ControlMode.Velocity, -1, 1, false, FeedbackDevice.QuadEncoder,
         new SubsystemBase[] { shooter }, Encoder.VersaPlanetary, shooter.flywheel);
     Robot.testCommand = tuneShooter;
+
+    //intake
+    IntakeCommand intakeCommand = new IntakeCommand(intake, intakeSpeed);
   }
 
   /**

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

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Intake;
+import frc.robot.utils.UserAnalog;
+
+public class IntakeCommand extends CommandBase {
+
+    private final double indexSpeed = 0.8;
+
+    private final Intake intake;
+    private final UserAnalog grabberSpeed;
+
+    public IntakeCommand(Intake intake, UserAnalog grabberSpeed) {
+        this.intake = intake;
+        this.grabberSpeed = grabberSpeed;
+
+        addRequirements(intake);
+        intake.setDefaultCommand(this);
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute() {
+        if (intake.getSwitch()) {
+            intake.index(indexSpeed);
+        } else {
+            intake.index(0);
+        }
+        intake.intake(grabberSpeed.get());
+    }
+}

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

@@ -1,42 +1,16 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.vision.PositionVisionCommand;
+import frc.robot.commands.vision.ShootVisionCommand;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Shooter;
 
-public class VisionCommand extends CommandBase {
-
-    private final DriveTrain driveTrain;
-    private final Shooter shooter;
-
-    public VisionCommand(Shooter shooter, DriveTrain driveTrain) {
-        this.driveTrain=driveTrain;
-        this.shooter=shooter;
-
-    }
-
-    @Override
-    public void initialize() {
-
-    }
-
-    @Override
-    public void execute() {
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    private void turnByAngle(double radians) {
-        DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.K_TRACK_WIDTH_METERS);
-        //kinematics works for speeds, but by inputing a position a position can also be yielded
-        DifferentialDriveWheelSpeeds turnPositions = kinematics.toWheelSpeeds(new ChassisSpeeds(0, 0, radians));
-        driveTrain.positionDrive(driveTrain.metersToRotations(turnPositions.rightMetersPerSecond),driveTrain.metersToRotations(turnPositions.leftMetersPerSecond));
+public class VisionCommand extends SequentialCommandGroup {
+    public VisionCommand(DriveTrain driveTrain, Shooter shooter) {
+        super();
+        PositionVisionCommand position = new PositionVisionCommand(shooter, driveTrain);
+        ShootVisionCommand shoot = new ShootVisionCommand(shooter);
+        addCommands(position, shoot);
     }
 }

src/main/java/frc/robot/commands/vision/PositionVisionCommand.java

@@ -0,0 +1,82 @@
+package frc.robot.commands.vision;
+
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.EntryNotification;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveTrain;
+import frc.robot.subsystems.Shooter;
+
+public class PositionVisionCommand extends CommandBase {
+
+    private final DriveTrain driveTrain;
+    private final Shooter shooter;
+    private final NetworkTableEntry angleEntry;
+    private int angleListenerHandle;
+    private final NetworkTableEntry distanceEntry;
+    private int distanceListenerHandle;
+    private boolean turning = false;
+    private double distance;
+    private double angle;
+
+    public PositionVisionCommand(Shooter shooter, DriveTrain driveTrain) {
+        this.driveTrain = driveTrain;
+        this.shooter = shooter;
+
+        NetworkTableInstance ntInst = NetworkTableInstance.getDefault();
+        NetworkTable table = ntInst.getTable("Vision");
+        angleEntry = table.getEntry("angle");
+        angleListenerHandle=angleEntry.addListener(this::listenAngle, EntryListenerFlags.kUpdate);
+        distanceEntry = table.getEntry("distance");
+        distanceListenerHandle=distanceEntry.addListener(this::listenDistance, EntryListenerFlags.kUpdate);
+
+    }
+
+    @Override
+    public void initialize() {
+        angleListenerHandle=angleEntry.addListener(this::listenAngle, EntryListenerFlags.kUpdate);
+        distanceListenerHandle=distanceEntry.addListener(this::listenDistance, EntryListenerFlags.kUpdate);    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public void end(boolean interupted){
+        angleEntry.removeListener(angleListenerHandle);
+        distanceEntry.removeListener(distanceListenerHandle);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !turning && angle<=0.01;
+    }
+
+    private void listenAngle(EntryNotification n){
+        angle=n.value.getDouble();
+        if(!turning && angle>0.01){
+            turnByAngle(angle);
+        }
+    }
+
+    private void listenDistance(EntryNotification n){
+        distance = n.value.getDouble();
+    }
+
+    private void turnByAngle(double radians) {
+        DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.K_TRACK_WIDTH_METERS);
+        // kinematics works for speeds, but by inputing a position a position can also
+        // be yielded
+        DifferentialDriveWheelSpeeds turnPositions = kinematics.toWheelSpeeds(new ChassisSpeeds(0, 0, radians));
+        driveTrain.positionDrive(driveTrain.metersToRotations(turnPositions.rightMetersPerSecond),
+                driveTrain.metersToRotations(turnPositions.leftMetersPerSecond));
+        turning=true;
+    }
+}

src/main/java/frc/robot/commands/vision/ShootVisionCommand.java

@@ -0,0 +1,33 @@
+package frc.robot.commands.vision;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Shooter;
+
+public class ShootVisionCommand extends CommandBase {
+
+    private final Shooter shooter;
+
+    public ShootVisionCommand(Shooter shooter) {
+        this.shooter = shooter;
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute(){
+    }
+
+    @Override
+    public void end(boolean isInterupted){
+
+    }
+
+    @Override
+    public boolean isFinished(){
+        return false;
+    }
+
+}

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

@@ -0,0 +1,47 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Intake extends SubsystemBase {
+
+    private final Spark indexMotor;
+    private final Spark intakeMotor;
+
+    private final DigitalInput indexSwitch;
+
+    public Intake() {
+        indexMotor = new Spark(Constants.K_INTAKE_INDEXER_SPARK);
+        intakeMotor = new Spark(Constants.K_INTAKE_GRABBER_SPARK);
+
+        indexSwitch = new DigitalInput(Constants.K_INTAKE_INDEX_SWITCH);
+    }
+
+    /**
+     * Indexes a ball once intaked.
+     * 
+     * @param speed - double from 1 (in) to -1 (out)
+     */
+    public void index(double speed) {
+        indexMotor.set(speed);
+    }
+
+    /**
+     * Returns whether the indexing switch is pressed
+     */
+    public boolean getSwitch() {
+        return indexSwitch.get();
+    }
+
+    /**
+     * runs the intake bar
+     * 
+     * @param double speed -
+     */
+    public void intake(double speed) {
+        intakeMotor.set(speed);
+    }
+
+}

src/main/java/frc/robot/utils/UserAnalog.java

@@ -1,9 +1,19 @@
 package frc.robot.utils;
 
 public interface UserAnalog {
+
+    public static UserAnalog fromDigital(UserDigital digital, double trueVal, double falseVal){
+        return ()->{
+            if(digital.get()){
+                return trueVal;
+            }else{
+                return falseVal;
+            }
+        };
+    }
+
     /**
      * @return value - a [-1,1] value based on the input from the user or controller
      */
     public double get();
 }
-