diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c551e6f..2247231 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 void setMapped(BiHashMap biMap, SD sd, K key) { getState().setMapped(biMap, sd, key);} public static void optionalSet(Optional 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() { diff --git a/src/main/java/frc/robot/commands/hopper/ElevatorControlCommand.java b/src/main/java/frc/robot/commands/hopper/ElevatorControlCommand.java new file mode 100644 index 0000000..da9197e --- /dev/null +++ b/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(); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/hopper/ElevatorHopperCommand.java b/src/main/java/frc/robot/commands/hopper/ElevatorHopperCommand.java index 2315c64..fe4abfe 100644 --- a/src/main/java/frc/robot/commands/hopper/ElevatorHopperCommand.java +++ b/src/main/java/frc/robot/commands/hopper/ElevatorHopperCommand.java @@ -9,5 +9,4 @@ public class ElevatorHopperCommand extends InstantCommand { protected void execute(){ Robot.hopperSubsystem.elevator(); } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/robotState/RobotState.java b/src/main/java/frc/robot/robotState/RobotState.java index d858903..d21dc95 100644 --- a/src/main/java/frc/robot/robotState/RobotState.java +++ b/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, diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 5c1c630..3d404c9 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/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);} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9ec42f2..75de07a 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/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; + } +} \ No newline at end of file diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 0000000..35a1bf0 --- /dev/null +++ b/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" + ] + } + ] +} \ No newline at end of file