Merge branch 'full-robot-test' into vision-odo-not-proto

src/main/java/frc4388/robot/Constants.java

@@ -119,6 +119,25 @@ public final class Constants {
     public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
   }
 
+  public static final class SerializerConstants {
+    public static final double SERIALIZER_BELT_SPEED = 0.1d;
+    
+    // CAN IDs
+    public static final int SERIALIZER_BELT = 16;
+    public static final int SERIALIZER_BELT_BEAM = 27; // TODO
+  }
+  
+  public static final class IntakeConstants {
+    // CAN IDs
+    public static final int INTAKE_MOTOR = 14;
+    public static final int EXTENDER_MOTOR = 15;
+  }
+  public static final class StorageConstants {
+    public static final int STORAGE_CAN_ID = 17;
+    public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
+    public static final int BEAM_SENSOR_INTAKE = 29; //TODO
+    public static final double STORAGE_SPEED = 0.3;
+  }
   public static final class LEDConstants {
     public static final int LED_SPARK_ID = 0;
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -56,12 +56,16 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.commands.Shoot;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Serializer;
+import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
@@ -84,17 +88,17 @@ public class RobotContainer {
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
-  /* Subsystems */
-  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
-      m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
-
+  // Subsystems 
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
+  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
+  private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
+  private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
   private final Hood m_robotHood = new Hood();
   private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
-
   private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
-  // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
+  private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
 
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -130,6 +134,7 @@ public class RobotContainer {
 
     m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
 
+    //Swerve Drive
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
             getDriverController().getLeftX(),
@@ -138,6 +143,20 @@ public class RobotContainer {
             getDriverController().getRightY(),
             true),
             m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
+    //Intake with Triggers
+    m_robotIntake.setDefaultCommand(
+        new RunCommand(() -> m_robotIntake.runWithTriggers(
+            getOperatorController().getLeftTriggerAxis(), 
+            getOperatorController().getRightTriggerAxis()),
+            m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
+    //Storage Management
+    m_robotStorage.setDefaultCommand(
+        new RunCommand(() -> m_robotStorage.manageStorage(), 
+        m_robotStorage).withName("Storage manageStorage defaultCommand"));
+    //Serializer Management
+    m_robotSerializer.setDefaultCommand(
+        new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), 
+        m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
     /*
@@ -156,9 +175,10 @@ public class RobotContainer {
    * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    
     /* Driver Buttons */
     // "XboxController.Button.kBack" was undefined yet, 7 works just fine
-    new JoystickButton(getDriverController(), 7)
+    new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
         .whenPressed(m_robotSwerveDrive::resetGyro);
 
     new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
@@ -173,27 +193,39 @@ public class RobotContainer {
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
         .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-    new JoystickButton(getDriverController(), XboxController.Button.kX.value)
+
+    new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
         .whenPressed(() -> m_robotMap.leftFront.reset())
         .whenPressed(() -> m_robotMap.rightFront.reset())
         .whenPressed(() -> m_robotMap.leftBack.reset())
         .whenPressed(() -> m_robotMap.rightBack.reset());
 
     /* Operator Buttons */
-    // activates "Lit Mode"
     /*
-     * new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-     * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-     * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-     * 
-     * new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-     * .whenPressed(new InstantCommand());
-     * 
      * // activates "BoomBoom"
      * new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
      * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
      * m_robotHood));
      */
+
+    //Extender
+    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+        .whenPressed(() -> m_robotIntake.runExtender(true));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+        .whenPressed(() -> m_robotIntake.runExtender(false));
+
+
+
+    //Storage
+    new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
+        .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))
+        .whenReleased(() -> m_robotStorage.runStorage(0.0));
+
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
+        .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
+        .whenReleased(() -> m_robotStorage.runStorage(0.0));
   }
 
   /**

src/main/java/frc4388/robot/RobotMap.java

@@ -15,10 +15,13 @@ import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.SerializerConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
 
@@ -208,6 +211,19 @@ public class RobotMap {
 
   }
 
-  }
 
-       
+       
+  /* Serializer Subsystem */
+  public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
+//   public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless);
+  public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
+  
+  /* Intake Subsytem */
+  public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
+  public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
+
+  /* Storage Subsystem */
+  public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+  public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
+  public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+}

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import javax.sql.rowset.WebRowSet;
+
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
@@ -15,6 +17,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.utility.Gains;
 

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

@@ -0,0 +1,80 @@
+// 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 frc4388.robot.subsystems;
+
+//Imported Limit switch ONLY
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.SparkMaxLimitSwitch.Type;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+
+public class Intake extends SubsystemBase {
+
+  private WPI_TalonFX m_intakeMotor;
+  private CANSparkMax m_extenderMotor;
+  private Serializer m_serializer;
+  private SparkMaxLimitSwitch m_inLimit;
+  private SparkMaxLimitSwitch m_outLimit;
+
+  public boolean toggle;
+
+  /** Creates a new Intake. */
+  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) {
+    m_intakeMotor = intakeMotor;
+    m_extenderMotor = extenderMotor; 
+    m_serializer = serializer;
+
+    m_extenderMotor.restoreFactoryDefaults();
+
+    m_intakeMotor.setNeutralMode(NeutralMode.Brake);
+    m_intakeMotor.setInverted(false);
+    m_extenderMotor.setInverted(true);
+    
+    m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_inLimit.enableLimitSwitch(true);
+    m_outLimit.enableLimitSwitch(true);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+  /**
+   * Runs The Intake With Triggers.
+   * @param leftTrigger Left Trigger to Run -
+   * @param rightTrigger Right Trigger to Run +
+   */
+  public void runWithTriggers(double leftTrigger, double rightTrigger) {
+    m_intakeMotor.set(rightTrigger - leftTrigger);
+  }
+  /**
+   * Runs The Extender
+   * @param extended Wether the Extender Is Extended
+   */
+  public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
+    if (!m_serializer.getBeam() && !extended) return;
+    double extenderMotorSpeed = extended ? 0.25d : -0.25d;
+    m_extenderMotor.set(extenderMotorSpeed);
+  }
+
+  public void runExtender(double input) {
+    if (!m_serializer.getBeam() && input < 0.) return;
+    m_extenderMotor.set(input);
+  }
+  /**
+   * Toggles The Extender
+  */
+  public void toggleExtender() {
+    toggle = !toggle;
+    runExtender(toggle);
+  }
+}

src/main/java/frc4388/robot/subsystems/Serializer.java

@@ -0,0 +1,53 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
+import frc4388.robot.Constants.SerializerConstants;
+import edu.wpi.first.wpilibj.DigitalInput;
+import com.revrobotics.CANSparkMax;
+
+public class Serializer extends SubsystemBase{
+  private CANSparkMax m_serializerBelt;
+  // private CANSparkMax m_serializerShooterBelt;
+  private DigitalInput m_serializerBeam;
+  private boolean serializerState;
+
+  public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { 
+    m_serializerBelt = serializerBelt;
+    m_serializerBeam = serializerBeam;
+
+    m_serializerBelt.set(0);
+    // m_serializerShooterBelt.set(0);
+    
+  }
+
+  public void setSerializer(double input){
+    m_serializerBelt.set(input);
+  }
+  /**
+   * Gets The State Of The Beam
+   * @return The State Of The Beam
+   */
+  public boolean getBeam() {
+    return m_serializerBeam.get();
+  }
+
+  /**
+   * Sets The Serializer State With The Beam
+   * @param state Your State Of The Button
+   * @param beambroken The State of the Beam Senser
+   */
+  public void setSerializerStateWithBeam() {
+    if (m_serializerBeam.get()) setSerializer(0.0);
+    else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED);
+  }
+  
+  /**
+   * Gets the Serializer State
+   * @return The Serializer State
+   */
+  public boolean getSerializerState() {
+    return serializerState;
+  }
+}

