diff --git a/src/main/deploy/pathplanner/JMove2.path b/src/main/deploy/pathplanner/JMove2.path index 5bd47dc..8b9f54e 100644 --- a/src/main/deploy/pathplanner/JMove2.path +++ b/src/main/deploy/pathplanner/JMove2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.66,"y":0.7},"prevControl":null,"nextControl":{"x":7.465685817447585,"y":2.0602977914816596},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":10.6,"y":2.118171324950245},"prevControl":{"x":10.796770013793191,"y":3.8080785022329553},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.66,"y":0.7},"prevControl":null,"nextControl":{"x":7.708937737734877,"y":2.3864441401993526},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":11.0,"y":2.118171324950245},"prevControl":{"x":11.180129214388481,"y":3.8472372199577447},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ 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 a68b9ac..8d03887 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -168,7 +168,7 @@ public final class Constants { public static final double STORAGE_SPEED = 1.0;//0.9; } public static final class LEDConstants { - public static final int LED_SPARK_ID = 3; + public static final int LED_SPARK_ID = 4;//3; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED; public static final LEDPatterns SHOOTING_PATTERN = LEDPatterns.SOLID_GREEN; @@ -343,6 +343,6 @@ public final class Constants { public static final double SHOOTER_CORRECTION = 1.d; public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV; - public static final double DEGREES_PER_PIXELS = 1 / PIXELS_PER_DEGREE; + public static final double DEGREES_PER_PIXEL = 1 / PIXELS_PER_DEGREE; } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index d09a3a3..59c67a5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -139,7 +139,7 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - // m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b8f4cee..c6ddb1e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.CommandGroupBase; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -454,14 +455,14 @@ public class RobotContainer { ); } - private SequentialCommandGroup shoot(double storageRunTime, double timeout) { + private ParallelRaceGroup shoot(double storageRunTime, double timeout) { return new SequentialCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withTimeout(timeout), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).until(TrackTarget::isTargetNotLocked), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true) ) - ); + ).withTimeout(timeout + storageRunTime); } SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), @@ -492,14 +493,14 @@ public class RobotContainer { SequentialCommandGroup extendThenAimTurret() { return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), - new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true) // TODO: optimize time + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 0.5, true) // TODO: optimize time ); } ParallelDeadlineGroup idleDrumUntilShootingFirstBall() { return new ParallelDeadlineGroup( extendThenAimTurret(), - new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom) + new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(9000), m_robotBoomBoom) ); } @@ -523,7 +524,7 @@ public class RobotContainer { // ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3); ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup( - intakeWithPath2(4.2), + intakeWithPath2(2.8), new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom), new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true) ); @@ -556,10 +557,11 @@ public class RobotContainer { // ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE) SequentialCommandGroup threeBallAuto = new SequentialCommandGroup( idleDrumUntilShootingFirstBall(), - shoot(1.0), // TODO: optimize time + shoot(0.8), // TODO: optimize time brakeStorage(0.1), - intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget - shoot(2.3), // TODO: optimize time + intakeWithPathAndTrackTarget, + // intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget + shoot(0.8), // TODO: optimize time brakeStorage(0.1), intakeWithPath2AndIdleShooterAndAimTurret, shoot(4.0), // TODO: optimize time diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 118792e..2a68991 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -16,6 +16,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; @@ -47,6 +48,7 @@ public class RobotMap { /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); +// public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() {} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 1dcc54b..b4313a4 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -105,6 +105,7 @@ public class TrackTarget extends CommandBase { } // points = getFakePoints(); //// points = filterPoints(points); + Point average = VisionOdometry.averagePoint(points); // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); @@ -116,7 +117,6 @@ public class TrackTarget extends CommandBase { // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), // true); - double regressedDistance = getDistance(average.y); // ! offset trig solution diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 7a24a0c..cb6f53e 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -6,6 +6,8 @@ package frc4388.robot.subsystems; import java.util.logging.Logger; +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; @@ -18,6 +20,7 @@ import frc4388.utility.LEDPatterns; public class LED extends SubsystemBase { private LEDPatterns m_currentPattern; + // private PWM newLED; private Spark m_LEDController; /** @@ -25,6 +28,7 @@ public class LED extends SubsystemBase { */ public LED(Spark LEDController){ m_LEDController = LEDController; + // this.newLED = newLED; setPattern(LEDConstants.DEFAULT_PATTERN); updateLED(); Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); @@ -39,6 +43,7 @@ public class LED extends SubsystemBase { * Add your docs here. */ public void updateLED(){ + // newLED.setRaw((int) m_currentPattern.percentToPWM()); m_LEDController.set(m_currentPattern.getValue()); } @@ -47,6 +52,7 @@ public class LED extends SubsystemBase { */ public void setPattern(LEDPatterns pattern){ m_currentPattern = pattern; + // newLED.setRaw((int) m_currentPattern.percentToPWM()); m_LEDController.set(m_currentPattern.getValue()); } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index fda2586..c5b6d7d 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -45,7 +45,7 @@ public class VisionOdometry extends SubsystemBase { public VisionOdometry(SwerveDrive drive, Turret shooter) { // do{ m_camera = new PhotonCamera(VisionConstants.NAME); - // }while(m_camera.getLatestResult().getLatencyMillis() == 0.d); + // } while (m_camera.getLatestResult().getLatencyMillis() == 0.d); m_drive = drive; m_shooter = shooter; diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java index 6df032c..bc318d9 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/LEDPatterns.java @@ -42,4 +42,8 @@ public enum LEDPatterns { public float getValue() { return id; } + + public float percentToPWM() { + return (1000 + (getValue() * 1000)); + } } \ No newline at end of file