diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1f1bf92..bd7c489 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,11 +5,8 @@ package frc.robot; -import java.util.ArrayList; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -36,15 +33,6 @@ public class Robot extends TimedRobot { @Override public void robotInit() { DriverStation.silenceJoystickConnectionWarning(true); - - // Starts recording to data log - // DataLogManager.start(); - // Record both DS control and joystick data - // DriverStation.startDataLog(DataLogManager.getLog()); - - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our autonomous chooser on the dashboard. - robotContainer = RobotContainer.getInstance(); // for debugging CommandScheduler.getInstance().onCommandInitialize((command) -> { @@ -59,6 +47,10 @@ public void robotInit() { var cmdName = command.getName(); System.out.println(cmdName + " ended."); }); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = RobotContainer.getInstance(); } /** @@ -79,7 +71,6 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the // robot's periodic // block in order for anything in the Command-based framework to work. - SmartDashboard.getEntry("LED State").getInstance().flush(); CommandScheduler.getInstance().run(); } @@ -96,9 +87,9 @@ public void disabledInit() { public void disabledPeriodic() { RobotContainer.intake.setSetpoint(RobotContainer.intake.getPosition()); RobotContainer.arm.setSetpoint(RobotContainer.arm.getPosition()); + SwerveDrive.kMaxAngularSpeedRadiansPerSecond = 3 * Math.PI; SwerveDrive.kMaxSpeedMetersPerSecond = 4.6; - // RobotContainer.shooter.setSetpoint(RobotContainer.shooter.getPosition()); } /** @@ -107,42 +98,22 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - autonomousRoutine = new ProxyCommand(() -> robotContainer.getAutonomousCommand()).andThen(new InstantCommand(() -> { - RobotContainer.swerveDrive.stop(); - RobotContainer.shooter.stopIndexer(); - RobotContainer.shooter.stopShooting(); - RobotContainer.intake.stop(); - }, RobotContainer.swerveDrive, RobotContainer.shooter, RobotContainer.intake)); - - // if autonomousRoutine is custom use RobotContainer.fullAuto(start, note...) - // somehow get and parse a string from glass for start and note... - if (autonomousRoutine.getName().equals("Dynamic")) { - String commandString = SmartDashboard.getString("DynamicAutoString", ""); - String[] split = commandString.split(" "); - ArrayList splitList = new ArrayList<>(); - for (String s : split) { - splitList.add(s); - } - ArrayList notePoseStrings = new ArrayList<>(); - - for (int i = 1; i < split.length; i++) { - notePoseStrings.add(splitList.get(i)); - } - - String[] notePoseStringsArray = new String[notePoseStrings.size()]; - for (int i = 0; i < notePoseStrings.size(); i++) { - notePoseStringsArray[i] = notePoseStrings.get(i); - } + autonomousRoutine = robotContainer.getAutonomousCommand(); - if (commandString.equals("")) { - autonomousRoutine = robotContainer.fullAuto("center"); - } else { - autonomousRoutine = robotContainer.fullAuto(splitList.get(0), notePoseStringsArray); - } + if (autonomousRoutine != null) { + // for the end of the auto routine + var stopAllCmd = new InstantCommand(() -> { + RobotContainer.swerveDrive.stop(); + RobotContainer.shooter.stopIndexer(); + RobotContainer.shooter.stopShooting(); + RobotContainer.intake.stop(); + }, RobotContainer.swerveDrive, RobotContainer.shooter, RobotContainer.intake); - } + + // get the auto routine as a proxy command so we are free to compose a sequential command group + // using it, run the auto routine then stop all motors + new ProxyCommand(() ->autonomousRoutine).andThen(stopAllCmd); - if (autonomousRoutine != null) { autonomousRoutine.schedule(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 03154ff..0e956c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; @@ -27,7 +26,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ProxyCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; @@ -35,7 +33,6 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.Dynamic; import frc.robot.commands.arm.SetArmState; import frc.robot.commands.drive.AutoIntake; import frc.robot.commands.drive.AutoPickupNote; @@ -81,30 +78,6 @@ public AutoBuilder getAutoBuilder() { public static final AHRS navx = new AHRS(Port.kUSB); - /** - * Returns the angle or "yaw" of the robot in degrees. CW positive ranging from - * [-180, 180]. - */ - public static double getRobotYaw() { - return navx.getYaw(); - } - - /** - * Returns the pitch angle of the robot in degrees. This tracks the - * forwards/backwards tilt of the robot. - */ - public static double getRobotPitch() { - return navx.getPitch(); - } - - /** - * Returns the roll angle of the robot in degrees. This tracks the - * left/right tilt of the robot. - */ - public static double getRobotRoll() { - return navx.getRoll(); - } - public static final Field2d field = new Field2d(); // Human Interface Devices (HIDs) @@ -117,7 +90,6 @@ public static double getRobotRoll() { public static final Shooter shooter = new Shooter(); public static final Arm arm = new Arm(); public static final Climber climber = new Climber(); - // public static final NewLEDStrip ledStrip = new NewLEDStrip(); public static final LEDStrip leds = new LEDStrip(); // Cameras @@ -368,9 +340,7 @@ private void configureAutonmous() { autoBuilder = new AutoBuilder(); autoChooser = AutoBuilder.buildAutoChooser(); - autoChooser.addOption("Dynamic", new Dynamic()); SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putString("DynamicAutoString", ""); configurePathPlannerLogging(); } @@ -413,7 +383,7 @@ private void registerNamedCommands() { // General NamedCommands.registerCommand("ShootSubwooferSequence", shootSubwooferSequence()); NamedCommands.registerCommand("InitSequence", initSequence()); - + NamedCommands.registerCommand("StopAll", stopAllCommand()); } private Command stopAllCommand() { @@ -444,79 +414,25 @@ private Command straightenSwervesCommand() { } private Command intakeSetpointAndRun() { - return ( - - new SetArmState(ArmState.kIntake) - .andThen( - new InstantCommand(() -> { - intake.intake(); - shooter.indexAuto(); - }, intake, shooter))); + return new SetArmState(ArmState.kIntake) + .andThen(new InstantCommand(() -> { + intake.intake(); + shooter.indexAuto(); + }, intake, shooter)); } private Command initSequence() { - return new ParallelDeadlineGroup(new SetIntakeState(IntakeState.kDown), - new SetArmState(ArmState.kDeployDemonHorns).alongWith( - new RampShooter(() -> 15)));//.alongWith(straightenSwervesCommand())); + return new ParallelDeadlineGroup( + new SetIntakeState(IntakeState.kDown), + new SetArmState(ArmState.kDeployDemonHorns).alongWith(new RampShooter(() -> 15))); } private Command shootSubwooferSequence() { - return new SequentialCommandGroup(new RampShooter(() -> 15) - .alongWith(new SetArmState(ArmState.kShootFromSubwooferAuto)), new WaitCommand(0.1), feedCommand(), - intakeSetpointAndRun()); - } - - private Command getPathCommand(String name) { - PathPlannerPath path = PathPlannerPath.fromPathFile(name); - return AutoBuilder.followPath(path); - } - - private PathPlannerPath getPath(String name) { - return PathPlannerPath.fromPathFile(name); - } - - private Pose2d pathStartPose(String name) { - return getPath(name).getPreviewStartingHolonomicPose(); - } - - // this will get a string or something from glass idk yet - private Command getNoteSequence(String startPosition, String notePosition) { - String getNotePathName = startPosition + " get " + notePosition; - String getReturnPathName = "return " + startPosition + " get " + notePosition; return new SequentialCommandGroup( - new ParallelCommandGroup( - getPathCommand(getNotePathName).andThen(new InstantCommand(() -> swerveDrive.stop(), swerveDrive)), - new WaitForNote().withTimeout(3)), - getPathCommand(getReturnPathName), - new InstantCommand(() -> swerveDrive.stop(), swerveDrive), - shootSubwooferSequence()); - } - - // THIS EXPECTS YOU TO PUT THE NOTE POSITIONS IN THE RIGHT ORDER!! (I don't want - // to sort them based on startPosition because it's 3am and I'm sleepy) - public Command fullAuto(String startPosition, String... notePositions) { - // if no note positions are passed shoot in place and stop everything - if (notePositions.length == 0) { - return initSequence().andThen(shootSubwooferSequence()).andThen(stopAllCommand()); - } - - // set all to lowercase to match the path names - for (String s : notePositions) { - s = s.toLowerCase(); - } - startPosition = startPosition.toLowerCase(); - - // get each note sequence, each noteSequence includes path to get, grab, if - // missed timeout after 3 secs, return path, shoot - Command[] noteSequences = new Command[notePositions.length]; - for (int i = 0; i < notePositions.length; i++) { - System.out.println(notePositions[i]); - noteSequences[i] = getNoteSequence(startPosition, notePositions[i]); - } - - // combine init and all noteSequence - System.out.println(startPosition + notePositions); - return new SequentialCommandGroup(initSequence(), new SequentialCommandGroup(noteSequences)); + new RampShooter(() -> 15).alongWith(new SetArmState(ArmState.kShootFromSubwooferAuto)), + new WaitCommand(0.1), + feedCommand(), + intakeSetpointAndRun()); } private void configurePathPlannerLogging() { diff --git a/src/main/java/frc/robot/commands/Dynamic.java b/src/main/java/frc/robot/commands/Dynamic.java deleted file mode 100644 index a459fcf..0000000 --- a/src/main/java/frc/robot/commands/Dynamic.java +++ /dev/null @@ -1,33 +0,0 @@ -// 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. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; - -public class Dynamic extends Command { - /** Creates a new Dynamic. */ - public Dynamic() { - this.setName("Dynamic"); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // 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 true; - } -} diff --git a/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java b/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java index 3b10f4b..ff7e8d5 100644 --- a/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java +++ b/src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java @@ -9,10 +9,11 @@ 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 { - private boolean fieldRelative = false; + private final boolean fieldRelative; public DriveSwerveWithXbox(boolean fod) { this.addRequirements(RobotContainer.swerveDrive); @@ -27,9 +28,6 @@ public void initialize() { // Called repeatedly when this Command is scheduled to run @Override public void execute() { - - // fieldRelative = RobotContainer.driverController.getHID().getRightTriggerAxis() > 0.5; - // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. var xSpeed = 0.0; @@ -57,7 +55,7 @@ public void execute() { * SwerveDrive.kMaxAngularSpeedRadiansPerSecond; } - var sign = fieldRelative ? 1 : -1; + var sign = fieldRelative || RobotContainer.arm.isAtState(ArmState.kAmp) ? 1 : -1; RobotContainer.swerveDrive.drive(-xSpeed*sign, -ySpeed*sign, rot, fieldRelative); } diff --git a/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java b/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java index 13e6617..bd119d3 100644 --- a/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java +++ b/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java @@ -3,15 +3,14 @@ import edu.wpi.first.math.geometry.Rotation2d; 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 { - private boolean fieldRelative = false; private Rotation2d angleSetpoint; - public HoldAngleWhileDriving(boolean fod) { + public HoldAngleWhileDriving() { this.addRequirements(RobotContainer.swerveDrive); - this.fieldRelative = fod; } // Called just before this Command runs the first time @@ -23,9 +22,6 @@ public void initialize() { // Called repeatedly when this Command is scheduled to run @Override public void execute() { - - fieldRelative = RobotContainer.driverController.getHID().getRightTriggerAxis() > 0.5; - // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. var xSpeed = 0.0; @@ -43,17 +39,8 @@ public void execute() { * SwerveDrive.kMaxSpeedMetersPerSecond; } - // Get the rate of angular rotation. We are inverting this because we want a - // positive value when we pull to the left (remember, CCW is positive in - // mathematics). Xbox controllers return positive values when you pull to - // the right by default. - double rot = 0; - if (Math.abs(RobotContainer.driverController.getRightX()) > 0.05) { - rot = RobotContainer.swerveDrive.sensControl(-RobotContainer.driverController.getRightX()) - * SwerveDrive.kMaxAngularSpeedRadiansPerSecond; - } - - var sign = fieldRelative ? 1 : -1; + var fieldRelative = RobotContainer.driverController.getHID().getRightTriggerAxis() > 0.5; + var sign = fieldRelative || RobotContainer.arm.isAtState(ArmState.kAmp) ? 1 : -1; RobotContainer.swerveDrive.holdAngleWhileDriving(-xSpeed*sign, -ySpeed*sign, angleSetpoint, fieldRelative); } diff --git a/src/main/java/frc/robot/subsystems/cameras/Note.java b/src/main/java/frc/robot/subsystems/cameras/Note.java index 1dec23b..22444e9 100644 --- a/src/main/java/frc/robot/subsystems/cameras/Note.java +++ b/src/main/java/frc/robot/subsystems/cameras/Note.java @@ -49,7 +49,7 @@ public boolean isInFov(Pose2d robotPose) { // weights should be based on yaw value, close to edge should be weighted very little // maybe 0-1 with a factor x^2 or x^3 // avg = (sum(W*X)/sum(W)) - private void updatePose() { + protected void updatePose() { double x = 0.0; double y = 0.0;