src/main/java/frc4388/robot/subsystems/Storage.java

@@ -0,0 +1,64 @@
+// 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 frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.Constants.StorageConstants;
+
+public class Storage extends SubsystemBase {
+  public CANSparkMax m_storageMotor;
+  private DigitalInput m_beamShooter;
+  private DigitalInput m_beamIntake;
+
+  /** Creates a new Storage. */
+  public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) {
+    m_storageMotor = storageMotor;
+    m_beamShooter = beamShooter;
+    m_beamIntake = beamIntake;
+  }
+  /**
+   * If The Beam Is Broken, Run Storage
+   * If Else, Stop Running Storage
+   */
+  public void manageStorage() {
+    if (getBeamIntake()) runStorage(0.d);
+    else runStorage(1.d);
+  }
+  
+  /**
+   * Runs The Storage at a Specifyed Speed
+   * @param input The Specifyed Speed
+   */
+  public void runStorage(double input) {
+    m_storageMotor.set(input);
+  }
+  /**
+   * Gets The Beam State On The Shooter
+   * @return The State Of The Beam on the Shooter
+   */
+  public boolean getBeamShooter(){
+    return m_beamShooter.get();//True if open
+  }
+
+  /**
+   * Gets The Beam State Of The Intake
+   * @return The Beam State Of The Intake
+   */
+  public boolean getBeamIntake(){
+    return m_beamIntake.get(); //True if open
+  }
+  
+
+  @Override
+  /**
+   * Every Robot Tick Manage The Storage
+   */
+  public void periodic() {
+    manageStorage();
+  }
+}