From a6f4e4ae80488af9ce3695467f0964c226e9c82f Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 09:55:34 +0300 Subject: [PATCH 01/24] Commit all, explain later --- src/main/java/frc/robot/RobotContainer.java | 83 +++----- .../java/frc/robot/auto/FiveCargoAuto.java | 32 +-- .../java/frc/robot/auto/FourBallAuto.java | 32 ++- .../java/frc/robot/auto/FourCargoAuto.java | 26 ++- .../java/frc/robot/auto/SaarIsAutonomous.java | 43 ++-- .../commandgroups/BackAndShootCargo2.java | 126 ------------ .../commandgroups/ConveyForShooting.java | 15 ++ .../frc/robot/commandgroups/JustShoot.java | 26 +++ .../robot/commandgroups/OdometryAdjust.java | 53 +++++ .../OneButtonAdjustAndShoot.java | 36 ++++ .../QuickReleaseBackAndShootCargo.java | 11 - .../robot/commandgroups/SafetyShootCargo.java | 30 --- .../frc/robot/commandgroups/ShootCargo.java | 29 --- .../frc/robot/commandgroups/VisionAdjust.java | 46 +++++ .../commandgroups/bits/TestBallFlow.java | 12 -- .../conveyor/commands/ConveyCargo.java | 61 ++++++ .../conveyor/commands/ConveyToShooter.java | 188 +++++++++--------- .../conveyor/commands/QuickReleaseConvey.java | 104 +++++----- .../subsystems/drivetrain/SwerveDrive.java | 11 +- .../commands/DriveAndAdjustWithVision.java | 6 +- .../subsystems/hood/commands/HoodCommand.java | 14 +- .../frc/robot/subsystems/shooter/Shooter.java | 1 - .../shooter/commands/BackAndShootCargo.java | 18 -- .../shooter/commands/ReachVelocity.java | 2 +- .../subsystems/shooter/commands/Shoot.java | 6 +- .../shooter/commands/bits/TryVelocities.java | 7 +- src/test/java/Test.java | 7 + 27 files changed, 502 insertions(+), 523 deletions(-) delete mode 100644 src/main/java/frc/robot/commandgroups/BackAndShootCargo2.java create mode 100644 src/main/java/frc/robot/commandgroups/ConveyForShooting.java create mode 100644 src/main/java/frc/robot/commandgroups/JustShoot.java create mode 100644 src/main/java/frc/robot/commandgroups/OdometryAdjust.java create mode 100644 src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java delete mode 100644 src/main/java/frc/robot/commandgroups/SafetyShootCargo.java delete mode 100644 src/main/java/frc/robot/commandgroups/ShootCargo.java create mode 100644 src/main/java/frc/robot/commandgroups/VisionAdjust.java create mode 100644 src/main/java/frc/robot/subsystems/conveyor/commands/ConveyCargo.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7b63455..2e1adc87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,7 @@ import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; -import frc.robot.subsystems.drivetrain.commands.DriveAndAdjustWithVision; +import frc.robot.subsystems.drivetrain.commands.OverpoweredDrive; import frc.robot.subsystems.drivetrain.commands.TurnToAngle; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.helicopter.Helicopter; @@ -23,7 +23,6 @@ import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.BackAndShootCargo; import frc.robot.subsystems.shooter.commands.ReachVelocity; import frc.robot.subsystems.shooter.commands.Shoot; import frc.robot.utils.LedSubsystem; @@ -39,21 +38,10 @@ public class RobotContainer { public static boolean hardCodedVelocity = false; public static double hardCodedDistance = 3.8; public static double hardCodedVelocityValue = Shoot.getSetpointVelocity(hardCodedDistance); - public static boolean shooting = false; - public static DoubleSupplier proximity; + public static boolean shooting = false; // If this is true, don't change the setpoint of the shooter during teleop + public static double setpointVelocity = 0; // Setpoint velocity for the shooter to reach at all times during teleop // The robot's subsystems and commands are defined here... - public static DoubleSupplier setpointSupplierForShooterFromVision; - public static DoubleSupplier distanceSupplierFromVision; - public static DoubleSupplier odometrySetpointSupplier; - public static DoubleSupplier odometryDistanceSupplier; - public static double cachedSetpointForShooter = 0; - public static double cachedDistanceForHood = 0; - public static double odometryCachedSetpoint = 0; - public static double odometryCachedDistance = 0; - public static boolean cachedHasTarget = true; - - public static DoubleSupplier yawSupplierFromVision; public static boolean warmUpShooting = false; @@ -75,15 +63,8 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - distanceSupplierFromVision = photonVisionModule::getDistance; - setpointSupplierForShooterFromVision = () -> Shoot.getSetpointVelocity(distanceSupplierFromVision.getAsDouble()); - odometrySetpointSupplier = () -> Shoot.getSetpointVelocity(swerve.getOdometryDistance()); - odometryDistanceSupplier = swerve::getOdometryDistance; hasTarget = photonVisionModule::hasTargets; - yawSupplierFromVision = () -> photonVisionModule.getYaw().orElse(0); - proximity = conveyor::getColorSensorProximity; - // autonomousCommand = null; // autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); // autonomousCommand = new TaxiFrom(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); @@ -91,6 +72,7 @@ public RobotContainer() { // autonomousCommand = new FourCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); autonomousCommand = new FourBallAuto(swerve, shooter, conveyor, intake, hood, flap, photonVisionModule); configureDefaultCommands(); + initSuppliers(); if (Robot.debug) { startFireLog(); } @@ -100,16 +82,22 @@ public RobotContainer() { } + private void initSuppliers() { + Suppliers.shooterVelocity = shooter::getVelocity; + Suppliers.yawSupplier = () -> photonVisionModule.getYaw().orElse(0); + Suppliers.distanceSupplier = photonVisionModule::getDistance; + Suppliers.odometryDistanceSupplier = swerve::getOdometryDistance; + Suppliers.preFlapBlocked = () -> !conveyor.isPreFlapBeamConnected(); + Suppliers.postFlapBlocked = () -> !conveyor.isPostFlapBeamConnected(); + } + private void configureDefaultCommands() { swerve.setDefaultCommand( - new DriveAndAdjustWithVision( + new OverpoweredDrive( swerve, () -> -Joysticks.leftJoystick.getY() * speedMultiplier, () -> -Joysticks.leftJoystick.getX() * speedMultiplier, - () -> -Joysticks.rightJoystick.getX() * thetaMultiplier, - () -> photonVisionModule.getYaw().orElse(0), - Joysticks.rightTrigger::get, - photonVisionModule::hasTargets + () -> -Joysticks.rightJoystick.getX() * thetaMultiplier ) ); helicopter.setDefaultCommand(new JoystickPowerHelicopter(helicopter, () -> -Xbox.controller.getLeftY())); @@ -119,12 +107,12 @@ private void configureDefaultCommands() { private void configureButtonBindings() { { // Xbox controller button bindings. - Xbox.b.whileHeld(new BackAndShootCargo2( - shooter, hood, conveyor, flap, - () -> 0)).whenInactive(() -> shooting = false); + Xbox.lt.whileActiveContinuous(new PickUpCargo(conveyor, flap, intake, 0.7, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.25, 0.1))); + + Xbox.lb.whileHeld(new Outtake(intake, conveyor, flap, shooter, hood, () -> false)); + Xbox.rb.whileHeld(new Convey(conveyor, -Constants.Conveyor.SHOOT_POWER)); + Xbox.x.whenPressed(intake::toggleRetractor); -// Xbox.x.whileHeld(()->shooter.setVelocity(3600)); -// Xbox.y.whenPressed(new RunCommand(() -> shooter.setVelocity(3530.0), shooter).withInterrupt(Xbox.rt::get)); Xbox.y.whenPressed(new InstantCommand(() -> warmUpShooting = !warmUpShooting)); Xbox.a.whileHeld(() -> playWithoutVision = true).whenReleased(() -> playWithoutVision = false); @@ -132,26 +120,8 @@ private void configureButtonBindings() { Xbox.downPov.whileActiveOnce(new LowGoalShot(shooter, flap, hood)); Xbox.upPov.whileActiveContinuous(new JoystickPowerHelicopter(helicopter, () -> -0.1)); - Xbox.rt.whileActiveContinuous(new BackAndShootCargo( - shooter, hood, conveyor, flap, - () -> 0)).whenInactive(() -> shooting = false); - Xbox.lt.whileActiveContinuous(new PickUpCargo(conveyor, flap, intake, 0.7, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.25, 0.1))); -// Xbox.lt.whileActiveContinuous(new SmartIndexing(shooter, conveyor, intake, flap, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.7, 0.4))); -// Xbox.lt.whileActiveContinuous(new ParallelCommandGroup( -// new InstantCommand(flap::blockShooter), -// new InstantCommand(intake::openRetractor), -// new IntakeWithPID(intake, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 20, 10)), -// new Convey(conveyor, Constants.Conveyor.DEFAULT_POWER::get) -// )); - Xbox.lb.whileHeld(new Outtake(intake, conveyor, flap, shooter, hood, () -> false)); - Xbox.rb.whileHeld(new Convey(conveyor, -Constants.Conveyor.SHOOT_POWER)); Xbox.back.whenPressed(new OneBallOuttake(intake, conveyor, () -> conveyor.getColorSensorProximity() >= 150)); - - Xbox.rightJoystickButton - .whileHeld(() -> hardCodedVelocity = true) - .whenReleased(() -> hardCodedVelocity = false); - } { // Joystick button bindings. @@ -163,6 +133,10 @@ private void configureButtonBindings() { thetaMultiplier = 1.5 * speedMultiplier; }); + Joysticks.rightTrigger.whileHeld(new OneButtonAdjustAndShoot(swerve, conveyor, flap, hood)).whenReleased( + () -> shooting = false + ); + Joysticks.leftTwo.whenPressed((Runnable) Robot::resetAngle); Joysticks.rightTwo.whileHeld(new TurnToAngle(swerve, Rotation2d::new)); } @@ -228,4 +202,13 @@ private static final class Xbox { public static final JoystickButton rightJoystickButton = new JoystickButton(controller, XboxController.Button.kRightStick.value); } + + public static final class Suppliers { + public static DoubleSupplier shooterVelocity = () -> 0; + public static DoubleSupplier yawSupplier = () -> 0; + public static DoubleSupplier distanceSupplier = () -> 0; + public static DoubleSupplier odometryDistanceSupplier = () -> 0; + public static BooleanSupplier preFlapBlocked = () -> false; + public static BooleanSupplier postFlapBlocked = () -> false; + } } diff --git a/src/main/java/frc/robot/auto/FiveCargoAuto.java b/src/main/java/frc/robot/auto/FiveCargoAuto.java index e450903a..aa2a398d 100644 --- a/src/main/java/frc/robot/auto/FiveCargoAuto.java +++ b/src/main/java/frc/robot/auto/FiveCargoAuto.java @@ -2,14 +2,18 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.RobotContainer; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.Constants; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.commands.Shoot; import frc.robot.utils.PhotonVisionModule; public class FiveCargoAuto extends SaarIsAutonomous { @@ -20,23 +24,21 @@ public class FiveCargoAuto extends SaarIsAutonomous { */ public FiveCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "FiveCargoAutoPart1"); - addCommands(shoot3(0.8)); - new InstantCommand(() -> RobotContainer.shooting = true); - + addCommands(reachVelocityByDistance(2.16)); + addCommands(new WaitUntilCommand(() -> Math.abs(shooter.getVelocity() - Shoot.getSetpointVelocity(2.16)) < Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get())); + confirmShooting().withTimeout(0.6); addCommands(followPathAndPickup("FiveCargoAutoPart1")); - - addCommands(turnToAngle(() -> Rotation2d.fromDegrees(40.15))); - - addCommands(quickReleaseBackShootAndAdjust(1.3)); // 1.8 - new InstantCommand(() -> RobotContainer.shooting = false); - - + addCommands(new ParallelDeadlineGroup( + turnToAngle(() -> Rotation2d.fromDegrees(40.15)), + new Convey(conveyor, -0.25).withTimeout(0.075) + )); + addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); + addCommands(confirmShooting().withTimeout(2)); + addCommands(reachVelocityByDistance(3.89)); addCommands(followPathAndPickup("FiveCargoAutoPart2")); - addCommands(followPathAndPickup("FiveCargoAutoPart3")); - addCommands(new InstantCommand(swerveDrive::terminate)); - - addCommands(quickReleaseBackShootAndAdjust(5)); + addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); + addCommands(confirmShooting()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/auto/FourBallAuto.java b/src/main/java/frc/robot/auto/FourBallAuto.java index c68d8889..173805d1 100644 --- a/src/main/java/frc/robot/auto/FourBallAuto.java +++ b/src/main/java/frc/robot/auto/FourBallAuto.java @@ -1,40 +1,32 @@ package frc.robot.auto; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.RobotContainer; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.ConveyToShooter; +import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.Shoot; import frc.robot.utils.PhotonVisionModule; public class FourBallAuto extends SaarIsAutonomous { public FourBallAuto(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Alon 4 ball #1"); - addCommands(new InstantCommand(() -> RobotContainer.cachedDistanceForHood = 3.4)); - addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = Shoot.getSetpointVelocity(RobotContainer.cachedDistanceForHood))); - addCommands(new InstantCommand(() -> shooter.setVelocity(RobotContainer.cachedSetpointForShooter))); + addCommands(reachVelocityByDistance(3.35)); addCommands(followPathAndPickup("Alon 4 ball #1")); - addCommands(turnToAngle(() -> Rotation2d.fromDegrees(79.28))); - new ParallelCommandGroup( - new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity), - new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets) - ).withTimeout(3); + addCommands(new ParallelDeadlineGroup( + turnToAngle(() -> Rotation2d.fromDegrees(79.28)), + new Convey(conveyor, -0.25).withTimeout(0.075) + )); + addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); + addCommands(confirmShooting().withTimeout(2)); + addCommands(reachVelocityByDistance(4.03)); addCommands(followPathAndPickup("Alon 4 ball #2")); addCommands(followPath("Alon 4 ball #3")); - addCommands(new InstantCommand(() -> RobotContainer.cachedDistanceForHood = 4.5)); - addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = Shoot.getSetpointVelocity(RobotContainer.cachedDistanceForHood))); - addCommands(new InstantCommand(() -> shooter.setVelocity(RobotContainer.cachedSetpointForShooter))); - new ParallelCommandGroup( - new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity), - new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets) - ).withTimeout(3); + addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); + addCommands(confirmShooting()); } } diff --git a/src/main/java/frc/robot/auto/FourCargoAuto.java b/src/main/java/frc/robot/auto/FourCargoAuto.java index d6bdbcaa..850d8dbd 100644 --- a/src/main/java/frc/robot/auto/FourCargoAuto.java +++ b/src/main/java/frc/robot/auto/FourCargoAuto.java @@ -1,12 +1,10 @@ package frc.robot.auto; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.RobotContainer; +import edu.wpi.first.wpilibj2.command.*; import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.intake.Intake; @@ -19,12 +17,14 @@ public class FourCargoAuto extends SaarIsAutonomous { // go to terminal, pickup cargo, park near low tarmac, shoot.(9) public FourCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "p2 - Taxi from low right tarmac and pickup low cargo(7.1)"); - addCommands(new InstantCommand(() -> RobotContainer.warmUpShooting = true)); + addCommands(reachVelocityByDistance(3.36)); addCommands(followPathAndPickup("p2 - Taxi from low right tarmac and pickup low cargo(7.1)")); - - addCommands(shootAndAdjust(2)); - addCommands(new InstantCommand(() -> RobotContainer.shooting = false)); - + addCommands(new ParallelDeadlineGroup( + new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3), + new Convey(conveyor, -0.25).withTimeout(0.075) + )); + addCommands(confirmShooting().withTimeout(2)); + addCommands(reachVelocityByDistance(4.05)); addCommands(new ParallelRaceGroup( followPath("p3 - Going to terminal(9.3)"), new SequentialCommandGroup( @@ -33,14 +33,12 @@ public FourCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor pickup(10) ) )); - addCommands(new ParallelRaceGroup( followPath("p3 - Going to middle tarmac(9.4.2)"), pickup(10) ) ); - addCommands(shootAndAdjust(10)); - addCommands(new InstantCommand(() -> RobotContainer.shooting = false)); - + new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3); + addCommands(confirmShooting()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/auto/SaarIsAutonomous.java b/src/main/java/frc/robot/auto/SaarIsAutonomous.java index 0ba95337..4e8fa7ac 100644 --- a/src/main/java/frc/robot/auto/SaarIsAutonomous.java +++ b/src/main/java/frc/robot/auto/SaarIsAutonomous.java @@ -9,19 +9,16 @@ import edu.wpi.first.wpilibj2.command.*; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.RobotContainer; import frc.robot.commandgroups.PickUpCargo; import frc.robot.commandgroups.QuickReleaseBackAndShootCargo; import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.Convey; -import frc.robot.subsystems.conveyor.commands.ConveyToShooter; +import frc.robot.subsystems.conveyor.commands.ConveyCargo; import frc.robot.subsystems.drivetrain.SwerveDrive; import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand; import frc.robot.subsystems.drivetrain.commands.TurnToAngle; import frc.robot.subsystems.drivetrain.commands.auto.FollowPath; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.commands.HoodCommand; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.commands.IntakeCargo; import frc.robot.subsystems.shooter.Shooter; @@ -140,32 +137,6 @@ protected CommandBase quickReleaseBackShootAndAdjust(double timeout) { } - protected CommandBase shoot3(double timeout) { - DoubleSupplier distanceFromTarget = visionModule::getDistance; - - return new SequentialCommandGroup( - new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = RobotContainer.setpointSupplierForShooterFromVision.getAsDouble()), - new InstantCommand(() -> RobotContainer.cachedDistanceForHood = RobotContainer.distanceSupplierFromVision.getAsDouble()), - new InstantCommand(() -> RobotContainer.odometryCachedSetpoint = RobotContainer.odometrySetpointSupplier.getAsDouble()), - new InstantCommand(() -> RobotContainer.odometryCachedDistance = RobotContainer.odometryDistanceSupplier.getAsDouble()), - new InstantCommand(() -> RobotContainer.cachedHasTarget = !RobotContainer.playWithoutVision && RobotContainer.hasTarget.getAsBoolean()), - new InstantCommand(() -> RobotContainer.shooting = true), - new InstantCommand(flap::allowShooting), - new InstantCommand(() -> shooter.setVelocity(Shoot.getSetpointVelocity(distanceFromTarget.getAsDouble()))), - new WaitUntilCommand(() -> Math.abs(shooter.getVelocity() - (RobotContainer.cachedSetpointForShooter)) <= Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get()), - new IntakeCargo(intake, Constants.Intake.DEFAULT_POWER::get), - new Convey(conveyor, Constants.Conveyor.SHOOT_POWER), - new HoodCommand(hood) - ); - } - - protected CommandBase shoot4(double timeout) { - return new SequentialCommandGroup( - new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity) - ); - } - - protected CommandBase pickup(double timeout) { return new PickUpCargo( conveyor, @@ -182,4 +153,16 @@ protected CommandBase turnToAngle(Supplier target) { target ); } + + protected CommandBase reachVelocityByDistance(double distance) { + return new SequentialCommandGroup( + new InstantCommand(() -> shooter.setVelocity(Shoot.getSetpointVelocity(distance))), + new InstantCommand(() -> hood.setSolenoid(distance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); + } + + protected CommandBase confirmShooting() { + return new ConveyCargo(conveyor); + } + + } diff --git a/src/main/java/frc/robot/commandgroups/BackAndShootCargo2.java b/src/main/java/frc/robot/commandgroups/BackAndShootCargo2.java deleted file mode 100644 index fe50eb7a..00000000 --- a/src/main/java/frc/robot/commandgroups/BackAndShootCargo2.java +++ /dev/null @@ -1,126 +0,0 @@ -package frc.robot.commandgroups; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.*; -import frc.robot.Constants; -import frc.robot.RobotContainer; -import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.Convey; -import frc.robot.subsystems.flap.Flap; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.commands.HoodCommand; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.Shoot; -import webapp.FireLog; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -public class BackAndShootCargo2 extends SequentialCommandGroup { - - class ConveyToShooter2 extends CommandBase { - private final Conveyor conveyor; - private final BooleanSupplier preFlapSupplier; - private final Timer timer = new Timer(); - private final DoubleSupplier velocitySupplier; - private final Timer delayTimer = new Timer(); - private boolean last = false; - private boolean getBallToPreFlap = true; - private boolean wait = true; - - public ConveyToShooter2(Conveyor conveyor, BooleanSupplier preFlapSupplier, DoubleSupplier velocitySupplier) { - this.conveyor = conveyor; - this.preFlapSupplier = preFlapSupplier; - this.velocitySupplier = velocitySupplier; - addRequirements(conveyor); - } - - @Override - public void initialize() { - timer.stop(); - delayTimer.stop(); - if (preFlapSupplier.getAsBoolean()) { - getBallToPreFlap = false; - } - wait = true; - last = false; - } - - @Override - public void execute() { - FireLog.log("Shooter velocity", velocitySupplier.getAsDouble()); - if (RobotContainer.hardCodedVelocity) { - FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); - } else { - if (RobotContainer.cachedHasTarget) { - FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); - } else { - FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); - } - } - - if (getBallToPreFlap) { - conveyor.setPower(Constants.Conveyor.SHOOT_POWER); - } else { - conveyor.setPower(0); - } - - if (preFlapSupplier.getAsBoolean()) { - if (!last) { - last = true; - getBallToPreFlap = false; - timer.start(); - timer.reset(); - delayTimer.start(); - delayTimer.reset(); - } - } else { - last = false; - getBallToPreFlap = true; - } - - if (timer.hasElapsed(0.3)) { - getBallToPreFlap = true; - SmartDashboard.putNumber("time", timer.get()); - timer.reset(); - timer.stop(); - } - } - - @Override - public void end(boolean interrupted) { - timer.stop(); - delayTimer.stop(); - conveyor.setPower(0); - } - - @Override - public boolean isFinished() { - return super.isFinished(); - } - } - public BackAndShootCargo2(Shooter shooter, Hood hood, Conveyor conveyor, Flap flap, DoubleSupplier distanceFromTarget) { - addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = RobotContainer.setpointSupplierForShooterFromVision.getAsDouble())); - addCommands(new InstantCommand(() -> { - RobotContainer.cachedDistanceForHood = RobotContainer.distanceSupplierFromVision.getAsDouble() - 0.5; - -// if (RobotContainer.distanceSupplier.getAsDouble() > 3.4 && RobotContainer.distanceSupplier.getAsDouble() < 4.5) -// RobotContainer.cachedDistance = RobotContainer.distanceSupplier.getAsDouble() - 0.3; -// else RobotContainer.cachedDistance = RobotContainer.distanceSupplier.getAsDouble(); - })); - addCommands(new InstantCommand(() -> RobotContainer.odometryCachedSetpoint = RobotContainer.odometrySetpointSupplier.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.odometryCachedDistance = RobotContainer.odometryDistanceSupplier.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.cachedHasTarget = !RobotContainer.playWithoutVision && RobotContainer.hasTarget.getAsBoolean())); - addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); - - addCommands(new Convey(conveyor, -0.25).withTimeout(0.075), - new ParallelCommandGroup( - new HoodCommand(hood), - new ConveyToShooter2(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity), - new InstantCommand(flap::allowShooting), - new Shoot(shooter, hood, distanceFromTarget) - )); - - } -} diff --git a/src/main/java/frc/robot/commandgroups/ConveyForShooting.java b/src/main/java/frc/robot/commandgroups/ConveyForShooting.java new file mode 100644 index 00000000..d1aa87e3 --- /dev/null +++ b/src/main/java/frc/robot/commandgroups/ConveyForShooting.java @@ -0,0 +1,15 @@ +package frc.robot.commandgroups; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.conveyor.commands.ConveyCargo; + +public class ConveyForShooting extends SequentialCommandGroup { + public ConveyForShooting(Conveyor conveyor) { + addCommands(new WaitUntilCommand(() -> Math.abs(RobotContainer.Suppliers.shooterVelocity.getAsDouble() - RobotContainer.setpointVelocity) < Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get())); + addCommands(new ConveyCargo(conveyor)); + } +} diff --git a/src/main/java/frc/robot/commandgroups/JustShoot.java b/src/main/java/frc/robot/commandgroups/JustShoot.java new file mode 100644 index 00000000..fb5ca78a --- /dev/null +++ b/src/main/java/frc/robot/commandgroups/JustShoot.java @@ -0,0 +1,26 @@ +package frc.robot.commandgroups; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.conveyor.commands.Convey; +import frc.robot.subsystems.flap.Flap; +import frc.robot.subsystems.hood.Hood; + +public class JustShoot extends SequentialCommandGroup { + public JustShoot(Conveyor conveyor, Flap flap, Hood hood) { + addCommands(new Convey(conveyor, -0.25).withTimeout(0.075)); + addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); +// if (RobotContainer.hardCodedVelocity) +// addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.hardCodedDistance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); +// else { +// if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) + addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.distanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); +// else +// addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.odometryDistanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); +// } + addCommands(new ConveyForShooting(conveyor)); + } +} diff --git a/src/main/java/frc/robot/commandgroups/OdometryAdjust.java b/src/main/java/frc/robot/commandgroups/OdometryAdjust.java new file mode 100644 index 00000000..1712212a --- /dev/null +++ b/src/main/java/frc/robot/commandgroups/OdometryAdjust.java @@ -0,0 +1,53 @@ +package frc.robot.commandgroups; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.drivetrain.SwerveDrive; + +public class OdometryAdjust extends CommandBase { + private final SwerveDrive swerveDrive; + private final PIDController adjustController = new PIDController(Constants.SwerveDrive.ADJUST_CONTROLLER_KP.get(), 0, 0) {{ + enableContinuousInput(-Math.PI, Math.PI); + setTolerance(Constants.SwerveDrive.ADJUST_CONTROLLER_TOLERANCE); + }}; + private Rotation2d target; + + + public OdometryAdjust(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + target = Robot.getAngle(); + addRequirements(swerveDrive); + } + + @Override + public void initialize() { + var robotAngle = Robot.getAngle(); + var robotPose = swerveDrive.getPose().getTranslation(); + var hubPose = Constants.Vision.HUB_POSE.getTranslation(); + var poseRelativeToTarget = hubPose.minus(robotPose); + target = robotAngle.plus(new Rotation2d( + Math.atan2( + poseRelativeToTarget.getY(), + poseRelativeToTarget.getX() + ))); + } + + @Override + public void execute() { + double rotation = adjustController.calculate(Robot.getAngle().getRadians(), target.getRadians()); + swerveDrive.holonomicDrive(0, 0, rotation); + } + + @Override + public boolean isFinished() { + return Math.abs(Robot.getAngle().minus(target).getDegrees()) < 2; + } + + @Override + public void end(boolean interrupted) { + swerveDrive.terminate(); + } +} diff --git a/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java b/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java new file mode 100644 index 00000000..0b4a3c4a --- /dev/null +++ b/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java @@ -0,0 +1,36 @@ +package frc.robot.commandgroups; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.conveyor.commands.Convey; +import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.flap.Flap; +import frc.robot.subsystems.hood.Hood; + +public class OneButtonAdjustAndShoot extends SequentialCommandGroup { + public OneButtonAdjustAndShoot(SwerveDrive swerve, Conveyor conveyor, Flap flap, Hood hood) { + addCommands(new InstantCommand(flap::allowShooting)); + + addCommands(new ParallelCommandGroup( + new Convey(conveyor, -0.25).withTimeout(0.075), + //if (!RobotContainer.hasTarget.getAsBoolean()) + new OdometryAdjust(swerve) + ).withTimeout(1)); +// if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) + addCommands(new VisionAdjust(swerve).withTimeout(0.5)); + addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); +// if (RobotContainer.hardCodedVelocity) +// addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.hardCodedDistance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); +// else { +// if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) + addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.distanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); +// else +// addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.odometryDistanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); +// } + addCommands(new ConveyForShooting(conveyor)); + } +} diff --git a/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java b/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java index 4b5f967d..cf7f72f7 100644 --- a/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java +++ b/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java @@ -17,16 +17,5 @@ public QuickReleaseBackAndShootCargo(Shooter shooter, Conveyor conveyor, Flap flap, DoubleSupplier distanceFromTarget) { - addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = RobotContainer.setpointSupplierForShooterFromVision.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.cachedDistanceForHood = RobotContainer.distanceSupplierFromVision.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.odometryCachedSetpoint = RobotContainer.odometrySetpointSupplier.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.odometryCachedDistance = RobotContainer.odometryDistanceSupplier.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.cachedHasTarget = !RobotContainer.playWithoutVision && RobotContainer.hasTarget.getAsBoolean())); - addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); - - addCommands(new Convey(conveyor, -0.25).withTimeout(0.075), - new QuickReleaseShootCargo(shooter, hood, conveyor, flap, distanceFromTarget)); - - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commandgroups/SafetyShootCargo.java b/src/main/java/frc/robot/commandgroups/SafetyShootCargo.java deleted file mode 100644 index 44c26ad6..00000000 --- a/src/main/java/frc/robot/commandgroups/SafetyShootCargo.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.commandgroups; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.Constants; -import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.Convey; -import frc.robot.subsystems.flap.Flap; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.Shoot; -import frc.robot.subsystems.shooter.commands.WarmUpShooter; -import frc.robot.utils.Utils; - -import java.util.function.DoubleSupplier; - -public class SafetyShootCargo extends ParallelCommandGroup { - - public SafetyShootCargo(Shooter shooter, Conveyor conveyor, Flap flap, Hood hood, DoubleSupplier distanceFromTarget) { - addCommands( - new InstantCommand(flap::allowShooting), - new InstantCommand(() -> hood.setSolenoid(Hood.Mode.getValue(distanceFromTarget.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD))), - new WarmUpShooter(shooter, distanceFromTarget), - new Convey(conveyor, Constants.Conveyor.SHOOT_POWER, - () -> Utils.conventionalDeadband( - shooter.getVelocity() - Shoot.getSetpointVelocity( - distanceFromTarget.getAsDouble()), Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get()) == 0) - ); - } -} diff --git a/src/main/java/frc/robot/commandgroups/ShootCargo.java b/src/main/java/frc/robot/commandgroups/ShootCargo.java deleted file mode 100644 index ad4459dc..00000000 --- a/src/main/java/frc/robot/commandgroups/ShootCargo.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.commandgroups; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.ConveyToShooter; -import frc.robot.subsystems.flap.Flap; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.commands.HoodCommand; -import frc.robot.subsystems.shooter.Shooter; - -import java.util.function.DoubleSupplier; - -public class ShootCargo extends ParallelCommandGroup { - - public ShootCargo(Shooter shooter, - Hood hood, - Conveyor conveyor, - Flap flap, - DoubleSupplier distanceFromTarget) { - addCommands( - new HoodCommand(hood), -// new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity), - new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity), - new InstantCommand(flap::allowShooting) -// new Shoot(shooter, hood, distanceFromTarget) - ); - } -} diff --git a/src/main/java/frc/robot/commandgroups/VisionAdjust.java b/src/main/java/frc/robot/commandgroups/VisionAdjust.java new file mode 100644 index 00000000..7fd0c8e4 --- /dev/null +++ b/src/main/java/frc/robot/commandgroups/VisionAdjust.java @@ -0,0 +1,46 @@ +package frc.robot.commandgroups; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.subsystems.drivetrain.SwerveDrive; + +public class VisionAdjust extends CommandBase { + private final SwerveDrive swerveDrive; + private final PIDController adjustController = new PIDController(Constants.SwerveDrive.ADJUST_CONTROLLER_KP.get(), 0, 0) {{ + enableContinuousInput(-Math.PI, Math.PI); + setTolerance(Constants.SwerveDrive.ADJUST_CONTROLLER_TOLERANCE); + }}; + private Rotation2d target; + + + public VisionAdjust(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + target = Robot.getAngle(); + addRequirements(swerveDrive); + } + + @Override + public void initialize() { + target = Robot.getAngle().minus(Rotation2d.fromDegrees(RobotContainer.Suppliers.yawSupplier.getAsDouble())); + } + + @Override + public void execute() { + double rotation = adjustController.calculate(Robot.getAngle().getRadians(), target.getRadians()); + swerveDrive.holonomicDrive(0, 0, rotation); + } + + @Override + public boolean isFinished() { + return Math.abs(Robot.getAngle().minus(target).getDegrees()) < 1.5; + } + + @Override + public void end(boolean interrupted) { + swerveDrive.terminate(); + } +} diff --git a/src/main/java/frc/robot/commandgroups/bits/TestBallFlow.java b/src/main/java/frc/robot/commandgroups/bits/TestBallFlow.java index b8a0c3af..2924cfeb 100644 --- a/src/main/java/frc/robot/commandgroups/bits/TestBallFlow.java +++ b/src/main/java/frc/robot/commandgroups/bits/TestBallFlow.java @@ -2,10 +2,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commandgroups.PickUpCargo; -import frc.robot.commandgroups.ShootCargo; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; @@ -24,14 +20,6 @@ public TestBallFlow(Hood hood, Flap flap, Conveyor conveyor, Intake intake, Shoo this.intake = intake; this.shooter = shooter; this.conveyor = conveyor; - ShootCargo shootCargo = new ShootCargo(shooter, hood, conveyor, flap, Constants.Conveyor.DEFAULT_POWER::get); - PickUpCargo pickUpCargo = new PickUpCargo(conveyor, flap, intake, Constants.Conveyor.DEFAULT_POWER.get(), () -> 0.5); - - addCommands( - pickUpCargo.withInterrupt(pickUpCargoIsFinished), - new WaitCommand(5), - shootCargo.withInterrupt(shootCargoIsFinished) - ); } @Override diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyCargo.java b/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyCargo.java new file mode 100644 index 00000000..9b95865f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyCargo.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.conveyor.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.subsystems.conveyor.Conveyor; + +public class ConveyCargo extends CommandBase { + private final Conveyor conveyor; + private final Timer timer = new Timer(); + private boolean stopForSecondBall = false; + private boolean lastPreFlapObservation = false; + + public ConveyCargo(Conveyor conveyor) { + this.conveyor = conveyor; + addRequirements(conveyor); + } + + @Override + public void initialize() { + timer.reset(); + timer.stop(); + } + + @Override + public void execute() { + if (RobotContainer.Suppliers.preFlapBlocked.getAsBoolean()) { + lastPreFlapObservation = true; + } else if (lastPreFlapObservation) { + stopForSecondBall = true; + } + if (RobotContainer.Suppliers.postFlapBlocked.getAsBoolean()) { + stopForSecondBall = true; + } + if (stopForSecondBall) { + if (!RobotContainer.Suppliers.preFlapBlocked.getAsBoolean()) { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + } else { + conveyor.setPower(0); + timer.start(); + } + if (timer.hasElapsed(0.3)) { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + } + } else { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + } + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + conveyor.setPower(0); + } + + @Override + public boolean isFinished() { + return super.isFinished(); + } +} diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java b/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java index 929a67e4..2d2f7a68 100644 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java @@ -30,98 +30,98 @@ public ConveyToShooter(Conveyor conveyor, BooleanSupplier preFlapSupplier, Doubl this.velocitySupplier = velocitySupplier; addRequirements(conveyor); } - - @Override - public void initialize() { - timer.stop(); - delayTimer.stop(); - if (preFlapSupplier.getAsBoolean()) { - getBallToPreFlap = false; - } - wait = true; - last = false; - firstBall = true; - } - - @Override - public void execute() { - if (RobotContainer.hardCodedVelocity) { - FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); - } else { - if (RobotContainer.cachedHasTarget) { - FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); - } else { - FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); - } - } - if (wait) { - FireLog.log("wait", 2000); - if (RobotContainer.hardCodedVelocity) { - if (Math.abs(RobotContainer.hardCodedVelocityValue - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } - } else { - if (RobotContainer.cachedHasTarget) { - if (Math.abs(RobotContainer.cachedSetpointForShooter - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } - } else { - if (Math.abs(RobotContainer.odometryCachedSetpoint - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } - } - } - SmartDashboard.putString("Saar", "Mama"); - } else { - FireLog.log("wait", 4000); - if (firstBall) { - conveyor.setPower(Constants.Conveyor.SHOOT_POWER); - if (preFlapSupplier.getAsBoolean()) { - firstBall = false; - } - } else { - SmartDashboard.putString("Saar", "Joe"); - SmartDashboard.putNumber("Saar2", timer.get()); - - if (getBallToPreFlap) { - conveyor.setPower(Constants.Conveyor.SHOOT_POWER); - } else { - conveyor.setPower(0); - } - - if (preFlapSupplier.getAsBoolean()) { - if (!last) { - last = true; - getBallToPreFlap = false; - timer.start(); - timer.reset(); - delayTimer.start(); - delayTimer.reset(); - } - } else { - last = false; - getBallToPreFlap = true; - } - - if (timer.hasElapsed(0.3)) { - getBallToPreFlap = true; - SmartDashboard.putNumber("time", timer.get()); - timer.reset(); - timer.stop(); - } - } - } - } - - @Override - public void end(boolean interrupted) { - timer.stop(); - delayTimer.stop(); - conveyor.setPower(0); - } - - @Override - public boolean isFinished() { - return super.isFinished(); - } +// +// @Override +// public void initialize() { +// timer.stop(); +// delayTimer.stop(); +// if (preFlapSupplier.getAsBoolean()) { +// getBallToPreFlap = false; +// } +// wait = true; +// last = false; +// firstBall = true; +// } +// +// @Override +// public void execute() { +// if (RobotContainer.hardCodedVelocity) { +// FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); +// } else { +// if (RobotContainer.cachedHasTarget) { +// FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); +// } else { +// FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); +// } +// } +// if (wait) { +// FireLog.log("wait", 2000); +// if (RobotContainer.hardCodedVelocity) { +// if (Math.abs(RobotContainer.hardCodedVelocityValue - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { +// wait = false; +// } +// } else { +// if (RobotContainer.cachedHasTarget) { +// if (Math.abs(RobotContainer.cachedSetpointForShooter - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { +// wait = false; +// } +// } else { +// if (Math.abs(RobotContainer.odometryCachedSetpoint - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { +// wait = false; +// } +// } +// } +// SmartDashboard.putString("Saar", "Mama"); +// } else { +// FireLog.log("wait", 4000); +// if (firstBall) { +// conveyor.setPower(Constants.Conveyor.SHOOT_POWER); +// if (preFlapSupplier.getAsBoolean()) { +// firstBall = false; +// } +// } else { +// SmartDashboard.putString("Saar", "Joe"); +// SmartDashboard.putNumber("Saar2", timer.get()); +// +// if (getBallToPreFlap) { +// conveyor.setPower(Constants.Conveyor.SHOOT_POWER); +// } else { +// conveyor.setPower(0); +// } +// +// if (preFlapSupplier.getAsBoolean()) { +// if (!last) { +// last = true; +// getBallToPreFlap = false; +// timer.start(); +// timer.reset(); +// delayTimer.start(); +// delayTimer.reset(); +// } +// } else { +// last = false; +// getBallToPreFlap = true; +// } +// +// if (timer.hasElapsed(0.3)) { +// getBallToPreFlap = true; +// SmartDashboard.putNumber("time", timer.get()); +// timer.reset(); +// timer.stop(); +// } +// } +// } +// } +// +// @Override +// public void end(boolean interrupted) { +// timer.stop(); +// delayTimer.stop(); +// conveyor.setPower(0); +// } +// +// @Override +// public boolean isFinished() { +// return super.isFinished(); +// } } diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java b/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java index 504d194e..121da2f4 100644 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java @@ -25,56 +25,56 @@ public QuickReleaseConvey(Conveyor conveyor, DoubleSupplier velocitySupplier) { addRequirements(conveyor); } - @Override - public void initialize() { - timer.stop(); - delayTimer.stop(); - wait = true; - } - - @Override - public void execute() { - FireLog.log("Shooter velocity", velocitySupplier.getAsDouble()); - if (RobotContainer.hardCodedVelocity) { - FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); - } else { - if (RobotContainer.cachedHasTarget) { - FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); - } else { - FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); - } - } - if (wait) { - if (RobotContainer.hardCodedVelocity) { - if (Math.abs(RobotContainer.hardCodedVelocityValue - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } - } else { - if (RobotContainer.cachedHasTarget) { - if (Math.abs(RobotContainer.cachedSetpointForShooter - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } - } else { - if (Math.abs(RobotContainer.odometryCachedSetpoint - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } - } - } - SmartDashboard.putString("Saar", "Mama"); - } else { - conveyor.setPower(Constants.Conveyor.SHOOT_POWER); - } - } - - @Override - public void end(boolean interrupted) { - timer.stop(); - delayTimer.stop(); - conveyor.setPower(0); - } - - @Override - public boolean isFinished() { - return super.isFinished(); - } +// @Override +// public void initialize() { +// timer.stop(); +// delayTimer.stop(); +// wait = true; +// } +// +// @Override +// public void execute() { +// FireLog.log("Shooter velocity", velocitySupplier.getAsDouble()); +// if (RobotContainer.hardCodedVelocity) { +// FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); +// } else { +// if (RobotContainer.cachedHasTarget) { +// FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); +// } else { +// FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); +// } +// } +// if (wait) { +// if (RobotContainer.hardCodedVelocity) { +// if (Math.abs(RobotContainer.hardCodedVelocityValue - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { +// wait = false; +// } +// } else { +// if (RobotContainer.cachedHasTarget) { +// if (Math.abs(RobotContainer.cachedSetpointForShooter - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { +// wait = false; +// } +// } else { +// if (Math.abs(RobotContainer.odometryCachedSetpoint - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { +// wait = false; +// } +// } +// } +// SmartDashboard.putString("Saar", "Mama"); +// } else { +// conveyor.setPower(Constants.Conveyor.SHOOT_POWER); +// } +// } +// +// @Override +// public void end(boolean interrupted) { +// timer.stop(); +// delayTimer.stop(); +// conveyor.setPower(0); +// } +// +// @Override +// public boolean isFinished() { +// return super.isFinished(); +// } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index d082500c..2bb3ba99 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -335,15 +335,20 @@ public void periodic() { rotationVelocity.append(speeds.omegaRadiansPerSecond); if (!RobotContainer.shooting && !DriverStation.isAutonomous()) { - RobotContainer.cachedSetpointForShooter = Shoot.getSetpointVelocity(getOdometryDistance()); + RobotContainer.setpointVelocity = Shoot.getSetpointVelocity(getOdometryDistance()); } + +// if (RobotContainer.hardCodedVelocity) { +// RobotContainer.setpointVelocity = RobotContainer.hardCodedVelocityValue; +// } + if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision && !DriverStation.isAutonomous()) { - if (Math.abs(RobotContainer.yawSupplierFromVision.getAsDouble()) < 6) { + if (Math.abs(RobotContainer.Suppliers.yawSupplier.getAsDouble()) < 6) { var odom = odometry.getPoseMeters().getTranslation(); var target = Constants.Vision.HUB_POSE.getTranslation(); Translation2d relative = odom.minus(target); double alpha = Math.atan2(relative.getY(), relative.getX()); - double visionDistance = RobotContainer.distanceSupplierFromVision.getAsDouble(); + double visionDistance = RobotContainer.Suppliers.distanceSupplier.getAsDouble(); Translation2d newTranslation = new Translation2d(Math.cos(alpha) * visionDistance, Math.sin(alpha) * visionDistance); odometry.resetPosition(new Pose2d(target.plus(newTranslation), Robot.getAngle()), Robot.getAngle()); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java b/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java index 057bc0de..ed23309e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java @@ -60,8 +60,8 @@ public void execute() { if (magnitude == 0) current = 0; current += magnitude / 5; if (current > magnitude) current = magnitude; - forward = Math.cos(alpha) * current; - strafe = Math.sin(alpha) * current; + forward = Math.cos(alpha) * magnitude; + strafe = Math.sin(alpha) * magnitude; double rotation = speeds.omegaRadiansPerSecond; if (rotation != 0) { @@ -133,7 +133,7 @@ public void execute() { } } else { - swerveDrive.defaultHolonomicDrive(forward, strafe, rotation * (1 + (current / VELOCITY_MULTIPLIER) / Constants.SwerveDrive.ROTATIONAL_ADDITION_RESTRAINT)); + swerveDrive.defaultHolonomicDrive(forward, strafe, rotation * (1 + (magnitude / VELOCITY_MULTIPLIER) / Constants.SwerveDrive.ROTATIONAL_ADDITION_RESTRAINT)); setpoint = Robot.getAngle(); } } diff --git a/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java b/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java index 936aa2ae..28c45f99 100644 --- a/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java +++ b/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java @@ -15,7 +15,7 @@ public HoodCommand(Hood hood) { this.hood = hood; addRequirements(hood); } - +/* @Override public void initialize() { timer.reset(); @@ -23,11 +23,11 @@ public void initialize() { if (RobotContainer.hardCodedVelocity) { mode = RobotContainer.hardCodedDistance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance; } else { - if (RobotContainer.cachedHasTarget) { - mode = RobotContainer.cachedDistanceForHood < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance; - } else { - mode = RobotContainer.odometryCachedSetpoint < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance; - } +// if (RobotContainer.cachedHasTarget) { +// mode = RobotContainer.cachedDistanceForHood < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance; +// } else { +// mode = RobotContainer.odometryCachedSetpoint < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance; +// } } } @@ -39,5 +39,5 @@ public void execute() { @Override public void end(boolean interrupted) { timer.stop(); - } + }*/ } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d07a15e8..3bd5c8fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -128,7 +128,6 @@ public void changePID() { @Override public void periodic() { FireLog.log("my Shooter velocity", getVelocity()); - FireLog.log("my Shooter setpoint", RobotContainer.cachedSetpointForShooter); shooterVelocity.append(getVelocity()); shooterVoltage.append(mainMotor.getMotorOutputVoltage()); FireLog.log("Shooter-velocity", getVelocity()); diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java b/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java index 25cd9344..627deaa7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.RobotContainer; -import frc.robot.commandgroups.ShootCargo; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.flap.Flap; @@ -18,23 +17,6 @@ public BackAndShootCargo(Shooter shooter, Conveyor conveyor, Flap flap, DoubleSupplier distanceFromTarget) { - addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); - addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = RobotContainer.setpointSupplierForShooterFromVision.getAsDouble())); - addCommands(new InstantCommand(() -> { -// double distance = RobotContainer.distanceSupplierFromVision.getAsDouble(); -// if (distance <= 4.5) -// RobotContainer.cachedDistanceForHood = distance - 0.5; -// else { - RobotContainer.cachedDistanceForHood = RobotContainer.distanceSupplierFromVision.getAsDouble(); -// } - })); - addCommands(new InstantCommand(() -> RobotContainer.odometryCachedSetpoint = RobotContainer.odometrySetpointSupplier.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.odometryCachedDistance = RobotContainer.odometryDistanceSupplier.getAsDouble())); - addCommands(new InstantCommand(() -> RobotContainer.cachedHasTarget = !RobotContainer.playWithoutVision && RobotContainer.hasTarget.getAsBoolean())); - - addCommands(new Convey(conveyor, -0.25).withTimeout(0.075), - new ShootCargo(shooter, hood, conveyor, flap, distanceFromTarget)); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java index 75784f2a..65843a87 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java @@ -16,7 +16,7 @@ public ReachVelocity(Shooter shooter) { @Override public void execute() { if (RobotContainer.warmUpShooting) { - shooter.setVelocity(RobotContainer.cachedSetpointForShooter); + shooter.setVelocity(RobotContainer.setpointVelocity); } else { shooter.setPower(0); } diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java b/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java index ec240884..aebdf855 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java @@ -73,7 +73,7 @@ public static double getSetpointVelocity(double distance) { double t = (distance - prevMeasuredDistance) / (nextMeasuredDistance - prevMeasuredDistance); return (1 - t) * y1 + t * y2; } - +/* @Override public void initialize() { RobotContainer.ledSubsystem.setNeutralMode(false); @@ -85,7 +85,7 @@ public void execute() { if (power.isEmpty()) { if (RobotContainer.hardCodedVelocity) { // shooter.setVelocity(RobotContainer.hardCodedVelocityValue); - SmartDashboard.putString("speed_state", Math.abs(RobotContainer.cachedSetpointForShooter - shooter.getVelocity()) <= 30 ? "green" : Math.abs(RobotContainer.cachedSetpointForShooter - shooter.getVelocity()) <= 100 ? "yellow" : "red"); +// SmartDashboard.putString("speed_state", Math.abs(RobotContainer.cachedSetpointForShooter - shooter.getVelocity()) <= 30 ? "green" : Math.abs(RobotContainer.cachedSetpointForShooter - shooter.getVelocity()) <= 100 ? "yellow" : "red"); } else { if (RobotContainer.cachedHasTarget) { // shooter.setVelocity(RobotContainer.cachedSetpointForShooter); @@ -133,5 +133,5 @@ public void end(boolean interrupted) { RobotContainer.ledSubsystem.setNeutralMode(true); RobotContainer.ledSubsystem.setPercent(0); SmartDashboard.putString("speed_state", "red"); - } + }*/ } diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java b/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java index 154475f9..05cd6dfc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.commandgroups.ShootCargo; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; @@ -36,9 +35,9 @@ public TryVelocities(Conveyor conveyor, Shooter shooter, Hood hood, Flap flap, this.hood = hood; this.flap = flap; this.distanceFromTarget = distanceFromTarget; - addCommandsForNextVelocity(); +// addCommandsForNextVelocity(); } - +/* @Override public void execute() { super.execute(); @@ -86,5 +85,5 @@ private void addCommandsForNextVelocity() { } }) ); - } + }*/ } diff --git a/src/test/java/Test.java b/src/test/java/Test.java index 140f052b..c3fe2933 100644 --- a/src/test/java/Test.java +++ b/src/test/java/Test.java @@ -1,4 +1,5 @@ import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.Constants; import frc.robot.subsystems.UnitModel; import static frc.robot.Constants.Shooter.TICKS_PER_REVOLUTION; @@ -19,4 +20,10 @@ public void test() { System.out.println(target.plus(newTranslation)); } + + @org.junit.Test + public void informal() { + var translation = new Translation2d(4.92, 1.78); + System.out.println(Constants.Vision.HUB_POSE.getTranslation().minus(translation).getNorm()); + } } From 22011a312b86b986e496560bad760fcd62c24f17 Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 11:46:08 +0300 Subject: [PATCH 02/24] Better LEDs, revert if it creates problems --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../OneButtonAdjustAndShoot.java | 9 + .../java/frc/robot/utils/LedSubsystem.java | 224 ++++-------------- 3 files changed, 54 insertions(+), 184 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e1adc87..1142dfce 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,10 @@ private void configureButtonBindings() { }); Joysticks.rightTrigger.whileHeld(new OneButtonAdjustAndShoot(swerve, conveyor, flap, hood)).whenReleased( - () -> shooting = false + () -> { + shooting = false; + ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.STATIC); + } ); Joysticks.leftTwo.whenPressed((Runnable) Robot::resetAngle); diff --git a/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java b/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java index 0b4a3c4a..8265bc24 100644 --- a/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java +++ b/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java @@ -10,16 +10,22 @@ import frc.robot.subsystems.drivetrain.SwerveDrive; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; +import frc.robot.utils.LedSubsystem; public class OneButtonAdjustAndShoot extends SequentialCommandGroup { public OneButtonAdjustAndShoot(SwerveDrive swerve, Conveyor conveyor, Flap flap, Hood hood) { addCommands(new InstantCommand(flap::allowShooting)); + addCommands(new InstantCommand(() -> RobotContainer.ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.ODOMETRY_ADJUST))); // LEDs + addCommands(new ParallelCommandGroup( new Convey(conveyor, -0.25).withTimeout(0.075), //if (!RobotContainer.hasTarget.getAsBoolean()) new OdometryAdjust(swerve) ).withTimeout(1)); + + addCommands(new InstantCommand(() -> RobotContainer.ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.VISION_ADJUST))); // LEDs + // if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) addCommands(new VisionAdjust(swerve).withTimeout(0.5)); addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); @@ -31,6 +37,9 @@ public OneButtonAdjustAndShoot(SwerveDrive swerve, Conveyor conveyor, Flap flap, // else // addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.odometryDistanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); // } + + addCommands(new InstantCommand(() -> RobotContainer.ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.SHOOTING))); // LEDs + addCommands(new ConveyForShooting(conveyor)); } } diff --git a/src/main/java/frc/robot/utils/LedSubsystem.java b/src/main/java/frc/robot/utils/LedSubsystem.java index 89c07c92..5bb34ef5 100644 --- a/src/main/java/frc/robot/utils/LedSubsystem.java +++ b/src/main/java/frc/robot/utils/LedSubsystem.java @@ -2,53 +2,31 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LedSubsystem extends SubsystemBase { - public static boolean climbTime = false; - // private final AddressableLED hoodLeds = new AddressableLED(6); -// private final AddressableLEDBuffer hoodLedBuffer = new AddressableLEDBuffer(18); //18 -// private final AddressableLED conveyorLeds = new AddressableLED(3); -// private final AddressableLEDBuffer conveyorLedBuffer = new AddressableLEDBuffer(6); //34 - private final int m_water = 80; - private final int sign = 1; - Timer timer = new Timer(); - Timer timer2 = new Timer(); - private int current = 1; - private int current2 = 0; + public static LedMode currentLedMode = LedMode.STATIC; + private final Timer timer = new Timer(); + private final Timer blinkTimer = new Timer(); private int m_rainbowFirstPixelHue = 0; - private int percent = 0; - private boolean neutralMode = true; private AddressableLED led; private AddressableLEDBuffer ledBuffer; + private int current = 0; public LedSubsystem() { led = new AddressableLED(6); - - // Reuse buffer - // Default to a length of 60, start empty output - // Length is expensive to set, so only set it once, then just update data ledBuffer = new AddressableLEDBuffer(18); led.setLength(ledBuffer.getLength()); // Set the data led.setData(ledBuffer); led.start(); -// new SnakeSubsystem(); -// hoodLeds.setLength(hoodLedBuffer.getLength()); -// hoodLeds.setData(hoodLedBuffer); -// hoodLeds.start(); -// conveyorLeds.setLength(conveyorLedBuffer.getLength()); -// conveyorLeds.setData(conveyorLedBuffer); -// conveyorLeds.start(); - timer.start(); timer.reset(); - timer2.start(); - timer2.reset(); - climbTime = false; + timer.start(); + blinkTimer.reset(); + blinkTimer.start(); } public static int[] switchOfGods(int idx) { // 6 states @@ -90,65 +68,6 @@ public static int[] switchOfGods(int idx) { // 6 states return null; } - public static int[] switchOfGods2(int idx) { // 3 states - switch (idx) { - case 0: - return new int[]{0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, - 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0}; - case 1: - return new int[]{0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0 - , - 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0}; - case 2: - return new int[]{0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0 - , - 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0}; - case 3: - return new int[]{1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, - 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1}; - case 4: - return new int[]{1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, - 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1}; - case 5: - return new int[]{0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, - 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0}; - case 6: - return new int[]{1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, - 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1}; - case 7: - return new int[]{1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, - 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1}; - case 8: - return new int[]{0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, - 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0}; - case 9: - return new int[]{1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, - 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1}; - case 10: - return new int[]{1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, - 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1}; - case 11: - return new int[]{0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, - 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0}; - case 12: - return new int[]{1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, - 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1}; - case 13: - return new int[]{1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, - 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1}; - case 14: - return new int[]{0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, - 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0}; - case 15: - return new int[]{1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, - 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1}; - case 16: - return new int[]{1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, - 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1}; - } - return null; - } - public void updateCurrent(int states) { current++; if (current == states) { @@ -156,124 +75,63 @@ public void updateCurrent(int states) { } } - public void updateCurrent2(int states) { - current2++; - if (current2 == states) { - current2 = 0; + private void rainbowHood() { + for (var i = 0; i < 9; i++) { + final var hue = ((180 - m_rainbowFirstPixelHue) + (i * 180 / 9)) % 180; + ledBuffer.setHSV(8 - i, hue, 223, 217); + ledBuffer.setHSV(i + 9, hue, 223, 217); + } + } + + public void blink(Color color) { + if (blinkTimer.hasElapsed(0.1)) { + for (int i = 0; i < ledBuffer.getLength(); i++) { + ledBuffer.setLED(i, color); + } + blinkTimer.reset(); + } else { + for (int i = 0; i < ledBuffer.getLength(); i++) { + ledBuffer.setLED(i, Color.kBlack); + } } } @Override public void periodic() { - if (DriverStation.isAutonomous() || DriverStation.getMatchTime() == -1) { - climbTime = false; - } - if (!DriverStation.isAutonomous() && DriverStation.getMatchTime() != -1 && DriverStation.getMatchTime() <= 30) { - climbTime = true; - } - double time = 0.2; - if (current == 9) { - time = 0.4; - } - if (timer.hasElapsed(time)) { + + if (timer.hasElapsed(0.2)) { updateCurrent(10); - updateCurrent2(17); timer.reset(); } - if (neutralMode) { + + if (currentLedMode == LedMode.STATIC) { for (var i = 0; i < 18; i++) { if (switchOfGods(current)[i] == 1) { -// m_ledBuffer.setHSV(i, 10, 255, 128); ledBuffer.setRGB(i, 0, 158, 189); } else { ledBuffer.setRGB(i, 25, 25, 25); } } + } else if (currentLedMode == LedMode.SHOOTING) { + rainbowHood(); } else { - for (var i = 0; i < 18; i++) { - if (switchOfGods(percent)[i] == 1) { - ledBuffer.setRGB(i, 0, 255, 0); - } else { - ledBuffer.setRGB(i, 25, 25, 25); - } - } - } - - if (climbTime) { - for (var i = 0; i < 18; i++) { - ledBuffer.setRGB(i, 0, 158, 189); - } + blink(currentLedMode.color); } - -/* for (var i = 20; i < m_ledBuffer.getLength(); i++) { -// final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180; -// final var hue = (m_water + (i * 35 / 34)); -// final var hue = (m_water); -// m_ledBuffer.setHSV(i, hue, 255, 128); - if (switchOfGods2(current2)[i - 20] == 1) { - m_ledBuffer.setRGB(i, 0, 156, 189); - } else { - m_ledBuffer.setRGB(i, 25, 25, 25); - } -// m_ledBuffer.setRGB(i, 0, 156, 189); - }*/ -// m_rainbowFirstPixelHue += 3; -// m_rainbowFirstPixelHue %= 180; -// m_water += sign; -// if (m_water >= 130) { -// sign = -1; -// } -// -// if (m_water <= 80) { -// sign = 1; -// } -// rainbow2(); - led.setData(ledBuffer); -// conveyorLeds.setData(conveyorLedBuffer); - // Increase by to make the rainbow "move" m_rainbowFirstPixelHue += 3; - // Check bounds m_rainbowFirstPixelHue %= 180; + led.setData(ledBuffer); } - private void rainbow2() { - // For every pixel - /* for (var i = 0; i < conveyorLedBuffer.getLength(); i++) { - // Calculate the hue - hue is easier for rainbows because the color - // shape is a circle so only one value needs to precess - final var hue = ((180 - m_rainbowFirstPixelHue) + (i * 180 / 17)) % 180; - // Set the value - if (neutralMode) { - conveyorLedBuffer.setLED(i, Color.kPurple); -// m_ledBuffer.setRGB(i, 219, 127, 142); - conveyorLedBuffer.setLED(36 - (i - 37), Color.kPurple); -// m_ledBuffer.setRGB(36 - (i - 37), 219, 127, 142); - } else { - conveyorLedBuffer.setHSV(i, hue, 223, 217); - conveyorLedBuffer.setHSV(36 - (i - 37), hue, 223, 217); - } - }*/ - // Increase by to make the rainbow "move" -// m_rainbowFirstPixelHue += 3; - // Check bounds -// m_rainbowFirstPixelHue %= 180; - -/* if (climbTime) { - for (var i = 0; i < conveyorLedBuffer.getLength(); i++) { - final var hue = ((180 - m_rainbowFirstPixelHue) + (i * 180 / 17)) % 180; - conveyorLedBuffer.setHSV(i, hue, 223, 217); - conveyorLedBuffer.setHSV(36 - (i - 37), hue, 223, 217); - } - }*/ + public void setCurrentLedMode(LedMode ledMode) { + currentLedMode = ledMode; } - public void setNeutralMode(boolean neutralMode) { - this.neutralMode = neutralMode; - } + public enum LedMode { + STATIC(Color.kBlack), ODOMETRY_ADJUST(Color.kOrange), VISION_ADJUST(Color.kGreen), SHOOTING(Color.kBlack); + private final Color color; - public void setPercent(int percent) { - this.percent = percent; + LedMode(Color color) { + this.color = color; + } } - - } \ No newline at end of file From 470771416b0cea871d8642a6a70c75f2258c947e Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 11:50:16 +0300 Subject: [PATCH 03/24] Add constants for tolerances --- src/main/java/frc/robot/Constants.java | 8 +++++--- src/main/java/frc/robot/commandgroups/OdometryAdjust.java | 2 +- src/main/java/frc/robot/commandgroups/VisionAdjust.java | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0a41948f..879557bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -79,10 +79,10 @@ public static final class SwerveDrive { public static final WebConstant ADJUST_CONTROLLER_KP = WebConstant.of("Swerve", "kp_heading", 12.5); public static final WebConstant ADJUST_CONTROLLER_KI = WebConstant.of("Swerve", "KI_heading", 0); public static final WebConstant ADJUST_CONTROLLER_KD = WebConstant.of("Swerve", "Kd_heading", 0.1); - + public static final double ODOMETRY_ADJUST_FINISHED_CONDITION = 2; // [deg] + public static final double VISION_ADJUST_FINISHED_CONDITION = 1.5; // [deg] private static final double Rx = SwerveDrive.ROBOT_LENGTH / 2; // [m] private static final double Ry = SwerveDrive.ROBOT_WIDTH / 2; // [m] - // Axis systems public static final Translation2d[] SWERVE_POSITIONS = new Translation2d[]{ new Translation2d(Rx, -Ry), @@ -90,6 +90,8 @@ public static final class SwerveDrive { new Translation2d(-Rx, -Ry), new Translation2d(-Rx, Ry) }; + + } public static class Shooter { @@ -105,7 +107,7 @@ public static class Shooter { public static final WebConstant kI = WebConstant.of("Shooter", "kI", 0.0000075); public static final WebConstant kD = WebConstant.of("Shooter", "kD", 9); public static final WebConstant kF = WebConstant.of("Shooter", "kf", 0.0465); -// public static final WebConstant kP = WebConstant.of("Shooter", "kP", 0.03); + // public static final WebConstant kP = WebConstant.of("Shooter", "kP", 0.03); // public static final WebConstant kI = WebConstant.of("Shooter", "kI", 0.00); // public static final WebConstant kD = WebConstant.of("Shooter", "kD", 0); // public static final WebConstant kF = WebConstant.of("Shooter", "kf", 0.0475); diff --git a/src/main/java/frc/robot/commandgroups/OdometryAdjust.java b/src/main/java/frc/robot/commandgroups/OdometryAdjust.java index 1712212a..dbabc6e9 100644 --- a/src/main/java/frc/robot/commandgroups/OdometryAdjust.java +++ b/src/main/java/frc/robot/commandgroups/OdometryAdjust.java @@ -43,7 +43,7 @@ public void execute() { @Override public boolean isFinished() { - return Math.abs(Robot.getAngle().minus(target).getDegrees()) < 2; + return Math.abs(Robot.getAngle().minus(target).getDegrees()) < Constants.SwerveDrive.ODOMETRY_ADJUST_FINISHED_CONDITION; } @Override diff --git a/src/main/java/frc/robot/commandgroups/VisionAdjust.java b/src/main/java/frc/robot/commandgroups/VisionAdjust.java index 7fd0c8e4..3362b085 100644 --- a/src/main/java/frc/robot/commandgroups/VisionAdjust.java +++ b/src/main/java/frc/robot/commandgroups/VisionAdjust.java @@ -36,7 +36,7 @@ public void execute() { @Override public boolean isFinished() { - return Math.abs(Robot.getAngle().minus(target).getDegrees()) < 1.5; + return Math.abs(Robot.getAngle().minus(target).getDegrees()) < Constants.SwerveDrive.VISION_ADJUST_FINISHED_CONDITION; } @Override From 56610366167588b922c36851285c9b38a97d1b8d Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 11:54:27 +0300 Subject: [PATCH 04/24] Delete some unused code --- .../java/frc/robot/auto/SaarIsAutonomous.java | 20 --- .../QuickReleaseBackAndShootCargo.java | 21 --- .../commandgroups/QuickReleaseShootCargo.java | 29 ---- .../conveyor/commands/ConveyToShooter.java | 127 ------------------ .../conveyor/commands/QuickReleaseConvey.java | 80 ----------- 5 files changed, 277 deletions(-) delete mode 100644 src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java delete mode 100644 src/main/java/frc/robot/commandgroups/QuickReleaseShootCargo.java delete mode 100644 src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java delete mode 100644 src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java diff --git a/src/main/java/frc/robot/auto/SaarIsAutonomous.java b/src/main/java/frc/robot/auto/SaarIsAutonomous.java index 4e8fa7ac..9a273477 100644 --- a/src/main/java/frc/robot/auto/SaarIsAutonomous.java +++ b/src/main/java/frc/robot/auto/SaarIsAutonomous.java @@ -10,7 +10,6 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.commandgroups.PickUpCargo; -import frc.robot.commandgroups.QuickReleaseBackAndShootCargo; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.ConveyCargo; import frc.robot.subsystems.drivetrain.SwerveDrive; @@ -118,25 +117,6 @@ protected CommandBase shoot(double timeout) { } - protected CommandBase quickReleaseBackShootAndAdjust(double timeout) { - DoubleSupplier distanceFromTarget = visionModule::getDistance; - DoubleSupplier conveyorPower = Constants.Conveyor.DEFAULT_POWER::get; - - return new SequentialCommandGroup( - new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.1), - new ParallelRaceGroup(new QuickReleaseBackAndShootCargo( - shooter, - hood, - conveyor, - flap, - distanceFromTarget) - .withTimeout(timeout), - new IntakeCargo(intake, Constants.Intake.DEFAULT_POWER::get), - new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets) - )); - } - - protected CommandBase pickup(double timeout) { return new PickUpCargo( conveyor, diff --git a/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java b/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java deleted file mode 100644 index cf7f72f7..00000000 --- a/src/main/java/frc/robot/commandgroups/QuickReleaseBackAndShootCargo.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.commandgroups; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.RobotContainer; -import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.Convey; -import frc.robot.subsystems.flap.Flap; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.shooter.Shooter; - -import java.util.function.DoubleSupplier; - -public class QuickReleaseBackAndShootCargo extends SequentialCommandGroup { - public QuickReleaseBackAndShootCargo(Shooter shooter, - Hood hood, - Conveyor conveyor, - Flap flap, - DoubleSupplier distanceFromTarget) { - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commandgroups/QuickReleaseShootCargo.java b/src/main/java/frc/robot/commandgroups/QuickReleaseShootCargo.java deleted file mode 100644 index 95f6686f..00000000 --- a/src/main/java/frc/robot/commandgroups/QuickReleaseShootCargo.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.commandgroups; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.QuickReleaseConvey; -import frc.robot.subsystems.flap.Flap; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.commands.HoodCommand; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.Shoot; - -import java.util.function.DoubleSupplier; - -public class QuickReleaseShootCargo extends ParallelCommandGroup { - - public QuickReleaseShootCargo(Shooter shooter, - Hood hood, - Conveyor conveyor, - Flap flap, - DoubleSupplier distanceFromTarget) { - addCommands( - new HoodCommand(hood), - new QuickReleaseConvey(conveyor, shooter::getVelocity), - new InstantCommand(flap::allowShooting), - new Shoot(shooter, hood, distanceFromTarget) - ); - } -} diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java b/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java deleted file mode 100644 index 2d2f7a68..00000000 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/ConveyToShooter.java +++ /dev/null @@ -1,127 +0,0 @@ -package frc.robot.subsystems.conveyor.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.RobotContainer; -import frc.robot.subsystems.conveyor.Conveyor; -import webapp.FireLog; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import static frc.robot.Constants.Shooter.SHOOTER_VELOCITY_DEADBAND; - -public class ConveyToShooter extends CommandBase { - private final Conveyor conveyor; - private final BooleanSupplier preFlapSupplier; - private final Timer timer = new Timer(); - private final DoubleSupplier velocitySupplier; - private final Timer delayTimer = new Timer(); - private boolean last = false; - private boolean getBallToPreFlap = true; - private boolean wait = true; - private boolean firstBall = true; - - public ConveyToShooter(Conveyor conveyor, BooleanSupplier preFlapSupplier, DoubleSupplier velocitySupplier) { - this.conveyor = conveyor; - this.preFlapSupplier = preFlapSupplier; - this.velocitySupplier = velocitySupplier; - addRequirements(conveyor); - } -// -// @Override -// public void initialize() { -// timer.stop(); -// delayTimer.stop(); -// if (preFlapSupplier.getAsBoolean()) { -// getBallToPreFlap = false; -// } -// wait = true; -// last = false; -// firstBall = true; -// } -// -// @Override -// public void execute() { -// if (RobotContainer.hardCodedVelocity) { -// FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); -// } else { -// if (RobotContainer.cachedHasTarget) { -// FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); -// } else { -// FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); -// } -// } -// if (wait) { -// FireLog.log("wait", 2000); -// if (RobotContainer.hardCodedVelocity) { -// if (Math.abs(RobotContainer.hardCodedVelocityValue - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { -// wait = false; -// } -// } else { -// if (RobotContainer.cachedHasTarget) { -// if (Math.abs(RobotContainer.cachedSetpointForShooter - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { -// wait = false; -// } -// } else { -// if (Math.abs(RobotContainer.odometryCachedSetpoint - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { -// wait = false; -// } -// } -// } -// SmartDashboard.putString("Saar", "Mama"); -// } else { -// FireLog.log("wait", 4000); -// if (firstBall) { -// conveyor.setPower(Constants.Conveyor.SHOOT_POWER); -// if (preFlapSupplier.getAsBoolean()) { -// firstBall = false; -// } -// } else { -// SmartDashboard.putString("Saar", "Joe"); -// SmartDashboard.putNumber("Saar2", timer.get()); -// -// if (getBallToPreFlap) { -// conveyor.setPower(Constants.Conveyor.SHOOT_POWER); -// } else { -// conveyor.setPower(0); -// } -// -// if (preFlapSupplier.getAsBoolean()) { -// if (!last) { -// last = true; -// getBallToPreFlap = false; -// timer.start(); -// timer.reset(); -// delayTimer.start(); -// delayTimer.reset(); -// } -// } else { -// last = false; -// getBallToPreFlap = true; -// } -// -// if (timer.hasElapsed(0.3)) { -// getBallToPreFlap = true; -// SmartDashboard.putNumber("time", timer.get()); -// timer.reset(); -// timer.stop(); -// } -// } -// } -// } -// -// @Override -// public void end(boolean interrupted) { -// timer.stop(); -// delayTimer.stop(); -// conveyor.setPower(0); -// } -// -// @Override -// public boolean isFinished() { -// return super.isFinished(); -// } -} diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java b/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java deleted file mode 100644 index 121da2f4..00000000 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/QuickReleaseConvey.java +++ /dev/null @@ -1,80 +0,0 @@ -package frc.robot.subsystems.conveyor.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.RobotContainer; -import frc.robot.subsystems.conveyor.Conveyor; -import webapp.FireLog; - -import java.util.function.DoubleSupplier; - -import static frc.robot.Constants.Shooter.SHOOTER_VELOCITY_DEADBAND; - -public class QuickReleaseConvey extends CommandBase { - private final Conveyor conveyor; - private final Timer timer = new Timer(); - private final DoubleSupplier velocitySupplier; - private final Timer delayTimer = new Timer(); - private boolean wait = true; - - public QuickReleaseConvey(Conveyor conveyor, DoubleSupplier velocitySupplier) { - this.conveyor = conveyor; - this.velocitySupplier = velocitySupplier; - addRequirements(conveyor); - } - -// @Override -// public void initialize() { -// timer.stop(); -// delayTimer.stop(); -// wait = true; -// } -// -// @Override -// public void execute() { -// FireLog.log("Shooter velocity", velocitySupplier.getAsDouble()); -// if (RobotContainer.hardCodedVelocity) { -// FireLog.log("Shooter setpoint", RobotContainer.hardCodedVelocityValue); -// } else { -// if (RobotContainer.cachedHasTarget) { -// FireLog.log("Shooter setpoint", RobotContainer.cachedSetpointForShooter); -// } else { -// FireLog.log("Shooter setpoint", RobotContainer.odometryCachedSetpoint); -// } -// } -// if (wait) { -// if (RobotContainer.hardCodedVelocity) { -// if (Math.abs(RobotContainer.hardCodedVelocityValue - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { -// wait = false; -// } -// } else { -// if (RobotContainer.cachedHasTarget) { -// if (Math.abs(RobotContainer.cachedSetpointForShooter - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { -// wait = false; -// } -// } else { -// if (Math.abs(RobotContainer.odometryCachedSetpoint - velocitySupplier.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { -// wait = false; -// } -// } -// } -// SmartDashboard.putString("Saar", "Mama"); -// } else { -// conveyor.setPower(Constants.Conveyor.SHOOT_POWER); -// } -// } -// -// @Override -// public void end(boolean interrupted) { -// timer.stop(); -// delayTimer.stop(); -// conveyor.setPower(0); -// } -// -// @Override -// public boolean isFinished() { -// return super.isFinished(); -// } -} From ba199052dda8717c5a0f0d2d7578de93534719c0 Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 12:03:22 +0300 Subject: [PATCH 05/24] Quick fix to LEDs --- src/main/java/frc/robot/utils/LedSubsystem.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/LedSubsystem.java b/src/main/java/frc/robot/utils/LedSubsystem.java index 5bb34ef5..3c03b423 100644 --- a/src/main/java/frc/robot/utils/LedSubsystem.java +++ b/src/main/java/frc/robot/utils/LedSubsystem.java @@ -14,6 +14,7 @@ public class LedSubsystem extends SubsystemBase { private AddressableLED led; private AddressableLEDBuffer ledBuffer; private int current = 0; + private boolean colorOn = true; public LedSubsystem() { led = new AddressableLED(6); @@ -85,10 +86,14 @@ private void rainbowHood() { public void blink(Color color) { if (blinkTimer.hasElapsed(0.1)) { + blinkTimer.reset(); + colorOn = !colorOn; + } + + if (colorOn) { for (int i = 0; i < ledBuffer.getLength(); i++) { ledBuffer.setLED(i, color); } - blinkTimer.reset(); } else { for (int i = 0; i < ledBuffer.getLength(); i++) { ledBuffer.setLED(i, Color.kBlack); From 47b0ddb56e1628d1e3b4d0852759450aeb93e030 Mon Sep 17 00:00:00 2001 From: eitan Date: Sat, 26 Mar 2022 12:20:04 +0300 Subject: [PATCH 06/24] Reformat code --- src/main/java/frc/robot/auto/Test.java | 2 +- src/main/java/frc/robot/commandgroups/SmartIndexing.java | 2 +- src/main/java/frc/robot/subsystems/conveyor/ColorSensor.java | 2 +- .../subsystems/conveyor/commands/bits/TestColorSensor.java | 1 - .../java/frc/robot/subsystems/hood/commands/HoodCommand.java | 4 +--- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 1 - .../robot/subsystems/shooter/commands/BackAndShootCargo.java | 3 --- .../java/frc/robot/subsystems/shooter/commands/Shoot.java | 4 ---- .../subsystems/shooter/commands/bits/TestShooterVelocity.java | 3 --- .../robot/subsystems/shooter/commands/bits/TryVelocities.java | 3 +-- src/main/java/frc/robot/utils/LedSubsystem.java | 4 ++-- 11 files changed, 7 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/auto/Test.java b/src/main/java/frc/robot/auto/Test.java index b6366800..b101427a 100644 --- a/src/main/java/frc/robot/auto/Test.java +++ b/src/main/java/frc/robot/auto/Test.java @@ -8,7 +8,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.utils.PhotonVisionModule; -public class Test extends SaarIsAutonomous{ +public class Test extends SaarIsAutonomous { public Test(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Meter"); addCommands(followPath("Meter")); diff --git a/src/main/java/frc/robot/commandgroups/SmartIndexing.java b/src/main/java/frc/robot/commandgroups/SmartIndexing.java index fb7389f5..e38b9ab1 100644 --- a/src/main/java/frc/robot/commandgroups/SmartIndexing.java +++ b/src/main/java/frc/robot/commandgroups/SmartIndexing.java @@ -18,9 +18,9 @@ public class SmartIndexing extends CommandBase { private final Intake intake; private final Flap flap; private final DoubleSupplier intakePower; + private final Timer timer = new Timer(); private boolean outtakingShooter = false; private boolean outtakingIntake = false; - private final Timer timer = new Timer(); public SmartIndexing(Shooter shooter, Conveyor conveyor, Intake intake, Flap flap, DoubleSupplier intakePower) { this.shooter = shooter; diff --git a/src/main/java/frc/robot/subsystems/conveyor/ColorSensor.java b/src/main/java/frc/robot/subsystems/conveyor/ColorSensor.java index f82fecc3..03f28eb7 100644 --- a/src/main/java/frc/robot/subsystems/conveyor/ColorSensor.java +++ b/src/main/java/frc/robot/subsystems/conveyor/ColorSensor.java @@ -13,9 +13,9 @@ public class ColorSensor { private final ColorSensorV3 sensor; private final ColorMatch colorMatch = new ColorMatch(); + private final DeadbandProximity proximity; private DriverStation.Alliance lastSeenColor = DriverStation.Alliance.Invalid; private DriverStation.Alliance currentColor = DriverStation.Alliance.Invalid; - private final DeadbandProximity proximity; public ColorSensor(I2C.Port colorSensorPort) { this.sensor = new ColorSensorV3(colorSensorPort); diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/bits/TestColorSensor.java b/src/main/java/frc/robot/subsystems/conveyor/commands/bits/TestColorSensor.java index d374b96f..e704b5a5 100644 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/bits/TestColorSensor.java +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/bits/TestColorSensor.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.conveyor.commands.bits; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java b/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java index 28c45f99..8e0832ed 100644 --- a/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java +++ b/src/main/java/frc/robot/subsystems/hood/commands/HoodCommand.java @@ -2,14 +2,12 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.RobotContainer; import frc.robot.subsystems.hood.Hood; public class HoodCommand extends CommandBase { private final Hood hood; private final Timer timer = new Timer(); - private Hood.Mode mode = Hood.Mode.ShortDistance; + private final Hood.Mode mode = Hood.Mode.ShortDistance; public HoodCommand(Hood hood) { this.hood = hood; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3bd5c8fc..08a7a0ec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.RobotContainer; import frc.robot.subsystems.UnitModel; import frc.robot.utils.Utils; import webapp.FireLog; diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java b/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java index 627deaa7..dd8c3f76 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/BackAndShootCargo.java @@ -1,10 +1,7 @@ package frc.robot.subsystems.shooter.commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.RobotContainer; import frc.robot.subsystems.conveyor.Conveyor; -import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.shooter.Shooter; diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java b/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java index aebdf855..e0cf9886 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/Shoot.java @@ -1,14 +1,10 @@ package frc.robot.subsystems.shooter.commands; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; -import frc.robot.RobotContainer; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.shooter.Shooter; -import webapp.FireLog; import java.util.HashMap; import java.util.OptionalDouble; diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/bits/TestShooterVelocity.java b/src/main/java/frc/robot/subsystems/shooter/commands/bits/TestShooterVelocity.java index 3cb263fb..25eeeb00 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/bits/TestShooterVelocity.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/bits/TestShooterVelocity.java @@ -1,11 +1,8 @@ package frc.robot.subsystems.shooter.commands.bits; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; import frc.robot.subsystems.shooter.Shooter; import frc.robot.utils.LedSubsystem; diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java b/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java index 05cd6dfc..e67889c4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/bits/TryVelocities.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.shooter.commands.bits; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; @@ -21,7 +20,7 @@ public class TryVelocities extends SequentialCommandGroup { private boolean lastButtonRunUnsuccessful; private boolean lastButtonRunSuccessful; private boolean lastButtonShoot; - private boolean addShot = false; + private final boolean addShot = false; private double shooterVelocity; public TryVelocities(Conveyor conveyor, Shooter shooter, Hood hood, Flap flap, diff --git a/src/main/java/frc/robot/utils/LedSubsystem.java b/src/main/java/frc/robot/utils/LedSubsystem.java index 3c03b423..f2d32953 100644 --- a/src/main/java/frc/robot/utils/LedSubsystem.java +++ b/src/main/java/frc/robot/utils/LedSubsystem.java @@ -11,8 +11,8 @@ public class LedSubsystem extends SubsystemBase { private final Timer timer = new Timer(); private final Timer blinkTimer = new Timer(); private int m_rainbowFirstPixelHue = 0; - private AddressableLED led; - private AddressableLEDBuffer ledBuffer; + private final AddressableLED led; + private final AddressableLEDBuffer ledBuffer; private int current = 0; private boolean colorOn = true; From e159e83a5d1afa9f8d7a07c769438186479b410a Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 16:36:17 +0300 Subject: [PATCH 07/24] Update zero positions --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 879557bb..dbce7389 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,7 +155,7 @@ public static final class SwerveModule { public static final double TRIGGER_THRESHOLD_TIME = 0.02; // [secs] public static final double RAMP_RATE = 0; // seconds from neutral to max - public static final int[] ZERO_POSITIONS = {-589, -430, -868, -519}; // fr, fl, rr, rl + public static final int[] ZERO_POSITIONS = {-582, -1451, -866, -493}; // fr, fl, rr, rl public static final SwerveModuleConfigBase frConfig = new SwerveModuleConfigBase.Builder(0) .configPorts(DRIVE_MOTOR_FR, ANGLE_MOTOR_FR) From 78e3c9c9d4eefe8d2e87d4030937e7c824205148 Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 17:54:32 +0300 Subject: [PATCH 08/24] Add paths --- src/main/deploy/pathplanner/Four- first shooting position.path | 1 + src/main/deploy/pathplanner/Four- go to terminal.path | 1 + src/main/deploy/pathplanner/Four- leave low left.path | 1 + src/main/deploy/pathplanner/Four- pickup from terminal.path | 1 + src/main/deploy/pathplanner/Four- second shooting position.path | 1 + src/main/deploy/pathplanner/p0- leave tarmac up low.path | 1 + 6 files changed, 6 insertions(+) create mode 100644 src/main/deploy/pathplanner/Four- first shooting position.path create mode 100644 src/main/deploy/pathplanner/Four- go to terminal.path create mode 100644 src/main/deploy/pathplanner/Four- leave low left.path create mode 100644 src/main/deploy/pathplanner/Four- pickup from terminal.path create mode 100644 src/main/deploy/pathplanner/Four- second shooting position.path create mode 100644 src/main/deploy/pathplanner/p0- leave tarmac up low.path diff --git a/src/main/deploy/pathplanner/Four- first shooting position.path b/src/main/deploy/pathplanner/Four- first shooting position.path new file mode 100644 index 00000000..6c9f5deb --- /dev/null +++ b/src/main/deploy/pathplanner/Four- first shooting position.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":5.59,"y":2.06},"prevControl":null,"nextControl":{"x":5.68380749106099,"y":2.101047099251879},"holonomicAngle":-164.29,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.777894753114187,"y":1.71425289653302},"prevControl":{"x":5.271566301321153,"y":1.8720242160630813},"nextControl":null,"holonomicAngle":15.762147763972642,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Four- go to terminal.path b/src/main/deploy/pathplanner/Four- go to terminal.path new file mode 100644 index 00000000..e40cec06 --- /dev/null +++ b/src/main/deploy/pathplanner/Four- go to terminal.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":4.78,"y":1.71},"prevControl":null,"nextControl":{"x":4.121362487972965,"y":1.5513921795987642},"holonomicAngle":15.76,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.7115994070108047,"y":1.5513921795987642},"prevControl":{"x":3.6327803371701948,"y":1.5666603718113503},"nextControl":null,"holonomicAngle":-155.72555886556043,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Four- leave low left.path b/src/main/deploy/pathplanner/Four- leave low left.path new file mode 100644 index 00000000..887fd917 --- /dev/null +++ b/src/main/deploy/pathplanner/Four- leave low left.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.6656763466332585,"y":2.6405468074855807},"prevControl":null,"nextControl":{"x":6.05876943741271,"y":2.3277563235026832},"holonomicAngle":-135.31654804727307,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.5872494540952085,"y":2.0616509863829044},"prevControl":{"x":5.582580939408897,"y":2.0523139570102797},"nextControl":null,"holonomicAngle":-164.29136217098434,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Four- pickup from terminal.path b/src/main/deploy/pathplanner/Four- pickup from terminal.path new file mode 100644 index 00000000..86740841 --- /dev/null +++ b/src/main/deploy/pathplanner/Four- pickup from terminal.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":2.71,"y":1.55},"prevControl":null,"nextControl":{"x":2.36552038352551,"y":1.429246641898072},"holonomicAngle":-155.73,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.5613955936626176,"y":1.5615709744071544},"prevControl":{"x":1.3323727104738194,"y":1.1849555664966847},"nextControl":null,"holonomicAngle":-138.53955916854994,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Four- second shooting position.path b/src/main/deploy/pathplanner/Four- second shooting position.path new file mode 100644 index 00000000..674e5e1d --- /dev/null +++ b/src/main/deploy/pathplanner/Four- second shooting position.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":1.56,"y":1.56},"prevControl":null,"nextControl":{"x":2.56,"y":1.56},"holonomicAngle":-138.54,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.963724348291743,"y":1.5004982055568075},"prevControl":{"x":4.963724348291743,"y":1.5004982055568075},"nextControl":null,"holonomicAngle":38.49104355949754,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/p0- leave tarmac up low.path b/src/main/deploy/pathplanner/p0- leave tarmac up low.path new file mode 100644 index 00000000..832e5f07 --- /dev/null +++ b/src/main/deploy/pathplanner/p0- leave tarmac up low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.0865374516672786,"y":3.962765462187816},"prevControl":null,"nextControl":{"x":4.942994427384167,"y":4.055106757438087},"holonomicAngle":1.134421630977088,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.055976636984904,"y":4.08778636024227},"prevControl":{"x":5.0783813532872095,"y":4.050438242751776},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file From 6ad7776ca6eb039c0627ee7f6609749633e66e1f Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 17:54:47 +0300 Subject: [PATCH 09/24] Add auto path --- .../java/frc/robot/auto/StraightLineFour.java | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 src/main/java/frc/robot/auto/StraightLineFour.java diff --git a/src/main/java/frc/robot/auto/StraightLineFour.java b/src/main/java/frc/robot/auto/StraightLineFour.java new file mode 100644 index 00000000..550278d5 --- /dev/null +++ b/src/main/java/frc/robot/auto/StraightLineFour.java @@ -0,0 +1,27 @@ +package frc.robot.auto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.flap.Flap; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.utils.PhotonVisionModule; + +public class StraightLineFour extends SaarIsAutonomous { + + public StraightLineFour(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { + super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, ""); + + addCommands( + followPathAndPickup("Four- leave low left"), + followPath("Four- first shooting position"), + shootAndAdjust(3), + followPath("Four- go to terminal"), + followPathAndPickup("Four- pickup from terminal"), + followPath("Four- second shooting position"), + shootAndAdjust(3) + ); + } +} From ad97c3c7c95730101d9ac1d17dce04fb4537d78d Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 18:18:44 +0300 Subject: [PATCH 10/24] Add warmup function --- src/main/java/frc/robot/auto/SaarIsAutonomous.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/auto/SaarIsAutonomous.java b/src/main/java/frc/robot/auto/SaarIsAutonomous.java index 0ba95337..788c8aba 100644 --- a/src/main/java/frc/robot/auto/SaarIsAutonomous.java +++ b/src/main/java/frc/robot/auto/SaarIsAutonomous.java @@ -73,8 +73,7 @@ protected CommandBase followPathAndPickup(String path) { return new ParallelRaceGroup( followPath(path), pickup(10), - new RunCommand(() -> shooter.setVelocity(3400), shooter) - ); + warmUp(3400)); } protected CommandBase shootAndAdjust(double timeout) { @@ -182,4 +181,11 @@ protected CommandBase turnToAngle(Supplier target) { target ); } + + protected CommandBase warmUp(double velocity) { + return new RunCommand( + () -> shooter.setVelocity(velocity), shooter + ); + } + } From c2acd5a9c91fc55ca13e8b483e54bc7cdfeef2d7 Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 18:20:24 +0300 Subject: [PATCH 11/24] Change name --- src/main/java/frc/robot/auto/SaarIsAutonomous.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/auto/SaarIsAutonomous.java b/src/main/java/frc/robot/auto/SaarIsAutonomous.java index 788c8aba..b78d2fd7 100644 --- a/src/main/java/frc/robot/auto/SaarIsAutonomous.java +++ b/src/main/java/frc/robot/auto/SaarIsAutonomous.java @@ -73,7 +73,7 @@ protected CommandBase followPathAndPickup(String path) { return new ParallelRaceGroup( followPath(path), pickup(10), - warmUp(3400)); + warmup(3400)); } protected CommandBase shootAndAdjust(double timeout) { @@ -182,7 +182,7 @@ protected CommandBase turnToAngle(Supplier target) { ); } - protected CommandBase warmUp(double velocity) { + protected CommandBase warmup(double velocity) { return new RunCommand( () -> shooter.setVelocity(velocity), shooter ); From 060df3c8313b4694e271fa98a2efa325aa516b8e Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 18:23:35 +0300 Subject: [PATCH 12/24] Change path --- .../java/frc/robot/auto/StraightLineFour.java | 44 ++++++++++++++++--- 1 file changed, 37 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/auto/StraightLineFour.java b/src/main/java/frc/robot/auto/StraightLineFour.java index 550278d5..c1d384d0 100644 --- a/src/main/java/frc/robot/auto/StraightLineFour.java +++ b/src/main/java/frc/robot/auto/StraightLineFour.java @@ -1,6 +1,6 @@ package frc.robot.auto; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.drivetrain.SwerveDrive; import frc.robot.subsystems.flap.Flap; @@ -12,16 +12,46 @@ public class StraightLineFour extends SaarIsAutonomous { public StraightLineFour(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { - super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, ""); + super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Four- leave low left"); addCommands( - followPathAndPickup("Four- leave low left"), + followPathAndPickup("Four- leave low left") + ); + + addCommands( + pickup(2) + ); + + addCommands(new ParallelRaceGroup( followPath("Four- first shooting position"), - shootAndAdjust(3), - followPath("Four- go to terminal"), - followPathAndPickup("Four- pickup from terminal"), - followPath("Four- second shooting position"), + warmup(3400) + )); + + addCommands( shootAndAdjust(3) ); + + addCommands( + followPath("Four- go to terminal") + ); + + addCommands( + followPathAndPickup("Four- pickup from terminal") + ); + + addCommands( + pickup(2) + ); + + addCommands( + new ParallelRaceGroup( + warmup(3400), + followPath("Four- second shooting position") + )); + + addCommands( + shootAndAdjust(3) + ); + } } From d0ee300cd4060264462bd9bf8eebb37b033cbb14 Mon Sep 17 00:00:00 2001 From: Alon Date: Sat, 26 Mar 2022 18:33:55 +0300 Subject: [PATCH 13/24] add fade and (5,6) ball --- .../deploy/pathplanner/Disappeare #1.path | 1 + .../deploy/pathplanner/Disappeare #2.1.path | 1 + .../deploy/pathplanner/Disappeare #2.2.path | 1 + .../From Tarmac to (5,6) ball.path | 1 + src/main/java/frc/robot/auto/Disappear1.java | 27 ++++++++++++++++ src/main/java/frc/robot/auto/Disappear2.java | 28 +++++++++++++++++ .../frc/robot/auto/FromTarmacTo56Ball.java | 31 +++++++++++++++++++ 7 files changed, 90 insertions(+) create mode 100644 src/main/deploy/pathplanner/Disappeare #1.path create mode 100644 src/main/deploy/pathplanner/Disappeare #2.1.path create mode 100644 src/main/deploy/pathplanner/Disappeare #2.2.path create mode 100644 src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path create mode 100644 src/main/java/frc/robot/auto/Disappear1.java create mode 100644 src/main/java/frc/robot/auto/Disappear2.java create mode 100644 src/main/java/frc/robot/auto/FromTarmacTo56Ball.java diff --git a/src/main/deploy/pathplanner/Disappeare #1.path b/src/main/deploy/pathplanner/Disappeare #1.path new file mode 100644 index 00000000..40d8cd49 --- /dev/null +++ b/src/main/deploy/pathplanner/Disappeare #1.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.720378838681553,"y":5.638283940927743},"prevControl":null,"nextControl":{"x":6.734616929441472,"y":5.638283940927743},"holonomicAngle":134.99999999999991,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.666760122447581,"y":7.375331013637806},"prevControl":{"x":5.6525220316876625,"y":7.375331013637806},"nextControl":null,"holonomicAngle":-54.86580694308438,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Disappeare #2.1.path b/src/main/deploy/pathplanner/Disappeare #2.1.path new file mode 100644 index 00000000..fab14998 --- /dev/null +++ b/src/main/deploy/pathplanner/Disappeare #2.1.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.108140936005055,"y":4.1290463203763785},"prevControl":null,"nextControl":{"x":7.108140936005055,"y":4.1290463203763785},"holonomicAngle":1.5074357587749554,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.086332048096622,"y":4.029379685056948},"prevControl":{"x":4.086332048096622,"y":4.01514159429703},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Disappeare #2.2.path b/src/main/deploy/pathplanner/Disappeare #2.2.path new file mode 100644 index 00000000..9d55dd58 --- /dev/null +++ b/src/main/deploy/pathplanner/Disappeare #2.2.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":4.072093957336704,"y":3.9439511404974366},"prevControl":null,"nextControl":{"x":4.086332048096622,"y":3.9439511404974366},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.309904349912506,"y":4.0578558665767845},"prevControl":{"x":1.2956662591525876,"y":4.0578558665767845},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path b/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path new file mode 100644 index 00000000..89442246 --- /dev/null +++ b/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.207807571324485,"y":5.139950764330594},"prevControl":null,"nextControl":{"x":6.207807571324485,"y":5.139950764330594},"holonomicAngle":135.97102193107926,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.3819983072492095,"y":5.851855302326522},"prevControl":{"x":5.396236398009128,"y":5.837617211566603},"nextControl":null,"holonomicAngle":136.58235392261864,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc/robot/auto/Disappear1.java b/src/main/java/frc/robot/auto/Disappear1.java new file mode 100644 index 00000000..ef46e9e0 --- /dev/null +++ b/src/main/java/frc/robot/auto/Disappear1.java @@ -0,0 +1,27 @@ +package frc.robot.auto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.flap.Flap; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.commands.Shoot; +import frc.robot.utils.PhotonVisionModule; + +public class Disappear1 extends SaarIsAutonomous { + public Disappear1(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { + super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Disappeare #1"); + addCommands( + new ParallelRaceGroup( + followPath("Disappeare #1"), + new RunCommand(() -> shooter.setVelocity(Shoot.getSetpointVelocity(Math.hypot(Constants.Vision.HUB_POSE.getX() - 5.67, Constants.Vision.HUB_POSE.getY() - 7.38))))) + ); + addCommands(shootAndAdjust(5)); + } +} diff --git a/src/main/java/frc/robot/auto/Disappear2.java b/src/main/java/frc/robot/auto/Disappear2.java new file mode 100644 index 00000000..c3ef39fc --- /dev/null +++ b/src/main/java/frc/robot/auto/Disappear2.java @@ -0,0 +1,28 @@ +package frc.robot.auto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.flap.Flap; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.commands.Shoot; +import frc.robot.utils.PhotonVisionModule; + +public class Disappear2 extends SaarIsAutonomous { + public Disappear2(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { + super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Disappeare #2.2"); + addCommands( + new ParallelRaceGroup( + followPathAndPickup("Disappeare #2.1"), + new RunCommand(() -> shooter.setVelocity(Shoot.getSetpointVelocity(Math.hypot(Constants.Vision.HUB_POSE.getX() - 4.09, Constants.Vision.HUB_POSE.getY() - 4.03)))))); + addCommands(shootAndAdjust(5)); + addCommands(followPath("Disappeare #2.2")); + } + +} diff --git a/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java b/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java new file mode 100644 index 00000000..bde86637 --- /dev/null +++ b/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java @@ -0,0 +1,31 @@ +package frc.robot.auto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants; +import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.drivetrain.commands.TurnToAngle; +import frc.robot.subsystems.flap.Flap; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.commands.Shoot; +import frc.robot.utils.PhotonVisionModule; + +public class FromTarmacTo56Ball extends SaarIsAutonomous { + public FromTarmacTo56Ball(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule, String initialPathPath) { + super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, initialPathPath); + addCommands(followPath("From Tarmac to (5,6) ball")); + + addCommands( + new ParallelRaceGroup( + turnToAngle(() -> Rotation2d.fromDegrees(-45)), + new RunCommand(() -> shooter.setVelocity(Shoot.getSetpointVelocity(Math.hypot(Constants.Vision.HUB_POSE.getX() - 5.38, Constants.Vision.HUB_POSE.getY() - 5.85)))) + ) + ); + addCommands(shootAndAdjust(2)); + } +} From 33033095f31c35582c859781332eeaf2ab7e5c05 Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 18:36:46 +0300 Subject: [PATCH 14/24] Change path --- src/main/deploy/pathplanner/Four- go to terminal.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/Four- go to terminal.path b/src/main/deploy/pathplanner/Four- go to terminal.path index e40cec06..2fdb35c7 100644 --- a/src/main/deploy/pathplanner/Four- go to terminal.path +++ b/src/main/deploy/pathplanner/Four- go to terminal.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":4.78,"y":1.71},"prevControl":null,"nextControl":{"x":4.121362487972965,"y":1.5513921795987642},"holonomicAngle":15.76,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.7115994070108047,"y":1.5513921795987642},"prevControl":{"x":3.6327803371701948,"y":1.5666603718113503},"nextControl":null,"holonomicAngle":-155.72555886556043,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":4.78,"y":1.71},"prevControl":null,"nextControl":{"x":4.121362487972965,"y":1.5513921795987642},"holonomicAngle":15.76,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.5919319780877905,"y":1.4801406159400265},"prevControl":{"x":2.513112908247183,"y":1.495408808152613},"nextControl":null,"holonomicAngle":-142.56839702158936,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file From 37c06f232aa97bf82aa3dc37baf035904df738e5 Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 18:36:59 +0300 Subject: [PATCH 15/24] Change path --- src/main/deploy/pathplanner/Four- second shooting position.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/Four- second shooting position.path b/src/main/deploy/pathplanner/Four- second shooting position.path index 674e5e1d..2f8fd123 100644 --- a/src/main/deploy/pathplanner/Four- second shooting position.path +++ b/src/main/deploy/pathplanner/Four- second shooting position.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":1.56,"y":1.56},"prevControl":null,"nextControl":{"x":2.56,"y":1.56},"holonomicAngle":-138.54,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.963724348291743,"y":1.5004982055568075},"prevControl":{"x":4.963724348291743,"y":1.5004982055568075},"nextControl":null,"holonomicAngle":38.49104355949754,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":1.59,"y":1.48},"prevControl":null,"nextControl":{"x":2.59,"y":1.48},"holonomicAngle":-142.57,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.963724348291743,"y":1.5004982055568075},"prevControl":{"x":4.963724348291743,"y":1.5004982055568075},"nextControl":null,"holonomicAngle":38.49104355949754,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file From 3fae4ee86430b4dbdb933d0c21db8ff87be0f46b Mon Sep 17 00:00:00 2001 From: Gaia Zano Date: Sat, 26 Mar 2022 18:37:11 +0300 Subject: [PATCH 16/24] Change path --- src/main/java/frc/robot/auto/StraightLineFour.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/auto/StraightLineFour.java b/src/main/java/frc/robot/auto/StraightLineFour.java index c1d384d0..e43c2021 100644 --- a/src/main/java/frc/robot/auto/StraightLineFour.java +++ b/src/main/java/frc/robot/auto/StraightLineFour.java @@ -32,11 +32,7 @@ public StraightLineFour(SwerveDrive swerveDrive, Shooter shooter, Conveyor conve ); addCommands( - followPath("Four- go to terminal") - ); - - addCommands( - followPathAndPickup("Four- pickup from terminal") + followPathAndPickup("Four- go to terminal") ); addCommands( @@ -50,7 +46,7 @@ public StraightLineFour(SwerveDrive swerveDrive, Shooter shooter, Conveyor conve )); addCommands( - shootAndAdjust(3) + shootAndAdjust(5) ); } From dd83b4bf69aadd1b41efc5407f343cb9a73c2936 Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 18:43:12 +0300 Subject: [PATCH 17/24] Working 4 ball --- .../deploy/pathplanner/Alon 4 ball #2.path | 2 +- .../deploy/pathplanner/Alon 4 ball #3.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/auto/FourBallAuto.java | 23 +++-- .../java/frc/robot/auto/SaarIsAutonomous.java | 16 +++- .../robot/commandgroups/OdometryAdjust.java | 10 +-- .../OneButtonAdjustAndShoot.java | 6 +- .../conveyor/commands/OldConvey.java | 85 +++++++++++++++++++ .../conveyor/commands/OldConveyNoWait.java | 76 +++++++++++++++++ .../subsystems/drivetrain/SwerveDrive.java | 28 ++++-- .../commands/AdjustToTargetOnCommand.java | 6 +- .../drivetrain/commands/OverpoweredDrive.java | 6 +- .../frc/robot/subsystems/shooter/Shooter.java | 6 +- .../frc/robot/utils/PhotonVisionModule.java | 4 +- src/test/java/Test.java | 14 ++- 15 files changed, 251 insertions(+), 40 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java create mode 100644 src/main/java/frc/robot/subsystems/conveyor/commands/OldConveyNoWait.java diff --git a/src/main/deploy/pathplanner/Alon 4 ball #2.path b/src/main/deploy/pathplanner/Alon 4 ball #2.path index 2fbaab34..12b0e712 100644 --- a/src/main/deploy/pathplanner/Alon 4 ball #2.path +++ b/src/main/deploy/pathplanner/Alon 4 ball #2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.68,"y":0.81},"prevControl":null,"nextControl":{"x":7.592110958862685,"y":2.057404114812666},"holonomicAngle":79.28,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.727465936471339,"y":1.72113244051184},"prevControl":{"x":7.014699442397051,"y":2.02486169471903},"nextControl":{"x":4.631195615678799,"y":1.4624619153810163},"holonomicAngle":-165.76271953423893,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.3306233993767749,"y":0.5351420193218593},"prevControl":{"x":2.6467835009412655,"y":1.1859904211944103},"nextControl":null,"holonomicAngle":-141.95295746817393,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.68,"y":0.81},"prevControl":null,"nextControl":{"x":7.592110958862685,"y":2.057404114812666},"holonomicAngle":79.28,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.727465936471339,"y":1.72113244051184},"prevControl":{"x":7.014699442397051,"y":2.02486169471903},"nextControl":{"x":4.631195615678799,"y":1.4624619153810163},"holonomicAngle":-165.76271953423893,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.1097731477424426,"y":1.0665352328953341},"prevControl":{"x":2.551984847157085,"y":1.4842748908264891},"nextControl":null,"holonomicAngle":-141.95295746817393,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Alon 4 ball #3.path b/src/main/deploy/pathplanner/Alon 4 ball #3.path index a94098b1..f499c54c 100644 --- a/src/main/deploy/pathplanner/Alon 4 ball #3.path +++ b/src/main/deploy/pathplanner/Alon 4 ball #3.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":1.33,"y":0.54},"prevControl":null,"nextControl":{"x":2.2845776560797413,"y":0.9594356367623105},"holonomicAngle":-139.67,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.268093581171338,"y":1.3668567129531706},"prevControl":{"x":4.268093581171338,"y":1.3668567129531706},"nextControl":null,"holonomicAngle":40.914383220025144,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":4.0,"maxAcceleration":2.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":1.11,"y":1.07},"prevControl":null,"nextControl":{"x":2.1909478711777037,"y":1.8194571906832087},"holonomicAngle":-139.67,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.268093581171338,"y":1.3668567129531706},"prevControl":{"x":4.268093581171338,"y":1.3668567129531706},"nextControl":null,"holonomicAngle":40.914383220025144,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":4.0,"maxAcceleration":2.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1142dfce..3ae43c9f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,7 +107,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { { // Xbox controller button bindings. - Xbox.lt.whileActiveContinuous(new PickUpCargo(conveyor, flap, intake, 0.7, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.25, 0.1))); + Xbox.lt.whileActiveContinuous(new PickUpCargo(conveyor, flap, intake, 0.7, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.4, 0.25))); Xbox.lb.whileHeld(new Outtake(intake, conveyor, flap, shooter, hood, () -> false)); Xbox.rb.whileHeld(new Convey(conveyor, -Constants.Conveyor.SHOOT_POWER)); @@ -133,12 +133,15 @@ private void configureButtonBindings() { thetaMultiplier = 1.5 * speedMultiplier; }); - Joysticks.rightTrigger.whileHeld(new OneButtonAdjustAndShoot(swerve, conveyor, flap, hood)).whenReleased( + Joysticks.rightTrigger.whileActiveOnce(new OneButtonAdjustAndShoot(swerve, conveyor, flap, hood)).whenInactive( () -> { shooting = false; ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.STATIC); } ); +// Joysticks.rightTrigger.whileActiveOnce( +// new OdometryAdjust(swerve) +// ); Joysticks.leftTwo.whenPressed((Runnable) Robot::resetAngle); Joysticks.rightTwo.whileHeld(new TurnToAngle(swerve, Rotation2d::new)); diff --git a/src/main/java/frc/robot/auto/FourBallAuto.java b/src/main/java/frc/robot/auto/FourBallAuto.java index 173805d1..3194d1d2 100644 --- a/src/main/java/frc/robot/auto/FourBallAuto.java +++ b/src/main/java/frc/robot/auto/FourBallAuto.java @@ -1,7 +1,9 @@ package frc.robot.auto; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; @@ -15,18 +17,27 @@ public class FourBallAuto extends SaarIsAutonomous { public FourBallAuto(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Alon 4 ball #1"); - addCommands(reachVelocityByDistance(3.35)); + addRequirements(shooter); + addCommands(reachVelocityByDistance(3.25)); + addCommands(new InstantCommand(intake::openRetractor)); + addCommands(new WaitCommand(0.1)); addCommands(followPathAndPickup("Alon 4 ball #1")); addCommands(new ParallelDeadlineGroup( turnToAngle(() -> Rotation2d.fromDegrees(79.28)), new Convey(conveyor, -0.25).withTimeout(0.075) )); addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); - addCommands(confirmShooting().withTimeout(2)); - addCommands(reachVelocityByDistance(4.03)); + addCommands(new InstantCommand((flap::allowShooting))); + addCommands(confirmShootingSlower().withTimeout(1.7)); + addCommands(reachVelocityByDistance(4)); + addCommands(new InstantCommand((flap::blockShooter))); addCommands(followPathAndPickup("Alon 4 ball #2")); - addCommands(followPath("Alon 4 ball #3")); - addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); - addCommands(confirmShooting()); + addCommands(followPathAndPickup("Alon 4 ball #3")); + addCommands(new ParallelDeadlineGroup( + new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3), + new Convey(conveyor, -0.25).withTimeout(0.075) + )); + addCommands(new InstantCommand((flap::allowShooting))); + addCommands(confirmShootingSlower()); } } diff --git a/src/main/java/frc/robot/auto/SaarIsAutonomous.java b/src/main/java/frc/robot/auto/SaarIsAutonomous.java index 9a273477..7620a3c6 100644 --- a/src/main/java/frc/robot/auto/SaarIsAutonomous.java +++ b/src/main/java/frc/robot/auto/SaarIsAutonomous.java @@ -6,12 +6,16 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; import frc.robot.Robot; import frc.robot.commandgroups.PickUpCargo; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.ConveyCargo; +import frc.robot.subsystems.conveyor.commands.OldConveyNoWait; import frc.robot.subsystems.drivetrain.SwerveDrive; import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand; import frc.robot.subsystems.drivetrain.commands.TurnToAngle; @@ -68,8 +72,8 @@ protected FollowPath followPath(String path) { protected CommandBase followPathAndPickup(String path) { return new ParallelRaceGroup( followPath(path), - pickup(10), - new RunCommand(() -> shooter.setVelocity(3400), shooter) + pickup(10) +// new RunCommand(() -> shooter.setVelocity(3400), shooter) ); } @@ -123,7 +127,7 @@ protected CommandBase pickup(double timeout) { flap, intake, Constants.Conveyor.DEFAULT_POWER.get(), - () -> Utils.map(MathUtil.clamp(Math.hypot(swerveDrive.getChassisSpeeds().vxMetersPerSecond, swerveDrive.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.3, 0.2) + () -> Utils.map(MathUtil.clamp(Math.hypot(swerveDrive.getChassisSpeeds().vxMetersPerSecond, swerveDrive.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.4, 0.25) ).withTimeout(timeout); } @@ -144,5 +148,9 @@ protected CommandBase confirmShooting() { return new ConveyCargo(conveyor); } + protected CommandBase confirmShootingSlower() { + return new OldConveyNoWait(conveyor, () -> !conveyor.isPreFlapBeamConnected()); + } + } diff --git a/src/main/java/frc/robot/commandgroups/OdometryAdjust.java b/src/main/java/frc/robot/commandgroups/OdometryAdjust.java index dbabc6e9..d155ae09 100644 --- a/src/main/java/frc/robot/commandgroups/OdometryAdjust.java +++ b/src/main/java/frc/robot/commandgroups/OdometryAdjust.java @@ -9,7 +9,7 @@ public class OdometryAdjust extends CommandBase { private final SwerveDrive swerveDrive; - private final PIDController adjustController = new PIDController(Constants.SwerveDrive.ADJUST_CONTROLLER_KP.get(), 0, 0) {{ + private final PIDController adjustController = new PIDController(8, 0, 0) {{ enableContinuousInput(-Math.PI, Math.PI); setTolerance(Constants.SwerveDrive.ADJUST_CONTROLLER_TOLERANCE); }}; @@ -18,21 +18,21 @@ public class OdometryAdjust extends CommandBase { public OdometryAdjust(SwerveDrive swerveDrive) { this.swerveDrive = swerveDrive; - target = Robot.getAngle(); + target = new Rotation2d(); addRequirements(swerveDrive); } @Override public void initialize() { - var robotAngle = Robot.getAngle(); var robotPose = swerveDrive.getPose().getTranslation(); var hubPose = Constants.Vision.HUB_POSE.getTranslation(); var poseRelativeToTarget = hubPose.minus(robotPose); - target = robotAngle.plus(new Rotation2d( + var value = new Rotation2d( Math.atan2( poseRelativeToTarget.getY(), poseRelativeToTarget.getX() - ))); + )); + target = value; } @Override diff --git a/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java b/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java index 8265bc24..ba88bad0 100644 --- a/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java +++ b/src/main/java/frc/robot/commandgroups/OneButtonAdjustAndShoot.java @@ -14,7 +14,6 @@ public class OneButtonAdjustAndShoot extends SequentialCommandGroup { public OneButtonAdjustAndShoot(SwerveDrive swerve, Conveyor conveyor, Flap flap, Hood hood) { - addCommands(new InstantCommand(flap::allowShooting)); addCommands(new InstantCommand(() -> RobotContainer.ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.ODOMETRY_ADJUST))); // LEDs @@ -22,12 +21,12 @@ public OneButtonAdjustAndShoot(SwerveDrive swerve, Conveyor conveyor, Flap flap, new Convey(conveyor, -0.25).withTimeout(0.075), //if (!RobotContainer.hasTarget.getAsBoolean()) new OdometryAdjust(swerve) - ).withTimeout(1)); + ).withTimeout(1.5)); addCommands(new InstantCommand(() -> RobotContainer.ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.VISION_ADJUST))); // LEDs // if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) - addCommands(new VisionAdjust(swerve).withTimeout(0.5)); + addCommands(new VisionAdjust(swerve).withTimeout(0.75)); addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); // if (RobotContainer.hardCodedVelocity) // addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.hardCodedDistance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); @@ -39,6 +38,7 @@ public OneButtonAdjustAndShoot(SwerveDrive swerve, Conveyor conveyor, Flap flap, // } addCommands(new InstantCommand(() -> RobotContainer.ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.SHOOTING))); // LEDs + addCommands(new InstantCommand(flap::allowShooting)); addCommands(new ConveyForShooting(conveyor)); } diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java new file mode 100644 index 00000000..fff8574d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java @@ -0,0 +1,85 @@ +package frc.robot.subsystems.conveyor.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.subsystems.conveyor.Conveyor; + +import java.util.function.BooleanSupplier; + +import static frc.robot.Constants.Shooter.SHOOTER_VELOCITY_DEADBAND; + +public class OldConvey extends CommandBase { + private final Conveyor conveyor; + private final BooleanSupplier preFlapSupplier; + private final Timer timer = new Timer(); + private final Timer delayTimer = new Timer(); + private boolean last = false; + private boolean getBallToPreFlap = true; + private boolean wait = true; + + public OldConvey(Conveyor conveyor, BooleanSupplier preFlapSupplier) { + this.conveyor = conveyor; + this.preFlapSupplier = preFlapSupplier; + addRequirements(conveyor); + } + + @Override + public void initialize() { + timer.stop(); + delayTimer.stop(); + if (preFlapSupplier.getAsBoolean()) { + getBallToPreFlap = false; + } + wait = true; + last = false; + } + + @Override + public void execute() { + if (wait) { + if (Math.abs(RobotContainer.setpointVelocity - RobotContainer.Suppliers.shooterVelocity.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { + wait = false; + } + } else { + if (getBallToPreFlap) { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + } else { + conveyor.setPower(0); + } + + if (preFlapSupplier.getAsBoolean()) { + if (!last) { + last = true; + getBallToPreFlap = false; + timer.start(); + timer.reset(); + delayTimer.start(); + delayTimer.reset(); + } + } else { + last = false; + getBallToPreFlap = true; + } + + if (timer.hasElapsed(0.3)) { + getBallToPreFlap = true; + timer.reset(); + timer.stop(); + } + } + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + delayTimer.stop(); + conveyor.setPower(0); + } + + @Override + public boolean isFinished() { + return super.isFinished(); + } +} diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/OldConveyNoWait.java b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConveyNoWait.java new file mode 100644 index 00000000..259289dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConveyNoWait.java @@ -0,0 +1,76 @@ +package frc.robot.subsystems.conveyor.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.conveyor.Conveyor; + +import java.util.function.BooleanSupplier; + +public class OldConveyNoWait extends CommandBase { + private final Conveyor conveyor; + private final BooleanSupplier preFlapSupplier; + private final Timer timer = new Timer(); + private final Timer delayTimer = new Timer(); + private boolean last = false; + private boolean getBallToPreFlap = true; + private boolean wait = true; + + public OldConveyNoWait(Conveyor conveyor, BooleanSupplier preFlapSupplier) { + this.conveyor = conveyor; + this.preFlapSupplier = preFlapSupplier; + addRequirements(conveyor); + } + + @Override + public void initialize() { + timer.stop(); + delayTimer.stop(); + if (preFlapSupplier.getAsBoolean()) { + getBallToPreFlap = false; + } + wait = true; + last = false; + } + + @Override + public void execute() { + if (getBallToPreFlap) { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + } else { + conveyor.setPower(0); + } + + if (preFlapSupplier.getAsBoolean()) { + if (!last) { + last = true; + getBallToPreFlap = false; + timer.start(); + timer.reset(); + delayTimer.start(); + delayTimer.reset(); + } + } else { + last = false; + getBallToPreFlap = true; + } + + if (timer.hasElapsed(0.3)) { + getBallToPreFlap = true; + timer.reset(); + timer.stop(); + } + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + delayTimer.stop(); + conveyor.setPower(0); + } + + @Override + public boolean isFinished() { + return super.isFinished(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 2bb3ba99..72759c4e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -33,6 +33,7 @@ public class SwerveDrive extends SubsystemBase { private final SwerveModule[] modules = new SwerveModule[4]; private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(Constants.SwerveDrive.SWERVE_POSITIONS); private final SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, new Rotation2d()); + private final SwerveDriveOdometry odometryForDistance = new SwerveDriveOdometry(kinematics, new Rotation2d()); private final ProfiledPIDController headingController = new ProfiledPIDController( Constants.SwerveDrive.HEADING_KP, @@ -244,6 +245,10 @@ public Pose2d getPose() { return odometry.getPoseMeters(); } + public Pose2d getDistanceOdometryPose() { + return odometryForDistance.getPoseMeters(); + } + /** * Resets the odometry. */ @@ -260,6 +265,11 @@ public void resetOdometry(Pose2d pose, Rotation2d angle) { odometry.resetPosition(new Pose2d(pose.getTranslation(), angle), angle); } + + public void resetDistanceOdometry(Pose2d pose, Rotation2d angle) { + odometryForDistance.resetPosition(new Pose2d(pose.getTranslation(), angle), angle); + } + /** * Resets the heading controller target angle. */ @@ -315,7 +325,7 @@ public void lock() { * @return distance from hub. [m] */ public double getOdometryDistance() { - return Constants.Vision.HUB_POSE.getTranslation().minus(getPose().getTranslation()).getNorm(); + return Constants.Vision.HUB_POSE.getTranslation().minus(getDistanceOdometryPose().getTranslation()).getNorm(); } @Override @@ -326,6 +336,12 @@ public void periodic() { getStates() ); + odometryForDistance.updateWithTime( + Timer.getFPGATimestamp(), + Robot.getAngle(), + getStates() + ); + String outputRotation = String.valueOf(Robot.getAngle().getDegrees()); SmartDashboard.putString("robot_rotation", outputRotation); @@ -343,19 +359,19 @@ public void periodic() { // } if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision && !DriverStation.isAutonomous()) { - if (Math.abs(RobotContainer.Suppliers.yawSupplier.getAsDouble()) < 6) { - var odom = odometry.getPoseMeters().getTranslation(); + if (Math.abs(RobotContainer.Suppliers.yawSupplier.getAsDouble()) < 10) { + var odom = odometryForDistance.getPoseMeters().getTranslation(); var target = Constants.Vision.HUB_POSE.getTranslation(); Translation2d relative = odom.minus(target); double alpha = Math.atan2(relative.getY(), relative.getX()); double visionDistance = RobotContainer.Suppliers.distanceSupplier.getAsDouble(); Translation2d newTranslation = new Translation2d(Math.cos(alpha) * visionDistance, Math.sin(alpha) * visionDistance); - odometry.resetPosition(new Pose2d(target.plus(newTranslation), Robot.getAngle()), Robot.getAngle()); + odometryForDistance.resetPosition(new Pose2d(target.plus(newTranslation), Robot.getAngle()), Robot.getAngle()); } } - System.out.println("Saar's distance: " + getOdometryDistance()); - System.out.println("hasTargetttt: " + RobotContainer.hasTarget.getAsBoolean()); +// System.out.println("Saar's distance: " + getOdometryDistance()); +// System.out.println("hasTargetttt: " + RobotContainer.hasTarget.getAsBoolean()); FireLog.log("distance", getOdometryDistance() * 1000); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/commands/AdjustToTargetOnCommand.java b/src/main/java/frc/robot/subsystems/drivetrain/commands/AdjustToTargetOnCommand.java index 0d8480f9..55920fe2 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/commands/AdjustToTargetOnCommand.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/commands/AdjustToTargetOnCommand.java @@ -32,15 +32,15 @@ public AdjustToTargetOnCommand(SwerveDrive swerveDrive, DoubleSupplier yawSuppli @Override public void initialize() { if (!hasTarget.getAsBoolean()) { - var robotAngle = Robot.getAngle(); var robotPose = swerveDrive.getPose().getTranslation(); var hubPose = Constants.Vision.HUB_POSE.getTranslation(); var poseRelativeToTarget = hubPose.minus(robotPose); - target = robotAngle.plus(new Rotation2d( + var value = new Rotation2d( Math.atan2( poseRelativeToTarget.getY(), poseRelativeToTarget.getX() - ))); + )); + target = value; } else { target = Robot.getAngle().minus(Rotation2d.fromDegrees(yawSupplier.getAsDouble())); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/commands/OverpoweredDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/commands/OverpoweredDrive.java index 041e6ba9..e3c7e3c7 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/commands/OverpoweredDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/commands/OverpoweredDrive.java @@ -47,8 +47,8 @@ public void execute() { if (magnitude == 0) current = 0; current += magnitude / 5; if (current > magnitude) current = magnitude; - forward = Math.cos(alpha) * current; - strafe = Math.sin(alpha) * current; + forward = Math.cos(alpha) * magnitude; + strafe = Math.sin(alpha) * magnitude; double rotation = speeds.omegaRadiansPerSecond; if (rotation != 0) { @@ -99,7 +99,7 @@ public void execute() { } } else { - swerveDrive.defaultHolonomicDrive(forward, strafe, rotation * (1 + (current / VELOCITY_MULTIPLIER) / Constants.SwerveDrive.ROTATIONAL_ADDITION_RESTRAINT)); + swerveDrive.defaultHolonomicDrive(forward, strafe, rotation * (1 + (magnitude / VELOCITY_MULTIPLIER) / Constants.SwerveDrive.ROTATIONAL_ADDITION_RESTRAINT)); setpoint = Robot.getAngle(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 08a7a0ec..2368ba2a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.RobotContainer; import frc.robot.subsystems.UnitModel; import frc.robot.utils.Utils; import webapp.FireLog; @@ -127,10 +128,11 @@ public void changePID() { @Override public void periodic() { FireLog.log("my Shooter velocity", getVelocity()); + FireLog.log("my shooter setpoint", RobotContainer.setpointVelocity); shooterVelocity.append(getVelocity()); shooterVoltage.append(mainMotor.getMotorOutputVoltage()); - FireLog.log("Shooter-velocity", getVelocity()); - System.out.println("Shooter'd velocity: " + getVelocity()); +// FireLog.log("Shooter-velocity", getVelocity()); +// System.out.println("Shooter'd velocity: " + getVelocity()); mainMotor.config_kP(0, kP.get()); mainMotor.config_kI(0, kI.get()); diff --git a/src/main/java/frc/robot/utils/PhotonVisionModule.java b/src/main/java/frc/robot/utils/PhotonVisionModule.java index d5992e9a..ab0eee80 100644 --- a/src/main/java/frc/robot/utils/PhotonVisionModule.java +++ b/src/main/java/frc/robot/utils/PhotonVisionModule.java @@ -149,7 +149,7 @@ public VisionEstimationData estimatePose() { @Override public void periodic() { - System.out.println("Distance: " + getDistance(CAMERA_HEIGHT, TARGET_HEIGHT_FROM_GROUND)); +// System.out.println("Distance: " + getDistance(CAMERA_HEIGHT, TARGET_HEIGHT_FROM_GROUND)); // System.out.println("Pose with vision = " + HUB_POSE.plus(poseRelativeToTarget())); @@ -157,7 +157,7 @@ public void periodic() { double yaw = getYaw().orElse(100); SmartDashboard.putString("aim_state", Math.abs(yaw) <= 5 ? "green" : Math.abs(yaw) <= 13 ? "yellow" : "red"); FireLog.log("filtered_distance", getDistance()); - System.out.println("target: " + hasTargets()); +// System.out.println("target: " + hasTargets()); } @Override diff --git a/src/test/java/Test.java b/src/test/java/Test.java index c3fe2933..84a71081 100644 --- a/src/test/java/Test.java +++ b/src/test/java/Test.java @@ -1,5 +1,7 @@ +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.subsystems.UnitModel; import static frc.robot.Constants.Shooter.TICKS_PER_REVOLUTION; @@ -23,7 +25,15 @@ public void test() { @org.junit.Test public void informal() { - var translation = new Translation2d(4.92, 1.78); - System.out.println(Constants.Vision.HUB_POSE.getTranslation().minus(translation).getNorm()); + var robotAngle = Robot.getAngle(); + var robotPose = new Translation2d(1,1); + var hubPose = Constants.Vision.HUB_POSE.getTranslation(); + var poseRelativeToTarget = hubPose.minus(robotPose); + var target = robotAngle.plus(new Rotation2d( + Math.atan2( + poseRelativeToTarget.getY(), + poseRelativeToTarget.getX() + ))); + System.out.println(target); } } From 7ca6fdef8edf8ac4a01a19dcad1ba8d3964dd3f1 Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 20:13:58 +0300 Subject: [PATCH 18/24] Five ball stuff, and just shoot --- .../pathplanner/FiveCargoAutoPart2.path | 2 +- .../pathplanner/FiveCargoAutoPart3.path | 2 +- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 39 ++++++++++--------- .../java/frc/robot/auto/FiveCargoAuto.java | 32 +++++++++++---- .../java/frc/robot/auto/SaarIsAutonomous.java | 6 +++ .../frc/robot/commandgroups/JustShoot.java | 27 ++++++++----- .../conveyor/commands/OldConvey.java | 3 +- .../commands/DriveAndAdjustWithVision.java | 3 ++ .../frc/robot/subsystems/shooter/Shooter.java | 6 ++- .../shooter/commands/ReachVelocity.java | 6 ++- 11 files changed, 88 insertions(+), 40 deletions(-) diff --git a/src/main/deploy/pathplanner/FiveCargoAutoPart2.path b/src/main/deploy/pathplanner/FiveCargoAutoPart2.path index 29a2408b..010ad282 100644 --- a/src/main/deploy/pathplanner/FiveCargoAutoPart2.path +++ b/src/main/deploy/pathplanner/FiveCargoAutoPart2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":4.96,"y":2.0},"prevControl":null,"nextControl":{"x":3.120852115904583,"y":2.4666138205738624},"holonomicAngle":40.150654652961755,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2004537190093711,"y":1.1715271233705913},"prevControl":{"x":2.776953181322883,"y":2.3864441401993526},"nextControl":null,"holonomicAngle":-139.51398845827583,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":4.0,"maxAcceleration":2.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":4.96,"y":2.0},"prevControl":null,"nextControl":{"x":3.120852115904583,"y":2.4666138205738624},"holonomicAngle":40.150654652961755,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.3259627219779835,"y":1.0232973180482268},"prevControl":{"x":2.902462184291495,"y":2.2382143348769885},"nextControl":null,"holonomicAngle":-139.51398845827583,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":4.0,"maxAcceleration":2.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FiveCargoAutoPart3.path b/src/main/deploy/pathplanner/FiveCargoAutoPart3.path index 03aa50bf..c6b014d7 100644 --- a/src/main/deploy/pathplanner/FiveCargoAutoPart3.path +++ b/src/main/deploy/pathplanner/FiveCargoAutoPart3.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":1.2,"y":1.17},"prevControl":null,"nextControl":{"x":1.5020361644941214,"y":1.4018664841196122},"holonomicAngle":-139.51,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.4009074380187423,"y":1.7789856317849724},"prevControl":{"x":1.9525452056176515,"y":1.619889355771682},"nextControl":null,"holonomicAngle":22.5795725640263,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":4.0,"maxAcceleration":2.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":1.3403753602603528,"y":1.02},"prevControl":null,"nextControl":{"x":1.6424115247544742,"y":1.2518664841196123},"holonomicAngle":-139.51,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.5798622525441197,"y":1.8015797852961732},"prevControl":{"x":2.131500020143029,"y":1.642483509282883},"nextControl":null,"holonomicAngle":22.5795725640263,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":4.0,"maxAcceleration":2.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3b9363c5..405270ad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.intake.Intake; +import frc.robot.utils.LedSubsystem; import frc.robot.valuetuner.NetworkTableConstant; /** @@ -131,6 +132,7 @@ public void teleopInit() { m_autonomousCommand.cancel(); } new InstantCommand(() -> RobotContainer.shooting = false); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3ae43c9f..77f0a279 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,12 +10,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.auto.FiveCargoAuto; import frc.robot.auto.FourBallAuto; import frc.robot.commandgroups.*; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; -import frc.robot.subsystems.drivetrain.commands.OverpoweredDrive; +import frc.robot.subsystems.drivetrain.commands.DriveAndAdjustWithVision; import frc.robot.subsystems.drivetrain.commands.TurnToAngle; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.helicopter.Helicopter; @@ -36,7 +37,7 @@ public class RobotContainer { public static boolean playWithoutVision = false; public static boolean hardCodedVelocity = false; - public static double hardCodedDistance = 3.8; + public static double hardCodedDistance = 3.35; public static double hardCodedVelocityValue = Shoot.getSetpointVelocity(hardCodedDistance); public static boolean shooting = false; // If this is true, don't change the setpoint of the shooter during teleop public static double setpointVelocity = 0; // Setpoint velocity for the shooter to reach at all times during teleop @@ -66,11 +67,11 @@ public RobotContainer() { hasTarget = photonVisionModule::hasTargets; // autonomousCommand = null; -// autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); + autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); // autonomousCommand = new TaxiFrom(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); // Configure the button bindings and default commands // autonomousCommand = new FourCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); - autonomousCommand = new FourBallAuto(swerve, shooter, conveyor, intake, hood, flap, photonVisionModule); +// autonomousCommand = new FourBallAuto(swerve, shooter, conveyor, intake, hood, flap, photonVisionModule); configureDefaultCommands(); initSuppliers(); if (Robot.debug) { @@ -93,11 +94,14 @@ private void initSuppliers() { private void configureDefaultCommands() { swerve.setDefaultCommand( - new OverpoweredDrive( + new DriveAndAdjustWithVision( swerve, () -> -Joysticks.leftJoystick.getY() * speedMultiplier, () -> -Joysticks.leftJoystick.getX() * speedMultiplier, - () -> -Joysticks.rightJoystick.getX() * thetaMultiplier + () -> -Joysticks.rightJoystick.getX() * thetaMultiplier, + () -> photonVisionModule.getYaw().orElse(0), + Joysticks.rightTrigger::get, + photonVisionModule::hasTargets ) ); helicopter.setDefaultCommand(new JoystickPowerHelicopter(helicopter, () -> -Xbox.controller.getLeftY())); @@ -119,12 +123,22 @@ private void configureButtonBindings() { Xbox.leftPov.whileActiveOnce(new InstantCommand(hood::toggle)); Xbox.downPov.whileActiveOnce(new LowGoalShot(shooter, flap, hood)); Xbox.upPov.whileActiveContinuous(new JoystickPowerHelicopter(helicopter, () -> -0.1)); + Xbox.rightPov.whileActiveContinuous(() -> hardCodedVelocity = true).whenInactive(() -> hardCodedVelocity = false); Xbox.back.whenPressed(new OneBallOuttake(intake, conveyor, () -> conveyor.getColorSensorProximity() >= 150)); + + Xbox.rt.whileActiveContinuous(new JustShoot(conveyor, flap, hood)).whenInactive( + () -> { + shooting = false; + ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.STATIC); + } + ); } { // Joystick button bindings. + + Joysticks.rightTrigger.whenInactive(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); Joysticks.leftTrigger.whileHeld(() -> { speedMultiplier = 0.5; thetaMultiplier = 1.5 * speedMultiplier; @@ -133,16 +147,6 @@ private void configureButtonBindings() { thetaMultiplier = 1.5 * speedMultiplier; }); - Joysticks.rightTrigger.whileActiveOnce(new OneButtonAdjustAndShoot(swerve, conveyor, flap, hood)).whenInactive( - () -> { - shooting = false; - ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.STATIC); - } - ); -// Joysticks.rightTrigger.whileActiveOnce( -// new OdometryAdjust(swerve) -// ); - Joysticks.leftTwo.whenPressed((Runnable) Robot::resetAngle); Joysticks.rightTwo.whileHeld(new TurnToAngle(swerve, Rotation2d::new)); } @@ -154,9 +158,6 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { -// return new TestColorSensor(conveyor, intake, ledSubsystem); -// return new RunAllBits(swerve, shooter, conveyor, intake, flap, hood); -// return new TestShooterVelocity(shooter, ledSubsystem); return autonomousCommand; } diff --git a/src/main/java/frc/robot/auto/FiveCargoAuto.java b/src/main/java/frc/robot/auto/FiveCargoAuto.java index aa2a398d..d8d0d375 100644 --- a/src/main/java/frc/robot/auto/FiveCargoAuto.java +++ b/src/main/java/frc/robot/auto/FiveCargoAuto.java @@ -13,7 +13,7 @@ import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.Shoot; +import frc.robot.utils.LedSubsystem; import frc.robot.utils.PhotonVisionModule; public class FiveCargoAuto extends SaarIsAutonomous { @@ -24,21 +24,39 @@ public class FiveCargoAuto extends SaarIsAutonomous { */ public FiveCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "FiveCargoAutoPart1"); - addCommands(reachVelocityByDistance(2.16)); - addCommands(new WaitUntilCommand(() -> Math.abs(shooter.getVelocity() - Shoot.getSetpointVelocity(2.16)) < Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get())); - confirmShooting().withTimeout(0.6); + addRequirements(shooter); + addCommands(reachVelocity(3350, 1)); + addCommands(new WaitUntilCommand(() -> Math.abs(shooter.getVelocity() - 3350) < Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get())); + addCommands(new InstantCommand(flap::allowShooting)); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING); + addCommands(confirmShooting().withTimeout(0.6)); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); + addCommands(reachVelocityByDistance(3.85)); addCommands(followPathAndPickup("FiveCargoAutoPart1")); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.ODOMETRY_ADJUST); addCommands(new ParallelDeadlineGroup( turnToAngle(() -> Rotation2d.fromDegrees(40.15)), new Convey(conveyor, -0.25).withTimeout(0.075) )); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.VISION_ADJUST); addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); - addCommands(confirmShooting().withTimeout(2)); - addCommands(reachVelocityByDistance(3.89)); + addCommands(new InstantCommand(flap::allowShooting)); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING); + addCommands(confirmShooting().withTimeout(1.6)); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); + addCommands(reachVelocityByDistance(5.6)); addCommands(followPathAndPickup("FiveCargoAutoPart2")); addCommands(followPathAndPickup("FiveCargoAutoPart3")); addCommands(new InstantCommand(swerveDrive::terminate)); - addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3)); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.VISION_ADJUST); + addCommands(new ParallelDeadlineGroup( + new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3), + new Convey(conveyor, -0.25).withTimeout(0.075) + + )); + addCommands(new InstantCommand(flap::allowShooting)); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING); addCommands(confirmShooting()); + new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/auto/SaarIsAutonomous.java b/src/main/java/frc/robot/auto/SaarIsAutonomous.java index 7620a3c6..4e2a94bd 100644 --- a/src/main/java/frc/robot/auto/SaarIsAutonomous.java +++ b/src/main/java/frc/robot/auto/SaarIsAutonomous.java @@ -144,6 +144,12 @@ protected CommandBase reachVelocityByDistance(double distance) { new InstantCommand(() -> hood.setSolenoid(distance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); } + protected CommandBase reachVelocity(double velocity, double distance) { + return new SequentialCommandGroup( + new InstantCommand(() -> shooter.setVelocity(velocity)), + new InstantCommand(() -> hood.setSolenoid(distance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); + } + protected CommandBase confirmShooting() { return new ConveyCargo(conveyor); } diff --git a/src/main/java/frc/robot/commandgroups/JustShoot.java b/src/main/java/frc/robot/commandgroups/JustShoot.java index fb5ca78a..27fab3f6 100644 --- a/src/main/java/frc/robot/commandgroups/JustShoot.java +++ b/src/main/java/frc/robot/commandgroups/JustShoot.java @@ -6,21 +6,30 @@ import frc.robot.RobotContainer; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; +import frc.robot.subsystems.conveyor.commands.OldConvey; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; +import frc.robot.utils.LedSubsystem; public class JustShoot extends SequentialCommandGroup { public JustShoot(Conveyor conveyor, Flap flap, Hood hood) { + addCommands(new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING)); + addCommands(new InstantCommand(flap::allowShooting)); addCommands(new Convey(conveyor, -0.25).withTimeout(0.075)); addCommands(new InstantCommand(() -> RobotContainer.shooting = true)); -// if (RobotContainer.hardCodedVelocity) -// addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.hardCodedDistance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); -// else { -// if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) - addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.distanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); -// else -// addCommands(new InstantCommand(() -> hood.setSolenoid(RobotContainer.Suppliers.odometryDistanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance))); -// } - addCommands(new ConveyForShooting(conveyor)); + addCommands( + new InstantCommand(() -> { + if (RobotContainer.hardCodedVelocity) { + hood.setSolenoid(RobotContainer.hardCodedDistance < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance); + } else { + if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision) + hood.setSolenoid(RobotContainer.Suppliers.distanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance); + else + hood.setSolenoid(RobotContainer.Suppliers.odometryDistanceSupplier.getAsDouble() < Constants.Hood.DISTANCE_FROM_TARGET_THRESHOLD ? Hood.Mode.ShortDistance : Hood.Mode.LongDistance); + } + }) + ); + + addCommands(new OldConvey(conveyor, () -> !conveyor.isPreFlapBeamConnected())); } } diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java index fff8574d..b4cf050e 100644 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java @@ -39,7 +39,8 @@ public void initialize() { @Override public void execute() { if (wait) { - if (Math.abs(RobotContainer.setpointVelocity - RobotContainer.Suppliers.shooterVelocity.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { + double setpoint = RobotContainer.hardCodedVelocity ? RobotContainer.hardCodedVelocityValue : RobotContainer.setpointVelocity; + if (Math.abs(setpoint - RobotContainer.Suppliers.shooterVelocity.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { wait = false; } } else { diff --git a/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java b/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java index ed23309e..8e6594c1 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java @@ -7,6 +7,7 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.utils.LedSubsystem; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -96,11 +97,13 @@ public void execute() { poseRelativeToTarget.getY(), poseRelativeToTarget.getX() )); + LedSubsystem.currentLedMode = LedSubsystem.LedMode.ODOMETRY_ADJUST; } else { if (sampleYawTimer.hasElapsed(Constants.SwerveDrive.SAMPLE_YAW_PERIOD)) { target = Robot.getAngle().minus(Rotation2d.fromDegrees(yawSupplier.getAsDouble())); sampleYawTimer.reset(); } + LedSubsystem.currentLedMode = LedSubsystem.LedMode.VISION_ADJUST; } rotation = adjustController.calculate(Robot.getAngle().getRadians(), target.getRadians()); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2368ba2a..ec291441 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -128,7 +128,11 @@ public void changePID() { @Override public void periodic() { FireLog.log("my Shooter velocity", getVelocity()); - FireLog.log("my shooter setpoint", RobotContainer.setpointVelocity); + if (RobotContainer.hardCodedVelocity) { + FireLog.log("my shooter setpoint", RobotContainer.hardCodedVelocityValue); + } else { + FireLog.log("my shooter setpoint", RobotContainer.setpointVelocity); + } shooterVelocity.append(getVelocity()); shooterVoltage.append(mainMotor.getMotorOutputVoltage()); // FireLog.log("Shooter-velocity", getVelocity()); diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java index 65843a87..04fd63f4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java @@ -16,7 +16,11 @@ public ReachVelocity(Shooter shooter) { @Override public void execute() { if (RobotContainer.warmUpShooting) { - shooter.setVelocity(RobotContainer.setpointVelocity); + if (RobotContainer.hardCodedVelocity) { + shooter.setVelocity(RobotContainer.hardCodedVelocityValue); + } else { + shooter.setVelocity(RobotContainer.setpointVelocity); + } } else { shooter.setPower(0); } From 58d6c7cba4694da54192ed924b2fea4bad5066d5 Mon Sep 17 00:00:00 2001 From: Saar Date: Sat, 26 Mar 2022 20:17:36 +0300 Subject: [PATCH 19/24] Fix warmup --- src/main/java/frc/robot/auto/StraightLineFour.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/auto/StraightLineFour.java b/src/main/java/frc/robot/auto/StraightLineFour.java index e43c2021..11ec89da 100644 --- a/src/main/java/frc/robot/auto/StraightLineFour.java +++ b/src/main/java/frc/robot/auto/StraightLineFour.java @@ -24,7 +24,7 @@ public StraightLineFour(SwerveDrive swerveDrive, Shooter shooter, Conveyor conve addCommands(new ParallelRaceGroup( followPath("Four- first shooting position"), - warmup(3400) + reachVelocityByDistance(3.5) )); addCommands( @@ -41,7 +41,7 @@ public StraightLineFour(SwerveDrive swerveDrive, Shooter shooter, Conveyor conve addCommands( new ParallelRaceGroup( - warmup(3400), + reachVelocityByDistance(3.5), followPath("Four- second shooting position") )); From 49133603a84e48c370fd8ad13471ab1646832db0 Mon Sep 17 00:00:00 2001 From: eitan Date: Sat, 26 Mar 2022 20:25:32 +0300 Subject: [PATCH 20/24] Add wait for vision adjustment in shooting --- src/main/java/frc/robot/commandgroups/JustShoot.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/commandgroups/JustShoot.java b/src/main/java/frc/robot/commandgroups/JustShoot.java index 27fab3f6..1c4b4e19 100644 --- a/src/main/java/frc/robot/commandgroups/JustShoot.java +++ b/src/main/java/frc/robot/commandgroups/JustShoot.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.conveyor.Conveyor; @@ -10,9 +11,11 @@ import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.utils.LedSubsystem; +import frc.robot.utils.Utils; public class JustShoot extends SequentialCommandGroup { public JustShoot(Conveyor conveyor, Flap flap, Hood hood) { + addCommands(new WaitUntilCommand(() -> Utils.conventionalDeadband(RobotContainer.Suppliers.yawSupplier.getAsDouble(), 5) == 0)); addCommands(new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING)); addCommands(new InstantCommand(flap::allowShooting)); addCommands(new Convey(conveyor, -0.25).withTimeout(0.075)); From ae5765688938a515ef41ac7689931b8928652c67 Mon Sep 17 00:00:00 2001 From: Saar Date: Sun, 27 Mar 2022 09:28:59 +0300 Subject: [PATCH 21/24] Latest (five cargo) --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/auto/FiveCargoAuto.java | 6 +-- .../frc/robot/commandgroups/JustShoot.java | 5 +- .../conveyor/commands/OldConvey.java | 54 ++++++++++--------- .../commands/DriveAndAdjustWithVision.java | 5 +- .../shooter/commands/ReachVelocity.java | 9 +++- 6 files changed, 46 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77f0a279..e06d78a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.auto.FiveCargoAuto; -import frc.robot.auto.FourBallAuto; import frc.robot.commandgroups.*; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; @@ -35,6 +34,7 @@ import java.util.function.DoubleSupplier; public class RobotContainer { + public static boolean overrideConveyor = false; public static boolean playWithoutVision = false; public static boolean hardCodedVelocity = false; public static double hardCodedDistance = 3.35; @@ -119,6 +119,7 @@ private void configureButtonBindings() { Xbox.x.whenPressed(intake::toggleRetractor); Xbox.y.whenPressed(new InstantCommand(() -> warmUpShooting = !warmUpShooting)); Xbox.a.whileHeld(() -> playWithoutVision = true).whenReleased(() -> playWithoutVision = false); + Xbox.b.whileHeld(() -> overrideConveyor = true).whenReleased(() -> overrideConveyor = false); Xbox.leftPov.whileActiveOnce(new InstantCommand(hood::toggle)); Xbox.downPov.whileActiveOnce(new LowGoalShot(shooter, flap, hood)); @@ -138,7 +139,6 @@ private void configureButtonBindings() { { // Joystick button bindings. - Joysticks.rightTrigger.whenInactive(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); Joysticks.leftTrigger.whileHeld(() -> { speedMultiplier = 0.5; thetaMultiplier = 1.5 * speedMultiplier; diff --git a/src/main/java/frc/robot/auto/FiveCargoAuto.java b/src/main/java/frc/robot/auto/FiveCargoAuto.java index d8d0d375..8094e11f 100644 --- a/src/main/java/frc/robot/auto/FiveCargoAuto.java +++ b/src/main/java/frc/robot/auto/FiveCargoAuto.java @@ -31,7 +31,7 @@ public FiveCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING); addCommands(confirmShooting().withTimeout(0.6)); new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); - addCommands(reachVelocityByDistance(3.85)); + addCommands(reachVelocityByDistance(3.78)); addCommands(followPathAndPickup("FiveCargoAutoPart1")); new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.ODOMETRY_ADJUST); addCommands(new ParallelDeadlineGroup( @@ -44,13 +44,13 @@ public FiveCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING); addCommands(confirmShooting().withTimeout(1.6)); new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); - addCommands(reachVelocityByDistance(5.6)); + addCommands(reachVelocityByDistance(5.8)); addCommands(followPathAndPickup("FiveCargoAutoPart2")); addCommands(followPathAndPickup("FiveCargoAutoPart3")); addCommands(new InstantCommand(swerveDrive::terminate)); new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.VISION_ADJUST); addCommands(new ParallelDeadlineGroup( - new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3), + new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.33), new Convey(conveyor, -0.25).withTimeout(0.075) )); diff --git a/src/main/java/frc/robot/commandgroups/JustShoot.java b/src/main/java/frc/robot/commandgroups/JustShoot.java index 1c4b4e19..0ef115e4 100644 --- a/src/main/java/frc/robot/commandgroups/JustShoot.java +++ b/src/main/java/frc/robot/commandgroups/JustShoot.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.conveyor.Conveyor; @@ -11,11 +10,11 @@ import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.utils.LedSubsystem; -import frc.robot.utils.Utils; public class JustShoot extends SequentialCommandGroup { public JustShoot(Conveyor conveyor, Flap flap, Hood hood) { - addCommands(new WaitUntilCommand(() -> Utils.conventionalDeadband(RobotContainer.Suppliers.yawSupplier.getAsDouble(), 5) == 0)); +// addCommands(new WaitUntilCommand(() -> Utils.conventionalDeadband(RobotContainer.Suppliers.yawSupplier.getAsDouble(), 5) == 0 || RobotContainer.playWithoutVision || RobotContainer.hardCodedVelocity)); + addCommands(new InstantCommand(() -> RobotContainer.warmUpShooting = true)); addCommands(new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING)); addCommands(new InstantCommand(flap::allowShooting)); addCommands(new Convey(conveyor, -0.25).withTimeout(0.075)); diff --git a/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java index b4cf050e..49e10b3c 100644 --- a/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java +++ b/src/main/java/frc/robot/subsystems/conveyor/commands/OldConvey.java @@ -38,36 +38,40 @@ public void initialize() { @Override public void execute() { - if (wait) { - double setpoint = RobotContainer.hardCodedVelocity ? RobotContainer.hardCodedVelocityValue : RobotContainer.setpointVelocity; - if (Math.abs(setpoint - RobotContainer.Suppliers.shooterVelocity.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { - wait = false; - } + if (RobotContainer.overrideConveyor) { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); } else { - if (getBallToPreFlap) { - conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + if (wait) { + double setpoint = RobotContainer.hardCodedVelocity ? RobotContainer.hardCodedVelocityValue : RobotContainer.setpointVelocity; + if (Math.abs(setpoint - RobotContainer.Suppliers.shooterVelocity.getAsDouble()) < SHOOTER_VELOCITY_DEADBAND.get()) { + wait = false; + } } else { - conveyor.setPower(0); - } + if (getBallToPreFlap) { + conveyor.setPower(Constants.Conveyor.SHOOT_POWER); + } else { + conveyor.setPower(0); + } - if (preFlapSupplier.getAsBoolean()) { - if (!last) { - last = true; - getBallToPreFlap = false; - timer.start(); - timer.reset(); - delayTimer.start(); - delayTimer.reset(); + if (preFlapSupplier.getAsBoolean()) { + if (!last) { + last = true; + getBallToPreFlap = false; + timer.start(); + timer.reset(); + delayTimer.start(); + delayTimer.reset(); + } + } else { + last = false; + getBallToPreFlap = true; } - } else { - last = false; - getBallToPreFlap = true; - } - if (timer.hasElapsed(0.3)) { - getBallToPreFlap = true; - timer.reset(); - timer.stop(); + if (timer.hasElapsed(0.3)) { + getBallToPreFlap = true; + timer.reset(); + timer.stop(); + } } } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java b/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java index 8e6594c1..5cc3c571 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/commands/DriveAndAdjustWithVision.java @@ -7,7 +7,6 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.drivetrain.SwerveDrive; -import frc.robot.utils.LedSubsystem; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -97,13 +96,13 @@ public void execute() { poseRelativeToTarget.getY(), poseRelativeToTarget.getX() )); - LedSubsystem.currentLedMode = LedSubsystem.LedMode.ODOMETRY_ADJUST; +// LedSubsystem.currentLedMode = LedSubsystem.LedMode.ODOMETRY_ADJUST; } else { if (sampleYawTimer.hasElapsed(Constants.SwerveDrive.SAMPLE_YAW_PERIOD)) { target = Robot.getAngle().minus(Rotation2d.fromDegrees(yawSupplier.getAsDouble())); sampleYawTimer.reset(); } - LedSubsystem.currentLedMode = LedSubsystem.LedMode.VISION_ADJUST; +// LedSubsystem.currentLedMode = LedSubsystem.LedMode.VISION_ADJUST; } rotation = adjustController.calculate(Robot.getAngle().getRadians(), target.getRadians()); } diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java index 04fd63f4..5af72e53 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter.commands; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RobotContainer; import frc.robot.subsystems.shooter.Shooter; @@ -19,7 +20,13 @@ public void execute() { if (RobotContainer.hardCodedVelocity) { shooter.setVelocity(RobotContainer.hardCodedVelocityValue); } else { - shooter.setVelocity(RobotContainer.setpointVelocity); + if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision && !DriverStation.isAutonomous()) { + if (Math.abs(RobotContainer.Suppliers.yawSupplier.getAsDouble()) < 10) { + shooter.setVelocity(RobotContainer.setpointVelocity - 100); + } + } else { + shooter.setVelocity(RobotContainer.setpointVelocity); + } } } else { shooter.setPower(0); From 450c5a103bf6ec9d6935e4b6d6fe96a3d128edda Mon Sep 17 00:00:00 2001 From: Galaxia Date: Sun, 27 Mar 2022 10:42:39 +0300 Subject: [PATCH 22/24] Warm up fix and a more reliable option --- src/main/java/frc/robot/RobotContainer.java | 1 + .../shooter/commands/ReachVelocity.java | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e06d78a8..4e08489f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,6 +37,7 @@ public class RobotContainer { public static boolean overrideConveyor = false; public static boolean playWithoutVision = false; public static boolean hardCodedVelocity = false; + public static boolean smartWarmUp = false; public static double hardCodedDistance = 3.35; public static double hardCodedVelocityValue = Shoot.getSetpointVelocity(hardCodedDistance); public static boolean shooting = false; // If this is true, don't change the setpoint of the shooter during teleop diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java index 5af72e53..3d4eed0f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java @@ -20,12 +20,20 @@ public void execute() { if (RobotContainer.hardCodedVelocity) { shooter.setVelocity(RobotContainer.hardCodedVelocityValue); } else { - if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision && !DriverStation.isAutonomous()) { - if (Math.abs(RobotContainer.Suppliers.yawSupplier.getAsDouble()) < 10) { - shooter.setVelocity(RobotContainer.setpointVelocity - 100); + if (RobotContainer.smartWarmUp) { + if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision && !DriverStation.isAutonomous()) { + if (Math.abs(RobotContainer.Suppliers.yawSupplier.getAsDouble()) < 10) { // with vision + shooter.setVelocity(RobotContainer.setpointVelocity); + } + } else { // with odometry + shooter.setVelocity(RobotContainer.setpointVelocity - 300); } } else { - shooter.setVelocity(RobotContainer.setpointVelocity); + if (RobotContainer.shooting) { + shooter.setVelocity(RobotContainer.setpointVelocity); + } else { + shooter.setVelocity(3530); + } } } } else { From cb28fd6483df5c36910ca2fffec3fac85c81aba1 Mon Sep 17 00:00:00 2001 From: Galaxia Date: Sun, 27 Mar 2022 11:46:24 +0300 Subject: [PATCH 23/24] Driver's practice and autonomous --- .../From Tarmac to (5,6) ball.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 ++- .../java/frc/robot/auto/FiveCargoAuto.java | 2 +- .../frc/robot/auto/FromTarmacTo56Ball.java | 36 ++++++++++--------- .../frc/robot/subsystems/shooter/Shooter.java | 3 ++ .../shooter/commands/ReachVelocity.java | 2 ++ 6 files changed, 30 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path b/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path index 89442246..b387a95e 100644 --- a/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path +++ b/src/main/deploy/pathplanner/From Tarmac to (5,6) ball.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":6.207807571324485,"y":5.139950764330594},"prevControl":null,"nextControl":{"x":6.207807571324485,"y":5.139950764330594},"holonomicAngle":135.97102193107926,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.3819983072492095,"y":5.851855302326522},"prevControl":{"x":5.396236398009128,"y":5.837617211566603},"nextControl":null,"holonomicAngle":136.58235392261864,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":6.21,"y":5.14},"prevControl":null,"nextControl":{"x":5.683180986615138,"y":5.405388025965925},"holonomicAngle":135.97,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.069721531848127,"y":5.949399240570633},"prevControl":{"x":5.347514492497339,"y":5.602158039759117},"nextControl":null,"holonomicAngle":135.97,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4e08489f..ab1dfcb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.auto.FiveCargoAuto; +import frc.robot.auto.FromTarmacTo56Ball; import frc.robot.commandgroups.*; import frc.robot.subsystems.conveyor.Conveyor; import frc.robot.subsystems.conveyor.commands.Convey; @@ -68,7 +69,8 @@ public RobotContainer() { hasTarget = photonVisionModule::hasTargets; // autonomousCommand = null; - autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); +// autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); + autonomousCommand = new FromTarmacTo56Ball(swerve,shooter, conveyor, intake, hood, flap, photonVisionModule); // autonomousCommand = new TaxiFrom(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); // Configure the button bindings and default commands // autonomousCommand = new FourCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); diff --git a/src/main/java/frc/robot/auto/FiveCargoAuto.java b/src/main/java/frc/robot/auto/FiveCargoAuto.java index 8094e11f..e0a41f3e 100644 --- a/src/main/java/frc/robot/auto/FiveCargoAuto.java +++ b/src/main/java/frc/robot/auto/FiveCargoAuto.java @@ -44,7 +44,7 @@ public FiveCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.SHOOTING); addCommands(confirmShooting().withTimeout(1.6)); new InstantCommand(() -> LedSubsystem.currentLedMode = LedSubsystem.LedMode.STATIC); - addCommands(reachVelocityByDistance(5.8)); + addCommands(reachVelocityByDistance(6)); addCommands(followPathAndPickup("FiveCargoAutoPart2")); addCommands(followPathAndPickup("FiveCargoAutoPart3")); addCommands(new InstantCommand(swerveDrive::terminate)); diff --git a/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java b/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java index bde86637..6c2ee20e 100644 --- a/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java +++ b/src/main/java/frc/robot/auto/FromTarmacTo56Ball.java @@ -1,31 +1,35 @@ package frc.robot.auto; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.conveyor.Conveyor; +import frc.robot.subsystems.conveyor.commands.Convey; import frc.robot.subsystems.drivetrain.SwerveDrive; -import frc.robot.subsystems.drivetrain.commands.TurnToAngle; +import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand; import frc.robot.subsystems.flap.Flap; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.commands.Shoot; import frc.robot.utils.PhotonVisionModule; public class FromTarmacTo56Ball extends SaarIsAutonomous { - public FromTarmacTo56Ball(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule, String initialPathPath) { - super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, initialPathPath); - addCommands(followPath("From Tarmac to (5,6) ball")); + public FromTarmacTo56Ball(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) { + super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "From Tarmac to (5,6) ball"); + addRequirements(shooter); + addCommands(reachVelocityByDistance(3.45)); + addCommands(new InstantCommand(intake::openRetractor)); + addCommands(new WaitCommand(0.3)); + addCommands(followPathAndPickup("From Tarmac to (5,6) ball")); - addCommands( - new ParallelRaceGroup( - turnToAngle(() -> Rotation2d.fromDegrees(-45)), - new RunCommand(() -> shooter.setVelocity(Shoot.getSetpointVelocity(Math.hypot(Constants.Vision.HUB_POSE.getX() - 5.38, Constants.Vision.HUB_POSE.getY() - 5.85)))) - ) - ); - addCommands(shootAndAdjust(2)); + addCommands(new ParallelDeadlineGroup( + turnToAngle(() -> Rotation2d.fromDegrees(-31.41)) + )); + addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.33)); + + addCommands(new Convey(conveyor, -0.25).withTimeout(0.1)); + addCommands(new InstantCommand(flap::allowShooting)); + addCommands(confirmShootingSlower()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ec291441..07a5fc16 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,6 +8,7 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.RobotContainer; @@ -133,6 +134,8 @@ public void periodic() { } else { FireLog.log("my shooter setpoint", RobotContainer.setpointVelocity); } + SmartDashboard.putString("speed_state", Math.abs(getVelocity() - RobotContainer.setpointVelocity) <= 50 ? "green" : Math.abs(getVelocity() - RobotContainer.setpointVelocity) <= 100 ? "yellow" : "red"); + shooterVelocity.append(getVelocity()); shooterVoltage.append(mainMotor.getMotorOutputVoltage()); // FireLog.log("Shooter-velocity", getVelocity()); diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java index 3d4eed0f..c8b7f875 100644 --- a/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java +++ b/src/main/java/frc/robot/subsystems/shooter/commands/ReachVelocity.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.shooter.commands; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RobotContainer; import frc.robot.subsystems.shooter.Shooter; @@ -19,6 +20,7 @@ public void execute() { if (RobotContainer.warmUpShooting) { if (RobotContainer.hardCodedVelocity) { shooter.setVelocity(RobotContainer.hardCodedVelocityValue); + SmartDashboard.putString("speed_state", Math.abs(shooter.getVelocity() - RobotContainer.hardCodedVelocityValue) <= 50 ? "green" : Math.abs(shooter.getVelocity() - RobotContainer.hardCodedVelocityValue) <= 100 ? "yellow" : "red"); } else { if (RobotContainer.smartWarmUp) { if (RobotContainer.hasTarget.getAsBoolean() && !RobotContainer.playWithoutVision && !DriverStation.isAutonomous()) { From 4483abad2e88f41ae32be716c0bfc73f5d70fd24 Mon Sep 17 00:00:00 2001 From: Saar Date: Sun, 27 Mar 2022 12:17:34 +0300 Subject: [PATCH 24/24] Change to 5 --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab1dfcb3..6229ab52 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -69,8 +69,8 @@ public RobotContainer() { hasTarget = photonVisionModule::hasTargets; // autonomousCommand = null; -// autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); - autonomousCommand = new FromTarmacTo56Ball(swerve,shooter, conveyor, intake, hood, flap, photonVisionModule); + autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); +// autonomousCommand = new FromTarmacTo56Ball(swerve,shooter, conveyor, intake, hood, flap, photonVisionModule); // autonomousCommand = new TaxiFrom(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule); // Configure the button bindings and default commands // autonomousCommand = new FourCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule);