diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd8b448..0db4e64 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,12 +32,14 @@ import frc.robot.commands.hoodedShooterCommands.ConstantShootCommand; import frc.robot.commands.hoodedShooterCommands.MaxPowerShootCommand; import frc.robot.commands.hoodedShooterCommands.PidShootCommand; +import frc.robot.commands.hoodedShooterCommands.SpinUpShooterCommand; import frc.robot.commands.drivingCommands.DisableRampingCommand; import frc.robot.commands.drivingCommands.DriveEncoders; import frc.robot.commands.drivingCommands.EnableBrakeModeCommand; import frc.robot.commands.drivingCommands.SetSpeedMode; import frc.robot.commands.drivingCommands.StopDrivetrainCommand; import frc.robot.commands.drivingCommands.TankDriveCommand; +import frc.robot.commands.drivingCommands.TurnCommand; import frc.robot.commands.drivingCommands.GTADriveCommand; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveTrainSubsystem; @@ -77,6 +79,8 @@ public class RobotContainer { enum PossibleAutos { IN_FRONT_OF_TARGET_MAX_POWER, IN_FRONT_OF_TARGET_MAX_POWER_THEN_BACK, + SHOOT_THEN_DRIVE_JULY_HEAT, + DRIVE_JULY_HEAT, } SendableChooser autoChoice = new SendableChooser(); @@ -238,6 +242,10 @@ public Command getAutonomousCommand() { return getAutoInFrontOfTarget(); case IN_FRONT_OF_TARGET_MAX_POWER_THEN_BACK: return getAutoInFrontOfTargetThenBack(); + case SHOOT_THEN_DRIVE_JULY_HEAT: + return getAutoShootThenDriveJulyHEAT(); + case DRIVE_JULY_HEAT: + return getAutoDriveJulyHEAT(); default: return null; } @@ -274,6 +282,41 @@ private Command getAutoInFrontOfTargetThenBack() { .andThen((shoot.withTimeout(5)).andThen(driveBack)); } + private Command getAutoShootThenDriveJulyHEAT() { + // This is the speed the shooter will spin + double shooterSpeed = 0.37; + // This command drives forward 4 feet when run + Command driveForward = new DriveEncoders(1.2192, .5, driveTrain); + // Turns around 180 degrees + Command turnAround = new TurnCommand(175, .5, driveTrain, navigation); + // This spins up the shooter - note: it doesn't stop the shooter, which might be concerning + Command spinUp = new SpinUpShooterCommand(shooter, shooterSpeed); + // This shoots with PID - We should adjust the value to the setpoint at wherever we start the + // bot + Command shoot = new PidShootCommand(shooter, shooterSpeed); + // This aims the turret + Command aimTurret = new PointTurretAtTargetWithAngleCommand(turret); + // This indexes the horizontal indexer in + Command horizIn = new HorizontalIndexerIntakeCommand(horizontalIndexer); + // This indexes the vertical indexer up + Command vertUp = new VerticalIndexerUpCommand(verticalIndexer); + + // This lets the shooter spin, then keeps it spinning and indexes up for 5 seconds to allow all + // balls to be shot, then drives forwards. While it does this the turret aims. Times can be + // adjusted as needed + return aimTurret.alongWith( + spinUp.andThen( + (shoot.alongWith(horizIn.alongWith(vertUp)).withTimeout(5)) + .andThen(turnAround.andThen(driveForward)))); + } + + private Command getAutoDriveJulyHEAT() { + // This command drives forward 4 feet when run + Command driveForward = new DriveEncoders(1.2192, .5, driveTrain); + + return driveForward; + } + public Command getStopDriveTrainCommand() { return new StopDrivetrainCommand(driveTrain); } diff --git a/src/main/java/frc/robot/commands/hoodedShooterCommands/SpinUpShooterCommand.java b/src/main/java/frc/robot/commands/hoodedShooterCommands/SpinUpShooterCommand.java new file mode 100644 index 0000000..77342ff --- /dev/null +++ b/src/main/java/frc/robot/commands/hoodedShooterCommands/SpinUpShooterCommand.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/*Note - this command works just like PidShootCommand with 3 changes: 1. It stops when the shooter is at the speed. 2. It doesn't stop the shooter when it ends +3. It prints to smart dashboard with a different prefix. Change 2 might be problematic +*/ + +package frc.robot.commands.hoodedShooterCommands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.HoodedShooterSubsystem; + +public class SpinUpShooterCommand extends CommandBase { + HoodedShooterSubsystem shooter; + double velocityFraction; + /** Creates a new SpinUpShooterCommand. */ + public SpinUpShooterCommand(HoodedShooterSubsystem shooter, double velocityFraction) { + this.shooter = shooter; + this.velocityFraction = velocityFraction; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double shootingVelocityFraction = + SmartDashboard.getNumber("Commands.SpinUpShooter.topSpeedFraction", this.velocityFraction); + SmartDashboard.putNumber("Commands.SpinUpShooter.topSpeedFraction", shootingVelocityFraction); + + shooter.setShooterVelFraction(velocityFraction); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // Update it every time to ensure that it keeps working - there was some weirdness this helped + // fix + SmartDashboard.putNumber("Commands.SpinUpShooter.speedFraction", velocityFraction); + shooter.setShooterVelFraction(velocityFraction); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return shooter.isShooterReady(); + } +}