Skip to content

Commit 0315035

Browse files
10239413114141023941311414
committed
Merge branch 'master' into feature/coral-station-autoalign
2 parents b005354 + 4585468 commit 0315035

File tree

13 files changed

+83
-88
lines changed

13 files changed

+83
-88
lines changed

‎src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 6.072698863636364,
12-
"y": 5.421022727272727
11+
"x": 5.751089743589744,
12+
"y": 5.5937179487179485
1313
},
1414
"isLocked": false,
1515
"linkedName": "OppositeStart"
@@ -20,8 +20,8 @@
2020
"y": 5.096
2121
},
2222
"prevControl": {
23-
"x": 5.595607954545455,
24-
"y": 5.345289772727273
23+
"x": 5.431153846153845,
24+
"y": 5.542115384615385
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -32,7 +32,7 @@
3232
"constraintZones": [
3333
{
3434
"name": "Constraints Zone",
35-
"minWaypointRelativePos": 0.769403824521935,
35+
"minWaypointRelativePos": 0.78,
3636
"maxWaypointRelativePos": 1.0,
3737
"constraints": {
3838
"maxVelocity": 1.5,

‎src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 4.37238446477125,
12-
"y": 6.006865646057453
11+
"x": 3.61474358974359,
12+
"y": 7.048910256410256
1313
},
1414
"isLocked": false,
1515
"linkedName": "ReefTopRight"
@@ -20,8 +20,8 @@
2020
"y": 7.1959659090909085
2121
},
2222
"prevControl": {
23-
"x": 2.582642045454545,
24-
"y": 6.368323863636363
23+
"x": 2.3246794871794867,
24+
"y": 6.997307692307693
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -32,7 +32,7 @@
3232
"constraintZones": [
3333
{
3434
"name": "Constraints Zone",
35-
"minWaypointRelativePos": 0.8256467941507301,
35+
"minWaypointRelativePos": 0.76,
3636
"maxWaypointRelativePos": 1.0,
3737
"constraints": {
3838
"maxVelocity": 1.5,

‎src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 2.2635511363636365,
12-
"y": 6.55778409090909
11+
"x": 2.2209556741938354,
12+
"y": 6.483850839917186
1313
},
1414
"isLocked": false,
1515
"linkedName": "TopSource"
@@ -20,8 +20,8 @@
2020
"y": 5.096
2121
},
2222
"prevControl": {
23-
"x": 3.5519090909090916,
24-
"y": 5.484892045454546
23+
"x": 3.5631410256410248,
24+
"y": 5.366666666666667
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -32,11 +32,11 @@
3232
"constraintZones": [
3333
{
3434
"name": "Constraints Zone",
35-
"minWaypointRelativePos": 0.708661417322838,
35+
"minWaypointRelativePos": 0.67,
3636
"maxWaypointRelativePos": 1.0,
3737
"constraints": {
38-
"maxVelocity": 0.9,
39-
"maxAcceleration": 1.5,
38+
"maxVelocity": 1.0,
39+
"maxAcceleration": 1.3,
4040
"maxAngularVelocity": 540.0,
4141
"maxAngularAcceleration": 720.0,
4242
"nominalVoltage": 12.0,

‎src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 3.442221590909091,
12-
"y": 5.524778409090909
11+
"x": 3.3257692307692306,
12+
"y": 5.4698717948717945
1313
},
1414
"isLocked": false,
1515
"linkedName": "ReefTopLeft_OffsetLeft"
@@ -20,8 +20,8 @@
2020
"y": 7.1959659090909085
2121
},
2222
"prevControl": {
23-
"x": 2.0940340909090915,
24-
"y": 6.747244318181817
23+
"x": 2.1698717948717943,
24+
"y": 6.625769230769231
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -32,7 +32,7 @@
3232
"constraintZones": [
3333
{
3434
"name": "Constraints Zone",
35-
"minWaypointRelativePos": 0.7984234234234222,
35+
"minWaypointRelativePos": 0.74,
3636
"maxWaypointRelativePos": 1.0,
3737
"constraints": {
3838
"maxVelocity": 1.0,

‎src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 2.0398863636363638,
12-
"y": 6.822954545454544
11+
"x": 2.135968509345801,
12+
"y": 6.8154492991273745
1313
},
1414
"isLocked": false,
1515
"linkedName": "TopSource"
@@ -20,8 +20,8 @@
2020
"y": 5.096
2121
},
2222
"prevControl": {
23-
"x": 3.472136363636363,
24-
"y": 5.4450056818181825
23+
"x": 3.5321794871794863,
24+
"y": 5.480192307692307
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -32,11 +32,11 @@
3232
"constraintZones": [
3333
{
3434
"name": "Constraints Zone",
35-
"minWaypointRelativePos": 0.8,
35+
"minWaypointRelativePos": 0.84,
3636
"maxWaypointRelativePos": 1.0,
3737
"constraints": {
38-
"maxVelocity": 1.5,
39-
"maxAcceleration": 1.5,
38+
"maxVelocity": 1.0,
39+
"maxAcceleration": 1.4,
4040
"maxAngularVelocity": 540.0,
4141
"maxAngularAcceleration": 720.0,
4242
"nominalVoltage": 12.0,

‎src/main/deploy/pathplanner/settings.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323
"robotTrackwidth": 0.546,
2424
"driveWheelRadius": 0.051,
2525
"driveGearing": 7.733,
26-
"maxDriveSpeed": 4.5,
26+
"maxDriveSpeed": 4.25,
2727
"driveMotorType": "vortex",
2828
"driveCurrentLimit": 45.0,
2929
"wheelCOF": 1.01,

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

Lines changed: 6 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ public class RobotConstants {
3030
public class RobotDimensions {
3131
public static final double robotWidth = 0.81;
3232
public static final double robotLength = 0.81; // 0.927
33-
public static final double robotLengthLocalAlign = 0.79;
33+
public static final double robotLengthLocalAlign = 0.79 + 0.16;
3434
public static final Translation2d robotXY = new Translation2d(robotWidth / 2, robotLength / 2);
3535
}
3636

@@ -144,7 +144,7 @@ public static class DriveConstants {
144144
public static final double driveGearRatio = 116.0 / 15.0;
145145
public static final double steerGearRatio = ((480.0 / 11.0)) * 1.0166667 * 0.99790377777778;
146146

147-
public static final double maxSpeed = 4.5;
147+
public static final double maxSpeed = 4.25;
148148
public static final double maxNorm =
149149
DriveSubsystem.computeMaxNorm(DriveConstants.positions, new Translation2d());
150150
public static final double maxOmega = (maxSpeed / maxNorm);
@@ -274,15 +274,9 @@ public static class LiftConstants {
274274
public static final double emaPeriod = 21;
275275

276276
public enum GantrySetpoint {
277-
LEFT(Units.inchesToMeters(13 / 2)),
278-
RIGHT(-Units.inchesToMeters(13 / 2)),
279-
CENTER(0);
280-
281-
public double alignOffset;
282-
283-
private GantrySetpoint(double alignOffset) {
284-
this.alignOffset = alignOffset;
285-
}
277+
LEFT,
278+
RIGHT,
279+
CENTER;
286280
}
287281

288282
public enum CoralPreset {
@@ -416,7 +410,7 @@ public static class GantryConstants {
416410
public static final double pulleyRadius =
417411
Units.inchesToMeters(0.5) * 1.13278894472 * 0.60103626943 * 1.58904109589 * 1.03571428571;
418412
// left -> right limit
419-
public static final Limits gantryLimits = new Limits(0.01, 0.36 + Units.inchesToMeters(1));
413+
public static final Limits gantryLimits = new Limits(0.01, 0.36 + Units.inchesToMeters(4));
420414
public static final double gantryLimitCenter = 0.223; // 0.198
421415
public static final double gantryPadding = 0.02;
422416
public static final int gantryLimitSwitchDIOPort = new RobotSwitch<Integer>(4).get();

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,8 @@ public static final MAXMotionConfig constructMaxMotionPos(PIDConstants constant)
9191
}
9292

9393
public static final PIDConstants drivePID = new PIDConstants(0.17189, 0.0, 0);
94-
public static final FeedForwardConstants driveFF = new FeedForwardConstants(0.12506, 2, 0.27879);
94+
public static final FeedForwardConstants driveFF =
95+
new FeedForwardConstants(0.12506, 2.3, 0.27879);
9596

9697
public static final PIDConstants steerPID = new PIDConstants(0.725, 0.0, 0.005);
9798

@@ -127,9 +128,9 @@ public static final MAXMotionConfig constructMaxMotionPos(PIDConstants constant)
127128

128129
public static final PIDConstants driveAntiTip = new PIDConstants(1, 0, 0);
129130

130-
public static final PIDConstants localTagAlign = new PIDConstants(1, 0, 0);
131+
public static final PIDConstants localTagAlign = new PIDConstants(0.9, 0.000, 0.0005);
131132
public static final PIDConstants localTagAlignVelocity = new PIDConstants(0.25, 0, 0);
132133
// public static final PIDConstants localTagAlignY = new PIDConstants(0.25, 0, 0);
133134
public static final PIDConstants localAnglePid = new PIDConstants(0.5, 0.001, 0.001);
134-
public static final PIDConstants localDriveProfiledPid = new PIDConstants(1, 0, 0);
135+
public static final PIDConstants localDriveProfiledPid = new PIDConstants(0.5, 0, 0);
135136
}

‎src/main/java/frc/robot/sensors/apriltag/AprilTagVision.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -296,6 +296,8 @@ public void periodic() {
296296
Logger.recordOutput(
297297
"AprilTagVisionLocal/" + displayName + "/IsPresent", localVector.isPresent());
298298

299+
Logger.recordOutput("idtouse", idToUse);
300+
299301
if (lastLocalVector.isPresent()) {
300302
Logger.recordOutput(
301303
"AprilTagVisionLocal/" + displayName + "/LocalAlignVectorLast", lastLocalVector.get());

‎src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java‎

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
package frc.robot.subsystems.drive.weights;
22

3-
import edu.wpi.first.math.MathUtil;
43
import edu.wpi.first.math.geometry.Pose2d;
54
import edu.wpi.first.math.geometry.Rotation2d;
65
import edu.wpi.first.math.geometry.Translation2d;
@@ -53,7 +52,7 @@ public ChassisSpeeds getSpeeds() {
5352
return autoAlignHelper.getLocalAlignSpeedsLine(
5453
vector.get(),
5554
gyro,
56-
new Rotation2d(MathUtil.angleModulus(robotRotation.get().getRadians())),
55+
new Rotation2d((robotRotation.get().getRadians())),
5756
new Rotation2d(
5857
FieldConstants.aprilTagLayout
5958
.getTagPose(getTargetTagId())
@@ -86,17 +85,20 @@ public boolean isReady() {
8685
boolean ready = false;
8786
if (vector.isPresent()) {
8887
double goalAngle =
89-
MathUtil.angleModulus(
90-
FieldConstants.aprilTagLayout
91-
.getTagPose(getTargetTagId())
92-
.get()
93-
.toPose2d()
94-
.getRotation()
95-
.unaryMinus()
96-
.getRadians());
88+
FieldConstants.aprilTagLayout
89+
.getTagPose(getTargetTagId())
90+
.get()
91+
.toPose2d()
92+
.getRotation()
93+
.plus(Rotation2d.k180deg)
94+
.getRadians();
95+
96+
Logger.recordOutput(
97+
"angledelta",
98+
Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()));
9799
ready =
98100
vector.get().getNorm() < 1
99-
&& MathUtil.angleModulus(robotRotation.get().getRadians()) - goalAngle < Math.PI / 18;
101+
&& Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) < 15;
100102
}
101103
Logger.recordOutput("LocalTagAlign/isAlignReady", ready);
102104
return ready;
@@ -114,7 +116,7 @@ public boolean isAutoalignComplete() {
114116
AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions);
115117
if (vector.isPresent()) {
116118
Rotation2d rotationError =
117-
new Rotation2d(MathUtil.angleModulus(robotRotation.get().getRadians()))
119+
new Rotation2d((robotRotation.get().getRadians()))
118120
.minus(
119121
new Rotation2d(
120122
FieldConstants.aprilTagLayout
@@ -131,6 +133,6 @@ public boolean isAutoalignComplete() {
131133
}
132134

133135
private boolean vectorDeadband(Translation2d vector) {
134-
return Math.abs(vector.getX()) < 0.03 && Math.abs(vector.getY()) < 0.03;
136+
return Math.abs(vector.getX()) < 0.015 && Math.abs(vector.getY()) < 0.015;
135137
}
136138
}

0 commit comments

Comments
 (0)