From: moo Date: Wed, 22 Apr 2026 00:35:56 +0000 (-0700) Subject: other auto work and shuttling works X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=406018dc55530535fad529c12f21904666eff7ab;p=FRC2026.git other auto work and shuttling works --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e1c335b..895a1e9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.commands.DoNothing; import frc.robot.commands.LogCommand; +import frc.robot.commands.Music; import frc.robot.commands.auto_comm.DynamicAutoBuilder; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.gpm.IntakeMovementCommand; @@ -293,8 +294,11 @@ public class RobotContainer { addAuto(leftDoNothing); addAuto(rightDoNothing); addAuto(centerDoNothing); + addAuto("LeftShallowDoubleSwipe"); + addAuto("RightShallowDoubleSwipe"); - DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake); + + DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, drive); // names String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe"; diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java index 09461d5..1fcdf01 100644 --- a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java +++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java @@ -1,5 +1,6 @@ package frc.robot.commands.auto_comm; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.*; import frc.robot.commands.gpm.IntakeCommand; @@ -8,6 +9,7 @@ import frc.robot.commands.gpm.RunSpindexerWithStop; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.PowerControl.BreakerConstants; import frc.robot.subsystems.PowerControl.EMABreaker; +import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.turret.Turret; @@ -20,17 +22,20 @@ public class DynamicAutoBuilder { private final Turret turret; private final Hood hood; private final Intake intake; + private final Drivetrain drive; - public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { + public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake, Drivetrain drive) { this.spindexer = spindexer; this.turret = turret; this.hood = hood; this.intake = intake; + this.drive = drive; } /* * Autos have no named commands within them. They must be added here - * Still need to make one method to call that four command block in each sequential + * Still need to make one method to call that four command block in each + * sequential */ public Command getDynamicDoubleLiberalSwipe(boolean left) { @@ -41,22 +46,24 @@ public class DynamicAutoBuilder { runSpindexerWithAbort(), departCommand(), + // new InstantCommand(() -> drive.setPose(Translation2d.kZero)), new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), runSpindexerWithAbort(), departCommand(), + // new InstantCommand(() -> drive.setPose(Translation2d.kZero)), new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), runSpindexerWithAbort(), - + departCommand(), + // new InstantCommand(() -> drive.setPose(Translation2d.kZero)), new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), - runSpindexerWithAbort() - ); + runSpindexerWithAbort()); } - + public Command getDynamicDoubleConservativeSwipe(boolean left) { return new SequentialCommandGroup( departCommand(), @@ -73,12 +80,11 @@ public class DynamicAutoBuilder { new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), runSpindexerWithAbort(), - + departCommand(), new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), - runSpindexerWithAbort() - ); + runSpindexerWithAbort()); } public Command getDynamicDoubleShallowSwipe(boolean left) { @@ -97,12 +103,11 @@ public class DynamicAutoBuilder { new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), runSpindexerWithAbort(), - + departCommand(), new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), startShootingCommand(), - runSpindexerWithAbort() - ); + runSpindexerWithAbort()); } private Command departCommand() { @@ -124,9 +129,12 @@ public class DynamicAutoBuilder { // timer.start(); // return new RunSpindexer(spindexer, turret, hood, intake).raceWith( - // new WaitUntilCommand(() -> spindexer.spinningAir() && timer.hasElapsed(3.0))); - // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new WaitCommand(3.0)); - // return new RunSpindexer(spindexer, turret, hood, intake).until(() -> spindexer.spinningAir() && timer.hasElapsed(3.0)); + // new WaitUntilCommand(() -> spindexer.spinningAir() && + // timer.hasElapsed(3.0))); + // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new + // WaitCommand(3.0)); + // return new RunSpindexer(spindexer, turret, hood, intake).until(() -> + // spindexer.spinningAir() && timer.hasElapsed(3.0)); // has an isFinnished so it should work return new RunSpindexerWithStop(spindexer, turret, hood, intake); diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java index 4ba1a7b..7cf9b4a 100644 --- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java +++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java @@ -21,7 +21,7 @@ public class ShuttleInterpolation { shooterVelocityMap.put(0.0, 9.0); shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned by wesley shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned by wesley - shooterVelocityMap.put(16.5, 70.0); // tuned by taren + shooterVelocityMap.put(16.0, 100.0); // tuned by taren // always shoot at low angle to ground. newHoodMap.put(0.0, 55.0); // min angle (w/ 0.5 deg buffer) diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 3569dba..fb8dfd3 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -125,7 +125,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Toggle spindexer controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue( - new RunSpindexerWithStop(spindexer, turret, hood, intake) + new RunSpindexer(spindexer, turret, hood, intake) ); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 0db0422..acd45cb 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -10,6 +10,7 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.hardware.TalonFX; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.VecBuilder; @@ -17,6 +18,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 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.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -28,6 +30,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.Music; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.constants.GyroBiasConstants; @@ -189,6 +192,25 @@ public class Drivetrain extends SubsystemBase { if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putData("Field", field); } + + addMusic(); + + } + + public void setPose(Translation2d pose) { + poseEstimator.resetTranslation(pose); + } + + public void addMusic() { + ArrayList motors = new ArrayList<>(); + for (Module m: modules) { + motors.add(m.getMotors()[0]); + motors.add(m.getMotors()[1]); + } + + TalonFX[] f = new TalonFX[8]; + + SmartDashboard.putData("Chirp", new Music(motors.toArray(f))); } public void close() {