Skip to content

Commit 7b77108

Browse files
committed
reverse in danger zone
1 parent 8c83d26 commit 7b77108

File tree

4 files changed

+33
-22
lines changed

4 files changed

+33
-22
lines changed

src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ public boolean isFinished() {
5858
return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
5959
|| (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
6060
&& !robotStateSubsystem.getIsAutoPlacing())
61-
|| !robotStateSubsystem.getIsBargeSafe();
61+
/*|| !robotStateSubsystem.getIsBargeSafe() */ ;
6262
}
6363
}
6464
}

src/main/java/frc/robot/constants/BargeAlignConstants.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,16 @@
44

55
public class BargeAlignConstants {
66
public static final double kXSpeed = 0.45;
7+
public static final double kXRevSpeed = 2;
78

89
public static final double kBlueEjectAlgaeX = 7.67; // 7.72
910
public static final double kRedEjectAlgaeX = 9.86; // 9.81
1011
public static final double kBlueRaiseElevatorX = 6.82; // 7.02
1112
public static final double kRedRaiseElevatorX = 10.71; // 10.51
1213

14+
public static final double kBlueRevDoneX = kBlueRaiseElevatorX + 0.25;
15+
public static final double kRedRevDoneX = kRedRaiseElevatorX - 0.25;
16+
1317
public static final double kBlueUnsafeX = 7.02;
1418
public static final double kRedUnsafeX = 10.51;
1519

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -741,18 +741,18 @@ public void toScoreAlgae() {
741741
isBargeSafe =
742742
poseX > RobotStateConstants.kRedBargeSafeX
743743
|| poseX < RobotStateConstants.kBlueBargeSafeX;
744-
if (isBargeSafe) {
745-
driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
746-
if (isAutoPlacing) {
747-
setAutoPlacingLed(true);
748-
driveSubsystem.setIgnoreSticks(true);
749-
bargeAlignSubsystem.startBargeAlign(allianceColor);
750-
setState(RobotStates.BARGE_ALIGN);
751-
} else {
752-
elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint);
753-
setState(RobotStates.TO_BARGE_ALGAE);
754-
}
744+
// if (isBargeSafe) {
745+
driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
746+
if (isAutoPlacing) {
747+
setAutoPlacingLed(true);
748+
driveSubsystem.setIgnoreSticks(true);
749+
bargeAlignSubsystem.startBargeAlign(allianceColor);
750+
setState(RobotStates.BARGE_ALIGN);
751+
} else {
752+
elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint);
753+
setState(RobotStates.TO_BARGE_ALGAE);
755754
}
755+
// }
756756
}
757757
}
758758
}

src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java

Lines changed: 17 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -39,12 +39,12 @@ public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem)
3939

4040
public void startBargeAlign(Alliance alliance) {
4141
this.alliance = alliance;
42-
if (isSafe()) {
43-
this.isOnBlueSide = isOnBlueSide();
44-
setState(BargeAlignStates.DRIVE);
45-
setupBargeAlign();
46-
driveOmega.reset();
47-
}
42+
// if (isSafe()) {
43+
this.isOnBlueSide = isOnBlueSide();
44+
setState(BargeAlignStates.DRIVE);
45+
setupBargeAlign();
46+
driveOmega.reset();
47+
// }
4848
}
4949

5050
public void killBargeAlign() {
@@ -96,6 +96,13 @@ private boolean shouldDriveBackwards() {
9696
: poseX < BargeAlignConstants.kRedUnsafeX;
9797
}
9898

99+
private boolean shouldStopDrivingBackwards() {
100+
double poseX = driveSubsystem.getPoseMeters().getX();
101+
return isOnBlueSide
102+
? poseX < BargeAlignConstants.kBlueRevDoneX
103+
: poseX > BargeAlignConstants.kRedRevDoneX;
104+
}
105+
99106
private boolean shouldEjectAlgae() {
100107
double poseX = driveSubsystem.getPoseMeters().getX();
101108
return isOnBlueSide
@@ -160,14 +167,14 @@ public void periodic() {
160167
double vOmega =
161168
driveOmega.calculate(
162169
driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians());
170+
double revX = BargeAlignConstants.kXRevSpeed * (isOnBlueSide ? -1 : 1);
171+
driveSubsystem.move(revX, getYStickReading(), vOmega, true);
163172

164-
driveSubsystem.move(-vX, getYStickReading(), vOmega, true);
165-
166-
Logger.recordOutput("BargeAlign/Vx", vX);
173+
Logger.recordOutput("BargeAlign/Vx", revX);
167174
Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError());
168175
Logger.recordOutput("BargeAlign/Vomega", vOmega);
169176

170-
if (shouldRaiseElevator()) {
177+
if (shouldStopDrivingBackwards()) {
171178
setState(BargeAlignStates.RAISE_ELEV);
172179
}
173180
}

0 commit comments

Comments
 (0)