From cce339dc566970cd4fed674d74690a8d5aa89364 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 7 Apr 2022 11:35:07 -0600 Subject: [PATCH] asdfghkl; --- src/main/java/frc4388/robot/Constants.java | 5 +- .../java/frc4388/robot/RobotContainer.java | 52 ++++++++++++------- src/main/java/frc4388/robot/RobotMap.java | 4 +- .../robot/commands/ShooterCommands/Seek.java | 9 ++-- .../commands/ShooterCommands/TrackTarget.java | 18 +++++-- 5 files changed, 59 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7bf945c..678889e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -168,9 +168,10 @@ 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 = 0; + public static final int LED_SPARK_ID = 3; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_BPM; + public static final LEDPatterns SHOOTING_PATTERN = LEDPatterns.SOLID_GREEN; } public static final class ClimberConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6495570..aeeecbc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -55,6 +55,7 @@ import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Serializer; import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; @@ -87,7 +88,7 @@ public class RobotContainer { public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); - // private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad + private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); @@ -352,7 +353,7 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0)); new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // * aim with turret to target); + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED)); // * aim with turret to target); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); @@ -525,21 +526,21 @@ public class RobotContainer { Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs. - SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(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, true), + 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), 1.0, true) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(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, true), + 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), 2.3, true) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(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, true), + 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), 4.0, true) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. @@ -641,9 +642,11 @@ public class RobotContainer { ParallelDeadlineGroup idleDrumUntilShootingFirstBall = new ParallelDeadlineGroup(extendThenAimTurret, new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom)); // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward")); - return new SequentialCommandGroup(idleDrumUntilShootingFirstBall, - //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), - // extendWhileTurretIsAiming,//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), // * aim with turret to target + + + + //! 2 BALL AUTO + return new SequentialCommandGroup(extendWhileTurretIsAiming,//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), // * aim with turret to target weirdAutoShootingGroup, // * shoot // extendWhileTurretIsAiming, new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true), @@ -652,14 +655,27 @@ public class RobotContainer { // intakeWithPath, // weirdAutoShootingGroup2, // 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)); // * aim with turret to target); // * shoot - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true), - // intakeWithPath2, - // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 1.0, true), - // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true), - // intakeWithPath2AndTrackTarget, - intakeWithPath2AndAimTurret, - weirdAutoShootingGroup3, new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true)); + //! 3 BALL AUTO + // return new SequentialCommandGroup(idleDrumUntilShootingFirstBall, + // //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), + // // extendWhileTurretIsAiming,//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), // * aim with turret to target + // weirdAutoShootingGroup, // * shoot + // // extendWhileTurretIsAiming, + // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true), + // // new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), + // intakeWithPathAndTrackTarget, + // // intakeWithPath, + // // weirdAutoShootingGroup2, + // // 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)); // * aim with turret to target); // * shoot + // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true), + // // intakeWithPath2, + // // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 1.0, true), + // // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true), + // // intakeWithPath2AndTrackTarget, + // intakeWithPath2AndAimTurret, + // weirdAutoShootingGroup3, + // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true)); // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond")); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index b76ab82..2545ecb 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -53,9 +53,9 @@ public class RobotMap { } /* LED Subsystem */ -// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); -// void configureLEDMotorControllers() {} + void configureLEDMotorControllers() {} /* Swerve Subsystem */ diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index 7d3c7f6..3de6da1 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -7,6 +7,7 @@ package frc4388.robot.commands.ShooterCommands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.VisionOdometry; @@ -21,10 +22,10 @@ public class Seek extends SequentialCommandGroup { * @author Aarav Shah * @blame Aarav Shah (thomas did this) */ - public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { + public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds)); } /** Seeks. @@ -32,9 +33,9 @@ public class Seek extends SequentialCommandGroup { * @author Aarav Shah * @blame Aarav Shah (thomas did this) */ - public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) { + public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds, double[] fakeOdo) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index a9388d3..bc2d8cd 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -17,14 +17,17 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.LEDPatterns; import frc4388.utility.Vector2D; import frc4388.utility.desmos.DesmosServer; @@ -36,6 +39,7 @@ public class TrackTarget extends CommandBase { VisionOdometry m_visionOdometry; BoomBoom m_boomBoom; Hood m_hood; + LED m_leds; boolean isAuto; @@ -57,11 +61,12 @@ public class TrackTarget extends CommandBase { long startTime; private double timeTolerance; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) { + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds, boolean isAuto) { m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; + m_leds = leds; this.isAuto = isAuto; this.timeTolerance = 1000; @@ -71,8 +76,8 @@ public class TrackTarget extends CommandBase { SmartDashboard.putBoolean("Target Locked", false); } - public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - this(turret, boomBoom, hood, visionOdometry, false); + public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds) { + this(turret, boomBoom, hood, visionOdometry, leds, false); } // Called when the command is initially scheduled. @@ -134,6 +139,13 @@ public class TrackTarget extends CommandBase { SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); SmartDashboard.putNumber("Vel Target Track", velocity); SmartDashboard.putBoolean("Target Locked", targetLocked); + + if (targetLocked){ + m_leds.setPattern(LEDConstants.SHOOTING_PATTERN); + } + else{ + m_leds.setPattern(LEDConstants.DEFAULT_PATTERN); + } } public ArrayList filterPoints(ArrayList points) {