diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d49e95d..7cb314e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,6 +62,7 @@ import frc4388.robot.commands.TrackTarget; import frc4388.robot.commands.StorageOutake; import frc4388.robot.commands.StoragePrepAim; import frc4388.robot.commands.StoragePrepIntake; +import frc4388.robot.commands.StorageRun; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.LimeLight; @@ -180,12 +181,12 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - .whileHeld(new ParallelCommandGroup( - new HoldTarget(m_robotShooter,m_robotShooterAim), - new HoodPositionPID(m_robotShooter))); - //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + //.whileHeld(new ParallelCommandGroup( + //new HoldTarget(m_robotShooter,m_robotShooterAim), + //new HoodPositionPID(m_robotShooter))) + .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java index dd276f0..d713f8b 100644 --- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java @@ -13,12 +13,13 @@ import frc4388.robot.subsystems.Shooter; public class HoodPositionPID extends CommandBase { Shooter m_shooter; + double firingAngle; /** * Creates a new HoodPositionPID. */ public HoodPositionPID(Shooter subSystem) { m_shooter = subSystem; - addRequirements(m_shooter); + //addRequirements(m_shooter); } // Called when the command is initially scheduled. @@ -29,7 +30,7 @@ public class HoodPositionPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double firingAngle = (-0.47*m_shooter.addFireAngle())+40.5; + firingAngle = (-0.47*m_shooter.addFireAngle())+40.5; SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooter.runAngleAdjustPID(firingAngle); @@ -43,6 +44,10 @@ public class HoodPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition(); + if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ + return true; + } return false; } } diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 10eb073..7d23c65 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -28,7 +28,7 @@ public class ShootFireGroup extends ParallelRaceGroup { new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), new HoldTarget(m_shooter, m_shooterAim), - new StoragePositionPID(m_storage) + new StorageRun(m_storage) ); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index 7407365..0615b7d 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java @@ -27,7 +27,8 @@ public class ShootPrepGroup extends ParallelCommandGroup { addCommands( new TrackTarget(m_shooter, m_shooterAim), new ShooterVelocityControlPID(m_shooter), - new StoragePrepAim(m_storage) + new StoragePrepAim(m_storage), + new HoodPositionPID(m_shooter) ); } } diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java index 643c858..6a9f36e 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java @@ -36,8 +36,7 @@ public class StoragePrepIntake extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println(m_storage.getBeam(0)); - if (m_storage.getBeam(0)){//&& System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed + if (m_storage.getBeam(0)){ m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ @@ -53,7 +52,7 @@ public class StoragePrepIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(0)){ + if (!m_storage.getBeam(0) || startTime + 2000 <= System.currentTimeMillis()){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/StorageRun.java b/src/main/java/frc4388/robot/commands/StorageRun.java index 5b56b52..b97d226 100644 --- a/src/main/java/frc4388/robot/commands/StorageRun.java +++ b/src/main/java/frc4388/robot/commands/StorageRun.java @@ -29,12 +29,14 @@ public class StorageRun extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + System.err.println("oiudhgisbjkljasbhfkofhdnsekdfjbsjfvsdkcfbsdjhfgvsdkjfbsd"); 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. diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 2e0e512..fc49d81 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -83,8 +83,8 @@ public class TrackTarget extends CommandBase { double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); double xVel = (distance * VisionConstants.GRAV) / (yVel); - fireVel = Math.sqrt((Math.pow(xVel, 2)) + (Math.pow(yVel, 2))); - fireAngle = Math.atan(yVel / xVel) * (180/Math.PI); + fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); + fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); m_shooter.m_fireVel = fireVel; m_shooter.m_fireAngle = fireAngle; } @@ -93,8 +93,7 @@ public class TrackTarget extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + } // Returns true when the command should end. diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index 638f140..dc7fa83 100644 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -93,7 +93,7 @@ public class ShooterTables { SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); } - public double getHood(double distance) { + public double getHood(double distance) { //Rotations of motor int i = 0; while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { i ++; @@ -108,7 +108,7 @@ public class ShooterTables { } } - public double getVelocity(double distance) { + public double getVelocity(double distance) { //Units per 100ms int i = 0; while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { i ++;