Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 08:48:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

florida comp

Browse Source
This commit is contained in:
Abhishrek05
2024-03-06 16:29:42 -07:00
parent c5f5e6c57f
commit a7e9ef9951
10 changed files with 225 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -5
View File
@@ -25,21 +25,23 @@ import frc4388.utility.Gains;
public final class Constants {
public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = 1.5;
public static final double MAX_ROT_SPEED = 3.5;
public static final double AUTO_MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
public static final double SLOW_SPEED = 0.4;
public static final double SLOW_SPEED = 0.2;
public static final double FAST_SPEED = 1.0;
public static final double TURBO_SPEED = 4.0;
public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 130.957 + 90;
public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 90;//-202.588;
public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 45 + 90 ;//-202.588;
public static final double BACK_LEFT_ROT_OFFSET = 347.871 + 90;
public static final double BACK_RIGHT_ROT_OFFSET = 52.646 + 90;
}
@@ -65,7 +67,7 @@ public final class Constants {
public static final class PIDConstants {
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0, 0.0, 0, 1.0);
public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.0, 0, 1.0);
}
public static final class AutoConstants {
@@ -168,7 +170,7 @@ public final class Constants {
public static final class ShooterConstants {
public static final int LEFT_SHOOTER_ID = 15; // final
public static final int RIGHT_SHOOTER_ID = 16; // final
public static final double SHOOTER_SPEED = 0.40; // final
public static final double SHOOTER_SPEED = 0.50; // final
public static final double SHOOTER_IDLE = 0.35; // final
public static final double SHOOTER_IDLE_LIMELIGHT = 0.8;
}
src/main/java/frc4388/robot/RobotContainer.java
+70 -23
View File
@@ -32,6 +32,7 @@ import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.DeferredBlock;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
@@ -76,6 +77,7 @@ public class RobotContainer {
// private final VirtualController m_virtualOperator = new VirtualController(1);
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.talonPIDIn())
@@ -94,6 +96,8 @@ public class RobotContainer {
// );
// ! Teleop Commands
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
@@ -124,22 +128,33 @@ public class RobotContainer {
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
interrupt,
new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup ampShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.ampPosition()),
new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed
);
// ! /* Autos */
private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand();
private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
//Help Simplify Shooting
private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
// private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotIntake.talonPIDIn()),
// new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
// new WaitCommand(1.4).asProxy(),
// new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
// new WaitCommand(0.5),
// new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
// );
private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
@@ -170,11 +185,6 @@ public class RobotContainer {
taxi.asProxy()
);
private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
interrupt,
new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
@@ -184,7 +194,13 @@ public class RobotContainer {
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
pullInArmtoShoot,
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
// new WaitCommand(1).asProxy(),
// new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
@@ -197,6 +213,23 @@ public class RobotContainer {
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
@@ -205,19 +238,32 @@ public class RobotContainer {
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
pullInArmtoShoot,
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
//? Create Another Parallel Command Group :(
pullInArmtoShoot,
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Taxi Auto", taxi.asProxy())
.addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker)
.addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary)
// .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
// .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
.addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker)
.addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy())
.addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy())
// .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
// .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
.addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
.addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
.buildDisplay();
@@ -228,6 +274,7 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
// configureVirtualButtonBindings();
new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
DriverStation.silenceJoystickConnectionWarning(true);
src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java
+1 -1
View File
@@ -26,6 +26,6 @@ public class ArmIntakeInAuto extends ParallelCommandGroup {
this.intake = intake;
this.shooter = shooter;
this.swerve = swerve;
addCommands(new ArmIntakeIn(intake, shooter), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
}
}
src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java
+58
View File
@@ -0,0 +1,58 @@
// 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 frc4388.robot.commands.Autos;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.SwerveDrive;
public class ArmIntakeInTimeout extends Command {
/** Creates a new ArmIntakeIn. */
private Intake robotIntake;
private Shooter robotShooter;
public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake;
this.robotShooter = robotShooter;
addRequirements(this.robotIntake, this.robotShooter);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
robotIntake.talonPIDOut();
robotIntake.talonSpinIntakeMotor();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
if(interrupted) {
robotIntake.talonPIDIn();
robotIntake.talonStopIntakeMotors();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return robotIntake.getTalonIntakeLimitSwitchState();
// if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0)))
// {
// return !true==true;
// }
// else
// {
// return !false==!(!(true));
// }
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+5 -1
View File
@@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase {
// new Translation2d(out.rightX, out.rightY),
// true);
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// new Translation2d(lerpRX, lerpRY),
// true);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+1 -1
View File
@@ -64,7 +64,7 @@ public class JoystickRecorder extends CommandBase {
outputs.add(inputs);
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
src/main/java/frc4388/robot/subsystems/Intake.java
+13
View File
@@ -145,6 +145,10 @@ public class Intake extends SubsystemBase {
talonIntake.set(IntakeConstants.INTAKE_SPEED);
}
public void talonSpinIntakeMotor(double speed) {
talonIntake.set(speed);
}
public boolean getTalonIntakeLimitSwitchState() {
if(r = talonIntake.getForwardLimit().getValue().value == 0) {
return true;
@@ -174,6 +178,15 @@ public class Intake extends SubsystemBase {
}
}
public void ampPosition() {
PositionVoltage request = new PositionVoltage(-0);
talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
}
public void ampShoot(double speed) {
talonSpinIntakeMotor(speed);
}
// ! NEO Methods
//hanoff
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+45 -1
View File
@@ -33,7 +33,8 @@ public class SwerveDrive extends SubsystemBase {
private RobotGyro gyro;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
@@ -87,6 +88,41 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
// SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
Translation2d rightStick = new Translation2d(rightX, rightY);
@@ -157,6 +193,11 @@ public class SwerveDrive extends SubsystemBase {
return this.kinematics;
}
public boolean getSpeedState() {
return false;
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
@@ -170,6 +211,7 @@ public class SwerveDrive extends SubsystemBase {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
}
}
@@ -220,4 +262,6 @@ public class SwerveDrive extends SubsystemBase {
}
}
}
src/main/java/frc4388/utility/RobotGyro.java
+24
View File
@@ -103,6 +103,7 @@ public class RobotGyro implements Gyro {
} else {
m_navX.reset();
}
}
public void resetFlip() {
@@ -113,6 +114,29 @@ public class RobotGyro implements Gyro {
} else {
m_navX.reset();
}
}
public void resetNinety() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(90);
} else {
m_navX.reset();
}
}
public void resetTwoSeventy() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(270);
} else {
m_navX.reset();
}
}
/**