diff --git a/src/main/deploy/pathplanner/autos/1-Leave-Delayed.auto b/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto similarity index 95% rename from src/main/deploy/pathplanner/autos/1-Leave-Delayed.auto rename to src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto index c7112f0..8e757da 100644 --- a/src/main/deploy/pathplanner/autos/1-Leave-Delayed.auto +++ b/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto @@ -24,7 +24,7 @@ { "type": "wait", "data": { - "waitTime": 12.0 + "waitTime": 8.0 } } ] diff --git a/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto b/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto new file mode 100644 index 0000000..7e3a5fe --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto @@ -0,0 +1,44 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.75, + "y": 6.75 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "MoveOut-Left" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto b/src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto new file mode 100644 index 0000000..fa76be9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto @@ -0,0 +1,38 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7516461186768542, + "y": 4.35 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MoveOut-Right" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto similarity index 87% rename from src/main/deploy/pathplanner/autos/2-Center-Stays.auto rename to src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto index fc696f2..407a129 100644 --- a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto @@ -25,12 +25,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto new file mode 100644 index 0000000..fe3a087 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays.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..8ac0f0a 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -25,12 +25,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, { "type": "path", "data": { @@ -49,6 +43,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { @@ -63,12 +63,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, { "type": "path", "data": { @@ -87,6 +81,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { @@ -101,12 +101,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, { "type": "path", "data": { @@ -125,6 +119,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/shoot only.auto b/src/main/deploy/pathplanner/autos/shoot only.auto new file mode 100644 index 0000000..362eaf9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/shoot only.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file 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 be75a1b..d504383 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,7 +106,8 @@ public static final class IntakeConstants { public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle) /** Encoder offset in rotations */ - public static final double kArmEncoderOffset = 0.3415; + //public static final double kArmEncoderOffset = 0.3415; + public static final double kArmEncoderOffset = 0.504; public static final double kIntakeSpeed = 0.5; @@ -145,7 +146,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 { @@ -154,8 +159,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 965463f..af63cc1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,8 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +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; @@ -18,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -25,6 +28,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; @@ -49,7 +53,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); @@ -64,9 +68,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( @@ -74,6 +78,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)); @@ -89,7 +96,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), @@ -150,9 +168,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)) @@ -175,18 +193,18 @@ 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(() -> { - return m_operatorController.getAButton() && m_operatorController.getRightBumper(); - }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); + // new Trigger(() -> { + // return m_operatorController.getAButton() && m_operatorController.getRightBumper(); + // }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); - // Climber Down, Operator Controller Right Bumper + B Button - new Trigger(() -> { - return m_operatorController.getBButton() && m_operatorController.getRightBumper(); - }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); + // // Climber Down, Operator Controller Right Bumper + B Button + // new Trigger(() -> { + // return m_operatorController.getBButton() && m_operatorController.getRightBumper(); + // }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); // Toggle Distance Sensor, Operatoe Controller Left Bumper + Start Button new Trigger(() -> { @@ -209,17 +227,29 @@ 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(); + + // return new SequentialCommandGroup( + // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 3), + // new ParallelDeadlineGroup(new WaitCommand(1.5), new NoteOuttakeCommand(m_intakeSubsystem)), + // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01) + // , new ParallelDeadlineGroup(new WaitCommand(20), new RepeatCommand(new InstantCommand(() -> m_robotDrive.autonDrive(new ChassisSpeeds(3, 0, 0))))) + // ); + + + } } 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 9da64d1..1dbfc8c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -135,15 +135,17 @@ public void periodic() { haveNote = getDistanceSensorToggle() ? getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold : false; // Note: negative because encoder goes from 0 to -193 cuz weird - double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.4, 0.4); + double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3); m_armMotor.set(armMotorSpeed); m_intakeMotor.set(m_intakeSpeed); 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); + SmartDashboard.putNumber("Get offset", m_armEncoder.getPositionOffset()); } public boolean haveNote() { 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 } }