From 66292f2a9dc17a82ef97134d57532340690ff088 Mon Sep 17 00:00:00 2001 From: Luca Huang Date: Mon, 8 Apr 2024 20:29:12 -0400 Subject: [PATCH 1/9] Created shooterOn for initializing shooter speeds without waiting. Updated DriveToNoteCommand so driver has full control until robot detects note. Set new current limits for shooters to decrease acceleration time. Updated shootSpeakerCommand and shooterOn to accept shooter speed as a parameter. Need testing. --- .../java/frc/robot/FortissiMOEContainer.java | 8 ++- .../robot/commands/DriveToNoteCommand.java | 32 ++++++----- .../commands/ShooterControllerCommand.java | 4 +- .../robot/commands/shootSpeakerCommand.java | 19 ++++--- .../java/frc/robot/commands/shooterOn.java | 54 +++++++++++++------ .../frc/robot/subsystems/HeadSubsystem.java | 0 .../robot/subsystems/ShooterSubsystem.java | 4 +- 7 files changed, 77 insertions(+), 44 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/HeadSubsystem.java diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index cfa7f03..ea0b488 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -360,11 +360,15 @@ private void configureBindings() { var driveToNote = new DriveToNoteCommand( swerveSubsystem, vision, - () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), +// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), + () -> -driverJoystick.getRawAxis(1), + () -> -driverJoystick.getRawAxis(0), + () -> -driverJoystick.getRawAxis(2), (rumblePercent) -> { SmartDashboard.putNumber("JoyRumble", rumblePercent); driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types. - } + }, + maxMPS ); new JoystickButton(driverJoystick, 8).whileTrue(driveToNote); // object detection note pickup button diff --git a/src/main/java/frc/robot/commands/DriveToNoteCommand.java b/src/main/java/frc/robot/commands/DriveToNoteCommand.java index 10198bd..c6a8210 100644 --- a/src/main/java/frc/robot/commands/DriveToNoteCommand.java +++ b/src/main/java/frc/robot/commands/DriveToNoteCommand.java @@ -20,21 +20,27 @@ public class DriveToNoteCommand extends Command { private final Vision vision; private final SwerveDrive subsystem; - private final DoubleSupplier speedSupplier; + private final Supplier xspdFunction,yspdFunction,turnspdFunction; private final DoubleConsumer rumbleCallback; + private final double maxMPS; private Translation2d target = null; private int idleLoopCount = 0; private final double Latency = 0.25;//Pose latency in seconds - public DriveToNoteCommand(SwerveDrive subsystem, Vision vision, DoubleSupplier speedSupplier) { - this(subsystem, vision, speedSupplier, null); + public DriveToNoteCommand(SwerveDrive subsystem, Vision vision, Supplier xspeed, Supplier yspeed, + Supplier turnspeed, double maxMPS) { + this(subsystem, vision, xspeed,yspeed,turnspeed, null, maxMPS); } - public DriveToNoteCommand(SwerveDrive subsystem, Vision vision, DoubleSupplier speedSupplier, DoubleConsumer rumbleCallback){ - this.speedSupplier = speedSupplier; + public DriveToNoteCommand(SwerveDrive subsystem, Vision vision, Supplier xspeed, Supplier yspeed, + Supplier turnspeed, DoubleConsumer rumbleCallback, double maxMPS){ + xspdFunction = xspeed; + yspdFunction = yspeed; + turnspdFunction = turnspeed; this.vision = vision; this.subsystem = subsystem; this.rumbleCallback = rumbleCallback; + this.maxMPS=maxMPS; addRequirements(subsystem); } // Called when the command is initially scheduled. @@ -67,11 +73,12 @@ private void updateTarget() { public void execute() { updateTarget(); if (target == null){ - idleLoopCount += 1; - - if (idleLoopCount >= 5 && rumbleCallback != null){ - rumbleCallback.accept(1); - } +// idleLoopCount += 1; +// +// if (idleLoopCount >= 5 && rumbleCallback != null){ +// rumbleCallback.accept(1); +// } + subsystem.driveAtSpeed(xspdFunction.get(), yspdFunction.get(), turnspdFunction.get(), true); return; } // Put the detection on NetworkTables, for debugging @@ -84,11 +91,11 @@ public void execute() { var robotAngle = unitDelta.getAngle(); if (delta.getNorm() <= Units.inchesToMeters(24)) robotAngle = robotPose.getRotation().times(1); - SmartDashboard.putNumber("robot Object Detection angle", robotAngle.getDegrees()); + SmartDashboard.putNumber("robot Object Detection angle", robotAngle.getDegrees()); // var yawOffset = subsystem.getRotation2d().minus(robotPose.getRotation()); subsystem.setDesiredYaw(robotAngle.getDegrees());//Set absolute heading - var speedVal = speedSupplier.getAsDouble(); + var speedVal = Math.max(0, Math.hypot(xspdFunction.get(), yspdFunction.get())-.05)*(maxMPS); if (speedVal < .1) speedVal = 0; subsystem.driveAtSpeed(robotAngle.getCos()*speedVal, @@ -98,7 +105,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ShooterControllerCommand.java b/src/main/java/frc/robot/commands/ShooterControllerCommand.java index 8939f75..21b0507 100644 --- a/src/main/java/frc/robot/commands/ShooterControllerCommand.java +++ b/src/main/java/frc/robot/commands/ShooterControllerCommand.java @@ -26,8 +26,8 @@ public class ShooterControllerCommand extends Command { */ public ShooterControllerCommand(ShooterSubsystem subsystem, Supplier desShoulder, Supplier on) { this.subsystem = subsystem; - this.shooterSpeedTop = 5000; - this.shooterSpeedBottom = 5000; + this.shooterSpeedTop = 3500; + this.shooterSpeedBottom = 3500; this.desShoulder = desShoulder; onoff = on; onState = 0; diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index 9524943..be455fb 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -14,27 +14,30 @@ public class shootSpeakerCommand extends Command { private final ShooterSubsystem shooter; private final CollectorSubsystem collector; + private final double speed; private Timer timer; private boolean shot = false; /** - * Creates a new ExampleCommand. + * Creates a new shootSpeakerCommand. * * @param shooterSubsystem The shooter subsystem used by this command. - * @param collectorSubsystem The collector subsystem used by this command + * @param collectorSubsystem The collector subsystem used by this command. + * @param speed The speed in RPM to set the shooter motors to. */ - public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem) { + public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem, double speed) { this.shooter = shooterSubsystem; this.collector = collectorSubsystem; + this.speed=speed; // Use addRequirements() here to declare subsystem dependencies. addRequirements(shooterSubsystem,collectorSubsystem); timer = new Timer(); shot = false; } + public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem) { + this(shooterSubsystem,collectorSubsystem,3000); + } //TODO: Calculate shooter speeds based on odometry. - public double speedCalc(){ - return 3000; - }//placeholder value // Called when the command is initially scheduled. @Override @@ -45,8 +48,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.setShooterTopSpeed(speedCalc()); - shooter.setShooterBottomSpeed(speedCalc()); + shooter.setShooterTopSpeed(speed); + shooter.setShooterBottomSpeed(speed); if(shooter.shooterAtSpeed() && !shot){ System.out.println("at speed, let's shoot"); shot = true; diff --git a/src/main/java/frc/robot/commands/shooterOn.java b/src/main/java/frc/robot/commands/shooterOn.java index 6ef428d..6aef6c6 100644 --- a/src/main/java/frc/robot/commands/shooterOn.java +++ b/src/main/java/frc/robot/commands/shooterOn.java @@ -1,24 +1,44 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ExampleSubsystem; - -public class shooterOn { - - private Solenoid shooter; - - public shooterOn(Solenoid shooter) { - this.shooter = shooter; - - - +import frc.robot.subsystems.ShooterSubsystem; + +public class shooterOn extends Command { + + private final ShooterSubsystem shooter; + private final double speed; + + /** + * Creates a new shootOn. + * + * @param shooterSubsystem The shooter subsystem used by this command. + * @param speed The speed in RPM to set the shooter motors to. + */ + public shooterOn(ShooterSubsystem shooterSubsystem, double speed) { + this.shooter = shooterSubsystem; + this.speed=speed; } - - - public void execute() { - shooter.set(true); - - } - + @Override + public void initialize(){ + shooter.setShooterSpeeds(speed,speed); + } + + @Override + public void execute() { + shooter.setShooterSpeeds(speed,speed); + } + + @Override + public void end(boolean interrupted){ + if(interrupted){ + shooter.setShooterSpeeds(0,0); + } + } + + public boolean isFinished(){ + return true; + } } diff --git a/src/main/java/frc/robot/subsystems/HeadSubsystem.java b/src/main/java/frc/robot/subsystems/HeadSubsystem.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 19560c8..73cadc4 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -62,8 +62,8 @@ public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, shooterRPMTolerance=5; maxBotSpeed = 3500; maxTopSpeed = 3500; desiredBotSpeed = 0; desiredTopSpeed = 0; - shooterTop.setSmartCurrentLimit(40); - shooterBottom.setSmartCurrentLimit(40); + shooterTop.setSmartCurrentLimit(40,60); + shooterBottom.setSmartCurrentLimit(40,60); ShooterTopFF=new SimpleMotorFeedforward(shooterTopkS,shooterTopkV,shooterTopkA); ShooterBotFF=new SimpleMotorFeedforward(shooterBotkS,shooterBotkV,shooterBotkA); //SYS ID for bottom shooter From b50add1f9f55070cf9d7e94f3679b6869c7f1de9 Mon Sep 17 00:00:00 2001 From: Luca Huang Date: Tue, 9 Apr 2024 20:18:13 -0400 Subject: [PATCH 2/9] Updated code to stop using getYaw and instead get yaw from odometry pose. Added a timer to DriveToNoteCommand. --- .../frc/robot/commands/DriveToNoteCommand.java | 15 ++++++++++++--- .../frc/robot/commands/autos/tripleNoteAutos.java | 4 ++-- src/main/java/frc/robot/commands/setHeading.java | 2 +- .../java/frc/robot/subsystems/SwerveDrive.java | 8 ++++---- 4 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveToNoteCommand.java b/src/main/java/frc/robot/commands/DriveToNoteCommand.java index c6a8210..3239959 100644 --- a/src/main/java/frc/robot/commands/DriveToNoteCommand.java +++ b/src/main/java/frc/robot/commands/DriveToNoteCommand.java @@ -23,9 +23,9 @@ public class DriveToNoteCommand extends Command { private final Supplier xspdFunction,yspdFunction,turnspdFunction; private final DoubleConsumer rumbleCallback; private final double maxMPS; + private Timer timer; private Translation2d target = null; - private int idleLoopCount = 0; private final double Latency = 0.25;//Pose latency in seconds public DriveToNoteCommand(SwerveDrive subsystem, Vision vision, Supplier xspeed, Supplier yspeed, @@ -41,13 +41,14 @@ public DriveToNoteCommand(SwerveDrive subsystem, Vision vision, Supplier this.subsystem = subsystem; this.rumbleCallback = rumbleCallback; this.maxMPS=maxMPS; + timer = new Timer(); addRequirements(subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { this.target = null; - idleLoopCount = 0; + timer.reset(); } private void updateTarget() { @@ -58,6 +59,14 @@ private void updateTarget() { } var detections = vision.detections(); + if(detections.isEmpty()){ + if(timer.get()>1) {//TODO: Update this value + detectionMaxThreshold = Double.POSITIVE_INFINITY; + target = null; + } + }else{ + timer.reset(); + } for (var detection : detections){ var distance = detection.getNorm(); if (distance < detectionMaxThreshold){ @@ -73,6 +82,7 @@ private void updateTarget() { public void execute() { updateTarget(); if (target == null){ + subsystem.setDesiredYaw(subsystem.getEstimatedPose().getRotation().getDegrees()); // idleLoopCount += 1; // // if (idleLoopCount >= 5 && rumbleCallback != null){ @@ -86,7 +96,6 @@ public void execute() { // Drive towards target var robotPose = subsystem.getEstimatedPose(); var delta = target.minus(robotPose.getTranslation()); - var unitDelta = delta.div(delta.getNorm());//.times(speedSupplier.getAsDouble()); var robotAngle = unitDelta.getAngle(); diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 9469bff..96d6d9e 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -339,8 +339,8 @@ public Command EW3W2(){ Rotation2d endRotation1 = (swerveDrive.getAngleBetweenSpeaker(UsefulPoints.Points.WingedNote3)); Pose2d endPose1 = new Pose2d(UsefulPoints.Points.WingedNote3, endRotation1); - Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); - Pose2d startPose2 = new Pose2d(UsefulPoints.Points.WingedNote3, startRotation2); +// Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); + Pose2d startPose2 = new Pose2d(UsefulPoints.Points.WingedNote3, endRotation1); Rotation2d endRotation2 = (swerveDrive.getAngleBetweenSpeaker(UsefulPoints.Points.WingedNote2)); Pose2d endPose2 = new Pose2d(UsefulPoints.Points.WingedNote2, endRotation2); diff --git a/src/main/java/frc/robot/commands/setHeading.java b/src/main/java/frc/robot/commands/setHeading.java index 16b0d14..eec652a 100644 --- a/src/main/java/frc/robot/commands/setHeading.java +++ b/src/main/java/frc/robot/commands/setHeading.java @@ -68,7 +68,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { //return true; - return Math.abs(MathUtil.inputModulus(swerveDrive.getYaw()-desiredYaw.get().getDegrees(),-180,180))<=2;//2 Degree Tolerance + return Math.abs(MathUtil.inputModulus(swerveDrive.getEstimatedPose().getRotation().getDegrees()-desiredYaw.get().getDegrees(),-180,180))<=2;//2 Degree Tolerance } diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java index 4ffcc28..af43a08 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -116,7 +116,7 @@ public void setDesiredYaw(double yaw){ } public double getYawCorrection(){ - return driveThetaController.calculate(getYaw()-desiredYaw); + return driveThetaController.calculate(getOdomOnlyPose().getRotation().getDegrees()-desiredYaw); } public double getYaw(){ @@ -161,7 +161,7 @@ public void periodic() { SmartDashboard.putNumber("desired yaw", getDesiredYaw()); odometer.update(getRotation2d(), getModulePositions()); field.getObject("odom").setPose(odometer.getPoseMeters()); -// vision.setOdometryPosition(odometer.getPoseMeters()); + vision.setOdometryPosition(odometer.getPoseMeters()); SmartDashboard.putNumber("Rotation",getEstimatedPose().getRotation().getDegrees()); swerveDrivePoseEstimator.update(getRotation2d(), getModulePositions()); var aprilTagVal = vision.getAprilTagPose(); @@ -242,8 +242,8 @@ public Command generateTrajectoryQuintic(ArrayList internalPoints, doubl SwerveControllerCommand trajCommand = new SwerveControllerCommand( trajectory, // vision::getRobotPosition, -// this::getEstimatedPose, - this::getOdomOnlyPose, + this::getEstimatedPose, +// this::getOdomOnlyPose, kDriveKinematics, xController, yController, From a08b67e1d7361157013b57fe5e75d72d8e10bfc8 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Wed, 17 Apr 2024 14:23:49 -0400 Subject: [PATCH 3/9] lehigh changes --- src/main/java/frc/robot/FortissiMOEContainer.java | 2 +- src/main/java/frc/robot/Robot.java | 6 ++++-- src/main/java/frc/robot/commands/autos/tripleNoteAutos.java | 6 +++--- src/main/java/frc/robot/subsystems/Climber.java | 5 +++++ src/main/java/frc/robot/subsystems/ClimberArm.java | 4 ++++ 5 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index cfa7f03..6a4538a 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -74,7 +74,7 @@ public class FortissiMOEContainer{ double maxRPS = 1.5*2*Math.PI; double maxRPS2 = Math.PI; - double maxMPSSquared = 2; + double maxMPSSquared = 3; Vision vision = new Vision(); private final SendableChooser m_chooser = new SendableChooser<>(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 995b513..53c5c54 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,9 +22,11 @@ public class Robot extends TimedRobot { private void initRobotContainer(boolean force) { if (m_robotContainer != null) return; - if (force || DriverStation.getAlliance().isPresent()) + if (force || DriverStation.getAlliance().isPresent()) { // m_robotContainer = new SwerveBotContainer(); - m_robotContainer = new FortissiMOEContainer(); + m_robotContainer = new FortissiMOEContainer(); + m_robotContainer.climber.clearStickyFaults(); + } } @Override diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 9469bff..c795599 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -210,21 +210,21 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), - Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()), collectNote2), - Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules())), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()), collectNote3), - Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 3eedd95..e8f3ac7 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -39,6 +39,11 @@ public void driveLeft(double speed){ climberArmLeft.drive(speed); } + public void clearStickyFaults(){ + climberArmLeft.clearStickyFault(); + climberArmRight.clearStickyFault(); + } + public void stopArms(){ climberArmRight.stop(); climberArmLeft.stop(); diff --git a/src/main/java/frc/robot/subsystems/ClimberArm.java b/src/main/java/frc/robot/subsystems/ClimberArm.java index 04f2818..207f022 100644 --- a/src/main/java/frc/robot/subsystems/ClimberArm.java +++ b/src/main/java/frc/robot/subsystems/ClimberArm.java @@ -111,6 +111,10 @@ public void setSpeed(double speed){ this.speed= speed; } + public void clearStickyFault(){ + climberMotor.clearFaults(); + } + @Override public void periodic() { // This method will be called once per scheduler From c2736d2fbfe36bc5cbfe21b2eca61d329e6c9a76 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Wed, 17 Apr 2024 19:59:43 -0400 Subject: [PATCH 4/9] houston autos w/ obj detect. work in AS --- .../java/frc/robot/FortissiMOEContainer.java | 3 +- .../robot/commands/autos/doubleNoteAutos.java | 23 +++--- .../robot/commands/autos/tripleNoteAutos.java | 71 +++++++++++++++++++ 3 files changed, 86 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 270a268..4dac048 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -281,9 +281,10 @@ public FortissiMOEContainer() { m_chooser.addOption("D Score Move", new doubleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).DMoveAuto()); m_chooser.addOption("D Score Collect (DC5)", new doubleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).DC5Auto()); m_chooser.addOption("4 Note Auto (CW2W1W3)", new tripleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).CW1W2W3()); - m_chooser.addOption("centerLine Auto", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3()); + m_chooser.addOption("CenterLine Pass Auto (DC5C4PassC3)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3()); m_chooser.addOption("driveForward", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).rollOutAuto()); m_chooser.addOption("3 Note Centerline Auto (DC3C2)", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC3C2()); + m_chooser.addOption("2 Note Centerline Auto Obj Detect", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DC3ObjDetect()); SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index ef31c36..d88cd9c 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -321,22 +321,25 @@ public Command DC5C4PassC3(){ return Commands.sequence( swerveDrive.setInitPosition(startPose), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), - Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)), + Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()),collectNote.withTimeout(5)), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - //goes to point C5 + Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(4), + Commands.runOnce(()->swerveDrive.stopModules()),//goes to point C5 Commands.race(headingCorrect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(passNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //heading correct and spit back Commands.race(headingCorrectCollect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //swap back to 90 - Commands.race(Commands.parallel(trajCommand2.andThen(()->swerveDrive.stopModules()),collectAnotherNote.withTimeout(2)), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - //goes to point C4 - Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules())), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(headingCorrect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand2.andThen(()->swerveDrive.stopModules()), collectAnotherNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), + Commands.runOnce(()->swerveDrive.stopModules()), //goes to point C4 + Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules())), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), + Commands.runOnce(()->swerveDrive.stopModules()),Commands.race(headingCorrect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(passAnotherNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //spit out and then reorient to collect Commands.race(headingCorrectCollect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules()),collectLastNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) - ); + Commands.race(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules()), collectLastNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), + Commands.runOnce(()->swerveDrive.stopModules())); } public Command DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot and collect commands diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 7e42e18..88241f9 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -8,18 +8,22 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.PS5Controller; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.AllianceFlip; import frc.robot.Constants; import frc.robot.UsefulPoints; import frc.robot.commands.Collect; +import frc.robot.commands.DriveToNoteCommand; import frc.robot.commands.setHeading; import frc.robot.commands.shootSpeakerCommand; import frc.robot.subsystems.Arm; import frc.robot.subsystems.CollectorSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDrive; +import frc.robot.vision.Vision; import javax.xml.crypto.dsig.TransformService; import java.util.ArrayList; @@ -36,6 +40,7 @@ public class tripleNoteAutos { private ShooterSubsystem shooter; private CollectorSubsystem collector; private Arm armSubsystem; + private final Vision vision; /** Example static factory for an autonomous command. */ public tripleNoteAutos(SwerveDrive subsystem, Arm armSubsystem, ShooterSubsystem shooter, CollectorSubsystem collector, double startVelocity, double endVelocity) { @@ -45,12 +50,14 @@ public tripleNoteAutos(SwerveDrive subsystem, Arm armSubsystem, ShooterSubsystem this.endVelocity = endVelocity; this.shooter = shooter; this.collector = collector; + this.vision = new Vision(); } public tripleNoteAutos(SwerveDrive subsystem, double startVelocity, double endVelocity) { swerveDrive=subsystem; this.startVelocity = startVelocity; this.endVelocity = endVelocity; + this.vision = new Vision(); } public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect commands //go to W1 collect; go to B; shoot; W2 collect; go to D; shoot @@ -333,6 +340,70 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect ); } + public Command DC3ObjDetect(){//TODO: Fix coordinates, create actual shoot and collect commands + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Rotation2d endRotation = new Rotation2d(0); + Translation2d finTranslation = UsefulPoints.Points.CenterNote3; + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.CenterNote3.getX()-Units.inchesToMeters(36), + UsefulPoints.Points.CenterNote3.getY()); + Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start D to C3 + + + Translation2d endTranslation2 = UsefulPoints.Points.StageEnterBottom; + Pose2d startPose2 = new Pose2d(endTranslation, endRotation); + Rotation2d endRotation2 = swerveDrive.getAngleBetweenSpeaker(endTranslation2); + Pose2d endPose2 = new Pose2d(endTranslation2, endRotation2); //goes from point C3 to Shoot point + + ArrayList internalPoints2 = new ArrayList<>(); + + ArrayList trajOnePoses = new ArrayList<>(); + trajOnePoses.add(startPose); + trajOnePoses.add(new Pose2d(3.01, 2.15, Rotation2d.fromDegrees(15.48))); + trajOnePoses.add(new Pose2d(4.77, 4.1, Rotation2d.fromDegrees(44.19))); + //trajOnePoses.add(new Pose2d(6.69, 4.22, Rotation2d.fromDegrees(-4.61))); + trajOnePoses.add(endPose); + + Command trajCommand = swerveDrive.generateTrajectoryQuintic(trajOnePoses,0,0); + Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints2,0,0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + + Command collectNote = new Collect(collector,0.4,false); + + var driveToNote = new DriveToNoteCommand( + swerveDrive, + vision, +// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), + () -> 1.0, + () -> 0.0, + () -> 0.0, + (rumblePercent) -> { + SmartDashboard.putNumber("JoyRumble", rumblePercent); + //driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types. + }, + .56 + ); + + return Commands.sequence( + swerveDrive.setInitPosition(startPose), + Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), + Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand.andThen(driveToNote.withTimeout(2)) + .andThen(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.defer(()->armSubsystem.goToPoint( + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getX()), + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY()-2)), Set.of(armSubsystem)) + .andThen(Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())).withTimeout(1)), + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2) + + ); + } + public Command EW3W2(){ Rotation2d startRotation1 = Rotation2d.fromDegrees(0); Pose2d startPose1 = new Pose2d(UsefulPoints.Points.StartingPointE, startRotation1); From 78eecc16def25c2c0f74f8e06c6a6c79c662db74 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Thu, 18 Apr 2024 12:02:22 -0400 Subject: [PATCH 5/9] Some auto and shooter motor rpm changes. --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/Robot.java | 4 ++- src/main/java/frc/robot/commands/Collect.java | 3 ++- .../robot/commands/autos/tripleNoteAutos.java | 25 +++++++++++++------ .../robot/commands/shootSpeakerCommand.java | 3 ++- .../frc/robot/subsystems/SwerveDrive.java | 1 - 6 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 84a322b..1dfc970 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,4 +20,5 @@ public static class OperatorConstants { } public static Rotation2d collectorShoulder = Rotation2d.fromDegrees(83.6); public static Rotation2d collectorWrist = Rotation2d.fromDegrees(-45); + public static double subShotSpeed = 2700; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 53c5c54..252ea8f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.pathfinding.LocalADStar; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -33,7 +34,8 @@ private void initRobotContainer(boolean force) { public void robotInit() { // m_robotContainer = new FortissiMOEContainer(); initRobotContainer(false); - + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); } diff --git a/src/main/java/frc/robot/commands/Collect.java b/src/main/java/frc/robot/commands/Collect.java index 33a0a20..2e6fc2b 100644 --- a/src/main/java/frc/robot/commands/Collect.java +++ b/src/main/java/frc/robot/commands/Collect.java @@ -39,10 +39,11 @@ public void execute() { if(speed<0){ finalSpeed=speed; } - if (!shouldStop && speed>0 && ((index&&collector.isCollected())||!collector.isCollected())) { //collector in no note + if (!shouldStop && speed>0 && ((index&&collector.isCollected())||!collector.isCollected())) { //collecting in, we have no note finalSpeed = speed; timer.restart(); } + // Run backwards for a bit if (collector.isCollected() && timer.get() <= .1 && speed > 0){ shouldStop = true; finalSpeed = -speed; diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 88241f9..7c3dd33 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -29,6 +29,7 @@ import java.util.ArrayList; import java.util.Set; +@SuppressWarnings("UnnecessaryLocalVariable") public class tripleNoteAutos { private SwerveDrive swerveDrive; @@ -138,7 +139,7 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect ); } - public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collect commands + public Command CW1W2W3(){ //go to W1 collect; go to B; shoot; W2 collect; go to D; shoot //traj 1 Rotation2d startRotation = new Rotation2d(0); @@ -214,10 +215,16 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec return Commands.sequence( swerveDrive.setInitPosition(startPose), + Commands.runOnce(()->shooter.setShooterSpeeds(Constants.subShotSpeed,Constants.subShotSpeed)), Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), - Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), - Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), + Commands.race(Commands.waitSeconds(.2).andThen(shootNote),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race( + Commands.parallel( + trajCommand.andThen(()->swerveDrive.stopModules()), + collectNote + ), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())) + ).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), @@ -302,7 +309,7 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints3,0,0); Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints4,0,0); Command shootNote = new shootSpeakerCommand(shooter,collector); - Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector, 3000); Command shootLastNote = new shootSpeakerCommand(shooter, collector); Command collectNote = new Collect(collector,0.4,false); @@ -312,8 +319,9 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); return Commands.sequence( swerveDrive.setInitPosition(startPose), + Commands.runOnce(()->shooter.setShooterSpeeds(Constants.subShotSpeed,Constants.subShotSpeed)), Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), - Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.waitSeconds(.2).andThen(shootNote),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), Commands.runOnce(()->swerveDrive.stopModules()), @@ -366,7 +374,7 @@ public Command DC3ObjDetect(){//TODO: Fix coordinates, create actual shoot and c Command trajCommand = swerveDrive.generateTrajectoryQuintic(trajOnePoses,0,0); Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints2,0,0); Command shootNote = new shootSpeakerCommand(shooter,collector); - Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector, 3000); Command collectNote = new Collect(collector,0.4,false); @@ -386,8 +394,9 @@ public Command DC3ObjDetect(){//TODO: Fix coordinates, create actual shoot and c return Commands.sequence( swerveDrive.setInitPosition(startPose), + Commands.runOnce(()->shooter.setShooterSpeeds(Constants.subShotSpeed,Constants.subShotSpeed)), Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), - Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.waitSeconds(.2).andThen(shootNote),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(driveToNote.withTimeout(2)) .andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index be455fb..5a751a6 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.CollectorSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -35,7 +36,7 @@ public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem shot = false; } public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem) { - this(shooterSubsystem,collectorSubsystem,3000); + this(shooterSubsystem,collectorSubsystem, Constants.subShotSpeed); } //TODO: Calculate shooter speeds based on odometry. diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java index af43a08..feb2ea0 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -223,7 +223,6 @@ public Command generateTrajectory(Pose2d start, Pose2d end, ArrayList field.getObject("traj").setTrajectory(trajectory)), trajCommand ); From 536ceefe1996a58525f07f517703b4731124e8dc Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Thu, 18 Apr 2024 12:25:31 -0400 Subject: [PATCH 6/9] Split collector commands. Committing to switch laptops. --- src/main/java/frc/robot/commands/Intake.java | 51 ++++++++++++++++++ src/main/java/frc/robot/commands/PopNote.java | 52 +++++++++++++++++++ .../robot/commands/autos/tripleNoteAutos.java | 14 ++--- 3 files changed, 111 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/commands/Intake.java create mode 100644 src/main/java/frc/robot/commands/PopNote.java diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java new file mode 100644 index 0000000..d018d79 --- /dev/null +++ b/src/main/java/frc/robot/commands/Intake.java @@ -0,0 +1,51 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CollectorSubsystem; + +public class Intake extends Command { + private final double speed; + private CollectorSubsystem collector; + public Intake(CollectorSubsystem collector, double speed){ + this.collector=collector; + this.speed=speed; + } + + + // 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() { + double finalSpeed = 0; + if(speed<0){ + finalSpeed=speed; + } + if (speed>0 && !collector.isCollected()) { //collecting in, we have no note + finalSpeed = speed; + } + collector.updateCollectorSpeed(finalSpeed); + SmartDashboard.putBoolean("started collector", collector.getCollectorState()); + SmartDashboard.putNumber("collector speed", finalSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + collector.stopCollector(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if(collector.isCollected()){ + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/PopNote.java b/src/main/java/frc/robot/commands/PopNote.java new file mode 100644 index 0000000..ba5ac10 --- /dev/null +++ b/src/main/java/frc/robot/commands/PopNote.java @@ -0,0 +1,52 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CollectorSubsystem; + +public class PopNote extends Command { + private final double speed; + private CollectorSubsystem collector; + public PopNote(CollectorSubsystem collector, double speed){ + this.collector=collector; + this.speed=speed; + timer = new Timer(); + } + + + // 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() { + double finalSpeed = 0; + if(speed<0){ + finalSpeed=speed; + } + if (collector.isCollected() && timer.get() <= .1 && speed > 0){ + shouldStop = true; + finalSpeed = -speed; + } + SmartDashboard.putBoolean("started collector", collector.getCollectorState()); + SmartDashboard.putNumber("collector speed", finalSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + collector.stopCollector(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if(collector.isCollected()){ + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 7c3dd33..a4178f9 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -15,10 +15,7 @@ import frc.robot.AllianceFlip; import frc.robot.Constants; import frc.robot.UsefulPoints; -import frc.robot.commands.Collect; -import frc.robot.commands.DriveToNoteCommand; -import frc.robot.commands.setHeading; -import frc.robot.commands.shootSpeakerCommand; +import frc.robot.commands.*; import frc.robot.subsystems.Arm; import frc.robot.subsystems.CollectorSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -221,12 +218,17 @@ public Command CW1W2W3(){ Commands.race( Commands.parallel( trajCommand.andThen(()->swerveDrive.stopModules()), - collectNote + new Intake(collector, 0.35) ), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())) ).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()), + Commands.deadline( + trajCommand2.andThen(()->swerveDrive.stopModules()), + Commands.sequence( + new Intake(collector, 0.35), + + ), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), From 7356cd155db737d5a0cf2b7043e8df0e4efb15a9 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Thu, 18 Apr 2024 16:32:26 -0400 Subject: [PATCH 7/9] Split collector commands. Committing to switch laptops. --- src/main/java/frc/robot/commands/PopNote.java | 18 +++++----- .../robot/commands/autos/tripleNoteAutos.java | 36 ++++++++++++------- 2 files changed, 32 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/commands/PopNote.java b/src/main/java/frc/robot/commands/PopNote.java index ba5ac10..0520288 100644 --- a/src/main/java/frc/robot/commands/PopNote.java +++ b/src/main/java/frc/robot/commands/PopNote.java @@ -7,6 +7,8 @@ public class PopNote extends Command { private final double speed; + private Timer timer; + private CollectorSubsystem collector; public PopNote(CollectorSubsystem collector, double speed){ this.collector=collector; @@ -18,19 +20,14 @@ public PopNote(CollectorSubsystem collector, double speed){ // Called when the command is initially scheduled. @Override public void initialize() { + timer.restart(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double finalSpeed = 0; - if(speed<0){ - finalSpeed=speed; - } - if (collector.isCollected() && timer.get() <= .1 && speed > 0){ - shouldStop = true; - finalSpeed = -speed; - } + double finalSpeed = -speed; + collector.updateCollectorSpeed(finalSpeed); SmartDashboard.putBoolean("started collector", collector.getCollectorState()); SmartDashboard.putNumber("collector speed", finalSpeed); } @@ -44,9 +41,10 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if(collector.isCollected()){ + if(timer.get() >= .1 || !collector.isCollected()){ return true; + } else { + return false; } - return false; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index a4178f9..4ca6422 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -227,22 +227,34 @@ public Command CW1W2W3(){ trajCommand2.andThen(()->swerveDrive.stopModules()), Commands.sequence( new Intake(collector, 0.35), - + new PopNote(collector,0.35) ), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()), collectNote2), + Commands.race( + Commands.parallel( + trajCommand3.andThen(()->swerveDrive.stopModules()), + new Intake(collector,0.35) + ), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules())), + Commands.deadline(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules())), + Commands.sequence( + new Intake(collector, 0.35), + new PopNote(collector,0.35) + ), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()), collectNote3), + Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()), new Intake(collector,0.35)), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()), + Commands.deadline(trajCommand6.andThen(()->swerveDrive.stopModules()), + Commands.sequence( + new Intake(collector, 0.35), + new PopNote(collector,0.35) + ), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootAnotherLastNote.andThen(()->shooter.stopShooter()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) @@ -331,8 +343,8 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.defer(()->armSubsystem.goToPoint( - Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getX()), - Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY()-2)), Set.of(armSubsystem)) + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getX()), + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY()-2)), Set.of(armSubsystem)) .andThen(Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())).withTimeout(1)), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(1)), @@ -400,7 +412,7 @@ public Command DC3ObjDetect(){//TODO: Fix coordinates, create actual shoot and c Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), Commands.race(Commands.waitSeconds(.2).andThen(shootNote),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(driveToNote.withTimeout(2)) - .andThen(()->swerveDrive.stopModules()), collectNote), + .andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()), @@ -436,11 +448,11 @@ public Command EW3W2(){ Command collectNote = new Collect(collector,1,false); Command collectNoteAgain = new Collect(collector,1,false); return Commands.sequence( - swerveDrive.setInitPosition(startPose1), + swerveDrive.setInitPosition(startPose1), shootNote, - Commands.parallel(traj1, collectNote), + Commands.parallel(traj1, collectNote), shootAnotherNote, - Commands.parallel(traj2, collectNoteAgain), + Commands.parallel(traj2, collectNoteAgain), shootLastNote ); } @@ -595,7 +607,7 @@ public Command BDetourTopC1C2(){ ArrayList internalPoints1 = new ArrayList(); ArrayList internalPoints2 = new ArrayList(); ArrayList internalPoints3 = new ArrayList(); - ArrayList internalPoints4 = new ArrayList(); + ArrayList internalPoints4 = new ArrayList(); Command traj1 = swerveDrive.generateTrajectory(startPose1,endPose1, internalPoints1, 0,0); From 327df5c276838a98fea2fcec2f7c2522492c0b31 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Fri, 19 Apr 2024 16:10:47 -0400 Subject: [PATCH 8/9] quick houston changes --- .../java/frc/robot/FortissiMOEContainer.java | 1 + .../commands/AutoDriveToNoteCommand.java | 4 + .../robot/commands/autos/doubleNoteAutos.java | 97 +++++++++++++++++++ src/main/java/frc/robot/vision/Vision.java | 2 +- 4 files changed, 103 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 4dac048..97078ee 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -285,6 +285,7 @@ public FortissiMOEContainer() { m_chooser.addOption("driveForward", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).rollOutAuto()); m_chooser.addOption("3 Note Centerline Auto (DC3C2)", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC3C2()); m_chooser.addOption("2 Note Centerline Auto Obj Detect", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DC3ObjDetect()); + m_chooser.addOption("CenterLine Pass Auto Obj Detect (DC5C4PassC3OD)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3ObjDet()); SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java b/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java index 5599ecd..2f1c733 100644 --- a/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java +++ b/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java @@ -1,5 +1,6 @@ package frc.robot.commands; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -55,7 +56,10 @@ public void execute() { updateTarget(); if (target == null){ idleLoopCount += 1; + return; } + // Put the detection on NetworkTables, for debugging + subsystem.field.getObject("NoteTarget").setPose(new Pose2d(target, new Rotation2d())); // Drive towards target var robotPose = subsystem.getEstimatedPose(); var delta = target.minus(robotPose.getTranslation()); diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index d88cd9c..c0e4126 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -341,6 +341,103 @@ public Command DC5C4PassC3(){ Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), Commands.runOnce(()->swerveDrive.stopModules())); } + public Command DC5C4PassC3ObjDet(){ + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Pose2d endPose = new Pose2d(UsefulPoints.Points.CenterNote5,Rotation2d.fromDegrees(0)); + Pose2d startPose2 = new Pose2d(endPose.getTranslation(),Rotation2d.fromDegrees(90)); + Pose2d endPose2 = new Pose2d(UsefulPoints.Points.CenterNote4,Rotation2d.fromDegrees(90)); + Pose2d startPose3 = new Pose2d(endPose2.getTranslation(),Rotation2d.fromDegrees(90)); + Pose2d endPose3 = new Pose2d(UsefulPoints.Points.CenterNote5,Rotation2d.fromDegrees(-25)); + Pose2d startPose4 = new Pose2d(endPose3.getTranslation(),Rotation2d.fromDegrees(90)); + Pose2d endPose4 = new Pose2d(UsefulPoints.Points.CenterNote3,Rotation2d.fromDegrees(90)); + Pose2d startPose5 = new Pose2d(endPose4.getTranslation(),Rotation2d.fromDegrees(90)); + Pose2d endPose5 = new Pose2d(UsefulPoints.Points.CenterNote5,Rotation2d.fromDegrees(-25)); + + ArrayList internalPoints = new ArrayList(); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0); + Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints,0,0); + Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints,0,0); + Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints,0,0); + + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command passNote = new NoteFeed(shooter,collector,()->1800); + Command passAnotherNote = new NoteFeed(shooter,collector,()->1800); + Command collectNote = new Collect(collector,.4,false); + Command collectAnotherNote = new Collect(collector,.4,false); + Command collectLastNote = new Collect(collector,.4,false); + Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(Rotation2d.fromDegrees(-25))); + Command headingCorrectCollect = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(90))); + Command headingCorrect2 = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(-25))); + Command headingCorrectCollect2 = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(90))); + Command headingCorrect3 = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(180))); + + var driveToNote = new DriveToNoteCommand( + swerveDrive, + vision, +// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), + () -> 1.0, + () -> 0.0, + () -> 0.0, + (rumblePercent) -> { + SmartDashboard.putNumber("JoyRumble", rumblePercent); + //driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types. + }, + .56 + ); + var driveToNoteAgain = new DriveToNoteCommand( + swerveDrive, + vision, +// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), + () -> 1.0, + () -> 0.0, + () -> 0.0, + (rumblePercent) -> { + SmartDashboard.putNumber("JoyRumble", rumblePercent); + //driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types. + }, + .56 + ); + + var driveToLastNote = new DriveToNoteCommand( + swerveDrive, + vision, +// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), + () -> 1.0, + () -> 0.0, + () -> 0.0, + (rumblePercent) -> { + SmartDashboard.putNumber("JoyRumble", rumblePercent); + //driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types. + }, + .56 + ); + + + return Commands.sequence( + swerveDrive.setInitPosition(startPose), + Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)), + Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand.andThen(driveToNote.withTimeout(2)) + .andThen(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), + Commands.runOnce(()->swerveDrive.stopModules()),//goes to point C5 + Commands.race(headingCorrect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(passNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //heading correct and spit back + Commands.race(headingCorrectCollect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //swap back to 90 + Commands.race(Commands.parallel(trajCommand2.andThen(driveToNoteAgain.withTimeout(2)) + .andThen(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), + Commands.runOnce(()->swerveDrive.stopModules()), //goes to point C4 + Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules())), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2), + Commands.runOnce(()->swerveDrive.stopModules()),Commands.race(headingCorrect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(passAnotherNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //spit out and then reorient to collect + Commands.race(headingCorrectCollect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand4.andThen(driveToLastNote.withTimeout(2)) + .andThen(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6), + Commands.runOnce(()->swerveDrive.stopModules())); + } public Command DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot and collect commands Rotation2d startRotation = new Rotation2d(0); diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index ed6a046..5c7ccca 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -65,7 +65,7 @@ public Optional getAprilTagPose() { } public List detections(){ - ArrayList dets = new ArrayList<>(); + List dets = new ArrayList<>(); var entry = subNotes.getAtomic(); if (entry.timestamp > oldDetTime && entry.value.length %3 == 0){ oldDetTime = entry.timestamp; From 5a8d0ca2e5476cb1094b65adaaae841f947dc046 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 20 Apr 2024 09:14:49 -0400 Subject: [PATCH 9/9] modified collector light for current spikes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/FortissiMOEContainer.java | 5 ++--- src/main/java/frc/robot/subsystems/CollectorSubsystem.java | 3 +++ 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1dfc970..0e6825c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,5 +20,5 @@ public static class OperatorConstants { } public static Rotation2d collectorShoulder = Rotation2d.fromDegrees(83.6); public static Rotation2d collectorWrist = Rotation2d.fromDegrees(-45); - public static double subShotSpeed = 2700; + public static double subShotSpeed = 2500; } diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 97078ee..2bc4b3d 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -223,8 +223,8 @@ public class FortissiMOEContainer{ SmartDashboard.putNumber("Roll", pigeon.getRoll()); SmartDashboard.putNumber("Pitch", pigeon.getPitch()); - pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0)) - || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0 && collectorSubsystem.isCollected())); + pdh.setSwitchableChannel(((collectorSubsystem.isCollected() || collectorSubsystem.getCollectorAmps() > 20) && ((System.currentTimeMillis()/100)%2 == 0)) + || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0 && (collectorSubsystem.isCollected() || collectorSubsystem.getCollectorAmps() > 20))); }); //weirdest command ever - climbing & pdh logic @@ -285,7 +285,6 @@ public FortissiMOEContainer() { m_chooser.addOption("driveForward", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).rollOutAuto()); m_chooser.addOption("3 Note Centerline Auto (DC3C2)", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC3C2()); m_chooser.addOption("2 Note Centerline Auto Obj Detect", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DC3ObjDetect()); - m_chooser.addOption("CenterLine Pass Auto Obj Detect (DC5C4PassC3OD)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3ObjDet()); SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/subsystems/CollectorSubsystem.java b/src/main/java/frc/robot/subsystems/CollectorSubsystem.java index 48063ba..d763418 100644 --- a/src/main/java/frc/robot/subsystems/CollectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CollectorSubsystem.java @@ -46,6 +46,9 @@ public void setCollectorSpeed(double speed){ } } + public double getCollectorAmps(){ + return collector.getOutputCurrent(); + } public Command runCollectorForAuto(final double speed) { Command cmd = Commands.run(() -> updateCollectorSpeed(speed));