diff --git a/src/main/java/frc/robot/AllianceFlip.java b/src/main/java/frc/robot/AllianceFlip.java index 5266a42..f731252 100644 --- a/src/main/java/frc/robot/AllianceFlip.java +++ b/src/main/java/frc/robot/AllianceFlip.java @@ -45,4 +45,7 @@ public static List apply(Listpts){ return pts.stream().map(AllianceFlip::apply).toList(); } + public static List apply(ArrayList pts) { + return pts.stream().map(AllianceFlip::apply).toList(); + } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 846fa6e..84a322b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,8 @@ package frc.robot; +import edu.wpi.first.math.geometry.Rotation2d; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -16,4 +18,6 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + public static Rotation2d collectorShoulder = Rotation2d.fromDegrees(83.6); + public static Rotation2d collectorWrist = Rotation2d.fromDegrees(-45); } diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index c241cfc..cfa7f03 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -6,27 +6,26 @@ import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.*; import frc.robot.commands.autos.doubleNoteAutos; 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.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.commands.CollectorControllerCommand; import frc.robot.commands.ShooterControllerCommand; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.SwerveController; import frc.robot.commands.autos.tripleNoteAutos; -import frc.robot.commands.setHeading; import frc.robot.subsystems.*; +import frc.robot.vision.Vision; import java.util.Set; @@ -46,8 +45,8 @@ public class FortissiMOEContainer{ 0, false, true, - 0.7 * ClimberArm.CONVERSION_FACTOR_INCHES, - 0.42 * ClimberArm.CONVERSION_FACTOR_INCHES, + 0 * ClimberArm.CONVERSION_FACTOR_INCHES, + 0 * ClimberArm.CONVERSION_FACTOR_INCHES, 3.94 * ClimberArm.CONVERSION_FACTOR_INCHES, 3.57 * ClimberArm.CONVERSION_FACTOR_INCHES, 0.52 * ClimberArm.CONVERSION_FACTOR_INCHES, @@ -71,10 +70,13 @@ public class FortissiMOEContainer{ double width = Units.inchesToMeters(14); double length = Units.inchesToMeters(14); double maxMPS = 174/39.3701; + double maxMPSAuto = 4; double maxRPS = 1.5*2*Math.PI; double maxRPS2 = Math.PI; - double maxMPSSquared = 2.5; + double maxMPSSquared = 2; + + Vision vision = new Vision(); private final SendableChooser m_chooser = new SendableChooser<>(); private final SwerveModule backLeftModule = new SwerveModule( @@ -122,24 +124,31 @@ public class FortissiMOEContainer{ driveP, driveI, driveD, driveFF ); private final SwerveDrive swerveSubsystem = new SwerveDrive(frontLeftModule, backLeftModule, frontRightModule, backRightModule, - pigeon, maxMPS, maxMPSSquared, maxRPS, maxRPS2,1.0, 0, 0, 1.0, 0, 0, 4e-2, 0,0); + pigeon, maxMPSAuto, maxMPS, maxMPSSquared, maxRPS, maxRPS2,1.0, 0, 0, 1.0, 0, 0, 4e-2, 0,0); /////////////////////////////////////////////////////////////////////////////drive subsystems end + /////////////////////////////////////////////////////////////////////////////arm subsystem start - private final Arm armSubsystem = new Arm(4, 15,14, 35, 36, - 1.5e-2, 1.5e-3, 1.0e-4, 4.0e-2, 0, 4.0e-4, 1.0e-2,1.0e-3,0,0, 0, Rotation2d.fromDegrees(102), - Rotation2d.fromDegrees(-67), 80,30); + private final Arm armSubsystem = new Arm(4, 15,14, 35, 37, + 24.0e-2, 24.0e-3, 48.0e-4, .1724,0/*3.0e-2*/, 2.0e-3, + .0185, .14833, 1.42e-4, 1.36e-4, + .95908/3, .54837/3, .033244/3, .00498/3, + .6048, .3615, .18133,.14154,10.725,27.837, + 5.6705,5.899,0,0, + Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(-60), Rotation2d.fromDegrees(123), Rotation2d.fromDegrees(-60), + 100,300); /////////////////////////////////////////////////////////////////////////// arm subsystem end ///////////////////////////////////////////////////////////////////////////////////////head subsystem private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(5, - 13,1.0e-4, 0,0,driveFF); + 13,1.9e-10, 0,0,0.40322,0.002178, + 0.001054, 0.22312,0.002178,0.00078615); private final CollectorSubsystem collectorSubsystem = new CollectorSubsystem(6, 0.01,0,0,0,7); ///////////////////////////////////////////////////////////////////////////////////////head subsystem - private final Joystick driverJoystick = new Joystick(1); ///joystick imports + private final XboxController driverJoystick = new XboxController(1); ///joystick imports TESTING OUT RUMBLE CHANGE TO PS5 CONTROLLER IF ANYTHING BREAKS private final Joystick functionJoystick = new Joystick(0); private final Joystick buttonBox = new Joystick(2); @@ -156,19 +165,9 @@ public class FortissiMOEContainer{ () -> driverJoystick.getRawButton(3), 2,3, maxMPS, maxRPS ); - /* turnToAmp = new setHeading( swerveSubsystem, - ()-> -driverJoystick.getRawAxis(1), - ()-> -driverJoystick.getRawAxis(0), - ()->(Rotation2d.fromDegrees(90))); - Command turnToSource = new setHeading(swerveSubsystem, - ()-> -driverJoystick.getRawAxis(1), - ()-> -driverJoystick.getRawAxis(0), - ()->(Rotation2d.fromDegrees(-60))); -*/ - - // private final Command turnRobotOn = new CollectorOnOrOffCommand(headSubsystem, true); + Command collectorCommand = new CollectorControllerCommand( - 0.45, + 0.35,//.45 ()->functionJoystick.getRawAxis(2)>=0.5, ()->functionJoystick.getRawAxis(3)>=0.5, ()->functionJoystick.getRawButton(6), @@ -189,14 +188,14 @@ public class FortissiMOEContainer{ private final Command climbUp= new ClimbUp( climber, - 0.7, + 1, ()-> -pigeon.getPitch() ); private final Command climbDown= new ClimbDown( climber, - 0.8 + 1 ); @@ -206,49 +205,45 @@ public class FortissiMOEContainer{ public final Command buttonsCommand = Commands.run(() -> { - SmartDashboard.putData("lmao", climbUp); if (driverJoystick.getRawButtonPressed(6)) { - if (climbingUp) { - CommandScheduler.getInstance().cancel(climbUp); - this.climbingUp = false; - } else { - climbUp.schedule(); - this.climbingUp = true; - SmartDashboard.putBoolean("ClimbingUp", climbingUp); - } + CommandScheduler.getInstance().cancel(climbDown); + climbUp.schedule(); } if (driverJoystick.getRawButtonPressed(5)) { - if (climbingDown) { - CommandScheduler.getInstance().cancel(climbDown); - this.climbingDown = false; - SmartDashboard.putBoolean("ClimbingDown", climbingDown); - } else { - climbDown.schedule(); - this.climbingDown = true; - } + CommandScheduler.getInstance().cancel(climbUp); + climbDown.schedule(); + } + if (!driverJoystick.getRawButton(5) && !driverJoystick.getRawButton(6)){ + CommandScheduler.getInstance().cancel(climbDown); + CommandScheduler.getInstance().cancel(climbUp); + climber.stopArms(); } SmartDashboard.putBoolean("ClimbingDown", climbingDown); SmartDashboard.putBoolean("ClimbingUp", climbingUp); SmartDashboard.putNumber("Roll", pigeon.getRoll()); SmartDashboard.putNumber("Pitch", pigeon.getPitch()); - pdh.setSwitchableChannel(collectorSubsystem.isCollected() && (System.currentTimeMillis()/100)%2 == 0); - + pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0)) + || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0 && collectorSubsystem.isCollected())); }); + //weirdest command ever - climbing & pdh logic ////////////////////////////////////////////////////////////////////////////commands end Command shooterControl = new ShooterControllerCommand(shooterSubsystem, armSubsystem::getShoulderDesState, ()->functionJoystick.getRawButtonPressed(5)); - Command setHeading = new setHeading(swerveSubsystem, () -> -driverJoystick.getRawAxis(1), - () -> -driverJoystick.getRawAxis(0), ()->(swerveSubsystem.getAngleBetweenSpeaker( - ()->swerveSubsystem.getEstimatedPose().getTranslation()))); ////////////////////////////////////////////////////////////////////////////commands end + Trigger shoulderUp = new Trigger(()-> functionJoystick.getRawAxis(1) <= -.3); + Trigger shoulderDown = new Trigger(()-> functionJoystick.getRawAxis(1) >= .3); + Trigger wristUp = new Trigger(()-> functionJoystick.getRawAxis(0) >= .3); + Trigger wristDown = new Trigger(()-> functionJoystick.getRawAxis(0) <= -.3); public FortissiMOEContainer() { - shooterSubsystem.setShooterRPMTolerance(500); + shooterSubsystem.setShooterRPMTolerance(250); + shooterSubsystem.setMaxShooterSpeeds(3200,3200); + swerveSubsystem.setDefaultCommand(drive); // collectorSubsystem.setDefaultCommand(collectorCommand); @@ -272,87 +267,107 @@ public FortissiMOEContainer() { SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); -// - SmartDashboard.putString("hi","hi"); + m_chooser.setDefaultOption("Double Note Auto 1 (CW2)", new doubleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem, collectorSubsystem,0,0).DoubleNoteAuto1()); m_chooser.addOption("Double Note Auto 2 (BW1)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DoubleNoteAuto2()); // m_chooser.addOption("Double Note Auto 3 (CW1)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DoubleNoteAuto3()); m_chooser.addOption("Double Note Auto 4 (DW3)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DoubleNoteAuto4()); // m_chooser.addOption("Triple Note Auto (BW1W2)", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).BW1W2()); + m_chooser.addOption("A Move Auto", new doubleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).AMoveAuto()); + m_chooser.addOption("Triple Note CW1W2", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).CW1W2()); + m_chooser.addOption("Double Note Auto 1 Return Sub (CW2)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DoubleNoteAuto1ScoreSub()); + m_chooser.addOption("Double Note Auto 2 Return Sub (BW1)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DoubleNoteAuto2ScoreSub()); + m_chooser.addOption("Double Note Auto 4 Return Sub (DW3)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DoubleNoteAuto4ScoreSub()); + 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("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()); SmartDashboard.putData("chooser", m_chooser); - } private void configureBindings() { + + new JoystickButton(driverJoystick, 1).onTrue(Commands.runOnce(() -> {pigeon.setYaw(0); swerveSubsystem.setDesiredYaw(0);})); - new JoystickButton(functionJoystick, 8).whileTrue(Commands.run(()->armSubsystem.shoulderPowerController(.1))); - new JoystickButton(buttonBox, 1).whileTrue(Commands.run(()->armSubsystem.wristPowerController(.1))); - new JoystickButton(functionJoystick, 7).whileTrue(Commands.run(()->armSubsystem.shoulderPowerController(-.1))); - new JoystickButton(buttonBox, 2).whileTrue(Commands.run(()->armSubsystem.wristPowerController(-.1))); - new JoystickButton(functionJoystick, 1).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(80), Rotation2d.fromDegrees(-38)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(4) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(2)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2) - || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); + // zero heading + + shoulderUp.whileTrue(Commands.run(()->armSubsystem.shoulderVoltageController(1.5))); + wristUp.whileTrue(Commands.run(()->armSubsystem.wristVoltageController(1.5))); //in volts lol + shoulderDown.whileTrue(Commands.run(()->armSubsystem.shoulderVoltageController(-1.5))); + wristDown.whileTrue(Commands.run(()->armSubsystem.wristVoltageController(-1.5))); + //manual shoulder/wrist control + + new JoystickButton(functionJoystick, 1).onTrue(Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)) + .until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(3) || + functionJoystick.getRawButton(8) || shoulderDown.getAsBoolean() || + functionJoystick.getRawButton(2)|| wristDown.getAsBoolean()|| wristUp.getAsBoolean() + || functionJoystick.getRawButton(10) || buttonBox.getRawButton(3)))); //collect - new JoystickButton(functionJoystick, 2).onTrue(Commands.defer(() -> armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41.5)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(4) || functionJoystick.getRawButton(1) || - functionJoystick.getRawButton(8)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2) - || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); - //podium shot - - new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2) - || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); - //mid shot - - new JoystickButton(functionJoystick, 9).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-36)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2) - || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(4)))); - //wing shot - - new JoystickButton(functionJoystick, 10).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(143), Rotation2d.fromDegrees(-71)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1) || functionJoystick.getRawButton(4) || buttonBox.getRawButton(1) - || buttonBox.getRawButton(2) || functionJoystick.getRawButton(9)))); + new JoystickButton(functionJoystick, 8).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(120), Rotation2d.fromDegrees(-44)), Set.of(armSubsystem)) + .until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(3) || + functionJoystick.getRawButton(2) || shoulderDown.getAsBoolean() || + functionJoystick.getRawButton(1)||wristDown.getAsBoolean()|| wristUp.getAsBoolean() + || functionJoystick.getRawButton(10) || buttonBox.getRawButton(3)))); + //all the shots smh until roshik chooses a new random button to be his favorite + + new JoystickButton(functionJoystick, 10).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(147), Rotation2d.fromDegrees(-78)), Set.of(armSubsystem)) + .until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(3) || + functionJoystick.getRawButton(2) || shoulderDown.getAsBoolean() || + functionJoystick.getRawButton(1) || functionJoystick.getRawButton(8) || wristDown.getAsBoolean() + || wristUp.getAsBoolean() || buttonBox.getRawButton(3)))); //amp shot - new JoystickButton(functionJoystick, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(109), Rotation2d.fromDegrees(-102.5)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(10) || - functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1) || functionJoystick.getRawButton(4)||buttonBox.getRawButton(1) - || buttonBox.getRawButton(2) || functionJoystick.getRawButton(9)))); + new JoystickButton(functionJoystick, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(113), Rotation2d.fromDegrees(-110)), Set.of(armSubsystem)) + .until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(10) || + functionJoystick.getRawButton(2) || shoulderDown.getAsBoolean() || + functionJoystick.getRawButton(1) || functionJoystick.getRawButton(8)|| wristDown.getAsBoolean() + || wristUp.getAsBoolean() || buttonBox.getRawButton(3)))); //safe pos new JoystickButton(buttonBox, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-135)), Set.of(armSubsystem)) - .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1) || functionJoystick.getRawButton(4)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2) - || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); + .until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(3) || + functionJoystick.getRawButton(2) || shoulderDown.getAsBoolean() || + functionJoystick.getRawButton(1) || functionJoystick.getRawButton(8)|| wristDown.getAsBoolean() + || wristUp.getAsBoolean() || functionJoystick.getRawButton(10)))); //start position -// new JoystickButton(driverJoystick, 7).onTrue(turnToAmp.until(()->(Math.abs(driverJoystick.getRawAxis(2)) >= .2))); -// new JoystickButton(driverJoystick, 8).onTrue(turnToSource.until(()->(Math.abs(driverJoystick.getRawAxis(2)) >= .2))); -// new JoystickButton(functionJoystick, 3).onTrue( -// Commands.parallel( -// Commands.defer(()->armSubsystem.goToPoint( -// Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getX()), -// Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getY())), Set.of(armSubsystem)) -// .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || -// functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || -// functionJoystick.getRawButton(1) || functionJoystick.getRawButton(4))), -// setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>.1))); //auto aim shot - //104,-41 - // new JoystickButton(driverJoystick, 7).whileTrue(setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>= .1)); + new JoystickButton(functionJoystick, 2).onTrue( + Commands.parallel( + Commands.defer(()->armSubsystem.goToPoint( + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getX()), + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getY())), Set.of(armSubsystem)).andThen( + Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()) + )) + .until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(10) || + functionJoystick.getRawButton(3) || shoulderDown.getAsBoolean() || + functionJoystick.getRawButton(1) || functionJoystick.getRawButton(8)|| wristUp.getAsBoolean() + || wristDown.getAsBoolean() || buttonBox.getRawButton(3))), + Commands.run(()->swerveSubsystem.setDesiredYaw(swerveSubsystem.getAngleBetweenSpeaker( + ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).until( + ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1 + ))); + //auto aim shot + + new JoystickButton(functionJoystick, 4).whileTrue(Commands.runOnce(()->shooterSubsystem.setMaxShooterSpeeds(2300,2300))). + whileFalse(Commands.runOnce(()->shooterSubsystem.setMaxShooterSpeeds(3200,3200))); + //half speed reduction + + var driveToNote = new DriveToNoteCommand( + swerveSubsystem, + vision, + () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS), + (rumblePercent) -> { + SmartDashboard.putNumber("JoyRumble", rumblePercent); + driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types. + } + ); + new JoystickButton(driverJoystick, 8).whileTrue(driveToNote); + // object detection note pickup button } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f2305a5..995b513 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,13 +39,16 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putData("running command", CommandScheduler.getInstance()); + initRobotContainer(false); } @Override public void disabledInit() {} @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + if (m_robotContainer!= null) m_robotContainer.resetArmPos(); + } @Override public void autonomousInit() { diff --git a/src/main/java/frc/robot/SwerveBotContainer.java b/src/main/java/frc/robot/SwerveBotContainer.java index 9806f90..25d5a27 100644 --- a/src/main/java/frc/robot/SwerveBotContainer.java +++ b/src/main/java/frc/robot/SwerveBotContainer.java @@ -111,7 +111,7 @@ public class SwerveBotContainer { driveP, driveI, driveD, driveFF ); private final SwerveDrive swerveSubsystem = new SwerveDrive(frontLeftModule, backLeftModule, frontRightModule, backRightModule, - pigeon, maxMPS,maxMPSSquared, maxRPS, maxRPSSquared,1,0,0, 1.0, 0, 0, + pigeon, maxMPS, maxMPS,maxMPSSquared, maxRPS, maxRPSSquared,1,0,0, 1.0, 0, 0, .04,0,0); /////////////////////////////////////////////////////////////////////////////drive subsystems end diff --git a/src/main/java/frc/robot/commands/ArmPathFollow.java b/src/main/java/frc/robot/commands/ArmPathFollow.java index 32d4d2e..a0c4576 100644 --- a/src/main/java/frc/robot/commands/ArmPathFollow.java +++ b/src/main/java/frc/robot/commands/ArmPathFollow.java @@ -12,13 +12,15 @@ import frc.robot.subsystems.Arm; import edu.wpi.first.wpilibj2.command.Command; +import static java.lang.Math.abs; + /** An example command that uses an example subsystem. */ public class ArmPathFollow extends Command { private final Arm armSubsystem; Translation2d desiredPoint; Translation2d startPoint, currPos; Timer timer; - double targetDist, speed, accel, s; + double targetDist, speed, accel, s, v_0; public ArmPathFollow(Arm subsystem, Rotation2d shoulderPos, Rotation2d wristPos, double speed, double accel) { @@ -34,27 +36,39 @@ public ArmPathFollow(Arm subsystem, Rotation2d shoulderPos, Rotation2d wristPos, @Override public void initialize() { + System.out.println("initialized"); SmartDashboard.putNumber("made it to init", timer.get()); startPoint = new Translation2d(armSubsystem.shoulderState().getDegrees(), armSubsystem.wristState().getDegrees()); targetDist = startPoint.getDistance(desiredPoint); + v_0 = armSubsystem.getArmSpeed(); timer.restart(); } @Override public void execute() { - //SmartDashboard.putNumber("made it", timer.get()); - //s = LineHelpers.getS(targetDist, speed, accel, timer.get()); - //SmartDashboard.putNumber("ArmPathFollow s", s); - s = Math.min(timer.get()*speed, startPoint.getDistance(desiredPoint)); - //var interPos = startPoint.getDistance(desiredPoint) - //var interPos = startPoint.interpolate(desiredPoint, s / startPoint.getDistance(desiredPoint)); -// double shoulderPos = LineHelpers.getPositionX(startPoint, desiredPoint, s); -// double wristPos = LineHelpers.getPositionY(startPoint, desiredPoint, s); - var shoulderPos = (desiredPoint.getX()-startPoint.getX())/startPoint.getDistance(desiredPoint)*s+startPoint.getX(); - var wristPos = (desiredPoint.getY()-startPoint.getY())/startPoint.getDistance(desiredPoint)*s+startPoint.getY(); + + s = LineHelpers.getS(desiredPoint.getDistance(startPoint), speed, accel, v_0, timer.get()); + double v = LineHelpers.getVel(desiredPoint.getDistance(startPoint), speed, accel, v_0, timer.get()); + var shoulderPos = (desiredPoint.getX()-startPoint.getX())/desiredPoint.getDistance(startPoint)*s+startPoint.getX(); + + double wristPos = (desiredPoint.getY()-startPoint.getY())/(desiredPoint.getX()-startPoint.getX())*(armSubsystem.shoulderState().getDegrees() + - startPoint.getX()) + startPoint.getY(); + + double wristVel = (desiredPoint.getY()-startPoint.getY())/(desiredPoint.getDistance(startPoint))*v; + double shoulderVel = (desiredPoint.getX()-startPoint.getX())/(desiredPoint.getDistance(startPoint))*v; + + if (Math.abs(desiredPoint.getX() - startPoint.getX()) <= 5){ + wristPos = (desiredPoint.getY()-startPoint.getY())/(desiredPoint.getDistance(startPoint))*s+startPoint.getY(); + } + if (desiredPoint.getDistance(startPoint) <= 5){ + s = desiredPoint.getDistance(startPoint); + } SmartDashboard.putNumber("ArmPathFollow writePos", wristPos); - SmartDashboard.putNumber("ArmPathFollow desiredWrite", desiredPoint.getY()); - armSubsystem.pathFollow(Rotation2d.fromDegrees(shoulderPos), Rotation2d.fromDegrees(wristPos)); + SmartDashboard.putNumber("ArmPathFollow shoulderPos", shoulderPos); + SmartDashboard.putNumber("ArmPathFollow desiredWrist", desiredPoint.getY()); + SmartDashboard.putNumber("ArmPathFollow desiredShoulder", desiredPoint.getX()); + armSubsystem.pathFollow(Rotation2d.fromDegrees(shoulderPos), Rotation2d.fromDegrees(wristPos), wristVel, shoulderVel); + armSubsystem.setWristDestState(wristPos); armSubsystem.setShoulderDesState(shoulderPos); currPos = new Translation2d(armSubsystem.shoulderState().getDegrees(), armSubsystem.wristState().getDegrees()); @@ -62,12 +76,17 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + System.out.println("finished this command"); + System.out.println(s); + armSubsystem.setWristDestState(desiredPoint.getY()); + armSubsystem.setShoulderDesState(desiredPoint.getX()); + } // Returns true when the command should end. @Override public boolean isFinished() { - return s >= startPoint.getDistance(desiredPoint); + return Math.abs(s - startPoint.getDistance(desiredPoint)) <= .1; //return currPos.getDistance(desiredPoint) <= 5; } } diff --git a/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java b/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java new file mode 100644 index 0000000..5599ecd --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java @@ -0,0 +1,80 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveDrive; +import frc.robot.vision.Vision; + + +public class AutoDriveToNoteCommand extends Command { + + private final Vision vision; + private final SwerveDrive subsystem; + + private Translation2d target = null; + private int idleLoopCount = 0; + private double speed; + private double speedMultiplier; + public AutoDriveToNoteCommand(SwerveDrive subsystem, Vision vision, double SpeedMultiplier){ + this.vision = vision; + this.subsystem = subsystem; + this.speedMultiplier=SpeedMultiplier; + addRequirements(subsystem); + } + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.target = null; + idleLoopCount = 0; + } + + private void updateTarget() { + var robotPose = subsystem.getEstimatedPose(); + var detectionMaxThreshold = Double.POSITIVE_INFINITY; + if (target != null) { + detectionMaxThreshold = robotPose.getTranslation().getDistance(target) + Units.feetToMeters(2); + } + + var detections = vision.detections(); + for (var detection : detections){ + var distance = detection.getNorm(); + if (distance < detectionMaxThreshold){ + var detectionFieldCoord = robotPose.transformBy(new Transform2d(detection, new Rotation2d())).getTranslation(); + target = detectionFieldCoord; + detectionMaxThreshold = distance; + } + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + updateTarget(); + if (target == null){ + idleLoopCount += 1; + } + // Drive towards target + var robotPose = subsystem.getEstimatedPose(); + var delta = target.minus(robotPose.getTranslation()); + speed = speedMultiplier*target.getDistance(robotPose.getTranslation())/Units.inchesToMeters(12); + var unitDelta = delta.div(delta.getNorm()).times(speed); + var robotAngle = unitDelta.getAngle(); + subsystem.setDesiredYaw(robotAngle.getDegrees()); + subsystem.driveAtSpeed(unitDelta.getX(), unitDelta.getY(), 0, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + subsystem.stopModules(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimbDown.java b/src/main/java/frc/robot/commands/ClimbDown.java index 11c0675..e76591e 100644 --- a/src/main/java/frc/robot/commands/ClimbDown.java +++ b/src/main/java/frc/robot/commands/ClimbDown.java @@ -41,17 +41,10 @@ public void initialize() { public void execute() { - if(climber.canGoUpRight()){ - climber.driveRight(speed); - } else { - climber.driveRight(0); - } - - if(climber.canGoUpLeft()){ - climber.driveLeft(speed); - } else { - climber.driveLeft(0); - } + + + climber.driveLeft(speed); + climber.driveRight(speed); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Collect.java b/src/main/java/frc/robot/commands/Collect.java index b09a8d7..33a0a20 100644 --- a/src/main/java/frc/robot/commands/Collect.java +++ b/src/main/java/frc/robot/commands/Collect.java @@ -56,6 +56,7 @@ public void execute() { @Override public void end(boolean interrupted) { collector.stopCollector(); + shouldStop = false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/CollectorControllerCommand.java b/src/main/java/frc/robot/commands/CollectorControllerCommand.java index b6bfbac..d10ba4a 100644 --- a/src/main/java/frc/robot/commands/CollectorControllerCommand.java +++ b/src/main/java/frc/robot/commands/CollectorControllerCommand.java @@ -53,14 +53,14 @@ public void execute() { double finalSpeed = 0; if (buttonOut.get() && !buttonIn.get()) { //collector out - finalSpeed = -speed; + finalSpeed = -1;//speed; } if (!buttonOut.get() && buttonIn.get() && !subsystem.isCollected()) { //collector in no note finalSpeed = speed; timer.restart(); } - if (subsystem.isCollected() && timer.get() <= .05){ - finalSpeed = -speed; + if (subsystem.isCollected() && timer.get() <= .1){ + finalSpeed = -speed/3; } if(index.get()){ finalSpeed = 1; diff --git a/src/main/java/frc/robot/commands/DriveToNoteCollectCommand.java b/src/main/java/frc/robot/commands/DriveToNoteCollectCommand.java new file mode 100644 index 0000000..abd440e --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToNoteCollectCommand.java @@ -0,0 +1,129 @@ +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; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Joystick; +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; +import frc.robot.subsystems.SwerveDrive; +import frc.robot.vision.Vision; + +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveToNoteCollectCommand extends Command { + + private final Vision vision; + private final SwerveDrive subsystem; + private final DoubleSupplier speedSupplier; + private final DoubleConsumer rumbleCallback; + private boolean shouldStop = false; + private Timer timer; + + private final CollectorSubsystem collector; + + private Translation2d target = null; + private int idleLoopCount = 0; + +// public DriveToNoteCollectCommand(SwerveDrive subsystem, Vision vision, DoubleSupplier speedSupplier) { +// this(subsystem, vision, speedSupplier, null,); +// } + public DriveToNoteCollectCommand(SwerveDrive subsystem, Vision vision, DoubleSupplier speedSupplier, DoubleConsumer rumbleCallback, CollectorSubsystem collector){ + this.speedSupplier = speedSupplier; + this.vision = vision; + this.subsystem = subsystem; + this.rumbleCallback = rumbleCallback; + this.collector = collector; + shouldStop=false; + addRequirements(subsystem,collector); + } + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.target = null; + idleLoopCount = 0; + shouldStop=false; + } + + private void updateTarget() { + var robotPose = subsystem.getEstimatedPose(); + var detectionMaxThreshold = Double.POSITIVE_INFINITY; + if (target != null) { + detectionMaxThreshold = robotPose.getTranslation().getDistance(target) + Units.feetToMeters(2); + } + + var detections = vision.detections(); + for (var detection : detections){ + var distance = detection.getNorm(); + if (distance < detectionMaxThreshold){ + var detectionFieldCoord = robotPose.transformBy(new Transform2d(detection, new Rotation2d())).getTranslation(); + target = detectionFieldCoord; + detectionMaxThreshold = distance; + } + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double finalSpeed = 0; + updateTarget(); + if (target == null){ + idleLoopCount += 1; + + if (idleLoopCount >= 5 && rumbleCallback != null){ + rumbleCallback.accept(1); + } + return; + } + // 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(); + subsystem.setDesiredYaw(robotAngle.getDegrees()); + + subsystem.driveAtSpeed(unitDelta.getX(), unitDelta.getY(), 0, true); + + if (delta.getNorm()<=Units.feetToMeters(1)&& !shouldStop && !collector.isCollected()) { //within 1 ft, collector in, no note + finalSpeed = 0.4; + timer.restart(); + } + /* if (collector.isCollected() && timer.get() <= .1){ + shouldStop = true; + finalSpeed = -0.4; + }*/ + collector.updateCollectorSpeed(finalSpeed); + SmartDashboard.putBoolean("started collector", collector.getCollectorState()); + SmartDashboard.putNumber("collector speed", finalSpeed); + + SmartDashboard.putBoolean("Should Stop", shouldStop); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (rumbleCallback != null){ + rumbleCallback.accept(0); + } + shouldStop=false; + collector.stopCollector(); + subsystem.stopModules(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if(shouldStop && timer.get() >= .1){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveToNoteCommand.java b/src/main/java/frc/robot/commands/DriveToNoteCommand.java new file mode 100644 index 0000000..10198bd --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToNoteCommand.java @@ -0,0 +1,109 @@ +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; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveDrive; +import frc.robot.vision.Vision; + +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveToNoteCommand extends Command { + + private final Vision vision; + private final SwerveDrive subsystem; + private final DoubleSupplier speedSupplier; + private final DoubleConsumer rumbleCallback; + + 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, DoubleSupplier speedSupplier, DoubleConsumer rumbleCallback){ + this.speedSupplier = speedSupplier; + this.vision = vision; + this.subsystem = subsystem; + this.rumbleCallback = rumbleCallback; + addRequirements(subsystem); + } + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.target = null; + idleLoopCount = 0; + } + + private void updateTarget() { + var robotPose = subsystem.getBufferedPose(Timer.getFPGATimestamp()-Latency); + var detectionMaxThreshold = Double.POSITIVE_INFINITY; + if (target != null) { + detectionMaxThreshold = robotPose.getTranslation().getDistance(target) + Units.feetToMeters(2); + } + + var detections = vision.detections(); + for (var detection : detections){ + var distance = detection.getNorm(); + if (distance < detectionMaxThreshold){ + var detectionFieldCoord = robotPose.transformBy(new Transform2d(detection, new Rotation2d())).getTranslation(); + target = detectionFieldCoord; + detectionMaxThreshold = distance; + } + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + updateTarget(); + if (target == null){ + idleLoopCount += 1; + + if (idleLoopCount >= 5 && rumbleCallback != null){ + rumbleCallback.accept(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()); + + var unitDelta = delta.div(delta.getNorm());//.times(speedSupplier.getAsDouble()); + + var robotAngle = unitDelta.getAngle(); + if (delta.getNorm() <= Units.inchesToMeters(24)) robotAngle = robotPose.getRotation().times(1); + 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(); + if (speedVal < .1) speedVal = 0; + + subsystem.driveAtSpeed(robotAngle.getCos()*speedVal, + robotAngle.getSin()*speedVal, 0, true); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/NoteFeed.java b/src/main/java/frc/robot/commands/NoteFeed.java new file mode 100644 index 0000000..95788aa --- /dev/null +++ b/src/main/java/frc/robot/commands/NoteFeed.java @@ -0,0 +1,69 @@ +// 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.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CollectorSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +import java.util.function.Supplier; + +/** An example command that uses an example subsystem. */ +public class NoteFeed extends Command { + // private final HeadSubsystem headSubsystem; + private final ShooterSubsystem shooter; + private final CollectorSubsystem collector; + private final Timer timer; + private boolean shot = false; + private final SuppliershooterRpm; + /** + * Creates a new ExampleCommand. + * + * @param shooterSubsystem The shooter subsystem used by this command. + * @param collectorSubsystem The collector subsystem used by this command + * @param shooterRPM The supplier for the RPM which shooters should be set to + */ + public NoteFeed(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem, Supplier shooterRPM) { + this.shooter = shooterSubsystem; + this.collector = collectorSubsystem; + timer = new Timer(); + this.shooterRpm=shooterRPM; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooterSubsystem,collectorSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + shot=false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooter.setShooterTopSpeed(shooterRpm.get()); + shooter.setShooterBottomSpeed(shooterRpm.get()); + if(shooter.shooterAtSpeed() && !shot) { + shot = true; + collector.setCollectorSpeed(1); + timer.restart(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooter.stopShooter(); + collector.stopCollector(); + shot=false; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (shot && timer.get() >= .75); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShooterControllerCommand.java b/src/main/java/frc/robot/commands/ShooterControllerCommand.java index 4c319b2..8939f75 100644 --- a/src/main/java/frc/robot/commands/ShooterControllerCommand.java +++ b/src/main/java/frc/robot/commands/ShooterControllerCommand.java @@ -47,8 +47,8 @@ public void execute() { onState %= 2; } if (onState%2 == 1){ - shooterSpeedTop = 4000; shooterSpeedBottom = 4000; - if (desShoulder.get() <= 85) {shooterSpeedTop = 3000; shooterSpeedBottom = 3000;} + shooterSpeedTop = subsystem.getMaxShooterSpeedTop(); shooterSpeedBottom = subsystem.getMaxShooterSpeedBot(); + if (desShoulder.get() <= 85) {shooterSpeedTop = subsystem.getMaxShooterSpeedTop()-500; shooterSpeedBottom = subsystem.getMaxShooterSpeedBot()-500;} subsystem.setShooterSpeeds(shooterSpeedTop, shooterSpeedBottom); } else{ shooterSpeedTop = 0; diff --git a/src/main/java/frc/robot/commands/SwerveController.java b/src/main/java/frc/robot/commands/SwerveController.java index 68ec258..0ebfb71 100644 --- a/src/main/java/frc/robot/commands/SwerveController.java +++ b/src/main/java/frc/robot/commands/SwerveController.java @@ -49,7 +49,7 @@ public void initialize() { @Override public void execute() { double div = 1.0; - if (half.get()) div = 2.0; + if (half.get()) div = 1.0; double xspd = xspdFunction.get()/div; double yspd = yspdFunction.get()/div; double turnspd = turnspdFunction.get()/div; @@ -85,12 +85,13 @@ public void execute() { SmartDashboard.putNumber("xspd", xspd); SmartDashboard.putNumber("yspd", yspd); SmartDashboard.putNumber("turnspd", turnspd); - if (xspd == 0 && yspd == 0 && turnspd == 0){ - m_subsystem.stopModules(); - } - else { - m_subsystem.driveAtSpeed(xspd, yspd, turnspd, !relativeDrive.get(), DriverStation.getAlliance().get()==DriverStation.Alliance.Red); - } + m_subsystem.driveAtSpeed(xspd, yspd, turnspd, !relativeDrive.get(), DriverStation.getAlliance().get()==DriverStation.Alliance.Red); +// if (xspd == 0 && yspd == 0 && turnspd == 0){ +// m_subsystem.stopModules(); +// } +// else { +// m_subsystem.driveAtSpeed(xspd, yspd, turnspd, !relativeDrive.get(), DriverStation.getAlliance().get()==DriverStation.Alliance.Red); +// } } @Override diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index 7d59567..ef31c36 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -13,14 +13,14 @@ 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.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; import frc.robot.subsystems.SwerveDrive; +import frc.robot.vision.Vision; import java.util.ArrayList; import java.util.Set; @@ -38,16 +38,27 @@ public class doubleNoteAutos { // private Command shootPosition = armSubsystem.goToPoint(Rotation2d.fromDegrees(104), Rotation2d.fromDegrees(-41)); private ShooterSubsystem shooter; private CollectorSubsystem collector; - - /** Example static factory for an autonomous command. */ - public doubleNoteAutos(SwerveDrive subsystem, Arm armSubsystem, ShooterSubsystem shooter, CollectorSubsystem collector, double startVelocity, double endVelocity) { - swerveDrive=subsystem; + private Vision vision; + + /** + * Example static factory for an autonomous command. + */ + public doubleNoteAutos(SwerveDrive subsystem, Arm armSubsystem, ShooterSubsystem shooter, CollectorSubsystem collector, double startVelocity, double endVelocity, Vision vision) { + this.vision=vision; + swerveDrive = subsystem; this.armSubsystem = armSubsystem; this.startVelocity = startVelocity; this.endVelocity = endVelocity; this.shooter = shooter; this.collector = collector; - } + } public doubleNoteAutos(SwerveDrive subsystem, Arm armSubsystem, ShooterSubsystem shooter, CollectorSubsystem collector, double startVelocity, double endVelocity) { + swerveDrive = subsystem; + this.armSubsystem = armSubsystem; + this.startVelocity = startVelocity; + this.endVelocity = endVelocity; + this.shooter = shooter; + this.collector = collector; + } public doubleNoteAutos(SwerveDrive subsystem, double startVelocity, double endVelocity) { swerveDrive=subsystem; @@ -61,7 +72,7 @@ public Command DoubleNoteAuto1(){//TODO: Fix coordinates, create actual shoot an //x = dist center of robot when robot is pushed against the wall. Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointC, startRotation); - Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote2.getX()-Units.inchesToMeters(20), + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote2.getX()-Units.inchesToMeters(8), UsefulPoints.Points.WingedNote2.getY()); Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(endTranslation)); Pose2d endPose = new Pose2d(endTranslation, endRotation); @@ -73,20 +84,129 @@ public Command DoubleNoteAuto1(){//TODO: Fix coordinates, create actual shoot an Command shootNote = new shootSpeakerCommand(shooter,collector); Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); Command collectNote = new Collect(collector,.4,false); - Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(endRotation)); return Commands.sequence( swerveDrive.setInitPosition(startPose), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(81), Rotation2d.fromDegrees(-38)), Set.of(armSubsystem)), + Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)), + 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()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112),Rotation2d.fromDegrees(-44)), Set.of(armSubsystem)), +// Commands.race(headingCorrect.withTimeout(3), Commands.run(()->armSubsystem.holdPos(113.5, -42.19))), +// Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + //collect and shoot + ); + } + + public Command DoubleNoteAuto1ScoreSub(){//TODO: Fix coordinates, create actual shoot and collect commands + + Rotation2d startRotation = new Rotation2d(0); + //x = dist center of robot when robot is pushed against the wall. + + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointC, startRotation); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote2.getX()-Units.inchesToMeters(8), + UsefulPoints.Points.WingedNote2.getY()); + Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(endTranslation)); + Pose2d endPose = new Pose2d(endTranslation, endRotation); + +// midPose = new Translation2d(endTranslation.getX()-Units.inchesToMeters(0),endTranslation.getY()); + ArrayList internalPoints = new ArrayList(); +// internalPoints.add(midPose); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints, 0, 0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command collectNote = new Collect(collector,.4,false); + Command trajCommand2 = swerveDrive.generateTrajectory(endPose,startPose,internalPoints,0,0); + return Commands.sequence( + swerveDrive.setInitPosition(startPose), + Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)), Commands.race(shootNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(81, -38))), Commands.runOnce(()->swerveDrive.stopModules()), // Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(113.5), Rotation2d.fromDegrees(-42.19)), Set.of(armSubsystem)), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), - Rotation2d.fromDegrees(-44)), Set.of(armSubsystem)), + Commands.race(trajCommand2.andThen(()-> swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), +// Commands.race(headingCorrect.withTimeout(3), Commands.run(()->armSubsystem.holdPos(113.5, -42.19))), +// Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + //collect and shoot + ); + } + + public Command DoubleNoteAuto1ScoreSubWithObject(){//TODO: Fix coordinates, create actual shoot and collect commands + + Rotation2d startRotation = new Rotation2d(0); + //x = dist center of robot when robot is pushed against the wall. + + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointC, startRotation); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote2.getX()-Units.inchesToMeters(8), + UsefulPoints.Points.WingedNote2.getY()); + Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(endTranslation)); + Pose2d endPose = new Pose2d(endTranslation, endRotation); + +// midPose = new Translation2d(endTranslation.getX()-Units.inchesToMeters(0),endTranslation.getY()); + ArrayList internalPoints = new ArrayList(); +// internalPoints.add(midPose); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints, 0, 0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command collectNote = new Collect(collector,.4,false); + Command trajCommand2 = swerveDrive.generateTrajectory(endPose,startPose,internalPoints,0,0); + Command objDetect = new AutoDriveToNoteCommand(swerveDrive,vision,1.0); + return Commands.sequence( + swerveDrive.setInitPosition(startPose), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(87), Rotation2d.fromDegrees(-56)), Set.of(armSubsystem)), + Commands.race(shootNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race( + Commands.parallel( + trajCommand.andThen(()->swerveDrive.stopModules()), + collectNote + ), + Commands.run(()->armSubsystem.holdPos(81, -38))), + Commands.runOnce(()->swerveDrive.stopModules()), +// Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(113.5), Rotation2d.fromDegrees(-42.19)), Set.of(armSubsystem)), + Commands.race(trajCommand2.andThen(()-> swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), +// Commands.race(headingCorrect.withTimeout(3), Commands.run(()->armSubsystem.holdPos(113.5, -42.19))), +// Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + //collect and shoot + ); + } + + public Command DoubleNoteAuto2ScoreSub(){//TODO: Fix coordinates, create actual shoot and collect commands + + Rotation2d startRotation = new Rotation2d(0); + //x = dist center of robot when robot is pushed against the wall. + + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointB, UsefulPoints.Points.RotationOfStartingPointB); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote1.getX(), + UsefulPoints.Points.WingedNote1.getY() + Units.inchesToMeters(6)); +// Translation2d endAnglePos = endTranslation.plus(new Translation2d(Units.inchesToMeters(7), Units.inchesToMeters(-10))); + Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(endTranslation).plus(Rotation2d.fromDegrees(-5))); + Pose2d endPose = new Pose2d(endTranslation,endRotation); + Pose2d endPose2 = new Pose2d(UsefulPoints.Points.StartingPointC,Rotation2d.fromDegrees(0)); + +// midPose = new Translation2d(endTranslation.getX()-Units.inchesToMeters(0),endTranslation.getY()); + ArrayList internalPoints = new ArrayList(); +// internalPoints.add(midPose); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints, 0, 0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command collectNote = new Collect(collector,.4,false); + Command trajCommand2 = swerveDrive.generateTrajectory(endPose,endPose2,internalPoints,0,0); + 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.race(shootNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(81, -38))), + Commands.runOnce(()->swerveDrive.stopModules()), +// Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(113.5), Rotation2d.fromDegrees(-42.19)), Set.of(armSubsystem)), + Commands.race(trajCommand2.andThen(()-> swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), // Commands.race(headingCorrect.withTimeout(3), Commands.run(()->armSubsystem.holdPos(113.5, -42.19))), // Commands.runOnce(()->swerveDrive.stopModules()), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) - //collect and shoot ); } @@ -106,28 +226,118 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an midPose = new Translation2d(Units.inchesToMeters(75), Units.inchesToMeters(260)); ArrayList internalPoints = new ArrayList(); + ArrayList intP = new ArrayList<>(); internalPoints.add(midPose); Command trajCommand = swerveDrive.generateTrajectory(initPose,endPose,internalPoints, 0, 0); - + Pose2d finalPos = new Pose2d(UsefulPoints.Points.WingedNote1.getX()+Units.inchesToMeters(60), + UsefulPoints.Points.WingedNote1.getY(),endRotation); + Command crossLine = swerveDrive.generateTrajectory(endPose, finalPos, intP, 0,0); Command shootNote = new shootSpeakerCommand(shooter,collector); Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); Command collectNote = new Collect(collector,.4,false); Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(endRotation)); return Commands.sequence( swerveDrive.setInitPosition(initPose), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(81), Rotation2d.fromDegrees(-38)), Set.of(armSubsystem)), + 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.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(headingCorrect.withTimeout(1).andThen(()->swerveDrive.stopModules())), collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), - Rotation2d.fromDegrees(-41.5)), Set.of(armSubsystem)), + Rotation2d.fromDegrees(-37.5)), Set.of(armSubsystem)), Commands.race(Commands.waitSeconds(1), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.parallel(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.parallel(crossLine.andThen(Commands.runOnce(()->swerveDrive.stopModules())), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); + } + public Command DMoveAuto(){ + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Pose2d endPose = new Pose2d(UsefulPoints.Points.CenterNote5.minus(new Translation2d(Units.inchesToMeters(54), Units.inchesToMeters(-18))),Rotation2d.fromDegrees(180)); + ArrayList internalPoints = new ArrayList(); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + 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.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules())), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()) ); } + public Command DC5Auto(){ + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Pose2d endPose = new Pose2d(UsefulPoints.Points.CenterNote5.plus(new Translation2d(Units.inchesToMeters(-12), 0)), + Rotation2d.fromDegrees(0)); + ArrayList internalPoints = new ArrayList(); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command collectNote = new Collect(collector,.4,false); + Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(Rotation2d.fromDegrees(180))); + 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.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()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(headingCorrect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); + } + + public Command DC5C4PassC3(){ + 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))); + + 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.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(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(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()))) + ); + } public Command DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot and collect commands Rotation2d startRotation = new Rotation2d(0); @@ -150,7 +360,8 @@ public Command DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot an Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(endRotation)); return Commands.sequence( swerveDrive.setInitPosition(startPose), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(81), Rotation2d.fromDegrees(-38)), Set.of(armSubsystem)), + 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.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(headingCorrect.withTimeout(1).andThen(()->swerveDrive.stopModules())), collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), @@ -164,15 +375,32 @@ public Command DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot an ); } + public Command AMoveAuto(){//Roll straight past strating line + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointA,Rotation2d.fromDegrees(0)); + Pose2d endPose = new Pose2d(new Translation2d(Units.inchesToMeters(150),UsefulPoints.Points.StartingPointA.getY()),Rotation2d.fromDegrees(0)); + ArrayList internalPoints = new ArrayList<>(); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0); + return Commands.sequence( + swerveDrive.setInitPosition(startPose), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), + Commands.parallel(trajCommand.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); + } + public Command DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot and collect commands //x = dist center of robot when robot is pushed against the wall. Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); - Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(22),UsefulPoints.Points.WingedNote3.getY()-Units.inchesToMeters(3)); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(19),UsefulPoints.Points.WingedNote3.getY()-Units.inchesToMeters(3)); Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(endTranslation)); SmartDashboard.putNumber("endRotationDub4", endRotation.getDegrees()); Pose2d endPose = new Pose2d(endTranslation, Rotation2d.fromDegrees(0)); - + Pose2d startPose2 = new Pose2d(endPose.getTranslation(),endRotation); + Pose2d endPose2 = new Pose2d(UsefulPoints.Points.DetourPointBottom,Rotation2d.fromDegrees(0)); + ArrayList internalPoints2 = new ArrayList(); + Translation2d midPose2 = endTranslation.minus(new Translation2d(Units.inchesToMeters(30),Units.inchesToMeters(60))); + internalPoints2.add(midPose2); + Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints2,0,0); midPose = new Translation2d(Units.inchesToMeters(65),Units.inchesToMeters(175)); ArrayList internalPoints = new ArrayList(); internalPoints.add(midPose); @@ -183,20 +411,55 @@ public Command DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot an Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(endRotation)); return Commands.sequence( swerveDrive.setInitPosition(startPose), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(81), Rotation2d.fromDegrees(-38)), Set.of(armSubsystem)), + 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.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(headingCorrect.withTimeout(1).andThen(()->swerveDrive.stopModules())), collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), - Rotation2d.fromDegrees(-46.5)), Set.of(armSubsystem)), + Rotation2d.fromDegrees(-44.5)), Set.of(armSubsystem)), Commands.race(Commands.waitSeconds(1), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), // Commands.runOnce(()->swerveDrive.stopModules()), - Commands.parallel(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.parallel(trajCommand2.andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) ); } + public Command DoubleNoteAuto4ScoreSub(){//TODO: Fix coordinates, create actual shoot and collect commands + + Rotation2d startRotation = new Rotation2d(0); + //x = dist center of robot when robot is pushed against the wall. + + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); +// Translation2d endAnglePos = endTranslation.plus(new Translation2d(Units.inchesToMeters(7), Units.inchesToMeters(-10))); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(22),UsefulPoints.Points.WingedNote3.getY()-Units.inchesToMeters(3)); + Pose2d endPose = new Pose2d(endTranslation, Rotation2d.fromDegrees(0)); + Pose2d endPose2 = new Pose2d(UsefulPoints.Points.StartingPointC,Rotation2d.fromDegrees(0)); + +// midPose = new Translation2d(endTranslation.getX()-Units.inchesToMeters(0),endTranslation.getY()); + ArrayList internalPoints = new ArrayList(); +// internalPoints.add(midPose); + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints, 0, 0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command collectNote = new Collect(collector,.4,false); + Command trajCommand2 = swerveDrive.generateTrajectory(endPose,endPose2,internalPoints,0,0); + 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.race(shootNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(81, -38))), + Commands.runOnce(()->swerveDrive.stopModules()), +// Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(113.5), Rotation2d.fromDegrees(-42.19)), Set.of(armSubsystem)), + Commands.race(trajCommand2.andThen(()-> swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), +// Commands.race(headingCorrect.withTimeout(3), Commands.run(()->armSubsystem.holdPos(113.5, -42.19))), +// Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); + } public Command CenterLineAuto1(){//TODO: Fix coordinates, create actual shoot and collect commands Rotation2d startRotation = new Rotation2d(0); @@ -255,6 +518,17 @@ public Command FCenterAuto(){//TODO: Create actual shoot and collect commands ); } + public Command rollOutAuto(){ + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Pose2d endPose = new Pose2d(UsefulPoints.Points.DetourPointBottom, new Rotation2d(0)); + var trajCommand = swerveDrive.generateTrajectory(startPose, endPose, new ArrayList<>(),0, 0); + return Commands.sequence( + swerveDrive.setInitPosition(startPose), + trajCommand, + Commands.runOnce(()->swerveDrive.stopModules()) + ); + } + } diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index f32ed14..9469bff 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -10,14 +10,18 @@ import edu.wpi.first.math.util.Units; 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.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 javax.xml.crypto.dsig.TransformService; import java.util.ArrayList; import java.util.Set; @@ -48,40 +52,285 @@ public tripleNoteAutos(SwerveDrive subsystem, double startVelocity, double endVe this.startVelocity = startVelocity; this.endVelocity = endVelocity; } + 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 + //traj 1 + Rotation2d startRotation = new Rotation2d(0); + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointC, startRotation); + Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(UsefulPoints.Points.WingedNote2)); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote2.getX()-Units.inchesToMeters(8), + UsefulPoints.Points.WingedNote2.getY()); + Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start c to point w2 + + Translation2d endTranslation2 = UsefulPoints.Points.StartingPointC.plus( + new Translation2d(Units.inchesToMeters(6), 0)); +// Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); + Pose2d startPose2 = new Pose2d(endTranslation, endRotation); + Rotation2d endRotation2 = Rotation2d.fromDegrees(0); + Pose2d endPose2 = new Pose2d(endTranslation2, endRotation2); //goes from point w2 to start c + + Translation2d endTranslation3 = new Translation2d(UsefulPoints.Points.WingedNote1.getX(), + UsefulPoints.Points.WingedNote1.getY() + Units.inchesToMeters(6)); + Rotation2d startRotation3 = endRotation2; + Pose2d startPose3 = new Pose2d(endTranslation2, startRotation3); + Rotation2d endRotation3 = (swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); + Pose2d endPose3 = new Pose2d(endTranslation3,endRotation3); //startC to W1 + + Translation2d endTranslation4 = endTranslation2; + Rotation2d startRotation4 = endRotation3; + Pose2d startPose4 = new Pose2d(endPose3.getTranslation(), startRotation4); + Rotation2d endRotation4= endRotation2; + Pose2d endPose4 = new Pose2d(endTranslation4, endRotation4); //W1 to start C + + Translation2d endTranslation5 = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(19),UsefulPoints.Points.WingedNote3.getY()-Units.inchesToMeters(3)); + Rotation2d endRotation5 = (swerveDrive.getAngleBetweenSpeaker(endTranslation)); + Rotation2d startRotation5 = endRotation4; + Pose2d startPose5 = new Pose2d(endPose4.getTranslation(), startRotation5); + Pose2d endPose5 = new Pose2d(endTranslation5, endRotation5);// start C to W3 + + ArrayList internalPoints = new ArrayList(); + ArrayList internalPoints2 = new ArrayList<>(); + ArrayList internalPoints3 = new ArrayList<>(); + ArrayList internalPoints4 = new ArrayList<>(); + ArrayList internalPoints5 = new ArrayList<>(); + + + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0); + Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints2,0,0); + Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints3,0,0); + Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints4,0,0); + Command trajCommand5 = swerveDrive.generateTrajectory(startPose5,endPose5,internalPoints5,0,0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command shootLastNote = new shootSpeakerCommand(shooter, collector); + + Command collectNote = new Collect(collector,0.4,false); + Command collectNote2 = new Collect(collector,0.4,false); + + Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation)); + Command headingCorrect2 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation3)); + Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); + 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(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + 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()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(Commands.parallel(trajCommand4.andThen(headingCorrect4.withTimeout(.25)).andThen(()->swerveDrive.stopModules())), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.parallel(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); + } - public Command AW1W2(){//TODO: Fix coordinates, create actual shoot and collect commands -//go to W1 collect; go to B; shoot; W2 collect; go to D; shoot + public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collect commands + //go to W1 collect; go to B; shoot; W2 collect; go to D; shoot //traj 1 - Rotation2d startRotation1 = new Rotation2d(0); - Pose2d startPose1 = new Pose2d(UsefulPoints.Points.StartingPointA, startRotation1); - Translation2d wingedNote1 = UsefulPoints.Points.WingedNote1; - Rotation2d endRotation1 = (swerveDrive.getAngleBetweenSpeaker(wingedNote1)); - Pose2d endPose1 = new Pose2d(UsefulPoints.Points.WingedNote1, endRotation1); - //shoot beginning note; collect winged 1 note - //start traj 2 - Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); - Pose2d startPose2 = new Pose2d(UsefulPoints.Points.WingedNote1, startRotation2); + Rotation2d startRotation = new Rotation2d(0); + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointC, startRotation); + Rotation2d endRotation = (swerveDrive.getAngleBetweenSpeaker(UsefulPoints.Points.WingedNote2)); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.WingedNote2.getX()-Units.inchesToMeters(10), + UsefulPoints.Points.WingedNote2.getY()); + Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start c to point w2 + + Translation2d endTranslation2 = UsefulPoints.Points.StartingPointC.plus( + new Translation2d(Units.inchesToMeters(7), 0)); +// Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); + Pose2d startPose2 = new Pose2d(endTranslation, endRotation); + Rotation2d endRotation2 = Rotation2d.fromDegrees(0); + Pose2d endPose2 = new Pose2d(endTranslation2, endRotation2); //goes from point w2 to start c + + Translation2d endTranslation3 = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(10), + UsefulPoints.Points.WingedNote3.getY()); + Rotation2d startRotation3 = endRotation2; + Pose2d startPose3 = new Pose2d(endTranslation2, startRotation3); + Rotation2d endRotation3 = Rotation2d.fromDegrees(0); + Pose2d endPose3 = new Pose2d(endTranslation3,endRotation3); //startC to W3 + + Translation2d endTranslation4 = endTranslation2; + Rotation2d startRotation4 = endRotation3; + Pose2d startPose4 = new Pose2d(endPose3.getTranslation(), startRotation4); + Rotation2d endRotation4= endRotation2; + Pose2d endPose4 = new Pose2d(endTranslation4, endRotation4); //W3 to start C + + Translation2d endTranslation5 = new Translation2d(UsefulPoints.Points.WingedNote1.getX(),UsefulPoints.Points.WingedNote1.getY() + Units.inchesToMeters(6)); + Rotation2d endRotation5 = (swerveDrive.getAngleBetweenSpeaker(endTranslation)); + Rotation2d startRotation5 = endRotation4; + Pose2d startPose5 = new Pose2d(endPose4.getTranslation(), startRotation5); + Pose2d endPose5 = new Pose2d(endTranslation5, endRotation5);// start C to W1 + + Translation2d endTranslation6 = endTranslation4; + Rotation2d endRotation6 = (swerveDrive.getAngleBetweenSpeaker(endTranslation6)); + Rotation2d startRotation6 = endRotation5; + Pose2d startPose6 = new Pose2d(endPose5.getTranslation(), startRotation6); + Pose2d endPose6 = new Pose2d(endTranslation6, endRotation6);// W1 to start C - Rotation2d endRotation2 = (swerveDrive.getAngleBetweenSpeaker(UsefulPoints.Points.WingedNote2)); - Pose2d endPose2 = new Pose2d(UsefulPoints.Points.WingedNote2, endRotation2); ArrayList internalPoints = new ArrayList(); + ArrayList internalPoints2 = new ArrayList<>(); + ArrayList internalPoints3 = new ArrayList<>(); + internalPoints3.add(endTranslation3.plus(new Translation2d(Units.inchesToMeters(-24),Units.inchesToMeters(5)))); + ArrayList internalPoints4 = new ArrayList<>(); + ArrayList internalPoints5 = new ArrayList<>(); + internalPoints5.add(endTranslation5.plus(new Translation2d(Units.inchesToMeters(-24), Units.inchesToMeters(-5)))); + ArrayList internalPoints6 = new ArrayList<>(); + internalPoints6.add(endTranslation6.plus(new Translation2d(Units.inchesToMeters(20),Units.inchesToMeters(5)))); + + + Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0); + Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints2,0,0); + Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints3,0,0); + Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints4,0,0); + Command trajCommand5 = swerveDrive.generateTrajectory(startPose5,endPose5,internalPoints5,0,0); + Command trajCommand6 = swerveDrive.generateTrajectory(startPose6,endPose6,internalPoints6,0,0); + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); + Command shootLastNote = new shootSpeakerCommand(shooter, collector); + Command shootAnotherLastNote = new shootSpeakerCommand(shooter, collector); - Command traj1 = swerveDrive.generateTrajectory(startPose1,endPose1,internalPoints,0,0); - Command traj2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints,0,0); + Command collectNote = new Collect(collector,0.35,false); + Command collectNote2 = new Collect(collector,0.35,false); + Command collectNote3 = new Collect(collector,0.35,false); + + Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation)); + Command headingCorrect2 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation3)); + Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); + Command headingCorrect6 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); + + 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(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3), + 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.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.runOnce(()->swerveDrive.stopModules()), + Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()), + 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()))) + ); + } + + public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect commands + //go to C3 collect; go to ~W3; shoot; C2 collect; go to ~W3; shoot + //traj 1 + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Rotation2d endRotation = new Rotation2d(0); + Translation2d endTranslation = new Translation2d(UsefulPoints.Points.CenterNote3.getX()-Units.inchesToMeters(6), + UsefulPoints.Points.CenterNote3.getY()); + Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start D to C3 + + + Translation2d endTranslation2 = UsefulPoints.Points.StageEnterBottom; +// Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); + Pose2d startPose2 = new Pose2d(endTranslation, endRotation); + Rotation2d endRotation2 = swerveDrive.getAngleBetweenSpeaker(endTranslation2); + Pose2d endPose2 = new Pose2d(endTranslation2, endRotation2); //goes from point C3 to Shoot point + + Translation2d endTranslation3 = new Translation2d(UsefulPoints.Points.CenterNote2.getX(), + UsefulPoints.Points.CenterNote2.getY() + Units.inchesToMeters(0)); + Rotation2d startRotation3 = endRotation2; + Pose2d startPose3 = new Pose2d(endTranslation2, startRotation3); + Rotation2d endRotation3 = new Rotation2d(0); + Pose2d endPose3 = new Pose2d(endTranslation3,endRotation3); //shoot point to C2 + + Translation2d endTranslation4 = endTranslation2; + Rotation2d startRotation4 = endRotation3; + Pose2d startPose4 = new Pose2d(endPose3.getTranslation(), startRotation4); + Rotation2d endRotation4= (swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); + Pose2d endPose4 = new Pose2d(endTranslation4, endRotation4); //C2 to shoot point + + + Translation2d stageEntrancePoint = new Translation2d(UsefulPoints.Points.WingedNote3.getX(),UsefulPoints.Points.WingedNote3.getY()-Units.inchesToMeters(51)); + Translation2d stageEntranceBottom = UsefulPoints.Points.StageEnterBottom.plus(new Translation2d(Units.inchesToMeters(-6), 0)); + Translation2d stageExit = UsefulPoints.Points.OutOfStage; + Translation2d stageMiddle = UsefulPoints.Points.CenterStage; + + ArrayList internalPoints = new ArrayList(); + ArrayList internalPoints2 = new ArrayList<>(); + ArrayList internalPoints3 = new ArrayList<>(); + ArrayList internalPoints4 = new ArrayList<>(); + internalPoints.add(stageEntrancePoint); + internalPoints.add(stageEntranceBottom); + internalPoints.add(stageMiddle); + internalPoints.add(stageExit); + internalPoints2.add(stageExit); + internalPoints2.add(stageMiddle); + internalPoints3.add(stageMiddle); + internalPoints3.add(stageExit); + internalPoints4.add(stageExit); + internalPoints4.add(stageMiddle); + + 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 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 shootLastNote = new shootSpeakerCommand(shooter, collector); - Command collectNote = new Collect(collector,1,false); - Command collectNoteAgain = new Collect(collector,1,false); + + Command collectNote = new Collect(collector,0.4,false); + Command collectNote2 = new Collect(collector,0.4,false); + + Command headingCorrect2 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation2)); + Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); return Commands.sequence( - swerveDrive.setInitPosition(startPose1), - shootNote, - Commands.parallel(traj1, collectNote), - shootAnotherNote, - Commands.parallel(traj2, collectNoteAgain), - shootLastNote - ); + 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(()->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), + Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(1)), + Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()), collectNote2), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(5), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(Commands.parallel(trajCommand4.andThen(headingCorrect4.withTimeout(.25)).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.parallel(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); } public Command EW3W2(){ @@ -149,8 +398,8 @@ public Command EW3W2(){ return Commands.sequence( //TODO: change values for arm and wrist swerveDrive.setInitPosition(startPose1), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(85), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)), - Commands.race(shootNote, Commands.run(()->armSubsystem.holdPos(85, -41))), + 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(traj1.andThen(()->swerveDrive.stopModules()), collectNote), Commands.run(()->armSubsystem.holdPos(85, -41))), Commands.runOnce(() -> swerveDrive.stopModules()), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(armSubsystem.autoAim(()->swerveDrive.getEstimatedPose()).getX()), diff --git a/src/main/java/frc/robot/commands/setHeading.java b/src/main/java/frc/robot/commands/setHeading.java index ca8cc72..16b0d14 100644 --- a/src/main/java/frc/robot/commands/setHeading.java +++ b/src/main/java/frc/robot/commands/setHeading.java @@ -67,6 +67,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { + //return true; return Math.abs(MathUtil.inputModulus(swerveDrive.getYaw()-desiredYaw.get().getDegrees(),-180,180))<=2;//2 Degree Tolerance } diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index cea482e..9524943 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -12,7 +12,6 @@ /** An example command that uses an example subsystem. */ public class shootSpeakerCommand extends Command { - // private final HeadSubsystem headSubsystem; private final ShooterSubsystem shooter; private final CollectorSubsystem collector; private Timer timer; @@ -40,6 +39,7 @@ public double speedCalc(){ // Called when the command is initially scheduled. @Override public void initialize() { + shot = false; } // Called every time the scheduler runs while the command is scheduled. @@ -48,6 +48,7 @@ public void execute() { shooter.setShooterTopSpeed(speedCalc()); shooter.setShooterBottomSpeed(speedCalc()); if(shooter.shooterAtSpeed() && !shot){ + System.out.println("at speed, let's shoot"); shot = true; collector.setCollectorSpeed(1); timer.restart(); @@ -58,13 +59,14 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - shooter.stopShooter(); + //shooter.stopShooter(); collector.stopCollector(); + shot = false; } // Returns true when the command should end. @Override public boolean isFinished() { - return (shot && timer.get() >= .75); + return (shot && timer.get() >= .25); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/helpers/LineHelpers.java b/src/main/java/frc/robot/helpers/LineHelpers.java index f1af894..4c3ca3e 100644 --- a/src/main/java/frc/robot/helpers/LineHelpers.java +++ b/src/main/java/frc/robot/helpers/LineHelpers.java @@ -23,42 +23,58 @@ public static double getPositionY(Translation2d p, Translation2d q, double s){ -public static Translation2d getPosition (Translation2d start, Translation2d end, double s){ - return start.interpolate(end, s/ start.getDistance(end)); -} + public static Translation2d getPosition (Translation2d start, Translation2d end, double s){ + return start.interpolate(end, s/ start.getDistance(end)); + } - public static double getS(double distance, double maxSpeed, double maxAccel, double t){ + public static double getS(double distance, double maxSpeed, double maxAccel, double v_0, double t){ // using max speed it finds max velocity that the robot should travel - double maxVel = Math.min(maxSpeed, Math.sqrt(distance*maxAccel)); + double maxVel = Math.min(maxSpeed, (-v_0 + Math.sqrt(v_0*v_0 + 2*maxAccel*distance)/2)); + double t_1 = (maxVel-v_0)/maxAccel; + double t_3 = maxVel/maxAccel; + double t_2 = Math.max(0,(distance-(.5*maxAccel*t_1*t_1+v_0*t_1-.5*maxAccel*t_3*t_3+maxVel*t_3))/maxVel); + // it finds time to max which is the time it takes to reach the max speed(beginning of the trapezoid) - double timeToMax = maxVel/maxAccel; - // calculates overlap time between ?? - double overLapTime = (distance-maxVel*timeToMax); + double d_1 = .5*maxAccel*t_1*t_1+v_0*t_1; + double d_2 = maxVel*t_2; + double d_3 = maxVel*t_3-.5*maxAccel*t_3*t_3; // if the amount of time you want it take is less than the time it takes to max - if (t < timeToMax){ - - // returns s - return .5*maxAccel*t*t; + if (t <= t_1){ + return .5*maxAccel*t*t+v_0*t; } + else if ((t-t_1) <= t_2){ + return maxVel*(t-t_1)+d_1; + } + else if (t-t_1-t_2 <= t_3){ + double offSetT = t - t_1-t_2; + return maxVel*(offSetT) - .5*maxAccel*offSetT*offSetT +d_1 + d_2; + } + else { + return distance; //make sure to cancel fully + } + } - // if the robot is speeding up? - - else if (t <= overLapTime+timeToMax){ - - // return s - return .5*maxAccel*timeToMax*timeToMax + maxVel*(t-timeToMax); - + public static double getVel(double distance, double maxSpeed, double maxAccel, double v_0, double t){ + double maxVel = Math.min(maxSpeed, (-v_0 + Math.sqrt(v_0*v_0 + 2*maxAccel*distance)/2)); + double t_1 = (maxVel-v_0)/maxAccel; + double t_3 = maxVel/maxAccel; + double t_2 = Math.max(0,(distance-(.5*maxAccel*t_1*t_1+v_0*t_1-.5*maxAccel*t_3*t_3+maxVel*t_3))/maxVel); + // if the amount of time you want it take is less than the time it takes to max + if (t <= t_1){ + return maxAccel*t+v_0; + } + else if ((t-t_1) <= t_2){ + return maxVel; } - // - else if (t < overLapTime+2*timeToMax){ - double prevDist = .5*maxAccel*timeToMax*timeToMax + maxVel*overLapTime; - return prevDist - .5*maxAccel*t*t + maxVel*t; + else if (t-t_1-t_2 <= t_3){ + double offSetT = t - t_1-t_2; + return maxVel - maxAccel*offSetT; } - else{ - return distance; + else { + return 0; } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index a5971a9..a8fc9e1 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -5,17 +5,19 @@ package frc.robot.subsystems; import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.*; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; 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.units.*; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.AllianceFlip; import frc.robot.UsefulPoints; import frc.robot.commands.ArmPathFollow; @@ -23,6 +25,8 @@ import java.util.function.Supplier; import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.*; public class Arm extends SubsystemBase { private final CANSparkMax shoulderMotorLeft, shoulderMotorRight; @@ -38,18 +42,31 @@ public class Arm extends SubsystemBase { private final PIDController wristController; - private Rotation2d interShoulder, interWrist; + private Rotation2d interShoulder, interWrist, highTransitionShoulderAngle, highTransitionWristAngle; private double currShoulder, currWrist; - private double maxSpeed, maxAccel, shoulderLength, wristLength; + private double maxSpeed, maxAccel, shoulderLength, wristLength, shoulderCOMLen, wristCOMLen, + shoulderCOMOffset, wristCOMOffset, shoulderMass, wristMass, shoulderGearing, wristGearing; private double wristOffset = 0; private double shoulderOffset = 90; + private final Measure>rampRate= Volts.of(0.25).per(Seconds.of(1)); + private final Measure stepVoltage = Volts.of(2); + private final Measure