Skip to content

Commit 136f0b9

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #239 from strykeforce/hotfix/v19.4.0
Hotfix/v19.4.0
2 parents 0226e8e + 876e327 commit 136f0b9

File tree

13 files changed

+99
-108
lines changed

13 files changed

+99
-108
lines changed

‎docs/Subsystem-Interface-Conversions.xlsx‎

24 Bytes
Binary file not shown.

‎src/main/java/frc/team2767/deepspace/command/approach/VisionAutoAlignPickupCommand.java‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -22,12 +22,13 @@ public class VisionAutoAlignPickupCommand extends Command {
2222
private static final double DRIVE_EXPO = 0.5;
2323
private static final double DEADBAND = 0.05;
2424
private static final double MIN_RANGE = 35.0;
25-
private static final double FWD_SCALE = 0.6;
26-
private static final double FWD_SCALE_FAST = 0.6;
27-
private static final double AUTON_OUTPUT = 0.35;
25+
private static final double FWD_SCALE = 0.5;
26+
private static final double FWD_SCALE_FAST = 0.5;
27+
private static final double AUTON_OUTPUT = 0.30;
2828

2929
private static final DriveSubsystem DRIVE = Robot.DRIVE;
3030
private static final VisionSubsystem VISION = Robot.VISION;
31+
3132
private final ExpoScale driveExpo;
3233
private final Logger logger = LoggerFactory.getLogger(this.getClass());
3334
private double range;
@@ -46,6 +47,7 @@ public VisionAutoAlignPickupCommand() {
4647

4748
@Override
4849
protected void initialize() {
50+
VISION.queryPyeye();
4951
isAuton = DriverStation.getInstance().isAutonomous();
5052
SmartDashboard.putBoolean("Game/haveHatch", false);
5153
logger.info("Begin Vision Auto Align Pickup");
@@ -110,7 +112,7 @@ protected boolean isFinished() {
110112
@Override
111113
protected void end() {
112114

113-
logger.debug("range = {}", range);
115+
logger.info("range = {}", range);
114116
logger.info("End Auto Align Pickup Vision");
115117
}
116118
}

‎src/main/java/frc/team2767/deepspace/command/approach/VisionAutoAlignPlaceCommand.java‎

Lines changed: 4 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
package frc.team2767.deepspace.command.approach;
22

3-
import edu.wpi.first.wpilibj.DriverStation;
43
import edu.wpi.first.wpilibj.command.Command;
54
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
65
import frc.team2767.deepspace.Robot;
@@ -29,14 +28,10 @@ public class VisionAutoAlignPlaceCommand extends Command {
2928
private static Controls CONTROLS;
3029
private final Logger logger = LoggerFactory.getLogger(this.getClass());
3130
private AutoPlaceSideChooser autoPlaceSideChooser = new AutoPlaceSideChooser();
32-
private double gyroOffset;
3331
private double range;
3432
private double forward;
35-
private double strafeError;
36-
private double yawError;
3733
private double strafeCorrection;
3834
private double targetYaw;
39-
private boolean isSandstorm;
4035
private boolean isGood = false;
4136
private RateLimit strafeRateLimit;
4237
private final ExpoScale driveExpo;
@@ -49,15 +44,14 @@ public VisionAutoAlignPlaceCommand() {
4944

5045
@Override
5146
protected void initialize() {
52-
gyroOffset =
47+
double gyroOffset =
5348
autoPlaceSideChooser.determineGyroOffset(
5449
VISION.direction, Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0));
5550
targetYaw = autoPlaceSideChooser.determineTargetYaw(VISION.direction);
5651

5752
logger.debug("offset = {} target = {}", gyroOffset, targetYaw);
5853

5954
CONTROLS = Robot.CONTROLS;
60-
isSandstorm = DriverStation.getInstance().isAutonomous();
6155
DRIVE.setGyroOffset(gyroOffset);
6256
SmartDashboard.putBoolean("Game/haveHatch", true);
6357
logger.info("Begin Vision Auto Align Place");
@@ -78,7 +72,7 @@ protected void execute() {
7872
isGood = range >= 0; // check if range is good (we have a target), not -1
7973

8074
// Calculate Yaw Term based on gyro
81-
yawError = targetYaw - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0);
75+
double yawError = targetYaw - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0);
8276
DRIVE.setYawError(yawError);
8377
double yaw = kP_YAW * yawError;
8478
if (yaw > MAX_YAW) yaw = MAX_YAW;
@@ -88,7 +82,8 @@ protected void execute() {
8882
boolean onTarget = Math.abs(yawError) <= goodEnoughYaw;
8983

9084
double strafe;
91-
strafeError = Math.sin(Math.toRadians(VISION.getCorrectedBearing())) * range - strafeCorrection;
85+
double strafeError =
86+
Math.sin(Math.toRadians(VISION.getCorrectedBearing())) * range - strafeCorrection;
9287
VISION.setStrafeError(strafeError);
9388
forward = driveExpo.apply(CONTROLS.getDriverControls().getForward()) * FORWARD_SCALE;
9489

‎src/main/java/frc/team2767/deepspace/command/sequences/pickup/CoconutPickupAutoRetryCommand.java‎

Lines changed: 30 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,18 @@ public class CoconutPickupAutoRetryCommand extends Command {
1515
private static final double PRESSURE_DIFFERENTIAL = 2.5; // inHg
1616
private static final double STABLE_COUNTS = 5;
1717
private static final double WAIT_TIME = 1000; // ms
18+
private static final double DOWN_TIMEOUT = 1500;
1819
private static final double RESET_HEIGHT = 20.25;
19-
private static final double STRING_COMPRESSED_LENGTH = 223.0; // full length is 248
20-
private static PickupState state;
21-
private static double initialPressure;
22-
private static double startSealTime;
23-
private static boolean hasRetried;
24-
private static double currentTime;
25-
private static double currentPressure;
26-
private static double stableCounts;
20+
private static final double STRING_COMPRESSED_LENGTH = 223.0; // full length is 252
2721
private static double DOWN_SPEED = -0.35;
22+
private PickupState state;
23+
private double initialPressure;
24+
private double startSealTime;
25+
private boolean hasRetried;
26+
private double downInitTime;
27+
private double currentTime;
28+
private double currentPressure;
29+
private double stableCounts;
2830
private final Logger logger = LoggerFactory.getLogger(this.getClass());
2931

3032
public CoconutPickupAutoRetryCommand() {
@@ -39,6 +41,7 @@ protected void initialize() {
3941
initialPressure = VACUUM.getPressure();
4042
stableCounts = 0;
4143
hasRetried = false;
44+
downInitTime = System.currentTimeMillis();
4245
}
4346

4447
@Override
@@ -49,6 +52,22 @@ protected void execute() {
4952
state = PickupState.WAIT_FOR_PRESSURE;
5053
ELEVATOR.setPosition(ELEVATOR.getPosition()); // inches
5154
startSealTime = System.currentTimeMillis();
55+
break;
56+
}
57+
58+
// TIMEOUT (on down) = RESET & TRY AGAIN
59+
if (System.currentTimeMillis() - downInitTime > DOWN_TIMEOUT) {
60+
if (!hasRetried) {
61+
state = PickupState.RESET;
62+
hasRetried = true;
63+
ELEVATOR.setPosition(RESET_HEIGHT);
64+
logger.info("retrying");
65+
break;
66+
} else {
67+
state = PickupState.DONE;
68+
ELEVATOR.setPosition(RESET_HEIGHT);
69+
break;
70+
}
5271
}
5372

5473
break;
@@ -61,7 +80,7 @@ protected void execute() {
6180
state = PickupState.RESET;
6281
hasRetried = true;
6382
ELEVATOR.setPosition(RESET_HEIGHT);
64-
logger.info("Retrying");
83+
logger.info("retrying");
6584
break;
6685
}
6786

@@ -78,8 +97,10 @@ protected void execute() {
7897

7998
break;
8099
case RESET:
100+
currentPressure = VACUUM.getPressure();
81101
if (ELEVATOR.onTarget() && (currentPressure - initialPressure) < PRESSURE_DIFFERENTIAL) {
82102
startSealTime = System.currentTimeMillis();
103+
downInitTime = System.currentTimeMillis();
83104
ELEVATOR.openLoopMove(DOWN_SPEED);
84105
state = PickupState.DOWN;
85106
logger.info("Moving Down");

‎src/main/java/frc/team2767/deepspace/command/sequences/pickup/SandstormHatchPickupCommandGroup.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public SandstormHatchPickupCommandGroup() {
2929
addParallel(new SetGamePieceCommand(GamePiece.HATCH));
3030
addParallel(new SetFieldDirectionCommand(FieldDirection.LEFT));
3131
addParallel(new SetLevelCommand(ElevatorLevel.ONE));
32-
addParallel(new IntakePositionCommand(IntakeSubsystem.kStowPositionDeg));
32+
addParallel(new IntakePositionCommand(IntakeSubsystem.kStowPositionDeg), 0.05);
3333
}
3434
});
3535
addSequential(new ElevatorSetPositionCommand(7.5));

‎src/main/java/frc/team2767/deepspace/control/SmartDashboardControls.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ public SmartDashboardControls() {
5252
addPitCommands();
5353
addAzimuthCommands();
5454
addVisionCommands();
55+
addVacuumCommands();
5556
addPathTest();
5657
}
5758
}
@@ -65,6 +66,7 @@ private void addMatchCommands() {
6566
SmartDashboard.putData("Game/Gyro", Robot.DRIVE.getGyro());
6667
SmartDashboard.putData("Game/Lvl2Deploy", new ClimbLevel2DeployCommand());
6768
SmartDashboard.putData("Game/Lvl2Climb", new ClimbLevel2AutoCommand());
69+
SmartDashboard.putData("Stop pump", new StopPumpCommandGroup());
6870
}
6971

7072
private void addClimbTab() {

‎src/main/java/frc/team2767/deepspace/subsystem/ClimbSubsystem.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ private void climbPrefs() {
7575
kHighRelease = (int) getPrefs("high_position", 189);
7676
kHighLvl2 = (int) getPrefs("Lvl2_high_position", 600);
7777
kClimb = (int) getPrefs("climb_position", 884);
78-
kTooLowIn = (int) getPrefs("too_low_position", 240);
78+
kTooLowIn = (int) getPrefs("too_low_position", 300);
7979
kTooLowLvl2 = (int) getPrefs("too_low_Lvl2", 656);
8080
kSealOutputVelocity = (int) getPrefs("seal_velocity", 300);
8181

‎src/main/java/frc/team2767/deepspace/subsystem/ElevatorSubsystem.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -257,6 +257,10 @@ public int getTicks() {
257257
@SuppressWarnings("Duplicates")
258258
@Override
259259
public void setLimits(int forward, int reverse) {
260+
if (elevator.hasResetOccurred()) {
261+
logger.warn("ELEVATOR TALON RESET");
262+
}
263+
260264
if (forward != currentForwardLimit) {
261265
elevator.configForwardSoftLimitThreshold(forward, 0);
262266
currentForwardLimit = forward;

‎src/main/java/frc/team2767/deepspace/subsystem/VisionSubsystem.java‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ public class VisionSubsystem extends Subsystem implements Item {
3030
private static final double CAMERA_Y_LEFT = -13.5;
3131
private static final double CAMERA_Y_RIGHT = 13.5;
3232
private static final double GLUE_CORRECTION_FACTOR_RIGHT = -2.21; // -2.26 comp
33-
private static final double GLUE_CORRECTION_FACTOR_LEFT = 0.21; // 0.6147 comp
33+
private static final double GLUE_CORRECTION_FACTOR_LEFT = 0.6417; // 0.6147 comp
3434
private static final double CAMERA_DEGREES_PER_PIXEL_ADJUSTMENT_RIGHT =
3535
1.0; // 1.0 is zero value 0.85
3636
private static final double CAMERA_DEGREES_PER_PIXEL_ADJUSTMENT_LEFT =
@@ -43,8 +43,8 @@ public class VisionSubsystem extends Subsystem implements Item {
4343
private static final double CAMERA_RANGE_OFFSET_LEFT = -4.92; // -5.6325
4444
// NEGATIVE = TOWARDS FIELD LEFT (this one was negative)
4545
private static final double STRAFE_CORRECTION_RIGHT =
46-
0.0; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE?
47-
private static final double STRAFE_CORRECTION_LEFT = 1.0; // was positive
46+
-0.5; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE?
47+
private static final double STRAFE_CORRECTION_LEFT = 0.5; // was positive
4848

4949
private final Logger logger = LoggerFactory.getLogger(this.getClass());
5050
private final DigitalOutput lightsOutput6 = new DigitalOutput(6);
@@ -55,23 +55,23 @@ public class VisionSubsystem extends Subsystem implements Item {
5555
public FieldDirection direction = FieldDirection.NOTSET;
5656
public ElevatorLevel elevatorLevel = ElevatorLevel.NOTSET;
5757
public StartSide startSide = StartSide.NOTSET;
58-
private NetworkTableEntry bearingEntry;
59-
private NetworkTableEntry cameraMode;
60-
private NetworkTableEntry rangeEntry;
61-
private NetworkTableEntry cameraIDEntry;
62-
private NetworkTableEntry targetYawEntry;
63-
private NetworkTableEntry tuningEntry;
64-
private NetworkTableEntry tuningFinished;
65-
private double rawRange;
66-
private double rawBearing;
67-
private double correctedRange;
68-
private double correctedHeading;
69-
private double targetYaw;
70-
private double blinkPeriod;
71-
private boolean blinkEnabled;
72-
private LightPattern currentPattern;
73-
private double lightState;
74-
private double strafeError = 0;
58+
private static NetworkTableEntry bearingEntry;
59+
private static NetworkTableEntry cameraMode;
60+
private static NetworkTableEntry rangeEntry;
61+
private static NetworkTableEntry cameraIDEntry;
62+
private static NetworkTableEntry targetYawEntry;
63+
private static NetworkTableEntry tuningEntry;
64+
private static NetworkTableEntry tuningFinished;
65+
private static double rawRange;
66+
private static double rawBearing;
67+
private static double correctedRange;
68+
private static double correctedHeading;
69+
private static double targetYaw;
70+
private static double blinkPeriod;
71+
private static boolean blinkEnabled;
72+
private static LightPattern currentPattern;
73+
private static double lightState;
74+
private static double strafeError = 0;
7575

7676
public VisionSubsystem() {
7777

‎src/main/java/frc/team2767/deepspace/subsystem/safety/IntakePosition.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
package frc.team2767.deepspace.subsystem.safety;
22

33
enum IntakePosition {
4-
INTAKE_INTAKE(12_081, -700),
4+
INTAKE_INTAKE(12_081, -800),
55
INTAKE_STOW(12_081, -475);
66

77
public final int forwardLimit;

0 commit comments

Comments
 (0)