Hoppersubsystem

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

@@ -65,6 +65,7 @@ public class Robot extends TimedRobot implements LogUpdater {
 
     public static double get(SD sd)            {return getState().get(sd);}
     public static void   set(SD sd, double val){       getState().set(sd, val);}
+    public static <K> void setMapped(BiHashMap<K,Double> biMap, SD sd, K key) { getState().setMapped(biMap, sd, key);}
     public static void optionalSet(Optional<SD> optionalSD , double val){       
         getState().optionalSet(optionalSD, val);
     }
@@ -174,8 +175,6 @@ public void robotInit() {
         addSDToLog(SD.Y);
         addSDToLog(SD.Angle);
         addSDToLog(SD.Time);
-
-
     }
 
     public void logDataPeriodic() {

src/main/java/frc/robot/commands/hopper/ElevatorControlCommand.java

@@ -0,0 +1,23 @@
+package frc.robot.commands.hopper;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+import frc.robot.robotState.RobotState;
+import frc.robot.robotState.RobotState.SD;
+
+public class ElevatorControlCommand extends Command {
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void execute() {
+        if (Robot.getState().getMapped(RobotState.BOOLEAN_MAPPING, SD.BallInElevator)) {
+            Robot.hopperSubsystem.elevator();
+        } else if (!Robot.shooterSubsystem.isRunning()) {
+            Robot.hopperSubsystem.stopElevator();
+        }
+    }
+}

src/main/java/frc/robot/commands/hopper/ElevatorHopperCommand.java

@@ -9,5 +9,4 @@ public class ElevatorHopperCommand extends InstantCommand {
     protected void execute(){
         Robot.hopperSubsystem.elevator();
     }
-
 }

src/main/java/frc/robot/robotState/RobotState.java

@@ -35,12 +35,16 @@ public enum SD {
         // Pneumatics
         PressureSensorVoltage,
 
+        // Hopper
+        BallInElevator,
+
         // Turret
         TurretAngle,
 
         // Shooter
         HoodAngle,
         ShooterOmega,
+        ShooterSparkValue,
 
         DistanceToPort,
         Time,

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

@@ -1,23 +1,41 @@
 package frc.robot.subsystems;
 
 import frc.robot.sciSensorsActuators.SciSpark;
+import com.revrobotics.Rev2mDistanceSensor;
+import com.revrobotics.Rev2mDistanceSensor.Port;
+import com.revrobotics.Rev2mDistanceSensor.Unit;
+
 import frc.robot.PortMap;
+import frc.robot.Robot;
+import frc.robot.robotState.RobotStateUpdater;
+import frc.robot.robotState.RobotState;
+import frc.robot.robotState.RobotState.SD;
+
+public class HopperSubsystem implements RobotStateUpdater {
+
+    public SciSpark elevatorSpark, suckSpark;
+    private static final double ELEVATOR_SPEED = 0.4, SUCK_SPEED = 0.5;
+    private Rev2mDistanceSensor distanceSensor;
 
-public class HopperSubsystem {
-
-    public SciSpark upSpark, inSpark;
-    private static final double UP_SPEED = 0.4, IN_SPEED = 0.5;
-
     public HopperSubsystem() {
-        this.upSpark = new SciSpark(PortMap.HOPPER_ELEVATOR_SPARK);
-        this.inSpark = new SciSpark(PortMap.HOPPER_SUCK_SPARK);
+        this.elevatorSpark = new SciSpark(PortMap.HOPPER_ELEVATOR_SPARK);
+        this.suckSpark = new SciSpark(PortMap.HOPPER_SUCK_SPARK);
+        this.distanceSensor = new Rev2mDistanceSensor(Port.kOnboard);
+        automateStateUpdating();
     }
 
-    public void setUpSpeed(double speed) { this.upSpark.set(speed); }
-    public void setInSpeed(double speed) { this.inSpark.set(speed); }
+    @Override
+    public void updateRobotState() {
+        double range = distanceSensor.getRange(Unit.kMillimeters);
+        Robot.setMapped(RobotState.BOOLEAN_MAPPING, SD.BallInElevator, range < 30 ? true : false);
+    }
+
+    public void setElevatorSpeed(double speed) { this.elevatorSpark.set(speed); }
+    public void setSuckSpeed(double speed)     { this.suckSpark.set(speed); }
+
+    public void elevator() { this.setElevatorSpeed(ELEVATOR_SPEED); }
+    public void suck() { this.setSuckSpeed(SUCK_SPEED); }
+    public void stopElevator() {this.setElevatorSpeed(0);}
+    public void stopSuck()     {this.setSuckSpeed(0);}
 
-    public void elevator() { this.setUpSpeed(UP_SPEED); }
-    public void suck() { this.setInSpeed(IN_SPEED); }
-    public void stopElevator() {this.setUpSpeed(0);}
-    public void stopSuck()     {this.setInSpeed(0);}
 }

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

@@ -11,6 +11,7 @@
 import frc.robot.robotState.RobotState.SD;
 import frc.robot.sciSensorsActuators.SciSpark;
 import frc.robot.sciSensorsActuators.SciThroughBoreEncoder;
+import frc.robot.sciSensorsActuators.SciSpark.SciSparkSD;
 
 public class ShooterSubsystem extends Subsystem implements RobotStateUpdater {
   private double HOOD_ANGLE_P = 0.65, HOOD_ANGLE_I = 0.01, HOOD_ANGLE_D = 0.0;
@@ -50,6 +51,8 @@ public ShooterSubsystem() {
     this.absEncoder.setDistancePerRotation(2 * Math.PI * HOOD_SPARK_GEAR_RATIO);
     this.absEncoder.setAngle(Math.toRadians(57));
     Robot.set(SD.HoodAngle, this.absEncoder.getRadians());
+
+    this.rightShooterSpark.assignSD(SciSparkSD.Value, SD.ShooterSparkValue);
 
     automateStateUpdating();
   }
@@ -68,9 +71,9 @@ public void testShooterSpark(double speed){
   }
 
   public void stopMotors() {
-    this.hoodSpark.stopMotor();
-    this.rightShooterSpark.stopMotor();
-    this.leftShooterSpark.stopMotor();
+    this.hoodSpark.set(0);
+    this.rightShooterSpark.set(0);
+    this.leftShooterSpark.set(0);
   }
 
   @Override
@@ -81,4 +84,8 @@ public void updateRobotState() {
     Robot.set(SD.HoodAngle, this.absEncoder.getRadians());
     Robot.set(SD.ShooterOmega, this.shooterSparkEncoder.getVelocity());
   }
-}
+
+  public boolean isRunning() {
+    return Robot.getState().get(SD.ShooterSparkValue) != 0;
+  }
+}

vendordeps/REV2mDistanceSensor.json

@@ -0,0 +1,55 @@
+{
+    "fileName": "REV2mDistanceSensor.json",
+    "name": "REV2mDistanceSensor",
+    "version": "0.3.0",
+    "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae",
+    "mavenUrls": [
+        "http://www.revrobotics.com/content/sw/max/sdk/maven/"
+    ],
+    "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "DistanceSensor-java",
+            "version": "0.3.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "DistanceSensor-driver",
+            "version": "0.3.0",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "DistanceSensor-cpp",
+            "version": "0.3.0",
+            "libName": "libDistanceSensor",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "DistanceSensor-driver",
+            "version": "0.3.0",
+            "libName": "libDistanceSensorDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ]
+        }
+    ]
+}