diff --git a/src/main/deploy/pathplanner/paths/center get amp.path b/src/main/deploy/pathplanner/paths/center get amp.path index d05c482..d3119fe 100644 --- a/src/main/deploy/pathplanner/paths/center get amp.path +++ b/src/main/deploy/pathplanner/paths/center get amp.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.3758851026473984, - "y": 6.126871025671877 + "x": 2.262597392527166, + "y": 6.241875878901544 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.992561413694256 }, "prevControl": { - "x": 2.339612047645148, - "y": 6.027120124415688 + "x": 2.250886555981385, + "y": 6.2770083885388885 }, "nextControl": null, "isLocked": false, @@ -40,7 +40,7 @@ "globalConstraints": { "maxVelocity": 1.5, "maxAcceleration": 2.0, - "maxAngularVelocity": 45.0, + "maxAngularVelocity": 90.0, "maxAngularAcceleration": 540.0 }, "goalEndState": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2abfed7..2bf5532 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,6 +38,8 @@ import frc.robot.commands.drive.AutoPickupNote; import frc.robot.commands.drive.DriveSwerveWithXbox; import frc.robot.commands.intake.SetIntakeState; +import frc.robot.commands.shooter.CenterFeedShot; +import frc.robot.commands.shooter.FeedShot; import frc.robot.commands.shooter.NoteVisible; import frc.robot.commands.shooter.RampShooter; import frc.robot.commands.shooter.ShootSpeaker; @@ -207,6 +209,12 @@ private void configureXboxControllerBindings() { shootSpeaker.onFalse(new InstantCommand(() -> { // shooter.setVelocity(15); })); + + var feedShot = new Trigger(() -> driverController.getHID().getBButton() && shooter.hasNote()); + feedShot.whileTrue(new FeedShot()); + + var centerFeedShot = new Trigger(() -> driverController.getHID().getYButton() && shooter.hasNote()); + centerFeedShot.whileTrue(new CenterFeedShot()); } /** Binds commands to the operator stick. */ @@ -366,26 +374,15 @@ private void registerNamedCommands() { NamedCommands.registerCommand("LiftArmToDeployDemonHorns", new SetArmState(ArmState.kDeployDemonHorns)); NamedCommands.registerCommand("SetArmAndShooterForIntake", intakeSetpointAndRun()); NamedCommands.registerCommand("SetShootFromSubwoofer", new SetArmState(ArmState.kShootFromSubwooferAuto)); - NamedCommands.registerCommand("SetArmForScore", new SetArmState(ArmState.kShoot).andThen(new WaitCommand(0.2))); // Intake + Indexing NamedCommands.registerCommand("AutoIntake", new AutoIntake()); NamedCommands.registerCommand("AutoPickupNote", new AutoPickupNote()); NamedCommands.registerCommand("NoteVisible", new NoteVisible()); NamedCommands.registerCommand("HasNote", new WaitForNote().withTimeout(3)); - NamedCommands.registerCommand("RetractIntake", new SetIntakeState(IntakeState.kUp)); NamedCommands.registerCommand("DeployIntake", new SetIntakeState(IntakeState.kDown)); - NamedCommands.registerCommand("StartIntake", new InstantCommand(() -> { - intake.intake(); - shooter.index(); - }, intake, shooter)); - NamedCommands.registerCommand("StopIntake", new InstantCommand(() -> { - intake.stop(); - shooter.stopIndexer(); - }, intake, shooter)); // Shooting - NamedCommands.registerCommand("ShootSpeaker", new ShootSpeaker()); NamedCommands.registerCommand("RampShooterForManualShot", new RampShooter(() -> 15)); NamedCommands.registerCommand("Feed", feedCommand()); NamedCommands.registerCommand("StopShooter", new InstantCommand(() -> shooter.stopShooting(), shooter)); diff --git a/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java b/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java index d552a29..6b4c7ab 100644 --- a/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java +++ b/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java @@ -1,6 +1,9 @@ package frc.robot.commands.drive; /*----------------------------------------------------------------------------*/ +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + /* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ @@ -9,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; -import frc.robot.subsystems.Arm.ArmState; import frc.robot.subsystems.drive.SwerveDrive; public class DriveSwerveWithXbox extends Command { @@ -55,8 +57,8 @@ public void execute() { * SwerveDrive.kMaxAngularSpeedRadiansPerSecond; } - var sign = fieldRelative || RobotContainer.arm.rotationSetpoint == ArmState.kAmp.rotations ? 1 : -1; - RobotContainer.swerveDrive.drive(-xSpeed*sign, -ySpeed*sign, rot, fieldRelative); + var sign = fieldRelative && !DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Red ? -1 : 1; + RobotContainer.swerveDrive.drive(sign*xSpeed, sign*ySpeed, rot, fieldRelative); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java b/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java index 7d9fa22..ba02d66 100644 --- a/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java +++ b/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java @@ -1,9 +1,10 @@ package frc.robot.commands.drive; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; -import frc.robot.subsystems.Arm.ArmState; import frc.robot.subsystems.drive.SwerveDrive; public class HoldAngleWhileDriving extends Command { @@ -40,8 +41,8 @@ public void execute() { } var fieldRelative = RobotContainer.driverController.getHID().getRightTriggerAxis() > 0.5; - var sign = fieldRelative || RobotContainer.arm.rotationSetpoint == ArmState.kAmp.rotations ? 1 : -1; - RobotContainer.swerveDrive.holdAngleWhileDriving(-xSpeed*sign, -ySpeed*sign, angleSetpoint, fieldRelative); + var sign = fieldRelative && !DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Red ? -1 : 1; + RobotContainer.swerveDrive.holdAngleWhileDriving(sign*xSpeed, sign*ySpeed, angleSetpoint, fieldRelative); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/shooter/CenterFeedShot.java b/src/main/java/frc/robot/commands/shooter/CenterFeedShot.java new file mode 100644 index 0000000..dbf3a66 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/CenterFeedShot.java @@ -0,0 +1,116 @@ +package frc.robot.commands.shooter; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Arm.ArmState; +import frc.robot.subsystems.drive.SwerveDrive; + +public class CenterFeedShot extends Command { + SwerveDrive swerveDrive; + Shooter shooter; + Alliance color; + boolean hasAlliance, hasNoteInitially; + double targetX, targetY, targetZ; + final double v0 = 17; + boolean goodTrajectory = true; + public Timer timer; + Rotation2d angleSetpoint; + + public CenterFeedShot() { + this.addRequirements(RobotContainer.swerveDrive, RobotContainer.arm, RobotContainer.shooter); + swerveDrive = RobotContainer.swerveDrive; + shooter = RobotContainer.shooter; + timer = new Timer(); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + // if (!shooter.hasNote()) { + // hasNoteInitially = false; + // return; + // } + // hasNoteInitially = true; + + Optional alliance = DriverStation.getAlliance(); + hasAlliance = alliance.isPresent(); + if (!hasAlliance) { + return; + } + + color = alliance.get(); + if (color == Alliance.Blue) { + targetX = 1.905217; + targetY = 6.5; + } else { + targetX = 14.642417; + targetY = 6.5; + } + shooter.setVelocity(14); + + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + shooter.setVelocity(14); + Pose2d robotPose = RobotContainer.swerveDrive.getPose(); + Translation2d vec = new Translation2d(targetX, targetY).minus(robotPose.getTranslation()); + // SmartDashboard.putNumber("targetx", targetX); + // SmartDashboard.putNumber("targety", targetY); + // double armAngle = Shooter.shooterTable.get(vec.getNorm()); + RobotContainer.arm.setSetpoint(ArmState.kFeedFromCenter.rotations); + SmartDashboard.putNumber("distance", vec.getNorm()); + + angleSetpoint = Rotation2d.fromRadians(Math.atan2(vec.getY(), vec.getX())).plus(Rotation2d.fromDegrees(180)); + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + var xSpeed = 0.0; + if (Math.abs(RobotContainer.driverController.getLeftY()) > 0.05) { + xSpeed = swerveDrive.sensControl(-RobotContainer.driverController.getLeftY()) * SwerveDrive.kMaxSpeedMetersPerSecond; + } + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + var ySpeed = 0.0; + if (Math.abs(RobotContainer.driverController.getLeftX()) > 0.05) { + ySpeed = swerveDrive.sensControl(-RobotContainer.driverController.getLeftX()) * SwerveDrive.kMaxSpeedMetersPerSecond; + } + boolean fieldRelative = (RobotContainer.driverController.getRightTriggerAxis() < 0.3); + + var sign = fieldRelative && !DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Red ? -1 : 1; + swerveDrive.holdAngleWhileDriving(sign*xSpeed, sign*ySpeed, angleSetpoint, fieldRelative); + + // for later + // if (RobotContainer.arm.isAtSetpoint()) { + // shooter.feed(); + // } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return false; //!hasNoteInitially || !hasAlliance || timer.hasElapsed(0.33); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + //shooter.stopShooting(); + shooter.stopIndexer(); + //swerveDrive.stop(); + timer.stop(); + timer.reset(); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/FeedShot.java b/src/main/java/frc/robot/commands/shooter/FeedShot.java new file mode 100644 index 0000000..7d34a14 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/FeedShot.java @@ -0,0 +1,115 @@ +package frc.robot.commands.shooter; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Arm.ArmState; +import frc.robot.subsystems.drive.SwerveDrive; + +public class FeedShot extends Command { + SwerveDrive swerveDrive; + Shooter shooter; + Alliance color; + boolean hasAlliance, hasNoteInitially; + double targetX, targetY, targetZ; + final double v0 = 17; + boolean goodTrajectory = true; + public Timer timer; + Rotation2d angleSetpoint; + + public FeedShot() { + this.addRequirements(RobotContainer.swerveDrive, RobotContainer.arm, RobotContainer.shooter); + swerveDrive = RobotContainer.swerveDrive; + shooter = RobotContainer.shooter; + timer = new Timer(); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + // if (!shooter.hasNote()) { + // hasNoteInitially = false; + // return; + // } + // hasNoteInitially = true; + + Optional alliance = DriverStation.getAlliance(); + hasAlliance = alliance.isPresent(); + if (!hasAlliance) { + return; + } + + color = alliance.get(); + if (color == Alliance.Blue) { + targetX = 1.905217; + targetY = 6.1; + } else { + targetX = 14.642417; + targetY = 6.1; + } + shooter.setVelocity(17); + + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + Pose2d robotPose = RobotContainer.swerveDrive.getPose(); + Translation2d vec = new Translation2d(targetX, targetY).minus(robotPose.getTranslation()); + // SmartDashboard.putNumber("targetx", targetX); + // SmartDashboard.putNumber("targety", targetY); + // double armAngle = Shooter.shooterTable.get(vec.getNorm()); + RobotContainer.arm.setSetpoint(ArmState.kFeedFromCenter.rotations); + SmartDashboard.putNumber("distance", vec.getNorm()); + + angleSetpoint = Rotation2d.fromRadians(Math.atan2(vec.getY(), vec.getX())).plus(Rotation2d.fromDegrees(180)); + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + var xSpeed = 0.0; + if (Math.abs(RobotContainer.driverController.getLeftY()) > 0.05) { + xSpeed = swerveDrive.sensControl(-RobotContainer.driverController.getLeftY()) * SwerveDrive.kMaxSpeedMetersPerSecond; + } + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + var ySpeed = 0.0; + if (Math.abs(RobotContainer.driverController.getLeftX()) > 0.05) { + ySpeed = swerveDrive.sensControl(-RobotContainer.driverController.getLeftX()) * SwerveDrive.kMaxSpeedMetersPerSecond; + } + boolean fieldRelative = (RobotContainer.driverController.getRightTriggerAxis() < 0.3); + + var sign = fieldRelative && !DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Red ? -1 : 1; + swerveDrive.holdAngleWhileDriving(sign*xSpeed, sign*ySpeed, angleSetpoint, fieldRelative); + + // for later + // if (RobotContainer.arm.isAtSetpoint()) { + // shooter.feed(); + // } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return false; //!hasNoteInitially || !hasAlliance || timer.hasElapsed(0.33); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + //shooter.stopShooting(); + shooter.stopIndexer(); + //swerveDrive.stop(); + timer.stop(); + timer.reset(); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java b/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java index a6516a0..62d6b30 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java +++ b/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java @@ -89,8 +89,8 @@ public void execute() { } boolean fieldRelative = (RobotContainer.driverController.getRightTriggerAxis() < 0.3); - - swerveDrive.holdAngleWhileDriving(-xSpeed, -ySpeed, angleSetpoint, fieldRelative); + var sign = fieldRelative && !DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Red ? -1 : 1; + swerveDrive.holdAngleWhileDriving(sign*xSpeed, sign*ySpeed, angleSetpoint, fieldRelative); // for later // if (RobotContainer.arm.isAtSetpoint()) { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 3e1d58d..c857115 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -89,9 +89,9 @@ public boolean isArmClearingIntake() { public enum ArmState { kStow(0.63), kDeployDemonHorns(0.605), - kIntake(0.827), + kIntake(0.822), kShoot(0.75), - kShootFromSubwoofer(0.75), + kShootFromSubwoofer(0.76), kShootFromSubwooferAuto(0.7575), kShootFromPodium(0.7075), kAmp(0.513), diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 36353a1..1ceb39e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -51,7 +51,7 @@ public Intake() { intakeMotor.setSmartCurrentLimit(20); pivotEncoder = pivotMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - pivotEncoder.setZeroOffset(0.36); + pivotEncoder.setZeroOffset(0.43); pivotPositionController = new PIDController(kP, kI, kD); pivotPositionController.setTolerance(1); @@ -116,9 +116,8 @@ public boolean hasNoteInside() { } public enum IntakeState { - kUp(0.43), - kMid(0.15), - kDown(0.01); + kUp(0.7), + kDown(0.25); public final double rotations; diff --git a/src/main/java/frc/robot/subsystems/LEDStrip.java b/src/main/java/frc/robot/subsystems/LEDStrip.java index b9d0431..0d3faf1 100644 --- a/src/main/java/frc/robot/subsystems/LEDStrip.java +++ b/src/main/java/frc/robot/subsystems/LEDStrip.java @@ -215,22 +215,28 @@ public void runDisabled() { // if there is a note flash on and off really fast, if there's not a note run // disabled pattern public void runIntake() { - ticksPerSecond = 50; - if (RobotContainer.shooter.hasNote() || RobotContainer.intake.hasNoteInside()) { - if (counter % 3 == 0) { + try { + ticksPerSecond = 50; + if (!RobotContainer.shooter.hasNote() && RobotContainer.intake.hasNoteInside()) { + for (int i = 0; i < Constants.LED_LENGTH; i++) { + setRGB(i, 255, 30, 0); + } + } else if (RobotContainer.shooter.hasNote()) { + for (int i = 0; i < Constants.LED_LENGTH; i++) { + setRGB(i, 255, 255, 0); + } + } else if (RobotContainer.intakeCamera.getNearestNote().isPresent()) { for (int i = 0; i < Constants.LED_LENGTH; i++) { - setRGB(i, 255, 50, 0); + setRGB(i, 0, 100, 0); } } else { + // runSquareWave(new Color(255, 50, 0), -0.6f, 10f); for (int i = 0; i < Constants.LED_LENGTH; i++) { - setRGB(i, 255, 50, 0); + setRGB(i, 0, 0, 0); } } - } else { - // runSquareWave(new Color(255, 50, 0), -0.6f, 10f); - for (int i = 0; i < Constants.LED_LENGTH; i++) { - setRGB(i, 0, 0, 0); - } + } catch (Exception exception) { + exception.printStackTrace(); } } @@ -330,20 +336,22 @@ public void update() { private void updateState() { var isDisabled = RobotState.isDisabled(); - // var isScoringSpeaker = RobotContainer.shooter.veloctiySetpoint == 15 || RobotContainer.shooter.veloctiySetpoint == 20; - // var isScoringAmp = Math.abs(RobotContainer.arm.getPosition() - ArmState.kAmp.rotations) < 0.02; + // var isScoringSpeaker = RobotContainer.shooter.veloctiySetpoint == 15 || + // RobotContainer.shooter.veloctiySetpoint == 20; + // var isScoringAmp = Math.abs(RobotContainer.arm.getPosition() - + // ArmState.kAmp.rotations) < 0.02; // var hasIntakeDeployed = RobotContainer.intake.isIntakeDown(); // if (isScoringSpeaker) - // state = LEDState.kShoot; + // state = LEDState.kShoot; // else if (isScoringAmp) - // state = LEDState.kAmp; + // state = LEDState.kAmp; // else if (hasIntakeDeployed) - // state = LEDState.kIntake; + // state = LEDState.kIntake; // else if (isDisabled) - // state = LEDState.kDisabled; - // else - // state = LEDState.kStowed; + // state = LEDState.kDisabled; + // else + // state = LEDState.kStowed; if (isDisabled) { state = LEDState.kDisabled; } else { diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 6819390..ae995b9 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -341,7 +341,7 @@ public void updateOdometry() { // vision if (RobotState.isTeleop() || RobotState.isDisabled()) { - updateOdometryUsingFrontCamera(); + // updateOdometryUsingFrontCamera(); updateOdometryUsingRearCamera(); } }