Skip to content

Commit 3e0f4d3

Browse files
Carbonic-AciddCarbonic-Acidd
committed
Added some fuel position tracking and fixed tunerconstants and motor ID
1 parent 6f8057f commit 3e0f4d3

4 files changed

Lines changed: 513 additions & 354 deletions

File tree

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

Lines changed: 68 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -164,10 +164,10 @@ public static class SwerveConstants {
164164
}
165165

166166
public static class IntakeConstants {
167-
public static final int armMainID = 15;
168-
public static final int armFollowerID = 16;
169-
public static final int intakeID = 17;
170-
public static final int armEncoderID = 18;
167+
public static final int armMainID = 14;
168+
public static final int armFollowerID = 15;
169+
public static final int intakeID = 16;
170+
public static final int armEncoderID = 17;
171171

172172
public static final double armGearRatio = 3.0;
173173

@@ -232,40 +232,81 @@ public static class VisionConstants {
232232

233233
public static final Transform3d arducamLeftTransform =
234234
new Transform3d(
235-
Units.inchesToMeters(-10.02),
236-
Units.inchesToMeters(10.02),
237-
Units.inchesToMeters(5),
235+
Units.inchesToMeters(0.036),
236+
Units.inchesToMeters(12.89),
237+
Units.inchesToMeters(11.8),
238238
new Rotation3d(
239-
0, Units.degreesToRadians(-25), Units.degreesToRadians(180 - 45))); // Pitch: 65
239+
0, Units.degreesToRadians(-25), Units.degreesToRadians(90))); // Pitch: 65
240240

241241
public static final String arducamRightName = "Arducam_Right";
242242

243243
public static final Transform3d arducamRightTransform =
244244
new Transform3d(
245-
Units.inchesToMeters(-10.02),
246-
Units.inchesToMeters(-10.02),
247-
Units.inchesToMeters(5),
245+
Units.inchesToMeters(2.8),
246+
Units.inchesToMeters(-12.8),
247+
Units.inchesToMeters(9.4),
248248
new Rotation3d(
249-
0, Units.degreesToRadians(-25), Units.degreesToRadians(180 + 45))); // Pitch: 65
249+
0, Units.degreesToRadians(-25), Units.degreesToRadians(-90))); // Pitch: 65
250250

251-
public static final String arducamFrontName = "Arducam_Front";
251+
public static final String arducamBackLeftName = "Arducam_BackLeft";
252252

253-
public static final Transform3d arducamFrontTransform =
253+
public static final Transform3d arducamBackLeftTransform =
254254
new Transform3d(
255-
Units.inchesToMeters(0),
256-
Units.inchesToMeters(-1),
257-
Units.inchesToMeters(10.07),
258-
new Rotation3d(0, Units.degreesToRadians(-15), Units.degreesToRadians(0))); // Pitch: 65
255+
Units.inchesToMeters(-9.6),
256+
Units.inchesToMeters(10.6),
257+
Units.inchesToMeters(8.319),
258+
new Rotation3d(
259+
0, Units.degreesToRadians(-25), Units.degreesToRadians(180 - 45))); // Pitch: 65
260+
261+
public static final String arducamBackRightName = "Arducam_BackRight";
262+
263+
public static final Transform3d arducamBackRightTransform =
264+
new Transform3d(
265+
Units.inchesToMeters(-12.147),
266+
Units.inchesToMeters(-12.505),
267+
Units.inchesToMeters(15),
268+
new Rotation3d(
269+
0, Units.degreesToRadians(-25), Units.degreesToRadians(180 + 45))); // Pitch: 65
270+
271+
// public static final String arducamFrontName = "Arducam_Front";
272+
273+
// public static final Transform3d arducamFrontTransform =
274+
// new Transform3d(
275+
// Units.inchesToMeters(0),
276+
// Units.inchesToMeters(-1),
277+
// Units.inchesToMeters(10.07),
278+
// new Rotation3d(0, Units.degreesToRadians(-15), Units.degreesToRadians(0))); // Pitch:
279+
// 65
259280

260281
public static final String arducamFuelName = "Arducam_Fuel";
261282

262283
// TODO: Update this transform
263284
public static final Transform3d arducamFuelTransform =
264285
new Transform3d(
286+
Units.inchesToMeters(24.468),
265287
Units.inchesToMeters(0),
266-
Units.inchesToMeters(0),
267-
Units.inchesToMeters(0),
268-
new Rotation3d(0, Units.degreesToRadians(0), Units.degreesToRadians(0)));
288+
Units.inchesToMeters(10.591),
289+
new Rotation3d(0, Units.degreesToRadians(-13), Units.degreesToRadians(0)));
290+
291+
public static InterpolatingDoubleTreeMap fuelAreaToDistanceMap =
292+
new InterpolatingDoubleTreeMap();
293+
294+
static {
295+
fuelAreaToDistanceMap.put(80640.0, 15.0); // PIXEL area - INCHES horizontal distnace
296+
fuelAreaToDistanceMap.put(56882.0, 18.0);
297+
fuelAreaToDistanceMap.put(43264.0, 20.0);
298+
fuelAreaToDistanceMap.put(33488.0, 23.0);
299+
fuelAreaToDistanceMap.put(26080.0, 26.0);
300+
fuelAreaToDistanceMap.put(21025.0, 29.0);
301+
fuelAreaToDistanceMap.put(14884.0, 33.0);
302+
fuelAreaToDistanceMap.put(13110.0, 37.0);
303+
fuelAreaToDistanceMap.put(10816.0, 40.0);
304+
fuelAreaToDistanceMap.put(9312.0, 43.0);
305+
fuelAreaToDistanceMap.put(8190.0, 46.0);
306+
fuelAreaToDistanceMap.put(7225.0, 49.0);
307+
fuelAreaToDistanceMap.put(6400.0, 52.0);
308+
fuelAreaToDistanceMap.put(5550.0, 56.0);
309+
}
269310
}
270311

271312
public static class FieldConstants {
@@ -458,9 +499,9 @@ public static class TurretConstants {
458499
public static final Transform2d robotToTurretTransform =
459500
new Transform2d(TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero);
460501

461-
public static final int turretMotorID = 19;
462-
public static final int encoderAID = 20;
463-
public static final int encoderBID = 21;
502+
public static final int turretMotorID = 18;
503+
public static final int encoderAID = 19;
504+
public static final int encoderBID = 20;
464505
}
465506

466507
public static class AutoConstants {
@@ -475,7 +516,7 @@ public static class AutoConstants {
475516
}
476517

477518
public static class HoodConstants {
478-
public static final int hoodMotorID = 22;
519+
public static final int hoodMotorID = 21;
479520

480521
public static final Angle minAngle = Degrees.of(21.448);
481522
public static final Angle maxAngle = Degrees.of(59.231);
@@ -529,8 +570,8 @@ public static class HoodConstants {
529570
}
530571

531572
public static class SpindexerConstants {
532-
public static final int SpindexerMotorID = 23;
533-
public static final int SpindexerLaserID = 24;
573+
public static final int SpindexerMotorID = 22;
574+
public static final int SpindexerLaserID = 23;
534575
public static final double SpindexerMotorSpeed = 0.5;
535576
public static final double SpindexerDistance = 100;
536577
}

‎src/main/java/frc/robot/commands/MoveToFuel.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ public MoveToFuel(Swerve swerve) {
4040
}
4141

4242
private void updateDesiredRotation() {
43-
desiredRotation = swerve.desiredFuelRotation;
43+
// desiredRotation = swerve.desiredFuelRotation;
4444
}
4545

4646
// Called when the command is initially scheduled.

0 commit comments

Comments
 (0)