From 9f90748ba971875c343ff445c3cc8b92aceff7c2 Mon Sep 17 00:00:00 2001 From: malti Date: Sat, 9 Mar 2024 01:29:41 -0500 Subject: [PATCH 01/79] potential fixes from everything breaking. --- src/main/java/frc/robot/FortissiMOEContainer.java | 4 ++-- src/main/java/frc/robot/commands/ArmPathFollow.java | 11 ++++++++--- src/main/java/frc/robot/subsystems/Arm.java | 5 ++--- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index c241cfc..4e4f813 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -126,8 +126,8 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////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); + 1.5e-2, 1.5e-3, 1.0e-4, 4.0e-2, 0, 4.0e-4, 23.839, 14.231, + Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 60,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end diff --git a/src/main/java/frc/robot/commands/ArmPathFollow.java b/src/main/java/frc/robot/commands/ArmPathFollow.java index 32d4d2e..9b3426e 100644 --- a/src/main/java/frc/robot/commands/ArmPathFollow.java +++ b/src/main/java/frc/robot/commands/ArmPathFollow.java @@ -12,6 +12,8 @@ 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; @@ -45,13 +47,16 @@ 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)); + //s = Math.min(timer.get()*speed, startPoint.getDistance(desiredPoint)); + s = Math.min(timer.get()*speed, desiredPoint.getDistance(startPoint)); //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(); + var shoulderPos = (desiredPoint.getX()-startPoint.getX())/desiredPoint.getDistance(startPoint)*s+startPoint.getX(); + + var wristPos = (desiredPoint.getY()-startPoint.getY())/(desiredPoint.getX()-startPoint.getX())*(armSubsystem.shoulderState().getDegrees() + - startPoint.getX()) + startPoint.getY(); SmartDashboard.putNumber("ArmPathFollow writePos", wristPos); SmartDashboard.putNumber("ArmPathFollow desiredWrite", desiredPoint.getY()); armSubsystem.pathFollow(Rotation2d.fromDegrees(shoulderPos), Rotation2d.fromDegrees(wristPos)); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index a5971a9..21c8310 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -47,7 +47,6 @@ public class Arm extends SubsystemBase { public Arm(int rightShoulderMotorID, int leftShoulderMotorID, int wristMotorID, int shoulderEncoderID, int wristEncoderID, double kPShoulder, double kIShoulder, double kDShoulder, double kPWrist, double kIWrist, double kDWrist, - double kP, double kI, double kD, double shoulderLength, double wristLength, Rotation2d criticalShoulderAngle, Rotation2d criticalWristAngle, double maxSpeed, double maxAccel) { @@ -101,10 +100,10 @@ public void pathFollow(Rotation2d shoulder, Rotation2d wrist){ double shoulderPow = (shoulderController.calculate(shoulderState().getDegrees())); wristController.setSetpoint(wrist.getDegrees()); double wristPow = (wristController.calculate(wristState().getDegrees())); - /*if (!boundChecker.inBounds(shoulder, wrist, shoulderLength, wristLength)){ + if (!boundChecker.inBounds(shoulder, wrist, shoulderLength, wristLength)){ if (boundChecker.negDerivShoulder(shoulderState(), shoulder, shoulderLength, wristLength)) shoulderPow = 0; if (boundChecker.negDerivWrist(wristState(),wrist, wristLength)) wristPow = 0; - }*/ + } shoulderPower(shoulderPow); wristPower(wristPow); } From f5aa953738eb851872a164a865754486d2fe8e5d Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 9 Mar 2024 11:55:29 -0500 Subject: [PATCH 02/79] changes from today --- .../java/frc/robot/FortissiMOEContainer.java | 8 +++---- src/main/java/frc/robot/Robot.java | 1 + .../frc/robot/commands/ArmPathFollow.java | 13 ++++++++-- .../robot/commands/autos/doubleNoteAutos.java | 24 ++++++++++++------- src/main/java/frc/robot/subsystems/Arm.java | 10 ++++---- 5 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 4e4f813..a88cb49 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -126,7 +126,7 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////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, 23.839, 14.231, + 1.5e-2, 1.5e-3, 1.0e-4, 4.0e-3, 0, 4.0e-5, 23.839, 14.231, Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 60,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end @@ -290,7 +290,7 @@ private void configureBindings() { 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)) + new JoystickButton(functionJoystick, 1).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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) @@ -318,14 +318,14 @@ private void configureBindings() { || 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)) + new JoystickButton(functionJoystick, 10).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(146), 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)))); //amp shot - new JoystickButton(functionJoystick, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(109), Rotation2d.fromDegrees(-102.5)), Set.of(armSubsystem)) + new JoystickButton(functionJoystick, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(120), 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) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f2305a5..7cef390 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,6 +39,7 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putData("running command", CommandScheduler.getInstance()); + initRobotContainer(false); } @Override diff --git a/src/main/java/frc/robot/commands/ArmPathFollow.java b/src/main/java/frc/robot/commands/ArmPathFollow.java index 9b3426e..d57d3b3 100644 --- a/src/main/java/frc/robot/commands/ArmPathFollow.java +++ b/src/main/java/frc/robot/commands/ArmPathFollow.java @@ -55,8 +55,14 @@ public void execute() { // double wristPos = LineHelpers.getPositionY(startPoint, desiredPoint, s); var shoulderPos = (desiredPoint.getX()-startPoint.getX())/desiredPoint.getDistance(startPoint)*s+startPoint.getX(); - var wristPos = (desiredPoint.getY()-startPoint.getY())/(desiredPoint.getX()-startPoint.getX())*(armSubsystem.shoulderState().getDegrees() + double wristPos = (desiredPoint.getY()-startPoint.getY())/(desiredPoint.getX()-startPoint.getX())*(armSubsystem.shoulderState().getDegrees() - startPoint.getX()) + startPoint.getY(); + 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)+1; + } SmartDashboard.putNumber("ArmPathFollow writePos", wristPos); SmartDashboard.putNumber("ArmPathFollow desiredWrite", desiredPoint.getY()); armSubsystem.pathFollow(Rotation2d.fromDegrees(shoulderPos), Rotation2d.fromDegrees(wristPos)); @@ -67,7 +73,10 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + armSubsystem.setWristDestState(desiredPoint.getY()); + armSubsystem.setShoulderDesState(desiredPoint.getX()); + } // Returns true when the command should end. @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..f40c6a8 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -61,7 +61,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); @@ -76,13 +76,14 @@ public Command DoubleNoteAuto1(){//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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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)), + Rotation2d.fromDegrees(-42)), 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()))) @@ -108,14 +109,16 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an ArrayList internalPoints = new ArrayList(); internalPoints.add(midPose); Command trajCommand = swerveDrive.generateTrajectory(initPose,endPose,internalPoints, 0, 0); - + Pose2d finalPos = new Pose2d(UsefulPoints.Points.WingedNote1, endRotation); + Command crossLine = swerveDrive.generateTrajectory(endPose, finalPos, internalPoints, 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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()))), @@ -123,8 +126,9 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41.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.race(crossLine.andThen(Commands.runOnce(()->swerveDrive.stopModules())), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) ); } @@ -150,7 +154,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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()))), @@ -183,7 +188,8 @@ 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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()))), diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 21c8310..159a6df 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -100,12 +100,12 @@ public void pathFollow(Rotation2d shoulder, Rotation2d wrist){ double shoulderPow = (shoulderController.calculate(shoulderState().getDegrees())); wristController.setSetpoint(wrist.getDegrees()); double wristPow = (wristController.calculate(wristState().getDegrees())); - if (!boundChecker.inBounds(shoulder, wrist, shoulderLength, wristLength)){ - if (boundChecker.negDerivShoulder(shoulderState(), shoulder, shoulderLength, wristLength)) shoulderPow = 0; - if (boundChecker.negDerivWrist(wristState(),wrist, wristLength)) wristPow = 0; - } +// if (!boundChecker.inBounds(shoulder, wrist, shoulderLength, wristLength)){ +// if (boundChecker.negDerivShoulder(shoulderState(), shoulder, shoulderLength, wristLength)) shoulderPow = 0; +// if (boundChecker.negDerivWrist(wristState(),wrist, wristLength)) wristPow = 0; +// } shoulderPower(shoulderPow); - wristPower(wristPow); + wristPower(Math.min(wristPow, .5)); } public void shoulderPower(double power){ From 201166502057ba5760ae9deca623d8e59bd34714 Mon Sep 17 00:00:00 2001 From: tanmay Date: Sat, 9 Mar 2024 12:31:57 -0500 Subject: [PATCH 03/79] Changed D auto to go to detour point. Added A move auto --- .../java/frc/robot/FortissiMOEContainer.java | 1 + .../robot/commands/autos/doubleNoteAutos.java | 24 ++++++++++++++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index a88cb49..eff3ce0 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -279,6 +279,7 @@ public FortissiMOEContainer() { // 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()); SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index f40c6a8..c5fb813 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -169,6 +169,18 @@ 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(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.race(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. @@ -177,7 +189,12 @@ public Command DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot an 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(30,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); @@ -198,8 +215,9 @@ public Command DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot an Rotation2d.fromDegrees(-46.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.race(trajCommand2.andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()) ); } From 66c09b6f7a8af26f2b2ac644cae3e0eedc25e008 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 9 Mar 2024 12:32:13 -0500 Subject: [PATCH 04/79] auto fixes --- .../java/frc/robot/commands/autos/doubleNoteAutos.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index f40c6a8..ab70013 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -107,10 +107,12 @@ 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, endRotation); - Command crossLine = swerveDrive.generateTrajectory(endPose, finalPos, internalPoints, 0,0); + Pose2d finalPos = new Pose2d(UsefulPoints.Points.WingedNote1.getX()+Units.inchesToMeters(20), + 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); @@ -124,7 +126,7 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an 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(-38.5)), Set.of(armSubsystem)), Commands.race(Commands.waitSeconds(1), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(crossLine.andThen(Commands.runOnce(()->swerveDrive.stopModules())), From 1b55472f1d6c9b41f112816446fed87326ee193c Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 9 Mar 2024 14:24:43 -0500 Subject: [PATCH 05/79] tests from today --- .../robot/commands/autos/doubleNoteAutos.java | 18 +++++++++--------- src/main/java/frc/robot/subsystems/Arm.java | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index 4045af4..b95147a 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -110,7 +110,7 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an 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(20), + 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); @@ -129,7 +129,7 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an Rotation2d.fromDegrees(-38.5)), Set.of(armSubsystem)), Commands.race(Commands.waitSeconds(1), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(crossLine.andThen(Commands.runOnce(()->swerveDrive.stopModules())), + Commands.parallel(crossLine.andThen(Commands.runOnce(()->swerveDrive.stopModules())), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) ); } @@ -173,13 +173,13 @@ 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(150,UsefulPoints.Points.StartingPointA.getY()),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.race(trajCommand.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + Commands.parallel(trajCommand.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) ); } @@ -187,14 +187,14 @@ public Command DoubleNoteAuto4(){//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.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(30,60)); + 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)); @@ -214,12 +214,12 @@ public Command DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot an 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.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.runOnce(()->swerveDrive.stopModules()) + Commands.parallel(trajCommand2.andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 159a6df..a2652b0 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -41,8 +41,8 @@ public class Arm extends SubsystemBase { private Rotation2d interShoulder, interWrist; private double currShoulder, currWrist; private double maxSpeed, maxAccel, shoulderLength, wristLength; - private double wristOffset = 0; - private double shoulderOffset = 90; + private double wristOffset = -2; + private double shoulderOffset = 98; public Arm(int rightShoulderMotorID, int leftShoulderMotorID, int wristMotorID, int shoulderEncoderID, int wristEncoderID, double kPShoulder, double kIShoulder, double kDShoulder, From 9ec01c34a99565d7a07875656f258b47e196cdb6 Mon Sep 17 00:00:00 2001 From: RCode2 Date: Sat, 9 Mar 2024 14:48:19 -0500 Subject: [PATCH 06/79] Solid light when shooter at max speed. --- src/main/java/frc/robot/FortissiMOEContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index eff3ce0..fd88e93 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -232,8 +232,7 @@ public class FortissiMOEContainer{ 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()); }); ////////////////////////////////////////////////////////////////////////////commands end From d3bc3dcf3a259da2549507a4d5c387f38175ebb0 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 9 Mar 2024 15:31:32 -0500 Subject: [PATCH 07/79] tests from today --- src/main/java/frc/robot/commands/autos/doubleNoteAutos.java | 2 +- src/main/java/frc/robot/subsystems/Arm.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index b95147a..167f94f 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -79,7 +79,7 @@ public Command DoubleNoteAuto1(){//TODO: Fix coordinates, create actual shoot an Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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.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(113.5), Rotation2d.fromDegrees(-42.19)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index a2652b0..159a6df 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -41,8 +41,8 @@ public class Arm extends SubsystemBase { private Rotation2d interShoulder, interWrist; private double currShoulder, currWrist; private double maxSpeed, maxAccel, shoulderLength, wristLength; - private double wristOffset = -2; - private double shoulderOffset = 98; + private double wristOffset = 0; + private double shoulderOffset = 90; public Arm(int rightShoulderMotorID, int leftShoulderMotorID, int wristMotorID, int shoulderEncoderID, int wristEncoderID, double kPShoulder, double kIShoulder, double kDShoulder, From 62299fb9bdfa874d1489778ce276cc1a49aeed92 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 9 Mar 2024 16:37:01 -0500 Subject: [PATCH 08/79] tests from today --- src/main/java/frc/robot/FortissiMOEContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index fd88e93..c3f46b0 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -232,7 +232,8 @@ public class FortissiMOEContainer{ SmartDashboard.putNumber("Roll", pigeon.getRoll()); SmartDashboard.putNumber("Pitch", pigeon.getPitch()); - pdh.setSwitchableChannel(collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0) || shooterSubsystem.shooterAtSpeed()); + pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0)) + || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesShooterSpeedTop() != 0)); }); ////////////////////////////////////////////////////////////////////////////commands end From c846ac7ebecf00265c1646f461955f557a0d50db Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 9 Mar 2024 18:29:18 -0500 Subject: [PATCH 09/79] tests from today --- src/main/java/frc/robot/commands/autos/doubleNoteAutos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index 167f94f..99f1db1 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -126,7 +126,7 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), - Rotation2d.fromDegrees(-38.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.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.parallel(crossLine.andThen(Commands.runOnce(()->swerveDrive.stopModules())), From 8196ed0a96ef87f798a532879324ae49b389fc5f Mon Sep 17 00:00:00 2001 From: tanmay Date: Sun, 10 Mar 2024 10:03:02 -0400 Subject: [PATCH 10/79] Added return to sub for all 3 double note autos. Added auto that scores D and moves away. --- .../java/frc/robot/FortissiMOEContainer.java | 5 +- .../robot/commands/autos/doubleNoteAutos.java | 120 ++++++++++++++++++ 2 files changed, 124 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index eff3ce0..af8e17c 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -281,7 +281,10 @@ public FortissiMOEContainer() { // 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()); SmartDashboard.putData("chooser", m_chooser); - + 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()); } diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index c5fb813..ba983a6 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -91,6 +91,77 @@ public Command DoubleNoteAuto1(){//TODO: Fix coordinates, create actual shoot an ); } + 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(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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 DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot and collect commands Rotation2d startRotation = new Rotation2d(0); @@ -181,6 +252,22 @@ public Command AMoveAuto(){//Roll straight past strating line ); } + public Command DMoveAuto(){ + Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); + Pose2d endPose = new Pose2d(UsefulPoints.Points.DetourPointBottom.minus(new Translation2d(Units.inchesToMeters(50), Units.inchesToMeters(0))),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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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 DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot and collect commands //x = dist center of robot when robot is pushed against the wall. @@ -221,6 +308,39 @@ public Command DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot an ); } + 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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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); From af38b1c25565897f75737c02963f830cb9bf6812 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sun, 10 Mar 2024 12:18:45 -0400 Subject: [PATCH 11/79] tests from today --- src/main/java/frc/robot/FortissiMOEContainer.java | 2 +- .../java/frc/robot/commands/CollectorControllerCommand.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index c3f46b0..04d2dcd 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -305,7 +305,7 @@ private void configureBindings() { || 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)) + new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-39)), 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) diff --git a/src/main/java/frc/robot/commands/CollectorControllerCommand.java b/src/main/java/frc/robot/commands/CollectorControllerCommand.java index b6bfbac..8abb1fe 100644 --- a/src/main/java/frc/robot/commands/CollectorControllerCommand.java +++ b/src/main/java/frc/robot/commands/CollectorControllerCommand.java @@ -60,7 +60,7 @@ public void execute() { timer.restart(); } if (subsystem.isCollected() && timer.get() <= .05){ - finalSpeed = -speed; + finalSpeed = -speed/4; } if(index.get()){ finalSpeed = 1; From be6424ebdde818fd62ffe4eb0d0e13050cced790 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Mon, 11 Mar 2024 18:59:40 -0400 Subject: [PATCH 12/79] tests from today --- .../java/frc/robot/FortissiMOEContainer.java | 7 +-- .../robot/commands/autos/doubleNoteAutos.java | 50 +++++++++++++------ 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index f9105b5..ccc2f87 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -46,8 +46,8 @@ public class FortissiMOEContainer{ 0, false, true, - 0.7 * ClimberArm.CONVERSION_FACTOR_INCHES, - 0.42 * ClimberArm.CONVERSION_FACTOR_INCHES, + -1000,//0.7 * ClimberArm.CONVERSION_FACTOR_INCHES, + -1000,//0.42 * ClimberArm.CONVERSION_FACTOR_INCHES, 3.94 * ClimberArm.CONVERSION_FACTOR_INCHES, 3.57 * ClimberArm.CONVERSION_FACTOR_INCHES, 0.52 * ClimberArm.CONVERSION_FACTOR_INCHES, @@ -285,6 +285,7 @@ public FortissiMOEContainer() { 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()); } @@ -308,7 +309,7 @@ private void configureBindings() { || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); //podium shot - new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-39)), Set.of(armSubsystem)) + new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-38.5)), 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) diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index a8250d9..e9bf4ec 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -205,6 +205,40 @@ public Command DoubleNoteAuto2(){//TODO: Fix coordinates, create actual shoot an ); } + 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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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,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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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 DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot and collect commands Rotation2d startRotation = new Rotation2d(0); @@ -254,22 +288,6 @@ public Command AMoveAuto(){//Roll straight past strating line ); } - public Command DMoveAuto(){ - Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD); - Pose2d endPose = new Pose2d(UsefulPoints.Points.DetourPointBottom.minus(new Translation2d(Units.inchesToMeters(50), Units.inchesToMeters(0))),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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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 DoubleNoteAuto4(){//TODO: Fix coordinates, create actual shoot and collect commands //x = dist center of robot when robot is pushed against the wall. From 34914b73f8f588bf27eb7181b25cc5376edc8db1 Mon Sep 17 00:00:00 2001 From: malti Date: Mon, 11 Mar 2024 19:56:45 -0400 Subject: [PATCH 13/79] fix global state --- src/main/java/frc/robot/commands/shootSpeakerCommand.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index cea482e..f097757 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -40,6 +40,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. @@ -60,6 +61,7 @@ public void execute() { public void end(boolean interrupted) { shooter.stopShooter(); collector.stopCollector(); + shot = false; } // Returns true when the command should end. From 07af3e272b9c35403f9739696438524b8d7981de Mon Sep 17 00:00:00 2001 From: malti Date: Mon, 11 Mar 2024 20:11:51 -0400 Subject: [PATCH 14/79] fix global state --- src/main/java/frc/robot/commands/Collect.java | 1 + 1 file changed, 1 insertion(+) 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. From 42297171a77f45b3cad9881e10f59da3175f02bb Mon Sep 17 00:00:00 2001 From: tanmay Date: Mon, 11 Mar 2024 20:56:21 -0400 Subject: [PATCH 15/79] added 3 note auto --- .../java/frc/robot/FortissiMOEContainer.java | 1 + .../robot/commands/autos/tripleNoteAutos.java | 93 ++++++++++++++----- 2 files changed, 71 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index c3f46b0..b11a79d 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -280,6 +280,7 @@ public FortissiMOEContainer() { 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()); + SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index f32ed14..9aaeca1 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -10,8 +10,10 @@ 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.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; @@ -49,39 +51,84 @@ public tripleNoteAutos(SwerveDrive subsystem, double startVelocity, double endVe this.endVelocity = endVelocity; } - public Command AW1W2(){//TODO: Fix coordinates, create actual shoot and collect commands + 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 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(8), + UsefulPoints.Points.WingedNote2.getY()); + Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start c to point w2 + + Translation2d endTranslation2 = UsefulPoints.Points.StartingPointC.minus( + 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.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 - 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<>(); + ArrayList internalPoints4 = new ArrayList<>(); + ArrayList internalPoints5 = new ArrayList<>(); - Command traj1 = swerveDrive.generateTrajectory(startPose1,endPose1,internalPoints,0,0); - Command traj2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints,0,0); + 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,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 collectNote3 = new Collect(collector,0.4,false); + Command collectNote4 = new Collect(collector,0.4,false); + Command collectNote5 = 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(startPose1), - shootNote, - Commands.parallel(traj1, collectNote), - shootAnotherNote, - Commands.parallel(traj2, collectNoteAgain), - shootLastNote - ); + swerveDrive.setInitPosition(startPose), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.5)), + 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(Commands.parallel(trajCommand2.andThen(headingCorrect2.withTimeout(1)).andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),collectNote2), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),collectNote3), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(Commands.parallel(trajCommand4.andThen(headingCorrect4.withTimeout(1)).andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),collectNote4), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.parallel(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); } public Command EW3W2(){ From 76af8e470ba48120c46dc085d7d7f5e4014a541d Mon Sep 17 00:00:00 2001 From: tanmay Date: Mon, 11 Mar 2024 20:57:55 -0400 Subject: [PATCH 16/79] added 3 note auto --- src/main/java/frc/robot/FortissiMOEContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 62ac6cc..f965e9c 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -280,13 +280,13 @@ public FortissiMOEContainer() { 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()); - - SmartDashboard.putData("chooser", m_chooser); + m_chooser.addOption("Triple Note", 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()); + SmartDashboard.putData("chooser", m_chooser); } From f6166b69c22cc07303d58bbd5073d9e730c804e2 Mon Sep 17 00:00:00 2001 From: malti Date: Mon, 11 Mar 2024 21:32:21 -0400 Subject: [PATCH 17/79] triple note tested and works --- .../java/frc/robot/FortissiMOEContainer.java | 4 ++-- .../robot/commands/autos/tripleNoteAutos.java | 19 ++++++++++--------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index f965e9c..38c66a1 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -127,7 +127,7 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////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-3, 0, 4.0e-5, 23.839, 14.231, - Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 60,30); + Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 90,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end @@ -280,7 +280,7 @@ public FortissiMOEContainer() { 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", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).CW1W2()); + 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()); diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 9aaeca1..61593f1 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -61,11 +61,11 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect UsefulPoints.Points.WingedNote2.getY()); Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start c to point w2 - Translation2d endTranslation2 = UsefulPoints.Points.StartingPointC.minus( + 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); + 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(), @@ -106,9 +106,6 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Command collectNote = new Collect(collector,0.4,false); Command collectNote2 = new Collect(collector,0.4,false); - Command collectNote3 = new Collect(collector,0.4,false); - Command collectNote4 = new Collect(collector,0.4,false); - Command collectNote5 = 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)); @@ -118,14 +115,18 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.5)), 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.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(Commands.parallel(trajCommand2.andThen(headingCorrect2.withTimeout(1)).andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),collectNote2), + 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()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),collectNote3), + 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(1)).andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),collectNote4), + Commands.race(Commands.parallel(trajCommand4.andThen(headingCorrect4.withTimeout(.5)).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()))) ); From a8b701df5ce9f854b8262d0a9c49991a08a37376 Mon Sep 17 00:00:00 2001 From: malti Date: Tue, 12 Mar 2024 17:26:03 -0400 Subject: [PATCH 18/79] sped up and left shooter on the whole time --- src/main/java/frc/robot/FortissiMOEContainer.java | 2 +- src/main/java/frc/robot/commands/shootSpeakerCommand.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 38c66a1..2317d8e 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -74,7 +74,7 @@ public class FortissiMOEContainer{ double maxRPS = 1.5*2*Math.PI; double maxRPS2 = Math.PI; - double maxMPSSquared = 2.5; + double maxMPSSquared = 4; private final SendableChooser m_chooser = new SendableChooser<>(); private final SwerveModule backLeftModule = new SwerveModule( diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index f097757..09632c2 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -59,7 +59,7 @@ 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; } @@ -67,6 +67,6 @@ public void end(boolean interrupted) { // 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 From ebd996bd3d9c9595dfbc3f9e29d761c42963f174 Mon Sep 17 00:00:00 2001 From: tanmay Date: Tue, 12 Mar 2024 17:32:41 -0400 Subject: [PATCH 19/79] added 4 note auto. --- .../robot/commands/autos/tripleNoteAutos.java | 100 +++++++++++++++++- 1 file changed, 99 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 61593f1..550b399 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -50,7 +50,6 @@ 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 @@ -132,6 +131,105 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect ); } + 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 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(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.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 + + Translation2d endTranslation6 = endTranslation4; + Rotation2d endRotation6 = (swerveDrive.getAngleBetweenSpeaker(endTranslation6)); + Rotation2d startRotation6 = endRotation5; + Pose2d startPose6 = new Pose2d(endPose5.getTranslation(), startRotation6); + Pose2d endPose6 = new Pose2d(endTranslation6, endRotation6);// start C to W3 + + ArrayList internalPoints = new ArrayList(); + ArrayList internalPoints2 = new ArrayList<>(); + ArrayList internalPoints3 = new ArrayList<>(); + ArrayList internalPoints4 = new ArrayList<>(); + ArrayList internalPoints5 = new ArrayList<>(); + ArrayList internalPoints6 = 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 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 collectNote = new Collect(collector,0.4,false); + Command collectNote2 = new Collect(collector,0.4,false); + Command collectNote3 = 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)); + Command headingCorrect6 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); + + 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(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.5)), + 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(.5)).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()))), + Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()), collectNote3), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(trajCommand6.andThen(headingCorrect6.withTimeout(.5))).andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.parallel(shootAnotherLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + ); + } + public Command EW3W2(){ Rotation2d startRotation1 = Rotation2d.fromDegrees(0); Pose2d startPose1 = new Pose2d(UsefulPoints.Points.StartingPointE, startRotation1); From d988e230fe6ad332713e2af36302512ac5493b20 Mon Sep 17 00:00:00 2001 From: tanmay Date: Tue, 12 Mar 2024 17:35:42 -0400 Subject: [PATCH 20/79] Added to chooser. --- src/main/java/frc/robot/FortissiMOEContainer.java | 4 ++-- src/main/java/frc/robot/commands/autos/tripleNoteAutos.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 2317d8e..59f6b63 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -272,8 +272,7 @@ 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()); @@ -286,6 +285,7 @@ public FortissiMOEContainer() { 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()); SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 550b399..f23eac4 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -171,7 +171,7 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Rotation2d endRotation6 = (swerveDrive.getAngleBetweenSpeaker(endTranslation6)); Rotation2d startRotation6 = endRotation5; Pose2d startPose6 = new Pose2d(endPose5.getTranslation(), startRotation6); - Pose2d endPose6 = new Pose2d(endTranslation6, endRotation6);// start C to W3 + Pose2d endPose6 = new Pose2d(endTranslation6, endRotation6);// W3 to start C ArrayList internalPoints = new ArrayList(); ArrayList internalPoints2 = new ArrayList<>(); From c3011daa3719cc28f7f1774f545e0131a3e82734 Mon Sep 17 00:00:00 2001 From: malti Date: Tue, 12 Mar 2024 18:59:18 -0400 Subject: [PATCH 21/79] 4 note auto --- .../java/frc/robot/FortissiMOEContainer.java | 2 +- .../robot/commands/autos/tripleNoteAutos.java | 35 ++++++++++--------- src/main/java/frc/robot/subsystems/Arm.java | 2 +- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 59f6b63..959d81f 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -127,7 +127,7 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////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-3, 0, 4.0e-5, 23.839, 14.231, - Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 90,30); + Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 70,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index f23eac4..32cab55 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -61,7 +61,7 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect 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)); + new Translation2d(Units.inchesToMeters(6), 0)); // Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw()); Pose2d startPose2 = new Pose2d(endTranslation, endRotation); Rotation2d endRotation2 = Rotation2d.fromDegrees(0); @@ -111,8 +111,8 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); return Commands.sequence( swerveDrive.setInitPosition(startPose), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.5)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-65)), Set.of(armSubsystem)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()))), @@ -124,7 +124,7 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect 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(.5)).andThen(()->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()))) @@ -148,30 +148,30 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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)); + Translation2d endTranslation3 = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(14), + UsefulPoints.Points.WingedNote3.getY()); 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 + 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); //W1 to start C + Pose2d endPose4 = new Pose2d(endTranslation4, endRotation4); //W3 to start C - Translation2d endTranslation5 = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(19),UsefulPoints.Points.WingedNote3.getY()-Units.inchesToMeters(3)); + 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 W3 + 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);// W3 to start C + Pose2d endPose6 = new Pose2d(endTranslation6, endRotation6);// W1 to start C ArrayList internalPoints = new ArrayList(); ArrayList internalPoints2 = new ArrayList<>(); @@ -193,9 +193,9 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Command shootLastNote = new shootSpeakerCommand(shooter, collector); Command shootAnotherLastNote = new shootSpeakerCommand(shooter, collector); - Command collectNote = new Collect(collector,0.4,false); - Command collectNote2 = new Collect(collector,0.4,false); - Command collectNote3 = new Collect(collector,0.4,false); + Command collectNote = new Collect(collector,0.45,false); + Command collectNote2 = new Collect(collector,0.45,false); + Command collectNote3 = new Collect(collector,0.45,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)); @@ -204,7 +204,7 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(112), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.5)), Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.race(Commands.parallel(trajCommand.andThen(()->swerveDrive.stopModules()), collectNote), @@ -220,11 +220,12 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Commands.race(Commands.parallel(trajCommand4.andThen(headingCorrect4.withTimeout(.5)).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()))), + 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()))), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(trajCommand6.andThen(headingCorrect6.withTimeout(.5))).andThen(()->swerveDrive.stopModules()),Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())), + Commands.race(trajCommand6.andThen(headingCorrect6.withTimeout(.5)).andThen(()->swerveDrive.stopModules()), + Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.parallel(shootAnotherLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) ); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 159a6df..50a8cfb 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -105,7 +105,7 @@ public void pathFollow(Rotation2d shoulder, Rotation2d wrist){ // if (boundChecker.negDerivWrist(wristState(),wrist, wristLength)) wristPow = 0; // } shoulderPower(shoulderPow); - wristPower(Math.min(wristPow, .5)); + wristPower(Math.min(wristPow, .6)); } public void shoulderPower(double power){ From f356ce50eb5fb822659f8c550c5ed8f55d7c60ee Mon Sep 17 00:00:00 2001 From: malti Date: Wed, 13 Mar 2024 12:12:55 -0400 Subject: [PATCH 22/79] 4 note auto fixes. shd be cleaner pickup --- .../robot/commands/autos/tripleNoteAutos.java | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 32cab55..e4ac93a 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -148,11 +148,11 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(14), + Translation2d endTranslation3 = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(12), UsefulPoints.Points.WingedNote3.getY()); Rotation2d startRotation3 = endRotation2; Pose2d startPose3 = new Pose2d(endTranslation2, startRotation3); - Rotation2d endRotation3 = (swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); + Rotation2d endRotation3 = endRotation2;//(swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); Pose2d endPose3 = new Pose2d(endTranslation3,endRotation3); //startC to W3 Translation2d endTranslation4 = endTranslation2; @@ -161,23 +161,27 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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)); + Translation2d endTranslation5 = UsefulPoints.Points.WingedNote1; + Rotation2d endRotation5 = endRotation2;//(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 endRotation6 = endRotation2;//(swerveDrive.getAngleBetweenSpeaker(endTranslation6)); Rotation2d startRotation6 = endRotation5; Pose2d startPose6 = new Pose2d(endPose5.getTranslation(), startRotation6); Pose2d endPose6 = new Pose2d(endTranslation6, endRotation6);// W1 to start C ArrayList internalPoints = new ArrayList(); ArrayList internalPoints2 = new ArrayList<>(); - ArrayList internalPoints3 = new ArrayList<>(); + ArrayList internalPoints3 = new ArrayList<>(); //need an internal point + internalPoints3.add(endTranslation3.minus(new Translation2d(Units.inchesToMeters(24), Units.inchesToMeters(0)))); + ArrayList internalPoints4 = new ArrayList<>(); - ArrayList internalPoints5 = new ArrayList<>(); + ArrayList internalPoints5 = new ArrayList<>(); //need an internal point here + internalPoints5.add(endTranslation5.minus(new Translation2d(Units.inchesToMeters(12), Units.inchesToMeters(0)))); + ArrayList internalPoints6 = new ArrayList<>(); @@ -197,15 +201,11 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Command collectNote2 = new Collect(collector,0.45,false); Command collectNote3 = new Collect(collector,0.45,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(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.5)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.25)), 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()))), @@ -217,14 +217,14 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(.5)).andThen(()->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()))), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(trajCommand6.andThen(headingCorrect6.withTimeout(.5)).andThen(()->swerveDrive.stopModules()), + Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.parallel(shootAnotherLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) From 2f6944c30630e7e145ddaa2ed358f4d9b10528a8 Mon Sep 17 00:00:00 2001 From: malti Date: Wed, 13 Mar 2024 12:13:41 -0400 Subject: [PATCH 23/79] faster auto accel? --- src/main/java/frc/robot/FortissiMOEContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 959d81f..4ba4123 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -74,7 +74,7 @@ public class FortissiMOEContainer{ double maxRPS = 1.5*2*Math.PI; double maxRPS2 = Math.PI; - double maxMPSSquared = 4; + double maxMPSSquared = 6; private final SendableChooser m_chooser = new SendableChooser<>(); private final SwerveModule backLeftModule = new SwerveModule( From 97f3d0d81038bb8734a9c556c4a051c9e9d31ac3 Mon Sep 17 00:00:00 2001 From: malti Date: Wed, 13 Mar 2024 21:10:35 -0400 Subject: [PATCH 24/79] faster auto accel? --- .../java/frc/robot/FortissiMOEContainer.java | 10 +++--- .../robot/commands/autos/tripleNoteAutos.java | 32 ++++++++++--------- src/main/java/frc/robot/subsystems/Arm.java | 6 ++-- 3 files changed, 26 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 4ba4123..cab348e 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -74,7 +74,7 @@ public class FortissiMOEContainer{ double maxRPS = 1.5*2*Math.PI; double maxRPS2 = Math.PI; - double maxMPSSquared = 6; + double maxMPSSquared = 5; private final SendableChooser m_chooser = new SendableChooser<>(); private final SwerveModule backLeftModule = new SwerveModule( @@ -126,7 +126,7 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////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-3, 0, 4.0e-5, 23.839, 14.231, + 2.0e-2, 2.0e-3, 4.0e-4, 8.0e-3, 0, 1.0e-4, 23.839, 14.231, Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 70,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end @@ -292,9 +292,9 @@ public FortissiMOEContainer() { 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(functionJoystick, 8).whileTrue(Commands.run(()->armSubsystem.shoulderPowerController(.2))); new JoystickButton(buttonBox, 1).whileTrue(Commands.run(()->armSubsystem.wristPowerController(.1))); - new JoystickButton(functionJoystick, 7).whileTrue(Commands.run(()->armSubsystem.shoulderPowerController(-.1))); + new JoystickButton(functionJoystick, 7).whileTrue(Commands.run(()->armSubsystem.shoulderPowerController(-.2))); new JoystickButton(buttonBox, 2).whileTrue(Commands.run(()->armSubsystem.wristPowerController(-.1))); new JoystickButton(functionJoystick, 1).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)) .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || @@ -317,7 +317,7 @@ private void configureBindings() { || 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)) + new JoystickButton(functionJoystick, 9).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-31)), 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) diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index e4ac93a..ca61e1d 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -132,7 +132,7 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect } 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 + //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); @@ -148,11 +148,11 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(12), + Translation2d endTranslation3 = new Translation2d(UsefulPoints.Points.WingedNote3.getX()-Units.inchesToMeters(14), UsefulPoints.Points.WingedNote3.getY()); Rotation2d startRotation3 = endRotation2; Pose2d startPose3 = new Pose2d(endTranslation2, startRotation3); - Rotation2d endRotation3 = endRotation2;//(swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); + Rotation2d endRotation3 = (swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); Pose2d endPose3 = new Pose2d(endTranslation3,endRotation3); //startC to W3 Translation2d endTranslation4 = endTranslation2; @@ -161,26 +161,24 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Rotation2d endRotation4= endRotation2; Pose2d endPose4 = new Pose2d(endTranslation4, endRotation4); //W3 to start C - Translation2d endTranslation5 = UsefulPoints.Points.WingedNote1; - Rotation2d endRotation5 = endRotation2;//(swerveDrive.getAngleBetweenSpeaker(endTranslation)); + 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 = endRotation2;//(swerveDrive.getAngleBetweenSpeaker(endTranslation6)); + 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 ArrayList internalPoints = new ArrayList(); ArrayList internalPoints2 = new ArrayList<>(); - ArrayList internalPoints3 = new ArrayList<>(); //need an internal point - internalPoints3.add(endTranslation3.minus(new Translation2d(Units.inchesToMeters(24), Units.inchesToMeters(0)))); - + ArrayList internalPoints3 = new ArrayList<>(); ArrayList internalPoints4 = new ArrayList<>(); - ArrayList internalPoints5 = new ArrayList<>(); //need an internal point here - internalPoints5.add(endTranslation5.minus(new Translation2d(Units.inchesToMeters(12), Units.inchesToMeters(0)))); + ArrayList internalPoints5 = new ArrayList<>(); + internalPoints5.add(endTranslation5.plus(new Translation2d(Units.inchesToMeters(-24), 0))); ArrayList internalPoints6 = new ArrayList<>(); @@ -201,11 +199,15 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec Command collectNote2 = new Collect(collector,0.45,false); Command collectNote3 = new Collect(collector,0.45,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(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)), - Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.25)), + Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()))), @@ -217,18 +219,18 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(()->swerveDrive.stopModules())), + Commands.race(Commands.parallel(trajCommand4.andThen(headingCorrect4.withTimeout(.15)).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()))), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()), + Commands.race(trajCommand6.andThen(headingCorrect6.withTimeout(.15)).andThen(()->swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), Commands.parallel(shootAnotherLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) - ); + ); } public Command EW3W2(){ diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 50a8cfb..4268b4b 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -76,6 +76,8 @@ public Arm(int rightShoulderMotorID, int leftShoulderMotorID, int wristMotorID, shoulderController = new PIDController(kPShoulder, kIShoulder, kDShoulder); wristController = new PIDController(kPWrist, kIWrist, kDWrist); + //wristController.setIZone(); + interShoulder = criticalShoulderAngle; interWrist = criticalWristAngle; setShoulderDesState(shoulderState().getDegrees()); setWristDestState(wristState().getDegrees()); @@ -104,8 +106,8 @@ public void pathFollow(Rotation2d shoulder, Rotation2d wrist){ // if (boundChecker.negDerivShoulder(shoulderState(), shoulder, shoulderLength, wristLength)) shoulderPow = 0; // if (boundChecker.negDerivWrist(wristState(),wrist, wristLength)) wristPow = 0; // } - shoulderPower(shoulderPow); - wristPower(Math.min(wristPow, .6)); + shoulderPower(Math.min(shoulderPow, 1)); + wristPower(Math.min(wristPow, .7)); } public void shoulderPower(double power){ From c535c818d7438bc80536e6d7d1d965e161b16ca5 Mon Sep 17 00:00:00 2001 From: malti Date: Thu, 14 Mar 2024 00:04:29 -0400 Subject: [PATCH 25/79] auto aim shot implemented --- .../java/frc/robot/FortissiMOEContainer.java | 30 +++++++++---------- .../frc/robot/subsystems/SwerveDrive.java | 10 +++++-- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index cab348e..d18f707 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -240,9 +240,6 @@ public class FortissiMOEContainer{ 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 @@ -310,11 +307,11 @@ private void configureBindings() { || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); //podium shot - new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-38.5)), Set.of(armSubsystem)) + /*new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-38.5)), 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)))); + || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9))));*/ //mid shot new JoystickButton(functionJoystick, 9).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-31)), Set.of(armSubsystem)) @@ -347,19 +344,20 @@ private void configureBindings() { // 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 + new JoystickButton(functionJoystick, 4).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(3))), + Commands.run(()->swerveSubsystem.setDesiredYaw(swerveSubsystem.getAngleBetweenSpeaker( + ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).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)); - - } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java index 236684d..7c994fe 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -107,6 +107,7 @@ public Command setInitPosition(Pose2d initPose){ ); } public void setDesiredYaw(double yaw){ + align = true; desiredYaw = yaw; } @@ -249,9 +250,12 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { } public void driveAtSpeed(double xspd, double yspd, double turnspd, boolean fieldOriented, boolean red){ -// if (align){ -// turnspd = thetaController.calculate(pigeon.getYaw(), desiredYaw); -// } + if (turnspd != 0) { + align = false; + } + if (align){ + turnspd = getYawCorrection(); + } ChassisSpeeds chassisSpeeds; if (fieldOriented){ Rotation2d currRot = getRotation2d(); From dd569744ea6c6981acdd6037f4935658fe8f3da4 Mon Sep 17 00:00:00 2001 From: malti Date: Thu, 14 Mar 2024 22:30:40 -0400 Subject: [PATCH 26/79] recalibs? --- .../java/frc/robot/FortissiMOEContainer.java | 16 +++++++++------- .../java/frc/robot/commands/ArmPathFollow.java | 4 +++- .../robot/commands/ShooterControllerCommand.java | 2 +- src/main/java/frc/robot/subsystems/Arm.java | 2 +- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index d18f707..9b7dd83 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -127,7 +127,7 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////arm subsystem start private final Arm armSubsystem = new Arm(4, 15,14, 35, 36, 2.0e-2, 2.0e-3, 4.0e-4, 8.0e-3, 0, 1.0e-4, 23.839, 14.231, - Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 70,30); + Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-47), 70,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end @@ -307,12 +307,12 @@ private void configureBindings() { || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); //podium shot - /*new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-38.5)), Set.of(armSubsystem)) + new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-47.5)), 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 + || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); + //all the shots smh until roshik chooses a new random button to be his favorite new JoystickButton(functionJoystick, 9).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-31)), Set.of(armSubsystem)) .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || @@ -344,18 +344,20 @@ private void configureBindings() { // 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, 4).onTrue( + /*new JoystickButton(functionJoystick, 4).onTrue( Commands.parallel( Commands.defer(()->armSubsystem.goToPoint( Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getX()), - Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getY())), Set.of(armSubsystem)) + Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getY())), Set.of(armSubsystem)).andThen( + Commands.run(()-> armSubsystem.holdPos(armSubsystem.shoulderPosRel(), armSubsystem.wristPosRel())) + ) .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || functionJoystick.getRawButton(1) || functionJoystick.getRawButton(3))), Commands.run(()->swerveSubsystem.setDesiredYaw(swerveSubsystem.getAngleBetweenSpeaker( ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).until( ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1 - ))); //auto aim shot + ))); //auto aim shot*/ //104,-41 // new JoystickButton(driverJoystick, 7).whileTrue(setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>= .1)); } diff --git a/src/main/java/frc/robot/commands/ArmPathFollow.java b/src/main/java/frc/robot/commands/ArmPathFollow.java index d57d3b3..57bb987 100644 --- a/src/main/java/frc/robot/commands/ArmPathFollow.java +++ b/src/main/java/frc/robot/commands/ArmPathFollow.java @@ -64,7 +64,9 @@ public void execute() { s = desiredPoint.getDistance(startPoint)+1; } SmartDashboard.putNumber("ArmPathFollow writePos", wristPos); - SmartDashboard.putNumber("ArmPathFollow desiredWrite", desiredPoint.getY()); + 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)); armSubsystem.setWristDestState(wristPos); armSubsystem.setShoulderDesState(shoulderPos); diff --git a/src/main/java/frc/robot/commands/ShooterControllerCommand.java b/src/main/java/frc/robot/commands/ShooterControllerCommand.java index 4c319b2..c4d61a1 100644 --- a/src/main/java/frc/robot/commands/ShooterControllerCommand.java +++ b/src/main/java/frc/robot/commands/ShooterControllerCommand.java @@ -47,7 +47,7 @@ public void execute() { onState %= 2; } if (onState%2 == 1){ - shooterSpeedTop = 4000; shooterSpeedBottom = 4000; + shooterSpeedTop = 3500; shooterSpeedBottom = 3500; if (desShoulder.get() <= 85) {shooterSpeedTop = 3000; shooterSpeedBottom = 3000;} subsystem.setShooterSpeeds(shooterSpeedTop, shooterSpeedBottom); } else{ diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 4268b4b..be675f2 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -138,7 +138,7 @@ public Command goToPoint(Rotation2d shoulderPos, Rotation2d wristPos) { public Translation2d autoAim(Supplier robotPos){ double dist = AllianceFlip.apply(UsefulPoints.Points.middleOfSpeaker).getDistance(robotPos.get().getTranslation()); - return new Translation2d(0.466*dist + 112, 5.56*dist - 69.1); + return new Translation2d(112, 3.8*dist-53.1); } From 20d8bcc0e20ef009504c1ee1eeadbbcdc165d01b Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Fri, 15 Mar 2024 21:40:59 -0400 Subject: [PATCH 27/79] tests from today --- .../java/frc/robot/FortissiMOEContainer.java | 53 ++++++++----------- .../frc/robot/commands/SwerveController.java | 13 ++--- .../frc/robot/subsystems/SwerveDrive.java | 1 + 3 files changed, 31 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 9b7dd83..a04984e 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -46,8 +46,8 @@ public class FortissiMOEContainer{ 0, false, true, - -1000,//0.7 * ClimberArm.CONVERSION_FACTOR_INCHES, - -1000,//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, @@ -206,26 +206,18 @@ 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); @@ -300,14 +292,14 @@ private void configureBindings() { || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); //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, 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)))); +// //auto aim shot - new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-47.5)), Set.of(armSubsystem)) + new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41.5)), 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) @@ -344,22 +336,23 @@ private void configureBindings() { // 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, 4).onTrue( + new JoystickButton(functionJoystick, 2).onTrue( Commands.parallel( - Commands.defer(()->armSubsystem.goToPoint( + /*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.shoulderPosRel(), armSubsystem.wristPosRel())) ) .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1) || functionJoystick.getRawButton(3))), + functionJoystick.getRawButton(1) || functionJoystick.getRawButton(3))),*/ Commands.run(()->swerveSubsystem.setDesiredYaw(swerveSubsystem.getAngleBetweenSpeaker( ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).until( ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1 - ))); //auto aim shot*/ + ))); //auto aim shot //104,-41 // new JoystickButton(driverJoystick, 7).whileTrue(setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>= .1)); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/SwerveController.java b/src/main/java/frc/robot/commands/SwerveController.java index 68ec258..bfcc9da 100644 --- a/src/main/java/frc/robot/commands/SwerveController.java +++ b/src/main/java/frc/robot/commands/SwerveController.java @@ -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/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java index 7c994fe..957af8a 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -256,6 +256,7 @@ public void driveAtSpeed(double xspd, double yspd, double turnspd, boolean field if (align){ turnspd = getYawCorrection(); } + if (xspd == 0 && yspd == 0 && turnspd == 0) stopModules(); ChassisSpeeds chassisSpeeds; if (fieldOriented){ Rotation2d currRot = getRotation2d(); From 9ea3665f1742430a1144bfd847fe4c9426b22159 Mon Sep 17 00:00:00 2001 From: tanmay Date: Sat, 16 Mar 2024 08:13:03 -0400 Subject: [PATCH 28/79] Need to check out another branch so I committed. --- src/main/java/frc/robot/commands/autos/tripleNoteAutos.java | 5 ++++- src/main/java/frc/robot/commands/shootSpeakerCommand.java | 4 ++-- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 ++ 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index ca61e1d..84a0685 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -31,6 +31,9 @@ public class tripleNoteAutos { private final double startVelocity; //Velocities are in meters/second. private final double endVelocity; + private final double subShooterSpeed = 3000; + private final double podiumShooterSpeed = 4000; + private final double passShooterSpeed = ; private ShooterSubsystem shooter; private CollectorSubsystem collector; private Arm armSubsystem; @@ -98,7 +101,7 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints3,0,0); Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints4,0,0); Command trajCommand5 = swerveDrive.generateTrajectory(startPose5,endPose5,internalPoints5,0,0); - + Command setShooterSpeed = new Command(Commands.parallel(()->shooter.setDesShooterSpeedTop(3000),()->shooter.getDesShooterSpeedBot(3000)); Command shootNote = new shootSpeakerCommand(shooter,collector); Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); Command shootLastNote = new shootSpeakerCommand(shooter, collector); diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index 09632c2..d9b0cce 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -46,8 +46,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.setShooterTopSpeed(speedCalc()); - shooter.setShooterBottomSpeed(speedCalc()); + shooter.setShooterTopSpeed(shooter.getDesShooterSpeedTop()); + shooter.setShooterBottomSpeed(shooter.getDesShooterSpeedBot()); if(shooter.shooterAtSpeed() && !shot){ shot = true; collector.setCollectorSpeed(1); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 1377525..9168bb8 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -98,6 +98,8 @@ public void setShooterRPMTolerance(double Tolerance){ SmartDashboard.putNumber("Shooter RPM Tolerance", Tolerance); } + + public double getShooterRPMTolerance(){ return shooterRPMTolerance; } From c656c54632b2973f0df6b63ddfc38a2e296811e8 Mon Sep 17 00:00:00 2001 From: tanmay Date: Sat, 16 Mar 2024 08:33:38 -0400 Subject: [PATCH 29/79] Need to check out another branch so I committed. --- .../robot/commands/autos/tripleNoteAutos.java | 4 +-- .../robot/commands/shootSpeakerCommand.java | 28 +++++++++++++++---- 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 84a0685..960bd51 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -33,7 +33,7 @@ public class tripleNoteAutos { private final double endVelocity; private final double subShooterSpeed = 3000; private final double podiumShooterSpeed = 4000; - private final double passShooterSpeed = ; + private final double passShooterSpeed = 1300; private ShooterSubsystem shooter; private CollectorSubsystem collector; private Arm armSubsystem; @@ -101,7 +101,6 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints3,0,0); Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints4,0,0); Command trajCommand5 = swerveDrive.generateTrajectory(startPose5,endPose5,internalPoints5,0,0); - Command setShooterSpeed = new Command(Commands.parallel(()->shooter.setDesShooterSpeedTop(3000),()->shooter.getDesShooterSpeedBot(3000)); Command shootNote = new shootSpeakerCommand(shooter,collector); Command shootAnotherNote = new shootSpeakerCommand(shooter,collector); Command shootLastNote = new shootSpeakerCommand(shooter, collector); @@ -114,6 +113,7 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); return Commands.sequence( swerveDrive.setInitPosition(startPose), + Commands.runOnce(Commands.parallel(()->shooter.setDesShooterSpeedTop(3000),shooter.setDesShooterSpeedBot(3000)),shooter); Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-65)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)), Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index d9b0cce..bad11db 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -17,6 +17,7 @@ public class shootSpeakerCommand extends Command { private final CollectorSubsystem collector; private Timer timer; private boolean shot = false; + private double desSpeed = 0; /** * Creates a new ExampleCommand. * @@ -32,6 +33,16 @@ public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem timer = new Timer(); shot = false; } + public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem,double desSpeed) { + this.shooter = shooterSubsystem; + this.collector = collectorSubsystem; + this.desSpeed = desSpeed; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooterSubsystem,collectorSubsystem); + + timer = new Timer(); + shot = false; + } //TODO: Calculate shooter speeds based on odometry. public double speedCalc(){ return 3000; @@ -46,13 +57,18 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.setShooterTopSpeed(shooter.getDesShooterSpeedTop()); - shooter.setShooterBottomSpeed(shooter.getDesShooterSpeedBot()); - if(shooter.shooterAtSpeed() && !shot){ - shot = true; - collector.setCollectorSpeed(1); - timer.restart(); + if(desSpeed ==0){ + shooter.setShooterTopSpeed(); + shooter.setShooterBottomSpeed(); + if(shooter.shooterAtSpeed() && !shot){ + shot = true; + collector.setCollectorSpeed(1); + timer.restart(); + } + } else { + } + SmartDashboard.putNumber("auto Time", timer.get()); } From 80eb7a25290ac773332e88967f66bb05ef5ff2e8 Mon Sep 17 00:00:00 2001 From: tanmay Date: Sat, 16 Mar 2024 09:23:49 -0400 Subject: [PATCH 30/79] Chnaged ShooterControllerCommand to allow variable speed set through shooter.setDesiredSpeed for each motor. Default value is 3000 rpm. --- .../java/frc/robot/FortissiMOEContainer.java | 3 ++ .../commands/ShooterControllerCommand.java | 4 +-- .../robot/commands/autos/tripleNoteAutos.java | 4 --- .../robot/commands/shootSpeakerCommand.java | 28 ++++--------------- .../robot/subsystems/ShooterSubsystem.java | 2 -- 5 files changed, 11 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index a04984e..c2588eb 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -238,6 +238,9 @@ public class FortissiMOEContainer{ public FortissiMOEContainer() { shooterSubsystem.setShooterRPMTolerance(500); + shooterSubsystem.setDesShooterSpeedBot(3000); + shooterSubsystem.setDesShooterSpeedTop(3000); + swerveSubsystem.setDefaultCommand(drive); // collectorSubsystem.setDefaultCommand(collectorCommand); diff --git a/src/main/java/frc/robot/commands/ShooterControllerCommand.java b/src/main/java/frc/robot/commands/ShooterControllerCommand.java index c4d61a1..87a47b2 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 = 3500; shooterSpeedBottom = 3500; - if (desShoulder.get() <= 85) {shooterSpeedTop = 3000; shooterSpeedBottom = 3000;} + shooterSpeedTop = subsystem.getDesShooterSpeedTop(); shooterSpeedBottom = subsystem.getDesShooterSpeedBot(); + if (desShoulder.get() <= 85) {shooterSpeedTop = subsystem.getDesShooterSpeedTop()-500; shooterSpeedBottom = subsystem.getDesShooterSpeedBot()-500;} subsystem.setShooterSpeeds(shooterSpeedTop, shooterSpeedBottom); } else{ shooterSpeedTop = 0; diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 960bd51..80cd59a 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -31,9 +31,6 @@ public class tripleNoteAutos { private final double startVelocity; //Velocities are in meters/second. private final double endVelocity; - private final double subShooterSpeed = 3000; - private final double podiumShooterSpeed = 4000; - private final double passShooterSpeed = 1300; private ShooterSubsystem shooter; private CollectorSubsystem collector; private Arm armSubsystem; @@ -113,7 +110,6 @@ public Command CW1W2(){//TODO: Fix coordinates, create actual shoot and collect Command headingCorrect4 = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()-> AllianceFlip.apply(endRotation4)); return Commands.sequence( swerveDrive.setInitPosition(startPose), - Commands.runOnce(Commands.parallel(()->shooter.setDesShooterSpeedTop(3000),shooter.setDesShooterSpeedBot(3000)),shooter); Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-65)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), Set.of(armSubsystem)), Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), diff --git a/src/main/java/frc/robot/commands/shootSpeakerCommand.java b/src/main/java/frc/robot/commands/shootSpeakerCommand.java index bad11db..09632c2 100644 --- a/src/main/java/frc/robot/commands/shootSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/shootSpeakerCommand.java @@ -17,7 +17,6 @@ public class shootSpeakerCommand extends Command { private final CollectorSubsystem collector; private Timer timer; private boolean shot = false; - private double desSpeed = 0; /** * Creates a new ExampleCommand. * @@ -33,16 +32,6 @@ public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem timer = new Timer(); shot = false; } - public shootSpeakerCommand(ShooterSubsystem shooterSubsystem,CollectorSubsystem collectorSubsystem,double desSpeed) { - this.shooter = shooterSubsystem; - this.collector = collectorSubsystem; - this.desSpeed = desSpeed; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(shooterSubsystem,collectorSubsystem); - - timer = new Timer(); - shot = false; - } //TODO: Calculate shooter speeds based on odometry. public double speedCalc(){ return 3000; @@ -57,18 +46,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(desSpeed ==0){ - shooter.setShooterTopSpeed(); - shooter.setShooterBottomSpeed(); - if(shooter.shooterAtSpeed() && !shot){ - shot = true; - collector.setCollectorSpeed(1); - timer.restart(); - } - } else { - + shooter.setShooterTopSpeed(speedCalc()); + shooter.setShooterBottomSpeed(speedCalc()); + if(shooter.shooterAtSpeed() && !shot){ + shot = true; + collector.setCollectorSpeed(1); + timer.restart(); } - SmartDashboard.putNumber("auto Time", timer.get()); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9168bb8..1377525 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -98,8 +98,6 @@ public void setShooterRPMTolerance(double Tolerance){ SmartDashboard.putNumber("Shooter RPM Tolerance", Tolerance); } - - public double getShooterRPMTolerance(){ return shooterRPMTolerance; } From 88b25b6705849b400017d6381cea075d22c97290 Mon Sep 17 00:00:00 2001 From: tanmay Date: Sat, 16 Mar 2024 10:29:08 -0400 Subject: [PATCH 31/79] Added DC5C4PassC3 auto. --- .../java/frc/robot/commands/NoteFeed.java | 69 +++++++++++++++++++ .../robot/commands/autos/doubleNoteAutos.java | 56 +++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 src/main/java/frc/robot/commands/NoteFeed.java 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/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index e9bf4ec..3f616bd 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -15,6 +15,7 @@ import frc.robot.AllianceFlip; import frc.robot.UsefulPoints; import frc.robot.commands.Collect; +import frc.robot.commands.NoteFeed; import frc.robot.commands.setHeading; import frc.robot.commands.shootSpeakerCommand; import frc.robot.subsystems.Arm; @@ -239,6 +240,61 @@ public Command DC5Auto(){ 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 trajCommand5 = swerveDrive.generateTrajectory(startPose5,endPose5,internalPoints,0,0); + + Command shootNote = new shootSpeakerCommand(shooter,collector); + Command passNote = new NoteFeed(shooter,collector,()->1300); + Command passAnotherNote = new NoteFeed(shooter,collector,()->1300); + 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(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()))), + Commands.race(passNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(headingCorrectCollect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand2.andThen(()->swerveDrive.stopModules()),collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()),collectNote), 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()))), + Commands.race(headingCorrectCollect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.race(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules()),collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()),collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + Commands.runOnce(()->swerveDrive.stopModules()), + Commands.race(headingCorrect3.andThen(()-> swerveDrive.stopModules()),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); From f16ea163b639559818ec37f65f53e8d73a3d9d52 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 16 Mar 2024 11:49:54 -0400 Subject: [PATCH 32/79] tests from today --- .../java/frc/robot/FortissiMOEContainer.java | 16 +++++++++------- src/main/java/frc/robot/subsystems/Arm.java | 9 ++++----- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index a04984e..21aa7a5 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -74,7 +74,7 @@ public class FortissiMOEContainer{ double maxRPS = 1.5*2*Math.PI; double maxRPS2 = Math.PI; - double maxMPSSquared = 5; + double maxMPSSquared = 3; private final SendableChooser m_chooser = new SendableChooser<>(); private final SwerveModule backLeftModule = new SwerveModule( @@ -292,7 +292,9 @@ private void configureBindings() { || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9)))); //collect -// new JoystickButton(functionJoystick, 2).onTrue(Commands.defer(() -> armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-41.5)), Set.of(armSubsystem)) +// new JoystickButton(functionJoystick, 2).onTrue(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(4) || functionJoystick.getRawButton(1) || // functionJoystick.getRawButton(8)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2) @@ -338,14 +340,14 @@ private void configureBindings() { // new JoystickButton(driverJoystick, 8).onTrue(turnToSource.until(()->(Math.abs(driverJoystick.getRawAxis(2)) >= .2))); new JoystickButton(functionJoystick, 2).onTrue( Commands.parallel( - /*Commands.defer(()->armSubsystem.goToPoint( + 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.shoulderPosRel(), armSubsystem.wristPosRel())) - ) + Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()) + )) .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) || - functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) || - functionJoystick.getRawButton(1) || functionJoystick.getRawButton(3))),*/ + functionJoystick.getRawButtonPressed(4) || functionJoystick.getRawButton(8) || + functionJoystick.getRawButton(1) || functionJoystick.getRawButton(3))), Commands.run(()->swerveSubsystem.setDesiredYaw(swerveSubsystem.getAngleBetweenSpeaker( ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).until( ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1 diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index be675f2..dc4ddaf 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -11,6 +11,7 @@ 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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -138,7 +139,9 @@ public Command goToPoint(Rotation2d shoulderPos, Rotation2d wristPos) { public Translation2d autoAim(Supplier robotPos){ double dist = AllianceFlip.apply(UsefulPoints.Points.middleOfSpeaker).getDistance(robotPos.get().getTranslation()); - return new Translation2d(112, 3.8*dist-53.1); + dist = Units.metersToInches(dist); + return new Translation2d(112, Math.min(Math.max(-45, 4.63e-5*Math.pow(dist, 3)-1.7e-2*Math.pow(dist, 2) + +2.13*dist-131.8), -30)); } @@ -214,10 +217,6 @@ public void holdPos(double shoulderRel, double wristRel){ //wristPower(0); //shoulderPower(0); } - public Translation2d getAutoArmPosition(Supplier currPose){ - //return a math function that interpolates this - return new Translation2d(105, -31); //this is a placeholder - } } \ No newline at end of file From cf35eb35f5e4b5a04ec5fdcf0b14cc50955117f9 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 16 Mar 2024 12:23:46 -0400 Subject: [PATCH 33/79] tests from today --- src/main/java/frc/robot/FortissiMOEContainer.java | 6 ++++-- .../java/frc/robot/subsystems/ShooterSubsystem.java | 10 +++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index a2d0fa3..e2da6a8 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -238,8 +238,7 @@ public class FortissiMOEContainer{ public FortissiMOEContainer() { shooterSubsystem.setShooterRPMTolerance(500); - shooterSubsystem.setDesShooterSpeedBot(3000); - shooterSubsystem.setDesShooterSpeedTop(3000); + shooterSubsystem.setDesShooterSpeeds(3500,3500); swerveSubsystem.setDefaultCommand(drive); // collectorSubsystem.setDefaultCommand(collectorCommand); @@ -355,6 +354,9 @@ private void configureBindings() { ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).until( ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1 ))); //auto aim shot + + new JoystickButton(buttonBox, 6).whileTrue(Commands.runOnce(()->shooterSubsystem.setDesShooterSpeeds(1800,1800))). + whileFalse(Commands.runOnce(()->shooterSubsystem.setDesShooterSpeeds(3500,3500))); //104,-41 // new JoystickButton(driverJoystick, 7).whileTrue(setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>= .1)); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 1377525..f7be21f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -41,7 +41,7 @@ public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, shooterBottomController.setD(shooterD); shooterBottomController.setFF(shooterFF); shooterTopController.setOutputRange(0, 1); shooterRPMTolerance=5; - desBotSpeed = 0; desTopSpeed = 0; + desBotSpeed = 3500; desTopSpeed = 3500; } @Override public void periodic(){ @@ -51,7 +51,7 @@ public void periodic(){ } public void setShooterTopSpeed(double speed){ - setDesShooterSpeedTop(speed); + //setDesShooterSpeedTop(speed); shooterSpeedTop=speed; SmartDashboard.putNumber("shooterTopDesired", speed); shooterTopController.setReference(speed, CANSparkBase.ControlType.kVelocity); @@ -59,7 +59,7 @@ public void setShooterTopSpeed(double speed){ } public void setShooterBottomSpeed(double speed){ - setDesShooterSpeedBot(speed); + //setDesShooterSpeedBot(speed); shooterSpeedBottom=speed; SmartDashboard.putNumber("shooterBottomDesired", speed); shooterBottomController.setReference(speed, CANSparkBase.ControlType.kVelocity); @@ -68,6 +68,10 @@ public void setShooterBottomSpeed(double speed){ public void setShooterSpeeds(double topSpeed, double bottomSpeed){ setShooterTopSpeed(topSpeed); setShooterBottomSpeed(bottomSpeed); } + public void setDesShooterSpeeds(double topSpeed, double bottomSpeed){ + setDesShooterSpeedTop(topSpeed); + setDesShooterSpeedBot(bottomSpeed); + } public void stopShooter(){ setShooterSpeeds(0,0); From 72f550bfd36cf7943166bee91b1c7da5badabc4f Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 16 Mar 2024 16:28:57 -0400 Subject: [PATCH 34/79] tests from today --- .../java/frc/robot/FortissiMOEContainer.java | 11 ++-- .../commands/ShooterControllerCommand.java | 4 +- .../robot/commands/autos/doubleNoteAutos.java | 25 ++++----- .../java/frc/robot/commands/setHeading.java | 1 + .../robot/subsystems/ShooterSubsystem.java | 52 ++++++++++++------- 5 files changed, 50 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index e2da6a8..f6c37b0 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -18,14 +18,12 @@ 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 java.util.Set; @@ -225,7 +223,7 @@ public class FortissiMOEContainer{ SmartDashboard.putNumber("Roll", pigeon.getRoll()); SmartDashboard.putNumber("Pitch", pigeon.getPitch()); pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0)) - || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesShooterSpeedTop() != 0)); + || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getMaxShooterSpeedTop() != 0)); }); ////////////////////////////////////////////////////////////////////////////commands end @@ -238,7 +236,7 @@ public class FortissiMOEContainer{ public FortissiMOEContainer() { shooterSubsystem.setShooterRPMTolerance(500); - shooterSubsystem.setDesShooterSpeeds(3500,3500); + shooterSubsystem.setMaxShooterSpeeds(3500,3500); swerveSubsystem.setDefaultCommand(drive); // collectorSubsystem.setDefaultCommand(collectorCommand); @@ -277,6 +275,7 @@ public FortissiMOEContainer() { m_chooser.addOption("D Score Move", new doubleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).DMoveAuto()); m_chooser.addOption("D Score Collect (DC5)", new doubleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).DC5Auto()); m_chooser.addOption("4 Note Auto (CW2W1W3)", new tripleNoteAutos(swerveSubsystem,armSubsystem,shooterSubsystem,collectorSubsystem,0,0).CW1W2W3()); + m_chooser.addOption("centerLine Auto", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3()); SmartDashboard.putData("chooser", m_chooser); } @@ -355,8 +354,8 @@ private void configureBindings() { ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1 ))); //auto aim shot - new JoystickButton(buttonBox, 6).whileTrue(Commands.runOnce(()->shooterSubsystem.setDesShooterSpeeds(1800,1800))). - whileFalse(Commands.runOnce(()->shooterSubsystem.setDesShooterSpeeds(3500,3500))); + new JoystickButton(buttonBox, 6).whileTrue(Commands.runOnce(()->shooterSubsystem.setMaxShooterSpeeds(2300,2300))). + whileFalse(Commands.runOnce(()->shooterSubsystem.setMaxShooterSpeeds(3500,3500))); //104,-41 // new JoystickButton(driverJoystick, 7).whileTrue(setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>= .1)); diff --git a/src/main/java/frc/robot/commands/ShooterControllerCommand.java b/src/main/java/frc/robot/commands/ShooterControllerCommand.java index 87a47b2..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 = subsystem.getDesShooterSpeedTop(); shooterSpeedBottom = subsystem.getDesShooterSpeedBot(); - if (desShoulder.get() <= 85) {shooterSpeedTop = subsystem.getDesShooterSpeedTop()-500; shooterSpeedBottom = subsystem.getDesShooterSpeedBot()-500;} + 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/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index 3f616bd..3b5f3e3 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -258,11 +258,10 @@ public Command DC5C4PassC3(){ 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 trajCommand5 = swerveDrive.generateTrajectory(startPose5,endPose5,internalPoints,0,0); Command shootNote = new shootSpeakerCommand(shooter,collector); - Command passNote = new NoteFeed(shooter,collector,()->1300); - Command passAnotherNote = new NoteFeed(shooter,collector,()->1300); + 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); @@ -278,21 +277,17 @@ public Command DC5C4PassC3(){ Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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()), + //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()))), - Commands.race(headingCorrectCollect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.race(Commands.parallel(trajCommand2.andThen(()->swerveDrive.stopModules()),collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()),collectNote), 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), 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()))), + 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()),collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()),collectNote), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), - Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(headingCorrect3.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 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/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f7be21f..7a5cc90 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,8 +4,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.Supplier; - public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax shooterTop; @@ -19,7 +17,8 @@ public class ShooterSubsystem extends SubsystemBase { private double shooterSpeedTop=0;//Store desired speeds private double shooterSpeedBottom=0; private double shooterRPMTolerance=0; - private double desTopSpeed, desBotSpeed; + private double maxTopSpeed, maxBotSpeed; + private double desiredTopSpeed, desiredBotSpeed; public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, double shooterI, double shooterD, double shooterFF) { shooterTop = new CANSparkMax(shooterTopID, CANSparkLowLevel.MotorType.kBrushless); @@ -41,17 +40,18 @@ public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, shooterBottomController.setD(shooterD); shooterBottomController.setFF(shooterFF); shooterTopController.setOutputRange(0, 1); shooterRPMTolerance=5; - desBotSpeed = 3500; desTopSpeed = 3500; + maxBotSpeed = 3500; maxTopSpeed = 3500; + desiredBotSpeed = 0; desiredTopSpeed = 0; } @Override public void periodic(){ SmartDashboard.putNumber("shooterTopSpeed", getShooterSpeedTop()); SmartDashboard.putNumber("shooterBotSpeed", getShooterSpeedBottom()); - SmartDashboard.putBoolean("shooterOn", getDesShooterSpeedTop() != 0); + SmartDashboard.putBoolean("shooterOn", getMaxShooterSpeedTop() != 0); } public void setShooterTopSpeed(double speed){ - //setDesShooterSpeedTop(speed); + setDesiredTopSpeed(speed); shooterSpeedTop=speed; SmartDashboard.putNumber("shooterTopDesired", speed); shooterTopController.setReference(speed, CANSparkBase.ControlType.kVelocity); @@ -59,7 +59,7 @@ public void setShooterTopSpeed(double speed){ } public void setShooterBottomSpeed(double speed){ - //setDesShooterSpeedBot(speed); + setDesiredBotSpeed(speed); shooterSpeedBottom=speed; SmartDashboard.putNumber("shooterBottomDesired", speed); shooterBottomController.setReference(speed, CANSparkBase.ControlType.kVelocity); @@ -68,11 +68,23 @@ public void setShooterBottomSpeed(double speed){ public void setShooterSpeeds(double topSpeed, double bottomSpeed){ setShooterTopSpeed(topSpeed); setShooterBottomSpeed(bottomSpeed); } - public void setDesShooterSpeeds(double topSpeed, double bottomSpeed){ - setDesShooterSpeedTop(topSpeed); - setDesShooterSpeedBot(bottomSpeed); + public void setMaxShooterSpeeds(double topSpeed, double bottomSpeed){ + setMaxShooterSpeedTop(topSpeed); + setMaxShooterSpeedBot(bottomSpeed); + } + public void setDesiredTopSpeed(double speed){ + desiredTopSpeed = speed; + } + public void setDesiredBotSpeed(double speed){ + desiredBotSpeed = speed; } + public double getDesiredTopSpeed(){ + return desiredTopSpeed; + } + public double getDesiredBotSpeed(){ + return desiredBotSpeed; + } public void stopShooter(){ setShooterSpeeds(0,0); } @@ -83,18 +95,18 @@ public double getShooterSpeedBottom(){ return shooterBottomEncoder.getVelocity(); } - public double getDesShooterSpeedTop(){ - return desTopSpeed; + public double getMaxShooterSpeedTop(){ + return maxTopSpeed; } - public double getDesShooterSpeedBot(){ - return desBotSpeed; + public double getMaxShooterSpeedBot(){ + return maxBotSpeed; } - public void setDesShooterSpeedTop(double speed){ - desTopSpeed = speed; + public void setMaxShooterSpeedTop(double speed){ + maxTopSpeed = speed; } - public void setDesShooterSpeedBot(double speed){ - desBotSpeed = speed; + public void setMaxShooterSpeedBot(double speed){ + maxBotSpeed = speed; } public void setShooterRPMTolerance(double Tolerance){ @@ -107,7 +119,7 @@ public double getShooterRPMTolerance(){ } public boolean shooterAtSpeed(){ - return ((Math.abs(getDesShooterSpeedTop() - getShooterSpeedTop()) <= getShooterRPMTolerance()) && - (Math.abs(getDesShooterSpeedBot() - getShooterSpeedBottom()) <= getShooterRPMTolerance())); + return ((Math.abs(getDesiredTopSpeed() - getShooterSpeedTop()) <= getShooterRPMTolerance()) && + (Math.abs(getDesiredBotSpeed() - getShooterSpeedBottom()) <= getShooterRPMTolerance())); } } \ No newline at end of file From bc7387c42414114cedba678d288ae44e7c08a2c9 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 16 Mar 2024 17:45:21 -0400 Subject: [PATCH 35/79] tests from today --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7a5cc90..52f5ce9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,6 +27,8 @@ public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, shooterBottom.setInverted(true); shooterTop.setIdleMode(CANSparkBase.IdleMode.kCoast); shooterBottom.setIdleMode(CANSparkBase.IdleMode.kCoast); + shooterBottom.setSmartCurrentLimit(30); + shooterTop.setSmartCurrentLimit(30); shooterTopEncoder = shooterTop.getEncoder(); shooterTopController = shooterTop.getPIDController(); From ebc95c9892f2c3e8f5732ffe4656fa66b799d18b Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sat, 16 Mar 2024 19:22:33 -0400 Subject: [PATCH 36/79] tests from today --- src/main/java/frc/robot/FortissiMOEContainer.java | 2 +- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index f6c37b0..9215af0 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -223,7 +223,7 @@ public class FortissiMOEContainer{ SmartDashboard.putNumber("Roll", pigeon.getRoll()); SmartDashboard.putNumber("Pitch", pigeon.getPitch()); pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0)) - || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getMaxShooterSpeedTop() != 0)); + || (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0)); }); ////////////////////////////////////////////////////////////////////////////commands end diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 52f5ce9..140a339 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,8 +27,8 @@ public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, shooterBottom.setInverted(true); shooterTop.setIdleMode(CANSparkBase.IdleMode.kCoast); shooterBottom.setIdleMode(CANSparkBase.IdleMode.kCoast); - shooterBottom.setSmartCurrentLimit(30); - shooterTop.setSmartCurrentLimit(30); + shooterBottom.setSmartCurrentLimit(38); + shooterTop.setSmartCurrentLimit(38); shooterTopEncoder = shooterTop.getEncoder(); shooterTopController = shooterTop.getPIDController(); From e99fcae066de34a143e9bcf3d2d589a680a4111f Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sun, 17 Mar 2024 10:50:27 -0400 Subject: [PATCH 37/79] new auto aim and new auto --- .../java/frc/robot/commands/SwerveController.java | 2 +- .../frc/robot/commands/autos/doubleNoteAutos.java | 15 +++++++++++++-- src/main/java/frc/robot/subsystems/Arm.java | 12 ++++++++++-- .../frc/robot/subsystems/ShooterSubsystem.java | 4 ++-- 4 files changed, 26 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/SwerveController.java b/src/main/java/frc/robot/commands/SwerveController.java index bfcc9da..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; diff --git a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java index 3b5f3e3..10f28ae 100644 --- a/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/doubleNoteAutos.java @@ -276,12 +276,12 @@ public Command DC5C4PassC3(){ Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(135), Rotation2d.fromDegrees(-35)), Set.of(armSubsystem)), Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(83), Rotation2d.fromDegrees(-41)), 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.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), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), + 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()))), @@ -470,6 +470,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/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index dc4ddaf..ee0af87 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -140,8 +140,16 @@ public Command goToPoint(Rotation2d shoulderPos, Rotation2d wristPos) { public Translation2d autoAim(Supplier robotPos){ double dist = AllianceFlip.apply(UsefulPoints.Points.middleOfSpeaker).getDistance(robotPos.get().getTranslation()); dist = Units.metersToInches(dist); - return new Translation2d(112, Math.min(Math.max(-45, 4.63e-5*Math.pow(dist, 3)-1.7e-2*Math.pow(dist, 2) - +2.13*dist-131.8), -30)); + double func; + if (-.901*dist+130.46 < 0.0){ + func = -38.5+Math.pow(.901*dist-130.46, 1.0/3.0); + } + else{ + func = -38.5-Math.pow(-.901*dist+130.46, 1.0/3.0); + } + return new Translation2d(112, Math.min(Math.max(-45, func), -30)); +// return new Translation2d(112, Math.min(Math.max(-45, 4.63e-5*Math.pow(dist, 3)-1.7e-2*Math.pow(dist, 2) +// +2.13*dist-131.8)-1, -30)); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 140a339..9a1cfb2 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,8 +27,8 @@ public ShooterSubsystem(int shooterTopID, int shooterBottomID, double shooterP, shooterBottom.setInverted(true); shooterTop.setIdleMode(CANSparkBase.IdleMode.kCoast); shooterBottom.setIdleMode(CANSparkBase.IdleMode.kCoast); - shooterBottom.setSmartCurrentLimit(38); - shooterTop.setSmartCurrentLimit(38); +// shooterBottom.setSmartCurrentLimit(38); +// shooterTop.setSmartCurrentLimit(38); shooterTopEncoder = shooterTop.getEncoder(); shooterTopController = shooterTop.getPIDController(); From c12a01657c486b165838fbc76ec78d887fcb16e0 Mon Sep 17 00:00:00 2001 From: MOE 365 Programming Laptop Date: Sun, 17 Mar 2024 15:23:48 -0400 Subject: [PATCH 38/79] new wrist logic --- src/main/java/frc/robot/FortissiMOEContainer.java | 3 ++- src/main/java/frc/robot/subsystems/Arm.java | 12 +++++++----- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index 9215af0..e84f8b9 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -124,7 +124,7 @@ public class FortissiMOEContainer{ /////////////////////////////////////////////////////////////////////////////drive subsystems end /////////////////////////////////////////////////////////////////////////////arm subsystem start private final Arm armSubsystem = new Arm(4, 15,14, 35, 36, - 2.0e-2, 2.0e-3, 4.0e-4, 8.0e-3, 0, 1.0e-4, 23.839, 14.231, + 2.0e-2, 2.0e-3, 4.0e-4, 8.0e-3,0, 1.0e-4, 23.839, 14.231, Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-47), 70,30); /////////////////////////////////////////////////////////////////////////// arm subsystem end @@ -276,6 +276,7 @@ public FortissiMOEContainer() { 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()); SmartDashboard.putData("chooser", m_chooser); } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index ee0af87..12f613a 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -178,16 +178,18 @@ public Rotation2d shoulderState() { } public Rotation2d wristState(){ - double deg = wristEncoder.getAbsolutePosition() + wristOffset; - if (deg < -180) deg = deg + 360; - if (deg > 180) deg = deg - 360; - return Rotation2d.fromDegrees(deg); + return Rotation2d.fromDegrees(wristPosRel()); +// double deg = wristEncoder.getAbsolutePosition() + wristOffset; +// if (deg < -180) deg = deg + 360; +// if (deg > 180) deg = deg - 360; +// return Rotation2d.fromDegrees(deg); } public double shoulderPosRel(){ return shoulderRelEncoder.getPosition(); } public double wristPosRel(){ - return wristRelEncoder.getPosition(); + double val = wristRelEncoder.getPosition()*(-126.3+2)/(-0.548-15.8)-126.3+2; + return val; } public void shoulderPowerController(double shoulderPow){ From 85f59ece4545c0029bfae597e9f73bb5fadc77c0 Mon Sep 17 00:00:00 2001 From: tanmay Date: Thu, 21 Mar 2024 20:32:25 -0400 Subject: [PATCH 39/79] 4 note auto code update --- src/main/java/frc/robot/FortissiMOEContainer.java | 4 ++-- .../frc/robot/commands/autos/tripleNoteAutos.java | 15 ++++++++------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index e84f8b9..ed92564 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -131,7 +131,7 @@ public class FortissiMOEContainer{ ///////////////////////////////////////////////////////////////////////////////////////head subsystem private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(5, - 13,1.0e-4, 0,0,driveFF); + 13,4.0e-4, 0,0,driveFF+2.4e-5); private final CollectorSubsystem collectorSubsystem = new CollectorSubsystem(6, 0.01,0,0,0,7); ///////////////////////////////////////////////////////////////////////////////////////head subsystem @@ -235,7 +235,7 @@ public class FortissiMOEContainer{ public FortissiMOEContainer() { - shooterSubsystem.setShooterRPMTolerance(500); + shooterSubsystem.setShooterRPMTolerance(150); shooterSubsystem.setMaxShooterSpeeds(3500,3500); swerveSubsystem.setDefaultCommand(drive); diff --git a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java index 80cd59a..659672c 100644 --- a/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java +++ b/src/main/java/frc/robot/commands/autos/tripleNoteAutos.java @@ -147,11 +147,11 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(14), + 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 = (swerveDrive.getAngleBetweenSpeaker(endTranslation3).plus(Rotation2d.fromDegrees(-5))); + Rotation2d endRotation3 = Rotation2d.fromDegrees(0); Pose2d endPose3 = new Pose2d(endTranslation3,endRotation3); //startC to W3 Translation2d endTranslation4 = endTranslation2; @@ -175,11 +175,12 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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), 0))); - + 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); @@ -218,17 +219,17 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec 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(.15)).andThen(()->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()))), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.race(trajCommand6.andThen(headingCorrect6.withTimeout(.15)).andThen(()->swerveDrive.stopModules()), + Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), Commands.runOnce(()->swerveDrive.stopModules()), - Commands.parallel(shootAnotherLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) + Commands.race(shootAnotherLastNote.andThen(()->shooter.stopShooter()), Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))) ); } From cc08756d5bd51ed2a39f14d1bb94fff92d3eb5eb Mon Sep 17 00:00:00 2001 From: Luca Huang Date: Sun, 24 Mar 2024 17:51:53 -0400 Subject: [PATCH 40/79] Set up SysId for wrist. Need to test on robot. Slowed down values so hopefully enough data. --- .../java/frc/robot/FortissiMOEContainer.java | 6 +++ src/main/java/frc/robot/subsystems/Arm.java | 38 ++++++++++++++++++- 2 files changed, 43 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java index ed92564..114bcc9 100644 --- a/src/main/java/frc/robot/FortissiMOEContainer.java +++ b/src/main/java/frc/robot/FortissiMOEContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +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; @@ -282,6 +283,11 @@ public FortissiMOEContainer() { private void configureBindings() { + new JoystickButton(driverJoystick,4).whileTrue(armSubsystem.wristQuasiStatic(SysIdRoutine.Direction.kForward)); + new JoystickButton(driverJoystick,2).whileTrue(armSubsystem.wristQuasiStatic(SysIdRoutine.Direction.kReverse)); + new JoystickButton(driverJoystick,7).whileTrue(armSubsystem.wristDynamic(SysIdRoutine.Direction.kForward)); + new JoystickButton(driverJoystick,8).whileTrue(armSubsystem.wristDynamic(SysIdRoutine.Direction.kReverse)); + new JoystickButton(driverJoystick, 1).onTrue(Commands.runOnce(() -> {pigeon.setYaw(0); swerveSubsystem.setDesiredYaw(0);})); new JoystickButton(functionJoystick, 8).whileTrue(Commands.run(()->armSubsystem.shoulderPowerController(.2))); new JoystickButton(buttonBox, 1).whileTrue(Commands.run(()->armSubsystem.wristPowerController(.1))); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 12f613a..7b82031 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -12,11 +12,16 @@ 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.units.Unit.*; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; 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; @@ -24,6 +29,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; @@ -44,6 +51,13 @@ public class Arm extends SubsystemBase { private double maxSpeed, maxAccel, shoulderLength, wristLength; private double wristOffset = 0; private double shoulderOffset = 90; + private final Measure>rampRate= Volts.of(0.1).per(Seconds.of(1)); + private final MeasurestepVolatage = Volts.of(2); + private final Measure