From 743b3d31e06480da4aa44affd82817643b3324e5 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Wed, 6 Mar 2024 18:51:36 -0800 Subject: [PATCH] Works --- simgui.json | 7 +++ .../deploy/pathplanner/autos/New Auto.auto | 50 +++++++++++++++++ .../{Center4Note.path => CenterNote.path} | 54 +------------------ src/main/java/frc/robot/RobotContainer.java | 43 +++++++++------ .../frc/robot/subsystems/IntakeSubsystem.java | 8 +-- 5 files changed, 88 insertions(+), 74 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto rename src/main/deploy/pathplanner/paths/{Center4Note.path => CenterNote.path} (57%) diff --git a/simgui.json b/simgui.json index 905d0ec..a484da0 100644 --- a/simgui.json +++ b/simgui.json @@ -2,10 +2,17 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/SmartDashboard/Auto Chooser": "String Chooser", "/SmartDashboard/Field": "Field2d" }, "windows": { "/SmartDashboard/Field": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, "window": { "visible": true } diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..5189c4b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3470318873095533, + "y": 5.5216374597852385 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center4Note.path b/src/main/deploy/pathplanner/paths/CenterNote.path similarity index 57% rename from src/main/deploy/pathplanner/paths/Center4Note.path rename to src/main/deploy/pathplanner/paths/CenterNote.path index 2273f2f..d41e21e 100644 --- a/src/main/deploy/pathplanner/paths/Center4Note.path +++ b/src/main/deploy/pathplanner/paths/CenterNote.path @@ -46,59 +46,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "ShootPreloaded", - "waypointRelativePos": 0.0, - "command": { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Shoot" - } - } - ] - } - } - }, - { - "name": "IntakeNote2", - "waypointRelativePos": 0.05, - "command": { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - }, - { - "name": "ShootNote2", - "waypointRelativePos": 2.0, - "command": { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Shoot" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index da889bc..b0103cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ 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; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -53,10 +54,23 @@ public class RobotContainer { private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + private final SendableChooser autoChooser; + /** * The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { + NamedCommands.registerCommand("Shoot", + new SequentialCommandGroup( + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), + new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)), + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off))); + + NamedCommands.registerCommand("Intake", + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))); AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, m_robotDrive::getChassisSpeeds, @@ -72,15 +86,6 @@ public RobotContainer() { new ReplanningConfig(true, true)), () -> false, m_robotDrive); - NamedCommands.registerCommand("Shoot", - new SequentialCommandGroup(new InstantCommand(() -> SmartDashboard.putBoolean("shooting", true)), - new WaitCommand(2), - new InstantCommand(() -> SmartDashboard.putBoolean("shooting", false)))); - - NamedCommands.registerCommand("Intake", - new SequentialCommandGroup(new InstantCommand(() -> SmartDashboard.putBoolean("Intaking", true)), - new WaitCommand(2), - new InstantCommand(() -> SmartDashboard.putBoolean("Intaking", false)))); // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, // ShootSpeed.Shooting), // new ParallelDeadlineGroup(new WaitCommand(0.5), new @@ -88,6 +93,9 @@ public RobotContainer() { m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + // Configure the trigger bindings configureBindings(); @@ -180,16 +188,17 @@ public void resetAllSubsystems() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - PathPlannerPath path = PathPlannerPath.fromPathFile("Center4Note"); + // PathPlannerPath path = PathPlannerPath.fromPathFile("Center4Note"); - var alliance = DriverStation.getAlliance(); - PathPlannerPath autonPath = path; - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - autonPath = autonPath.flipPath(); - } - m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); + // var alliance = DriverStation.getAlliance(); + // PathPlannerPath autonPath = path; + // if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + // autonPath = autonPath.flipPath(); + // } + // m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - return AutoBuilder.followPath(autonPath); + // return AutoBuilder.followPath(autonPath); // return null; + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 08eac22..d372c3b 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -27,14 +27,14 @@ public class IntakeSubsystem extends SubsystemBase { private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); - private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port; + //private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port; private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { - m_distanceSensor.setAutomaticMode(true); + //m_distanceSensor.setAutomaticMode(true); m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); @@ -97,7 +97,7 @@ public void stopIntake() { * Gets distance from Rev 2m sensor */ public double getDistanceSensor() { - return m_distanceSensor.getRange(); + return 0;//m_distanceSensor.getRange(); } @Override @@ -112,7 +112,7 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); SmartDashboard.putBoolean("Have Note?", haveNote); - SmartDashboard.putNumber("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches)); + //SmartDashboard.putNumber("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches)); SmartDashboard.putNumber("pid output", setMotorSpeed); }