diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index 5a625da..a63fa24 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,9 +1,14 @@ -Distance (in),Hood Ext. (u),Drum Velocity (u/ds) -74,20,8000 -144,23,10000 -162,28,10000 -208,29,10000 -296,32,12500 -418,33,16000 -430,31,16000 -450,31,16000 \ No newline at end of file +Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) +1,10,8000,2 +70,21,8000,2, check +100,24,9000,2 +145,28,10000,1 +230,31,12000,0,Add a 270 +246,32,15000,0 +320,32,17000,0,change 17000 and mark them lower +331,33,17000,0 +397,33,16000,0 +415,33,16250,0 +436,31,18000,0 +500,33,18000,0 +501,33,18000,0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9a194ee..7443120 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -109,12 +109,6 @@ public final class Constants { public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; } - - public static final class IntakeConstants {; - public static final double EXTENDER_SPEED = 0.3; - public static final int INTAKE_SPARK_ID = 12; - public static final int EXTENDER_SPARK_ID = 13; - } public static final class ShooterConstants { /* Motor IDs */ @@ -126,8 +120,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); - //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; @@ -160,29 +153,40 @@ public final class Constants { public static final class LevelerConstants { public static final int LEVELER_CAN_ID = 15; } + + public static final class IntakeConstants {; + public static final double EXTENDER_SPEED = 0.3; + public static final double INTAKE_SPEED = 1.0; + + public static final int INTAKE_SPARK_ID = 12; + public static final int EXTENDER_SPARK_ID = 13; + } public static final class StorageConstants { public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; - public static final double STORAGE_SPEED = 0.5; - public static final double STORAGE_TIMEOUT = 2000; + public static final double STORAGE_SPEED = 1.0; + public static final double STORAGE_TIMEOUT = 3000; + + /* Storage Characteristics */ + public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; //For the first storage belt + public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt /* Ball Indexes */ - public static final int BEAM_SENSOR_SHOOTER = 1; - public static final int BEAM_SENSOR_USELESS = 2; - public static final int BEAM_SENSOR_STORAGE = 3; - public static final int BEAM_SENSOR_INTAKE = 4; + public static final int BEAM_SENSOR_SHOOTER = 11; + public static final int BEAM_SENSOR_USELESS = 12; + public static final int BEAM_SENSOR_STORAGE = 13; + public static final int BEAM_SENSOR_INTAKE = 14; + + /* PID Gains */ + public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); /* PID Values */ public static final int SLOT_DISTANCE = 0; /* PID Indexes */ public static final int PID_PRIMARY = 0; - - /* PID Gains */ - public static final double STORAGE_MIN_OUTPUT = -1.0; - public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); } public static final class PneumaticsConstants { @@ -202,9 +206,9 @@ public final class Constants { public static final class VisionConstants { public static final double FOV = 29.8; //Field of view of limelight - public static final double TARGET_HEIGHT = 71; - public static final double LIME_ANGLE = 25; - public static final double TURN_P_VALUE = 0.65; + public static final double TARGET_HEIGHT = 71.5; + public static final double LIME_ANGLE = 24.7; + public static final double TURN_P_VALUE = 0.6; public static final double X_ANGLE_ERROR = 1.3; public static final double MOTOR_DEAD_ZONE = 0.3; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5f82551..3ff294f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -38,6 +38,8 @@ import frc4388.robot.commands.auto.DriveOffLineBackward; import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; import frc4388.robot.commands.auto.SixBallAutoMiddle; +import frc4388.robot.commands.InterruptSubystem; +import frc4388.robot.commands.auto.AutoPath1FromCenter; import frc4388.robot.commands.auto.Wait; import frc4388.robot.commands.climber.DisengageRachet; import frc4388.robot.commands.climber.RunClimberWithTriggers; @@ -50,6 +52,12 @@ import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.ShootFireGroup; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; +import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.shooter.TrackTarget; +import frc4388.robot.commands.shooter.TrimShooter; +import frc4388.robot.commands.storage.ManageStorage; +import frc4388.robot.commands.storage.StoragePrep; +import frc4388.robot.commands.storage.ManageStorage.StorageMode; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; @@ -136,7 +144,7 @@ public class RobotContainer { // runs the turret with joystick m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter)); // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller @@ -145,7 +153,7 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the storage not //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -190,17 +198,19 @@ public class RobotContainer { /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); + //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + .whenReleased(new InterruptSubystem(m_robotStorage)); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); + //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + .whenReleased(new InterruptSubystem(m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -220,7 +230,8 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooterAim)) .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) - //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)) + //.whenPressed(new StoragePrep(m_robotStorage)) + //.whenReleased(new InterruptSubystem(m_robotStorage)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) @@ -247,7 +258,8 @@ public class RobotContainer { //Run drum new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); } diff --git a/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/InterruptSubystem.java similarity index 79% rename from src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java rename to src/main/java/frc4388/robot/commands/InterruptSubystem.java index 0d8c447..5eae65f 100644 --- a/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java +++ b/src/main/java/frc4388/robot/commands/InterruptSubystem.java @@ -5,16 +5,18 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands.storage; +package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class StorageIntakeFinal extends CommandBase { +public class InterruptSubystem extends CommandBase { /** - * Creates a new StorageIntakeFinal. + * Creates a new InterruptSubystem. */ - public StorageIntakeFinal() { + public InterruptSubystem(SubsystemBase subsystem) { // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); } // Called when the command is initially scheduled. @@ -35,6 +37,6 @@ public class StorageIntakeFinal extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java deleted file mode 100644 index c0ef7a6..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java +++ /dev/null @@ -1,75 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; - -public class PrepChecker extends CommandBase { - Shooter m_shooter; - ShooterAim m_shooterAim; - ShooterHood m_shooterHood; - Storage m_storage; - - boolean m_isDrumReady = false; - boolean m_isAimReady = false; - boolean m_isHoodReady = false; - boolean m_isStorageReady = false; - - /** - * Creates a new PrepChecker. - * @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands. - * @param storage reads storage in a similar way to shooter. Not used as a requirement. - */ - public PrepChecker(Shooter shooter, Storage storage) { - m_shooter = shooter; - m_shooterAim = m_shooter.m_shooterAimSubsystem; - m_shooterHood = m_shooter.m_shooterHoodSubsystem; - m_storage = storage; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_isDrumReady = false; - m_isAimReady = false; - m_isHoodReady = false; - m_isStorageReady = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady); - m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady); - m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady); - m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_shooter.m_isDrumReady = false; - m_shooterAim.m_isAimReady = false; - m_shooterHood.m_isHoodReady = false; - m_storage.m_isStorageReadyToFire = false; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) { - return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java deleted file mode 100644 index 15ee080..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ /dev/null @@ -1,36 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc4388.robot.commands.storage.StorageRun; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShootFireGroup extends ParallelRaceGroup { - /** - * Fires the shooter - * @param m_shooter The Shooter subsytem - * @param m_shooterAim The ShooterAim subsystem - * @param m_storage The Storage subsytem - */ - public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter), - new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood), - new TrackTarget(m_shooterAim) - //new StorageRun(m_storage) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java deleted file mode 100644 index 8863636..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java +++ /dev/null @@ -1,32 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShootFullGroup extends SequentialCommandGroup { - /** - * Preps and Fires the Shooter - * @param m_shooter The Shooter subsytem - * @param m_shooterAim The ShooterAim subsystem - * @param m_storage The Storage subsytem - */ - public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( - new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage), - new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index 14accc3..df92eb4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -8,7 +8,7 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import frc4388.robot.commands.storage.StoragePrepAim; +import frc4388.robot.commands.storage.StoragePrep; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; @@ -26,11 +26,9 @@ public class ShootPrepGroup extends ParallelDeadlineGroup { */ public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { super( - new PrepChecker(m_shooter, m_storage), new TrackTarget(m_shooterAim), new ShooterVelocityControlPID(m_shooter), - new HoodPositionPID(m_shooterHood), - new StoragePrepAim(m_storage) + new HoodPositionPID(m_shooterHood) ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 215b6bd..75a6896 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -34,10 +34,7 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); - //m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()); - //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); - //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); + m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 6d35f49..a1e688e 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; @@ -31,6 +32,7 @@ public class TrackTarget extends CommandBase { double yAngle = 0; double target = 0; public double distance; + public double realDistance; public static double fireVel; public static double fireAngle; @@ -48,6 +50,7 @@ public class TrackTarget extends CommandBase { m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); } // Called when the command is initially scheduled. @@ -65,8 +68,14 @@ public class TrackTarget extends CommandBase { xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + // Finding Distance + distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + realDistance = (1.09 * distance) - 12.8; + + if (target == 1.0) { // If target in view // Aiming Left/Right + xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance); turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { turnAmount = 0; @@ -79,10 +88,8 @@ public class TrackTarget extends CommandBase { } m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); - // Finding Distance - distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - SmartDashboard.putNumber("Distance to Target", distance); - + SmartDashboard.putNumber("Distance to Target", realDistance); + SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance)); //START Equation Code double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); double xVel = (distance * VisionConstants.GRAV) / (yVel); @@ -92,8 +99,8 @@ public class TrackTarget extends CommandBase { //END Equation Code //START CSV Code - fireVel = m_shooter.m_shooterTable.getVelocity(distance); - fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + fireVel = m_shooter.m_shooterTable.getVelocity(realDistance); + fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different //fireAngle = 33; //END CSV Code @@ -115,7 +122,7 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (xAngle < 1 && xAngle > -1 && target == 1) + if (xAngle < 0.5 && xAngle > -0.5 && target == 1) { m_shooterAim.m_isAimReady = true; } else { diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java new file mode 100644 index 0000000..68cfd71 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java @@ -0,0 +1,141 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.storage; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; +import frc4388.robot.subsystems.Storage; + +public class ManageStorage extends CommandBase { + Storage m_storage; + + /* Timer */ + long m_resetStartTime; + + /* Keeps track of which beam breaks are pressed */ + boolean m_isBallInIntake = false; + boolean m_isBallInStorage = false; + boolean m_isBallInUseless = false; + boolean m_isBallInShooter = false; + + /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ + boolean m_isStorageEmpty = true; + + public enum StorageMode{IDLE, INTAKE, RESET}; + StorageMode m_storageMode = StorageMode.IDLE; + + /** + * Creates a new ManageStorage. + */ + public ManageStorage(Storage m_robotStorage, StorageMode storageMode) { + // Use addRequirements() here to declare subsystem dependencies. + m_storage = m_robotStorage; + m_storageMode = storageMode; + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_isBallInIntake = !m_storage.getBeamIntake(); + m_isBallInStorage = !m_storage.getBeamStorage(); + m_isBallInUseless = !m_storage.getBeamUseless(); + m_isBallInShooter = !m_storage.getBeamShooter(); + + m_isStorageEmpty = !m_isBallInStorage; + + if (m_storageMode == StorageMode.RESET) { + m_resetStartTime = System.currentTimeMillis(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_isBallInIntake = !m_storage.getBeamIntake(); + m_isBallInStorage = !m_storage.getBeamStorage(); + m_isBallInUseless = !m_storage.getBeamUseless(); + m_isBallInShooter = !m_storage.getBeamShooter(); + + /// TODO: Delete/Comment these when done + SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake); + SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage); + SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter); + + if (m_storageMode == StorageMode.IDLE) { + runIdle(); + } else if (m_storageMode == StorageMode.INTAKE) { + runIntake(); + } else if (m_storageMode == StorageMode.RESET) { + runReset(); + } + } + + /** + * Intakes a ball. + * Runs until the storage ball has moved past the + * storage sensor and the intake ball has taken its place. + */ + private void runIntake() { + if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter + m_storage.runStorage(StorageConstants.STORAGE_SPEED); + + if (!m_isStorageEmpty && !m_isBallInStorage) { // If ball moves out of storage, set storage to empty + m_isStorageEmpty = true; + } + if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode + m_isStorageEmpty = false; + m_storageMode = StorageMode.IDLE; + } + } else { + m_storageMode = StorageMode.IDLE; + } + } + + /** + * Idles until a ball is in the intake. + * Also updates the status of the storage position + */ + private void runIdle() { + m_storage.runStorage(0); + + if (m_isBallInIntake) { + m_storageMode = StorageMode.INTAKE; + } + m_isStorageEmpty = !m_isBallInStorage; + } + + /** + * Runs Storage out until the Intake Sensor is tripped. + * Then switches into intake mode. This effectively + * resets the position of the balls back to the bottom of the storage. + */ + private void runReset() { + m_storage.runStorage(-StorageConstants.STORAGE_SPEED); + + if (m_isBallInIntake) { + m_storageMode = StorageMode.INTAKE; + } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) { + m_storageMode = StorageMode.IDLE; + } + m_isStorageEmpty = !m_isBallInStorage; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_storageMode = StorageMode.RESET; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java b/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java new file mode 100644 index 0000000..6ed4fd3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java @@ -0,0 +1,145 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.storage; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; +import frc4388.robot.subsystems.Storage; + +public class ManageStoragePID extends CommandBase { + Storage m_storage; + + /* Timer */ + long m_resetStartTime; + + /* Start Positions */ + double m_intakeStartPos; + + /* Keeps track of which beam breaks are pressed */ + boolean m_isBallInIntake = false; + boolean m_isBallInStorage = false; + boolean m_isBallInUseless = false; + boolean m_isBallInShooter = false; + + /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ + boolean m_isStorageEmpty = true; + + public enum StorageMode{IDLE, INTAKE, RESET}; + StorageMode m_storageMode = StorageMode.IDLE; + + /** + * Creates a new ManageStorage. + */ + public ManageStoragePID(Storage m_robotStorage, StorageMode storageMode) { + // Use addRequirements() here to declare subsystem dependencies. + m_storage = m_robotStorage; + m_storageMode = storageMode; + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_isBallInIntake = !m_storage.getBeamIntake(); + m_isBallInStorage = !m_storage.getBeamStorage(); + m_isBallInUseless = !m_storage.getBeamUseless(); + m_isBallInShooter = !m_storage.getBeamShooter(); + + m_isStorageEmpty = !m_isBallInStorage; + + m_intakeStartPos = m_storage.getEncoderPosInches(); + + if (m_storageMode == StorageMode.RESET) { + m_resetStartTime = System.currentTimeMillis(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_isBallInIntake = !m_storage.getBeamIntake(); + m_isBallInStorage = !m_storage.getBeamStorage(); + m_isBallInUseless = !m_storage.getBeamUseless(); + m_isBallInShooter = !m_storage.getBeamShooter(); + + /// TODO: Delete/Comment these when done + SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake); + SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage); + SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter); + + if (m_storageMode == StorageMode.IDLE) { + runIdle(); + } else if (m_storageMode == StorageMode.INTAKE) { + runIntake(); + } else if (m_storageMode == StorageMode.RESET) { + runReset(); + } + } + + /** + * Intakes a ball. + * Runs until the storage ball has moved past the + * storage sensor and the intake ball has taken its place. + */ + private void runIntake() { + if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter + m_storage.runStoragePositionPID(m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL); + + double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches(); + if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) { + m_storageMode = StorageMode.IDLE; + } + } else { + m_storageMode = StorageMode.IDLE; + } + } + + /** + * Idles until a ball is in the intake. + * Also updates the status of the storage position + */ + private void runIdle() { + m_storage.runStorage(0); + + if (m_isBallInIntake) { + m_storageMode = StorageMode.INTAKE; + m_intakeStartPos = m_storage.getEncoderPosInches(); + } + m_isStorageEmpty = !m_isBallInStorage; + } + + /** + * Runs Storage out until the Intake Sensor is tripped. + * Then switches into intake mode. This effectively + * resets the position of the balls back to the bottom of the storage. + */ + private void runReset() { + m_storage.runStorage(-StorageConstants.STORAGE_SPEED); + + if (m_isBallInIntake) { + m_storageMode = StorageMode.INTAKE; + m_intakeStartPos = m_storage.getEncoderPosInches(); + } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) { + m_storageMode = StorageMode.IDLE; + } + m_isStorageEmpty = !m_isBallInStorage; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_storageMode = StorageMode.RESET; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/storage/StorageIntake.java b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java deleted file mode 100644 index 47769c4..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StorageIntake.java +++ /dev/null @@ -1,63 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Storage; - -public class StorageIntake extends CommandBase { - public Intake m_intake; - public Storage m_storage; - public boolean intook; - /** - * Runs the Storage in for intaking - * @param inSub The Intake subsystem - * @param storeSub The Storage subsytem - */ - public StorageIntake(Intake inSub, Storage storeSub) { - m_intake = inSub; - m_storage = storeSub; - addRequirements(m_intake); - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - intook = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){ - m_storage.runStorage(StorageConstants.STORAGE_SPEED); - intook = true; - } - else{ - m_storage.runStorage(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){ - return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/StorageOutake.java b/src/main/java/frc4388/robot/commands/storage/StorageOutake.java deleted file mode 100644 index 658d8e5..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StorageOutake.java +++ /dev/null @@ -1,46 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StorageOutake extends CommandBase { - Storage m_storage; - /** - * Runs the Storage out for outaking - * @param storeSub The Storage subsystem - */ - public StorageOutake(Storage storeSub) { - m_storage = storeSub; - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_storage.runStorage(-StorageConstants.STORAGE_SPEED); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java deleted file mode 100644 index 2a27b41..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StoragePositionPID extends CommandBase { - public Storage m_storage; - double startPos; - /** - * Moves the storage a number of rotations - * @param subsystem The Storage subsystem - */ - public StoragePositionPID(Storage subsystem) { - m_storage = subsystem; - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - /*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos()) - { - return true; - }*/ - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java similarity index 73% rename from src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java rename to src/main/java/frc4388/robot/commands/storage/StoragePrep.java index 6d47abf..9fcee06 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java @@ -7,19 +7,18 @@ package frc4388.robot.commands.storage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; -public class StoragePrepAim extends CommandBase { +public class StoragePrep extends CommandBase { Storage m_storage; - double startTime; + double m_startTime; /** * Prepares the Storage for aiming * @param storeSub The Storage subsystem */ - public StoragePrepAim(Storage storeSub) { + public StoragePrep(Storage storeSub) { m_storage = storeSub; addRequirements(m_storage); } @@ -27,17 +26,18 @@ public class StoragePrepAim extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - startTime = System.currentTimeMillis(); + m_startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(1)){ - //m_storage.runStorage(StorageConstants.STORAGE_SPEED); - } - else{ + if ((m_storage.getBeamUseless() && m_storage.getBeamShooter()) || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) { + m_storage.runStorage(StorageConstants.STORAGE_SPEED); + m_storage.m_isStorageReadyToFire = false; + } else { m_storage.runStorage(0); + m_storage.m_isStorageReadyToFire = true; } } @@ -49,13 +49,6 @@ public class StoragePrepAim extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - /*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ - m_storage.m_isStorageReadyToFire = true; - return true; - } else { - m_storage.m_isStorageReadyToFire = false; - return false; - }*/ - return true; + return false; } } diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java deleted file mode 100644 index b9a9cf1..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java +++ /dev/null @@ -1,62 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Storage; - -public class StoragePrepIntake extends CommandBase { - public Intake m_intake; - public Storage m_storage; - public double startTime; - - /** - * Prepares the Storage for intaking - * @param inSub The Intake subsystem - * @param storeSub the Storage subsystem - */ - public StoragePrepIntake(Intake inSub, Storage storeSub) { - m_intake = inSub; - m_storage = storeSub; - addRequirements(m_intake); - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - startTime = System.currentTimeMillis(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){ - m_storage.runStorage(-StorageConstants.STORAGE_SPEED); - } - else{ - m_storage.runStorage(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ - return false; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/StorageRun.java b/src/main/java/frc4388/robot/commands/storage/StorageRun.java deleted file mode 100644 index 11dbc4b..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StorageRun.java +++ /dev/null @@ -1,46 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StorageRun extends CommandBase { - Storage m_storage; - /** - * Runs the Storage at a speed - * @param storageSub The Storage subsytem - */ - public StorageRun(Storage storageSub) { - m_storage = storageSub; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_storage.runStorage(StorageConstants.STORAGE_SPEED); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_storage.runStorage(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index cbad21c..a86e369 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -17,6 +17,7 @@ import frc4388.utility.LEDPatterns; public class LED extends SubsystemBase { public static float currentLED; + public static float defaultLED; public static Spark LEDController; /** @@ -25,7 +26,8 @@ public class LED extends SubsystemBase { */ public LED(){ LEDController = new Spark(LEDConstants.LED_SPARK_ID); - setPattern(LEDConstants.DEFAULT_PATTERN); + defaultLED = LEDConstants.DEFAULT_PATTERN.getValue(); + runDefaultLED(); LEDController.set(currentLED); System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); } @@ -38,6 +40,37 @@ public class LED extends SubsystemBase { LEDController.set(currentLED); } + /** + * + */ + public void runDefaultLED() { + setPattern(defaultLED); + } + + /** + * Changes the default LED by an amount + * @param amount the amount to increment led by + */ + public void incrementLED(float amount) { + defaultLED += amount; + if (defaultLED > 1) { + defaultLED -= 2; + } + if (defaultLED < -1) { + defaultLED += 2; + } + } + + /** + * Sets the current LED pattern. This method should only be run + * whenever you want to change the current LED. + * @param pattern LEDPattern to set the Blinkin to. + */ + public void setPattern(float pattern){ + currentLED = pattern; + LEDController.set(pattern); + } + /** * Sets the current LED pattern. This method should only be run * whenever you want to change the current LED. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index d8bb022..1b7a0c2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -92,6 +92,8 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); + + SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady); } catch(Exception e) diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index a34d59f..424175c 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -17,6 +17,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.ControlType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; @@ -57,6 +58,7 @@ public class ShooterAim extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 4caa519..403de25 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -21,7 +21,10 @@ import frc4388.utility.Gains; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - private DigitalInput[] m_beamSensors = new DigitalInput[6]; + private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); + private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); + private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); CANPIDController m_storagePIDController = m_storageMotor.getPIDController(); @@ -38,26 +41,29 @@ public class Storage extends SubsystemBase { */ public Storage() { resetEncoder(); - m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); - m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); - m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); - m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); + + // Set PID Coefficients + m_storagePIDController.setP(storageGains.m_kP); + m_storagePIDController.setI(storageGains.m_kI); + m_storagePIDController.setD(storageGains.m_kD); + m_storagePIDController.setIZone(storageGains.m_kIzone); + m_storagePIDController.setFF(storageGains.m_kF); + m_storagePIDController.setOutputRange(storageGains.m_kminOutput, storageGains.m_kmaxOutput); } @Override public void periodic() { - //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get()); - //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get()); - //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get()); - //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get()); + SmartDashboard.putBoolean("Intake Beam", getBeamIntake()); + SmartDashboard.putBoolean("Storage Beam", getBeamStorage()); + SmartDashboard.putBoolean("Upper Beam", getBeamUseless()); + SmartDashboard.putBoolean("Shooter Beam", getBeamShooter()); } /** * Runs storage motor * - * @param input the voltage to run motor at + * @param input the percent output to run motor at */ - public void runStorage(double input) { m_storageMotor.set(input); } @@ -66,36 +72,66 @@ public class Storage extends SubsystemBase { m_encoder.setPosition(0); } - public void testBeams(){ - SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get()); - SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get()); - } - - /* Storage PID Control */ + /** + * Runs Storage to a particular position + * @param targetPos in inches + */ public void runStoragePositionPID(double targetPos){ - // Set PID Coefficients - m_storagePIDController.setP(storageGains.m_kP); - m_storagePIDController.setI(storageGains.m_kI); - m_storagePIDController.setD(storageGains.m_kD); - m_storagePIDController.setIZone(storageGains.m_kIzone); - m_storagePIDController.setFF(storageGains.m_kF); - m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); - //SmartDashboard.putNumber("Storage Position PID Target", targetPos); //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); + targetPos = InchesToMotorRots(targetPos); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } + /** + * Runs Storage to a particular position + * @param position in motor rotations + */ + public void setStoragePID(double position){ + m_storagePIDController.setReference(position, ControlType.kPosition); + } public double getEncoderPos(){ return m_encoder.getPosition(); } - public boolean getBeam(int id){ - return m_beamSensors[id].get(); + public double getEncoderPosInches(){ + return motorRotsToInches(getEncoderPos()); } - public void setStoragePID(double position){ - m_storagePIDController.setReference(position , ControlType.kPosition); + public double getEncoderVel(){ + return m_encoder.getVelocity(); + } + + /** + * @param motorRots + * @return inches + */ + public double motorRotsToInches(double motorRots) { + return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT); + } + + /** + * @param inches + * @return motorRots + */ + public double InchesToMotorRots(double inches) { + return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT); + } + + public boolean getBeamShooter(){ + return m_beamShooter.get(); + } + + public boolean getBeamUseless(){ + return m_beamUseless.get(); + } + + public boolean getBeamStorage(){ + return m_beamStorage.get(); + } + + public boolean getBeamIntake(){ + return m_beamIntake.get(); } } diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index f1b78f9..cfbb689 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -27,7 +27,7 @@ public class Gains { * @param kD The D value. * @param kF The F value. * @param kIzone The zone of the I value. - * @param kPeakOutput The peak output setting the motors to run the gains at. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. */ public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput) { @@ -37,5 +37,29 @@ public class Gains { m_kF = kF; m_kIzone = kIzone; m_kPeakOutput = kPeakOutput; + m_kmaxOutput = m_kPeakOutput; + m_kminOutput = -m_kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIzone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput) + { + m_kP = kP; + m_kI = kI; + m_kD = kD; + m_kF = kF; + m_kIzone = kIzone; + m_kminOutput = kMinOutput; + m_kmaxOutput = kMaxOutput; + m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput); } } diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index 85244ce..3e5dae3 100644 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -20,12 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; * Add your docs here. */ public class ShooterTables { - double[][] m_distance = new double[50][3]; + double[][] m_distance = new double[50][4]; double[][] m_angle = new double[50][2]; final int m_columnDistance = 0; final int m_columnHood = 1; final int m_columnVel = 2; + final int m_columnCenterDisplacement = 3; final int m_columnAngle = 0; final int m_columnDisplacement = 1; @@ -55,6 +56,7 @@ public class ShooterTables { m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); + m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]); lineNum ++; } @@ -80,6 +82,9 @@ public class ShooterTables { m_angleLength = lineNum-1; + distanceReader.close(); + angleReader.close(); + } catch (FileNotFoundException e) { e.printStackTrace(); } catch (IOException e) { @@ -123,6 +128,23 @@ public class ShooterTables { } } + public double getCenterDisplacement(double distance) { //Degrees of Lime FOV + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnCenterDisplacement]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance); + } + } + + + public double getAngleDisplacement(double angle) { int i = 0; while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {