Skip to content

Commit 863ce3e

Browse files
AshOnDiscordAshOnDiscord
committed
3/13 - 4 spec, maybe 5
Revert the pp path i think
1 parent fc43bb1 commit 863ce3e

File tree

7 files changed

+64
-27
lines changed

7 files changed

+64
-27
lines changed

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/commands/drive/PurePursuitCommand.kt‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ import com.millburnx.utils.Vec2d
88
import com.qualcomm.robotcore.util.ElapsedTime
99
import org.firstinspires.ftc.teamcode.common.Robot
1010
import org.firstinspires.ftc.teamcode.common.utils.Pose2d
11+
import org.firstinspires.ftc.teamcode.common.utils.normalizeDegrees
1112
import kotlin.math.pow
1213

1314
@Config
@@ -39,7 +40,13 @@ class PurePursuitCommand(
3940
val results =
4041
purePursuit.calc(
4142
pose.position,
42-
if (headingLock) virtualHeading else pose.heading,
43+
if (headingLock) {
44+
virtualHeading
45+
} else if (backwards) {
46+
normalizeDegrees(180 - pose.heading)
47+
} else {
48+
pose.heading
49+
},
4350
robot.deltaTime.deltaTime,
4451
)
4552
robot.telemetry.addData("lookahead", results.lookahead)
@@ -79,7 +86,7 @@ class PurePursuitCommand(
7986

8087
robot.drive.pidManager.isOn = false
8188
robot.drive.robotCentric(
82-
powerF * multiF * if (backwards) -1.0 else 1.0,
89+
-powerF * multiF * if (backwards) -1.0 else 1.0,
8390
0.0,
8491
-powerH * multiH,
8592
speed = speed,
@@ -99,18 +106,18 @@ class PurePursuitCommand(
99106

100107
override fun isFinished(): Boolean {
101108
if (exitOnStuck && elapsedTime.milliseconds() > minStuckThreshold && robot.drive.stuckDectector.isStuck) return true
102-
return robot.drive.pidManager.atTarget()
109+
return robot.drive.pidManager.atTarget() && robot.drive.pidManager.isOn
103110
}
104111

105112
companion object {
106113
@JvmField
107114
var multiF = 1.0
108115

109116
@JvmField
110-
var multiH = 1.0
117+
var multiH = .1
111118

112119
@JvmField
113-
var minStuckThreshold: Long = 125L
120+
var minStuckThreshold: Long = 500L
114121

115122
@JvmField
116123
var useCorrection = false

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/drive/Drive.kt‎

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -128,13 +128,24 @@ open class Drive(
128128
path: String,
129129
heading: Double,
130130
useStuckDectector: Boolean = false,
131-
) = purePursuit(Path.loadPath(path), heading, useStuckDectector)
131+
headingLock: Boolean = true,
132+
backwards: Boolean = false,
133+
) = purePursuit(Path.loadPath(path), heading, useStuckDectector, headingLock, backwards)
132134

133135
fun purePursuit(
134136
path: Path,
135137
heading: Double,
136138
useStuckDectector: Boolean = false,
137-
) = PurePursuitCommand(robot, heading, path.points, exitOnStuck = useStuckDectector)
139+
headingLock: Boolean = true,
140+
backwards: Boolean = false,
141+
) = PurePursuitCommand(
142+
robot,
143+
heading,
144+
path.points,
145+
exitOnStuck = useStuckDectector,
146+
headingLock = headingLock,
147+
backwards = backwards,
148+
)
138149

139150
companion object {
140151
@JvmField

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/drive/PIDManager.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ open class PIDManager(
135135

136136
val motorThreshold = robot.drive.motors.all { abs(it.power) < wheelThreshold }
137137

138-
println("$motorThreshold ${robot.drive.motors.map { abs(it.power) }}")
138+
// println("$motorThreshold ${robot.drive.motors.map { abs(it.power) }}")
139139

140140
if (!motorThreshold && usePowerSettling) return false
141141

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/outtake/Slides.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -100,7 +100,7 @@ class Slides(
100100
else -> return
101101
}
102102

103-
println("slide target $target")
103+
// println("slide target $target")
104104

105105
this.target = target
106106
this.actualTarget = target

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecimenAuton.kt‎

Lines changed: 27 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,10 @@ class SpecimenAuton : OpMode() {
8787
SequentialCommandGroup(
8888
drive.pid(Pose2d(startingPose), tolerance = tolerance * 2.0),
8989
ConditionalCommand(
90-
intake.sweepAsync(),
90+
ParallelCommandGroup(
91+
intake.sweepAsync(),
92+
WaitCommand(250L),
93+
),
9194
intake.sweep(),
9295
) { intake.linkage.target == 1.0 },
9396
drive.pid(Pose2d(endingPose), tolerance = tolerance * 2.0),
@@ -131,7 +134,7 @@ class SpecimenAuton : OpMode() {
131134
),
132135
drive.pid(Pose2d(pickupPose), tolerance = tolerance * 2.0),
133136
),
134-
drive.pid(Pose2d(pickupPoseDeep), useStuckDectector = true, speed = 0.75),
137+
drive.pid(Pose2d(pickupPoseDeep), useStuckDectector = true, speed = slowSpeed),
135138
outtake.close(),
136139
WaitCommand(pickupDuration),
137140
outtake.slides.goTo(Slides.State.WALL),
@@ -149,15 +152,15 @@ class SpecimenAuton : OpMode() {
149152
return Path(
150153
listOf(
151154
pickupPos,
152-
Vec2d(-48, pickupPos.y),
155+
Vec2d(pickupPos.x, pickupPos.y + (scoringPos.y - pickupPos.y) / 2),
153156
) +
154157
listOf(
155158
Vec2d(-48, scoringPos.y) + offset,
156159
scoringPos + offset,
157-
Vec2d(-30, scoringPos.y) + offset,
160+
Vec2d(-36, scoringPos.y) + offset,
158161
) +
159162
listOf(
160-
Vec2d(-30.0, 0.0) + offset,
163+
Vec2d(-36.0, 0.0) + offset,
161164
scoringPosDeep + offset,
162165
),
163166
)
@@ -177,17 +180,21 @@ class SpecimenAuton : OpMode() {
177180
intake.arm.safe(),
178181
readySpecimen(),
179182
ParallelCommandGroup(
180-
purePursuitPath,
183+
InstantCommand({ println(purePursuitPath) }),
184+
namedCommand(
185+
"pathing",
186+
purePursuitPath,
187+
),
181188
SequentialCommandGroup(
182189
WaitUntilCommand({
183190
val pp = purePursuitPath.purePursuit
184191
val pastSegment = pp.beziers.indexOf(pp.lastIntersection.line) >= scoringSlowSegment
185192
val pastT = pp.lastIntersection.t > scoringSlowT
186-
print("segment ${pp.beziers.indexOf(pp.lastIntersection.line)} t ${pp.lastIntersection.t}")
193+
// print("segment ${pp.beziers.indexOf(pp.lastIntersection.line)} t ${pp.lastIntersection.t}")
187194
return@WaitUntilCommand pastSegment && pastT
188195
}),
189196
InstantCommand({
190-
purePursuitPath.speed = 0.75
197+
purePursuitPath.speed = slowSpeed
191198
}),
192199
),
193200
),
@@ -205,7 +212,7 @@ class SpecimenAuton : OpMode() {
205212
ParallelCommandGroup(
206213
SequentialCommandGroup(
207214
WaitUntilCommand {
208-
outtake.slides.position > Slides.autonHighRung * 3 / 4
215+
outtake.slides.position > Slides.autonHighRung * 1 / 4
209216
},
210217
drive.pid(
211218
Pose2d(scoringPose),
@@ -216,7 +223,7 @@ class SpecimenAuton : OpMode() {
216223
drive.pid(
217224
Pose2d(scoringPoseDeep),
218225
useStuckDectector = true,
219-
speed = 0.75,
226+
speed = slowSpeed,
220227
),
221228
WaitCommand(clipSpecimenDuration),
222229
releaseSpecimen(),
@@ -298,7 +305,7 @@ class SpecimenAuton : OpMode() {
298305
robot.telemetry.addData("pid at target", robot.drive.pidManager.atTarget())
299306
robot.telemetry.addData("current commands", currentCommands.joinToString(", "))
300307
robot.telemetry.addData("time", robot.matchTimer.seconds())
301-
println(currentCommands.joinToString(", "))
308+
println(currentCommands.joinToString(" | "))
302309
}
303310

304311
companion object {
@@ -308,11 +315,14 @@ class SpecimenAuton : OpMode() {
308315
@JvmField
309316
var parkPose = arrayOf(-54.0, -36.0, 180.0)
310317

318+
@JvmField
319+
var slowSpeed = .625
320+
311321
// make this pickup and scoring into a pure pursuit path
312322

313323
// <editor-fold desc="Pickup Config">
314324
@JvmField
315-
var pickupPose = arrayOf(-54.0, -36.0, 180.0)
325+
var pickupPose = arrayOf(-58.0, -36.0, 180.0)
316326

317327
@JvmField
318328
var pickupPoseDeep = arrayOf(-72.0, -36.0, 180.0)
@@ -323,7 +333,7 @@ class SpecimenAuton : OpMode() {
323333

324334
// <editor-fold desc="Scoring Config">
325335
@JvmField
326-
var scoringPose = arrayOf(-48.0, -2.0, 180.0)
336+
var scoringPose = arrayOf(-40.0, -2.0, 180.0)
327337

328338
@JvmField
329339
var scoringSlowSegment = 1
@@ -332,22 +342,22 @@ class SpecimenAuton : OpMode() {
332342
var scoringSlowT = 0.5
333343

334344
@JvmField
335-
var scoringPoseDeep = arrayOf(-24.0, -2.0, 180.0)
345+
var scoringPoseDeep = arrayOf(-32.0, -2.0, 180.0)
336346

337347
@JvmField
338348
var scoringOffset = arrayOf(0.0, 2.0, 0.0)
339349

340350
@JvmField
341-
var exitingScoring = arrayOf(-48.0, -2.0, 180.0)
351+
var exitingScoring = arrayOf(-40.0, -2.0, 180.0)
342352

343353
@JvmField
344354
var clipSpecimenDuration: Long = 500
345355

346356
@JvmField
347-
var minDistanceForArm = 64.0
357+
var minDistanceForArm = 52.0
348358

349359
@JvmField
350-
var minDistanceForLowering = 52.0
360+
var minDistanceForLowering = 44.0
351361
// </editor-fold>
352362

353363
// <editor-fold desc="Sweep Poses">

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/MainTeleop.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -430,7 +430,7 @@ open class MainTeleopBlue : OpMode() {
430430
var baseIntakeDuration: Long = 1000
431431

432432
@JvmField
433-
var intakeDuration: Long = 1000
433+
var intakeDuration: Long = 750
434434

435435
// <editor-fold desc="Heading Assist">
436436
@JvmField

‎paths/specimenMod.tsv‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
-64.0 -40.0
2+
-52.0 -40.0
3+
4+
-48.0 0.0
5+
-36.0 0.0
6+
-24.0 0.0
7+
8+
-12.0 0.0
9+
0.0 0.0

0 commit comments

Comments
 (0)