Skip to content

Commit 56b4bdf

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #87 from strykeforce/can-range
Added CAN range sensor for scoring over a single coral between the robot and the reef (level 2 and 3 only)
2 parents d4596c5 + b396db6 commit 56b4bdf

File tree

10 files changed

+162
-13
lines changed

10 files changed

+162
-13
lines changed

‎README.md‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,8 @@
5252
| Funnel | FXS | rollers | 40 | rio | | | Minion | |
5353
| Climb | FX | frontMain | 45 | rio | | | Minion | |
5454
| Climb | FX | backFollow | 46 | rio | | | kraken | |
55-
| Climb | CANcoder | CANCoder | 47 | ri0 | | | n/a | |
55+
| Climb | CANcoder | CANCoder | 47 | rio | | | n/a | |
56+
| Laser | CANRange | CANRange | 50 | rio | | | n/a | |
5657
| - | - | rio | - | both | | 12 | | |
5758
| - | - | vrm (radio, pigeon) | - | - | | 13 | | |
5859
| - | - | custom circuit (pi power) | - | - | | | | |

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,8 @@
9191
import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates;
9292
import frc.robot.subsystems.funnel.FunnelIOFXS;
9393
import frc.robot.subsystems.funnel.FunnelSubsystem;
94+
import frc.robot.subsystems.laser.LaserIOCANRange;
95+
import frc.robot.subsystems.laser.LaserSubsystem;
9496
import frc.robot.subsystems.led.LEDIO;
9597
import frc.robot.subsystems.led.LEDSubsystem;
9698
import frc.robot.subsystems.pathHandler.PathHandler;
@@ -138,6 +140,9 @@ public class RobotContainer {
138140
private final LEDIO ledIO;
139141
private final LEDSubsystem ledSubsystem;
140142

143+
private final LaserIOCANRange laserIO;
144+
private final LaserSubsystem laserSubsystem;
145+
141146
private final TagAlignSubsystem tagAlignSubsystem;
142147
private final BargeAlignSubsystem bargeAlignSubsystem;
143148

@@ -200,9 +205,12 @@ public RobotContainer() {
200205
ledIO = new LEDIO();
201206
ledSubsystem = new LEDSubsystem(ledIO);
202207

208+
laserIO = new LaserIOCANRange();
209+
laserSubsystem = new LaserSubsystem(laserIO);
210+
203211
visionSubsystem = new VisionSubsystem(driveSubsystem);
204212

205-
tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
213+
tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem, laserSubsystem);
206214
bargeAlignSubsystem = new BargeAlignSubsystem(flysky, driveSubsystem);
207215

208216
robotStateSubsystem =
@@ -300,6 +308,7 @@ private void configureTelemetry() {
300308
biscuitSubsystem.registerWith(telemetryService);
301309
ledSubsystem.registerWith(telemetryService);
302310
climbSubsystem.registerWith(telemetryService);
311+
laserSubsystem.registerWith(telemetryService);
303312
telemetryService.start();
304313
}
305314

‎src/main/java/frc/robot/constants/LaserConstants.java‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
package frc.robot.constants;
2+
3+
import com.ctre.phoenix6.configs.CANrangeConfiguration;
4+
import com.ctre.phoenix6.signals.UpdateModeValue;
5+
6+
public class LaserConstants {
7+
public static final int LaserCanId = 50;
8+
9+
public static CANrangeConfiguration getLaserConfig() {
10+
CANrangeConfiguration config = new CANrangeConfiguration();
11+
config.ToFParams =
12+
config.ToFParams.withUpdateMode(UpdateModeValue.LongRangeUserFreq).withUpdateFrequency(50);
13+
config.ProximityParams = config.ProximityParams.withProximityThreshold(1.0);
14+
config.FovParams = config.FovParams.withFOVRangeX(10.0).withFOVRangeY(10.0);
15+
config.ToFParams = config.ToFParams.withUpdateFrequency(25.0);
16+
return config;
17+
}
18+
}

‎src/main/java/frc/robot/constants/TagServoingConstants.java‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -95,10 +95,9 @@ public class TagServoingConstants {
9595
public static final double kEndVelThreshold = 5;
9696

9797
// Stuck coral
98-
public static final double kMaxStalledDer = 0.5;
99-
public static final double kMinStuckCounts = 5;
100-
public static final double kCoralStuckRadius = 1.4;
101-
public static final double kCoralStuckAllowence = 0.03;
98+
public static final double kMinStuckCounts = 3;
99+
public static final double kCoralStuckDistance = 0.19;
100+
public static final double kUnfixableCoralStuckDistance = 0.34;
102101

103102
// Reef
104103
public static final Translation2d kBlueReefPose =

‎src/main/java/frc/robot/subsystems/laser/LaserIO.java‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
package frc.robot.subsystems.laser;
2+
3+
import org.littletonrobotics.junction.AutoLog;
4+
5+
public interface LaserIO {
6+
7+
@AutoLog
8+
public static class LaserIOInputs {
9+
public double distance = 0.0;
10+
}
11+
12+
public void updateInputs(LaserIOInputs inputs);
13+
14+
public double getDistanceMeters();
15+
}

‎src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package frc.robot.subsystems.laser;
2+
3+
import static edu.wpi.first.units.Units.Meters;
4+
5+
import com.ctre.phoenix6.hardware.CANrange;
6+
import frc.robot.constants.LaserConstants;
7+
8+
public class LaserIOCANRange implements LaserIO {
9+
private CANrange laser;
10+
11+
public LaserIOCANRange() {
12+
laser = new CANrange(LaserConstants.LaserCanId);
13+
laser.getConfigurator().apply(LaserConstants.getLaserConfig());
14+
}
15+
16+
@Override
17+
public void updateInputs(LaserIOInputs inputs) {
18+
inputs.distance = getDistanceMeters();
19+
}
20+
21+
public double getDistanceMeters() {
22+
if (laser.getIsDetected().getValue()) {
23+
return laser.getDistance().getValue().in(Meters);
24+
} else {
25+
return -1.0;
26+
}
27+
}
28+
}

‎src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
package frc.robot.subsystems.laser;
2+
3+
import java.util.Set;
4+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
5+
import org.strykeforce.telemetry.measurable.Measure;
6+
7+
public class LaserSubsystem extends MeasurableSubsystem {
8+
9+
private final LaserIO io;
10+
private final LaserIOInputsAutoLogged inputs = new LaserIOInputsAutoLogged();
11+
12+
public LaserSubsystem(LaserIO laserio) {
13+
this.io = laserio;
14+
}
15+
16+
public double getDistance() {
17+
return io.getDistanceMeters();
18+
}
19+
20+
@Override
21+
public void periodic() {
22+
io.updateInputs(inputs);
23+
org.littletonrobotics.junction.Logger.processInputs("LaserInputs", inputs);
24+
}
25+
26+
@Override
27+
public Set<Measure> getMeasures() {
28+
return Set.of(
29+
new Measure("distance", "Distance measured by the laser in meters", () -> getDistance()));
30+
}
31+
}

‎src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,6 @@ public void reassignAlliance() {
172172
mirrorTrajectory = driveSubsystem.shouldFlip();
173173
Optional<Trajectory<SwerveSample>> temp;
174174
for (int i = 0; i < 12; i++) {
175-
logger.info(i + "");
176175
temp = Choreo.loadTrajectory(pathNames[0][i]);
177176
if (!temp.isEmpty()) {
178177
fetchPaths.add(temp.get());

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

Lines changed: 27 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -673,6 +673,7 @@ public void toPlaceCoral() {
673673
scoringTimer.stop();
674674
scoringTimer.reset();
675675
scoringTimer.start();
676+
reefCoralStuckFixable = false;
676677
setState(RobotStates.PLACE_CORAL);
677678
}
678679

@@ -1043,7 +1044,29 @@ public void periodic() {
10431044
}
10441045
} */
10451046

1046-
if ((isAutoPlacing
1047+
if (tagAlignSubsystem.fixableStuckCoral() == true) {
1048+
switch (scoringLevel) {
1049+
case L1 -> {}
1050+
case L2 -> {
1051+
setBiscuitTransfer(RobotConstants.kL2CoralSetpoint, true);
1052+
elevatorSubsystem.setPosition(
1053+
ElevatorConstants.kL2CoralSetpoint.plus(
1054+
RobotStateConstants.kStuckCoralElevatorOffset));
1055+
setState(RobotStates.PLACE_CORAL, true);
1056+
reefCoralStuckFixable = tagAlignSubsystem.fixableStuckCoral();
1057+
}
1058+
case L3 -> {
1059+
setBiscuitTransfer(RobotConstants.kL3CoralSetpoint, true);
1060+
elevatorSubsystem.setPosition(
1061+
ElevatorConstants.kL3CoralSetpoint.plus(
1062+
RobotStateConstants.kStuckCoralElevatorOffset));
1063+
setState(RobotStates.PLACE_CORAL, true);
1064+
reefCoralStuckFixable = tagAlignSubsystem.fixableStuckCoral();
1065+
}
1066+
case L4 -> {}
1067+
}
1068+
tagAlignSubsystem.terminate();
1069+
} else if ((isAutoPlacing
10471070
&& (tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE
10481071
/* || reefCoralStuckFixable
10491072
&& tagAlignSubsystem.isAligned()
@@ -1053,7 +1076,6 @@ public void periodic() {
10531076
toPlaceCoral();
10541077
// tagAlignSubsystem.terminate();
10551078
isAutoReadyForEject = false;
1056-
reefCoralStuckFixable = false;
10571079
}
10581080
}
10591081
case REMOVE_ALGAE -> {
@@ -1078,7 +1100,9 @@ public void periodic() {
10781100
}
10791101

10801102
case PLACE_CORAL -> {
1081-
if (!hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
1103+
if (reefCoralStuckFixable) {
1104+
toPlaceCoral();
1105+
} else if (!hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
10821106

10831107
coralLoc = CoralLoc.NONE;
10841108
visionSubsystem.setYawUpdateCamera(-1);

‎src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java‎

Lines changed: 28 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import frc.robot.constants.DriveConstants;
1010
import frc.robot.constants.TagServoingConstants;
1111
import frc.robot.subsystems.drive.DriveSubsystem;
12+
import frc.robot.subsystems.laser.LaserSubsystem;
1213
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
1314
import frc.robot.subsystems.vision.VisionSubsystem;
1415
import java.util.Set;
@@ -22,6 +23,7 @@ public class TagAlignSubsystem extends MeasurableSubsystem {
2223
private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class);
2324
private DriveSubsystem driveSubsystem;
2425
private VisionSubsystem visionSubsystem;
26+
private LaserSubsystem laserSubsystem;
2527

2628
private PIDController driveX;
2729
private PIDController driveY;
@@ -51,11 +53,16 @@ public class TagAlignSubsystem extends MeasurableSubsystem {
5153
private double coralOffset = 0;
5254
private double noProgressCounts = 0;
5355
private double yAutoOffset = 0;
56+
private boolean behindCoral = false;
5457
private boolean justAlgae = false;
5558

56-
public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) {
59+
public TagAlignSubsystem(
60+
DriveSubsystem driveSubsystem,
61+
VisionSubsystem visionSubsystem,
62+
LaserSubsystem laserSubsystem) {
5763
this.driveSubsystem = driveSubsystem;
5864
this.visionSubsystem = visionSubsystem;
65+
this.laserSubsystem = laserSubsystem;
5966

6067
this.driveX = new PIDController(4, 0, 0);
6168
this.driveY = new PIDController(4, 0, 0);
@@ -211,6 +218,10 @@ public double getErrYaw() {
211218
return yawError;
212219
}
213220

221+
public boolean getBehindCoral() {
222+
return behindCoral;
223+
}
224+
214225
public boolean isFinalDrive() {
215226
return finalDrive;
216227
}
@@ -220,8 +231,10 @@ public boolean stalled() {
220231
}
221232

222233
public boolean fixableStuckCoral() {
223-
return FastMath.abs(getCurRadius() - TagServoingConstants.kCoralStuckRadius)
224-
< TagServoingConstants.kCoralStuckAllowence;
234+
return laserSubsystem.getDistance() > TagServoingConstants.kCoralStuckDistance
235+
&& laserSubsystem.getDistance() < TagServoingConstants.kUnfixableCoralStuckDistance
236+
&& FastMath.abs(driveOmega.getError()) < TagServoingConstants.kAngleCloseEnough
237+
&& stalled();
225238
}
226239

227240
public boolean isAligned() {
@@ -432,6 +445,18 @@ public void periodic() {
432445
case TAG_ALIGN -> {
433446
if (justAlgae && (FastMath.abs(alignY.getError()) < driveYCloseEnough)) {
434447
finalDrive = true;
448+
if (scoreLeft) {
449+
visionSubsystem.setUsingLeftCam(false);
450+
visionSubsystem.setUsingRightCam(true);
451+
} else {
452+
visionSubsystem.setUsingLeftCam(true);
453+
visionSubsystem.setUsingRightCam(false);
454+
}
455+
} else if (stalled()
456+
&& fixableStuckCoral()
457+
&& FastMath.abs(alignY.getError()) < driveYCloseEnough) {
458+
finalDrive = true;
459+
behindCoral = true;
435460
} else {
436461
if ((
437462
/*isAuto ? true :*/ FastMath.abs(alignX.getError()) < driveXCloseEnough)

0 commit comments

Comments
 (0)