diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto index fc696f2..a2e0b01 100644 --- a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/2-Right-Leave.auto b/src/main/deploy/pathplanner/autos/2-Right-Leave.auto index 9c80858..995cddf 100644 --- a/src/main/deploy/pathplanner/autos/2-Right-Leave.auto +++ b/src/main/deploy/pathplanner/autos/2-Right-Leave.auto @@ -21,6 +21,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "path", "data": { @@ -42,6 +48,12 @@ "name": "Shoot" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto new file mode 100644 index 0000000..fe3a087 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto @@ -0,0 +1,101 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3470318873095533, + "y": 5.5216374597852385 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CenterNote-Center" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftNote-Center" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index 6e755e0..613aa97 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { @@ -49,6 +49,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { @@ -66,7 +72,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { @@ -87,6 +93,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { @@ -104,7 +116,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { @@ -125,6 +137,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/CenterNote-Center.path b/src/main/deploy/pathplanner/paths/CenterNote-Center.path index 0776d16..b50dfaa 100644 --- a/src/main/deploy/pathplanner/paths/CenterNote-Center.path +++ b/src/main/deploy/pathplanner/paths/CenterNote-Center.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -64,5 +64,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftNote-Center.path b/src/main/deploy/pathplanner/paths/LeftNote-Center.path index 2a7d69e..d87c8f2 100644 --- a/src/main/deploy/pathplanner/paths/LeftNote-Center.path +++ b/src/main/deploy/pathplanner/paths/LeftNote-Center.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -70,5 +70,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8fb314f..17cf494 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -119,7 +119,11 @@ public static final class ShooterConstants { public static final int kBottomShooterMotorPort = 35; public static final double kShooterSpeedTop = 0.8; public static final double kShooterSpeedBottom = 0.9; + public static final double kPreShooterSpeed = 0.4; public static final double kShooterOff = 0; + + public static final double kShooterOffTime = 0.04; + public static final double kShooterOnTime = 1.9; } public static class ClimberConstants { @@ -128,8 +132,8 @@ public static class ClimberConstants { public final static int leftReverseChannel = 2; public final static int rightReverseChannel = 3; - public final static double minPressure = 100.0; - public final static double maxPressure = 110.0; + public final static double minPressure = 95.0; + public final static double maxPressure = 105.0; } public static final class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index daaa2fc..0431825 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -25,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.commands.IntakeArmPositionCommand; import frc.robot.commands.NoteIntakeCommand; import frc.robot.commands.NoteOuttakeCommand; @@ -62,9 +64,9 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), - new ParallelDeadlineGroup(new WaitCommand(2), new NoteOuttakeCommand(m_intakeSubsystem)), - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off))); + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), + new ParallelDeadlineGroup(new WaitCommand(0.8), new NoteOuttakeCommand(m_intakeSubsystem)), + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", new SequentialCommandGroup( @@ -72,6 +74,9 @@ public RobotContainer() { new NoteIntakeCommand(m_intakeSubsystem), new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))); + NamedCommands.registerCommand("Pre-Speed - 30%", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Halfway, 0.01)); + NamedCommands.registerCommand("Intake in", new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); @@ -87,7 +92,18 @@ public RobotContainer() { // from robot center to // furthest module. new ReplanningConfig(false, false)), - () -> false, m_robotDrive); + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, m_robotDrive); // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, // ShootSpeed.Shooting), @@ -147,9 +163,9 @@ private void configureBindings() { // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), + .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)); + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); new JoystickButton(m_driverController, Button.kRightBumper.value) .onTrue(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp)) @@ -172,8 +188,8 @@ private void configureBindings() { new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; - }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting)) - .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)); + }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime)) + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); // Climber Up, Operator Controller Right Bumper + A Button new Trigger(() -> { @@ -206,17 +222,20 @@ public void resetAllSubsystems() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // PathPlannerPath path = PathPlannerPath.fromPathFile("Center4Note"); + + /// List pathGroup = + /// PathPlannerAuto.getPathGroupFromAutoFile(autoChooser.getSelected().getName()); + // PathPlannerAuto path = PathPlannerAuto.getPathGroupFromAutoFile(pathGroup); // var alliance = DriverStation.getAlliance(); - // PathPlannerPath autonPath = path; + // PathPlannerPath pathGroup = path; // if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { // autonPath = autonPath.flipPath(); // } // m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - // return AutoBuilder.followPath(autonPath); - // return null; + // return new PathPlannerAuto(autoChooser.getSelected().getName()); return autoChooser.getSelected(); + } } diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java index 46d32c9..ecb4581 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -13,15 +13,19 @@ public class ShooterSetSpeedCommand extends Command { ShooterSubsystem m_ShooterSubsystem; double m_shooterSpeed = 0; + + double set_time = 1.5; Timer m_timer = new Timer(); ShootSpeed m_shootSpeed; /** Creates a new ShootCommand. */ - public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed) { + public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed, double time) { m_ShooterSubsystem = shooterSubsystem; addRequirements(m_ShooterSubsystem); + set_time = time; + m_shootSpeed = shootSpeed; } @@ -47,6 +51,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_timer.get() > 1.5; + return m_timer.get() > set_time; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e630d6d..80e1453 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -126,6 +126,7 @@ public void periodic() { SmartDashboard.putNumber("intakespeed", m_intakeSpeed); SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); + SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches) : -1); SmartDashboard.putNumber("pid output", armMotorSpeed); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3629c74..3e0ccb9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -42,6 +42,10 @@ public void setShootingSpeed(ShootSpeed speed) { m_topSpeed = ShooterConstants.kShooterSpeedTop; m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; + case Halfway: + m_topSpeed = ShooterConstants.kPreShooterSpeed; + m_bottomSpeed = ShooterConstants.kPreShooterSpeed; + break; case Off: m_topSpeed = 0.0; m_bottomSpeed = 0.0; @@ -65,6 +69,7 @@ public void periodic() { public static enum ShootSpeed { Shooting, + Halfway, Off } }