diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bdc74e7..54265cb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,7 +19,6 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -31,7 +30,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics; import frc.lib.W8.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics; @@ -86,13 +84,7 @@ public class HopperConstants { public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(15.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); - - // CHANGE TO PROPER RPMS !!!! - public static final double SLOW_SPEED_RPM = 0.0; - public static final double FAST_SPEED_RPM = 0.0; - public static final double REVERSE_SPEED_RPM = 0.0; - public static final Voltage VOLTAGE = Volts.of(12.0); - public static final Distance DRUM_RADIUS = Inches.of(2.0); + private static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); public static final AngularVelocity ANGULAR_VELOCITY = RotationsPerSecond.of(1); public static final AngularAcceleration ANGULAR_ACCELERATION = @@ -121,13 +113,9 @@ public final class ShooterConstants { public static final Angle MAX_ANGLE = Rotations.of(10.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); + public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); - public static final double IDLE_SPEED_RPM = (1.0); - public static final double HUB_SPEED_RPM = (1.0); - public static final double TOWER_SPEED_RPM = (1.0); - public static final double DEFAULT_SPEED_RPM = (1.0); - public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; } public static final int CANDLE_ID = 50; @@ -167,5 +155,6 @@ public class ClimberConstants { MAX_DISTANCE, STARTING_DISTANCE, CONVERTER); + public static final double CLIMB_SPEED = 1.0; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 7be04be..07f4a55 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -10,6 +13,18 @@ public Climber(LinearMechanism io) { io = _io; } + public enum State { + IDLE(Units.MetersPerSecond.of(0.0)), + ASCENDING(Units.MetersPerSecond.of(ClimberConstants.CLIMB_SPEED)), + DESCENDING(Units.MetersPerSecond.of(-ClimberConstants.CLIMB_SPEED)); + + private final LinearVelocity velocity; + + private State(LinearVelocity velocity) { + this.velocity = velocity; + } + } + @Override public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index a9f17fe..6d6a3d0 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -14,18 +14,18 @@ public class Hopper extends SubsystemBase { private FlywheelMechanism _io; - public enum State { - OFF(RevolutionsPerSecond.of(0.0)), - FORWARD_SLOW(RevolutionsPerSecond.of(HopperConstants.SLOW_SPEED_RPM / 60)), - FORWARD_FAST(RevolutionsPerSecond.of(HopperConstants.FAST_SPEED_RPM / 60)), - REVERSE(RevolutionsPerSecond.of(HopperConstants.REVERSE_SPEED_RPM / 60)); - - private final AngularVelocity _stateVelocity; - - private State(AngularVelocity stateVelocity) { - _stateVelocity = stateVelocity; - } - } + // public enum State { + // OFF(RevolutionsPerSecond.of(0.0)), + // FORWARD_SLOW(RevolutionsPerSecond.of(HopperConstants.SLOW_SPEED_RPM / 60)), + // FORWARD_FAST(RevolutionsPerSecond.of(HopperConstants.FAST_SPEED_RPM / 60)), + // REVERSE(RevolutionsPerSecond.of(HopperConstants.REVERSE_SPEED_RPM / 60)); + + // private final AngularVelocity _stateVelocity; + + // private State(AngularVelocity stateVelocity) { + // _stateVelocity = stateVelocity; + // } + // } public Hopper(FlywheelMechanism io) { _io = io; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fd86a44..f156702 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -37,20 +36,20 @@ public void setFlywheelVelocity(double velocity) { _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } - public enum State { - OFF(Units.RevolutionsPerSecond.of(0.0)), - IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), - SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), - SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), - SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), - SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); + // public enum State { + // OFF(Units.RevolutionsPerSecond.of(0.0)), + // IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), + // SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), + // SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), + // SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), + // SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); - private final AngularVelocity stateVelocity; + // private final AngularVelocity stateVelocity; - State(AngularVelocity stateVelocity) { - this.stateVelocity = stateVelocity; - } - } + // State(AngularVelocity stateVelocity) { + // this.stateVelocity = stateVelocity; + // } + // } // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() {