Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add codes

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-19 21:28:28 -07:00
parent 2641abe8a4
commit c5c6dd2eed
5 changed files with 108 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -3
View File
@@ -242,7 +242,7 @@ public final class Constants {
public static final int SECONDS_TO_MICROS = 1000000;
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 1; // Degrees
public static final double ROT_TOLERANCE = 5; // Degrees
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
// public static final Pose2d targetpos =
@@ -365,9 +365,11 @@ public final class Constants {
;;;;;;;;;;;;;;;;;;;;;;;;;;;;
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17);
public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
@@ -413,6 +415,7 @@ public final class Constants {
public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
public static final double SCORING_THREE_ELEVATOR = -9.25;
public static final double DEALGAE_L2_ELEVATOR = -23.5;
public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
@@ -421,7 +424,7 @@ public final class Constants {
public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_THREE_ENDEFECTOR = 0.375 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
src/main/java/frc4388/robot/RobotContainer.java
+76 -10
View File
@@ -16,6 +16,7 @@ import java.util.List;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -115,19 +116,74 @@ public class RobotContainer {
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlign = new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarAlignLeft = new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarAlignL3 = new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarLeft = new SequentialCommandGroup(
AprilLidarAlign.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_lidar)
@@ -265,12 +321,22 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(AprilLidarAlignLeft);
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(AprilLidarAlign);
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(AprilLidarAlign);
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(AprilLidarAlign);
// ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -281,17 +347,17 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(AprilLidarAlignL3Left);
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(AprilLidarAlignL3);
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
src/main/java/frc4388/robot/commands/GotoLastApril.java
+2
View File
@@ -78,6 +78,8 @@ public class GotoLastApril extends Command {
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
// System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput);
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1)
src/main/java/frc4388/robot/commands/LidarAlign.java
+2 -2
View File
@@ -39,7 +39,7 @@ public class LidarAlign extends Command {
this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.headedRight = (GotoLastApril.tagRelativeXError < 0);
}
@@ -52,7 +52,7 @@ public class LidarAlign extends Command {
return;
}
if (currentFinderTick > 100) { //arbutrary threshhold for now.
if (currentFinderTick > 40) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+22 -12
View File
@@ -106,12 +106,6 @@ public class Elevator extends SubsystemBase {
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_THREE_ENDEFECTOR);
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
@@ -123,6 +117,22 @@ public class Elevator extends SubsystemBase {
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR);
break;
}
case PrimedThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break;
}
default: {
assert false;
}
}
}
@@ -148,7 +158,7 @@ public class Elevator extends SubsystemBase {
private void periodicWaiting() {
if (!basinBeamBreak.get())
transitionState(CoordinationState.WatingBeamTriped);
transitionState(CoordinationState.Ready);
}
private void periodicWaitingTripped() {
@@ -157,8 +167,8 @@ public class Elevator extends SubsystemBase {
}
private void periodicReady() {
if (elevatorAtRefrence())
transitionState(CoordinationState.Hovering);
// if (elevatorAtRefrence())
// transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
@@ -173,11 +183,11 @@ public class Elevator extends SubsystemBase {
SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
// periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
periodicWaitingTripped();
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
periodicReady();
// periodicReady();
}
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
// periodicScoring();