Skip to content

Commit 7cf2d7d

Browse files
Braedeng0Braedeng0
committed
Update thresholds and PID coefficients
1 parent b2879cf commit 7cf2d7d

File tree

8 files changed

+45
-27
lines changed

8 files changed

+45
-27
lines changed

‎TeamCode/src/main/kotlin/pioneer/Constants.kt‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -103,10 +103,10 @@ object Constants {
103103
@Config
104104
object Follower {
105105
/** The threshold in cm to consider the target reached. */
106-
const val POSITION_THRESHOLD = 1.0
106+
const val POSITION_THRESHOLD = 2.0
107107

108108
/** The threshold in radians to consider the target heading reached. */
109-
const val ROTATION_THRESHOLD = 0.05
109+
const val ROTATION_THRESHOLD = 0.075
110110

111111
/** The maximum drive velocity in cm per second. */
112112
const val MAX_DRIVE_VELOCITY = 110.0
@@ -173,22 +173,21 @@ object Constants {
173173

174174
@Config
175175
object Spindexer {
176-
@JvmField var KP = 0.000425
177-
@JvmField var KI = 0.0
178-
@JvmField var KD = 0.02
176+
@JvmField var KP = 0.00025
177+
@JvmField var KI = 0.0035
178+
@JvmField var KD = 0.0075
179179

180180
@JvmField var KS_START = 0.05
181-
@JvmField var KS_STEP = 0.01
181+
@JvmField var KS_STEP = 0.015
182182

183-
@JvmField var MAX_POWER_RATE = 5.0
183+
@JvmField var MAX_POWER_RATE = 15.0
184184

185185
const val POSITION_TOLERANCE_TICKS = 100
186-
const val VELOCITY_TOLERANCE_TPS = 125
186+
const val VELOCITY_TOLERANCE_TPS = 100
187187
const val TICKS_PER_REV = 8192
188188

189-
// TODO: Tune these values when we test on the real hardware
190189
// Time required to confirm an artifact has been intaken (ms)
191-
const val CONFIRM_INTAKE_MS = 50
190+
const val CONFIRM_INTAKE_MS = 25
192191

193192
// Max time the artifact can disappear without resetting confirmation (ms)
194193
const val CONFIRM_LOSS_MS = 10

‎TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ enum class GoalTag(
4545
val height: Double
4646
get() = position[2].toDouble()
4747

48-
val shootingHeight = height + 50.0
48+
val shootingHeight = height + 30.0
4949

5050
val shootingPose: Pose
5151
get() =

‎TeamCode/src/main/kotlin/pioneer/decode/Points.kt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -47,13 +47,13 @@ class Points(
4747

4848
private fun collectY(i: Int) = 30 - (i * 60.0) // 30, -30, -90
4949

50-
private val prepCollectX = 75.0
50+
private val prepCollectX = 80.0
5151
private val collectTheta = -PI / 2 // Point to right
5252
val PREP_COLLECT_GOAL = Pose(prepCollectX, collectY(0), theta=collectTheta).T(color)
5353
val PREP_COLLECT_MID = Pose(prepCollectX, collectY(1), theta=collectTheta).T(color)
5454
val PREP_COLLECT_AUDIENCE = Pose(prepCollectX, collectY(2), theta=collectTheta).T(color)
5555

56-
private val collectX = 120.0
56+
private val collectX = 117.5
5757
val COLLECT_GOAL = Pose(collectX, collectY(0), theta=collectTheta).T(color)
5858
val COLLECT_MID = Pose(collectX, collectY(1), theta=collectTheta).T(color)
5959
val COLLECT_AUDIENCE = Pose(collectX, collectY(2), theta=collectTheta).T(color)

‎TeamCode/src/main/kotlin/pioneer/hardware/Spindexer.kt‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,7 @@ class Spindexer(
145145
private var offsetTicks = 0
146146
private var lastPower = 0.0
147147
private val chrono = Chrono(autoUpdate = false, units = DurationUnit.MILLISECONDS)
148-
private val ticksPerRadian: Int = -(Constants.Spindexer.TICKS_PER_REV / (2 * PI)).toInt() // FIXME: Negative
148+
private val ticksPerRadian: Int = (Constants.Spindexer.TICKS_PER_REV / (2 * PI)).toInt()
149149
private val motorPID =
150150
PIDController(
151151
Constants.Spindexer.KP,
@@ -320,7 +320,7 @@ class Spindexer(
320320
val rawError = targetMotorPosition - currentMotorPosition
321321
val error = wrapTicks(rawError)
322322

323-
// PID -> -1..1 output
323+
// PID
324324
var power = motorPID.update(error.toDouble(), chrono.dt)
325325

326326
// Ramp power to prevent sudden acceleration
@@ -330,9 +330,9 @@ class Spindexer(
330330
val adjustedKS = Constants.Spindexer.KS_START + Constants.Spindexer.KS_STEP * numStoredArtifacts
331331
power += if (abs(error) > 100) adjustedKS * sign(error.toDouble()) else 0.0
332332

333-
// Apply power (inverted due to motor orientation)
334-
val maxPower = 0.75
335-
motor.power = -power.coerceIn(-maxPower, maxPower)
333+
// Apply power
334+
val maxPower = 1.0
335+
motor.power = power.coerceIn(-maxPower, maxPower)
336336
}
337337

338338
private fun checkForArtifact() {

‎TeamCode/src/main/kotlin/pioneer/opmodes/auto/GoalSideAuto.kt‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -99,20 +99,21 @@ class GoalSideAuto : BaseOpMode() {
9999
targetVelocity = bot.flywheel!!.estimateVelocity(targetGoal.shootingPose, bot.pinpoint!!.pose, targetGoal.shootingHeight)
100100
bot.turret?.autoTrack(bot.pinpoint!!.pose, targetGoal.shootingPose)
101101

102+
telemetry.addData("Pose", bot.pinpoint!!.pose.toString())
103+
telemetry.addData("Follower Done", bot.follower.done)
102104
telemetry.addData("Next Artifact", motifOrder.currentArtifact)
103105
telemetry.addData("Detected Motif", motifOrder.toString())
104106
telemetry.addData("Looking for Motif", lookForTag)
105107
telemetry.addData("State", state)
106108
telemetry.addData("Collect State", collectState)
107-
telemetry.addData("Pose", bot.pinpoint!!.pose.toString())
108109
}
109110

110111
private fun state_goto_shoot() {
111112
bot.flywheel?.velocity = targetVelocity
112113
if (!bot.follower.isFollowing) { // Starting path
113114
bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact)
114115
val endPose = if (lookForTag) P.SHOOT_GOAL_CLOSE.copy(theta=0.0) else P.SHOOT_GOAL_CLOSE
115-
FileLogger.debug("GoalSideAuto", "Set Target Pose: $endPose")
116+
// FileLogger.debug("GoalSideAuto", "Set Target Pose: $endPose")
116117
bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, endPose))
117118
}
118119
if (lookForTag) {
@@ -125,6 +126,7 @@ class GoalSideAuto : BaseOpMode() {
125126
}
126127
}
127128
if (bot.follower.done) { // Ending path
129+
bot.follower.reset()
128130
state = State.SHOOT
129131
}
130132
}
@@ -138,18 +140,21 @@ class GoalSideAuto : BaseOpMode() {
138140
when (autoType) {
139141
AutoOptions.PRELOAD_ONLY -> {
140142
if (collectState == CollectState.GOAL) {
143+
bot.follower.reset()
141144
bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION))
142145
state = State.STOP
143146
}
144147
}
145148
AutoOptions.FIRST_ROW -> {
146149
if (collectState == CollectState.MID) {
150+
bot.follower.reset()
147151
bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION))
148152
state = State.STOP
149153
}
150154
}
151155
AutoOptions.SECOND_ROW -> {
152156
if (collectState == CollectState.AUDIENCE) {
157+
bot.follower.reset()
153158
bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION))
154159
state = State.STOP
155160
}
@@ -195,6 +200,7 @@ class GoalSideAuto : BaseOpMode() {
195200
}
196201

197202
if (bot.follower.done) { // Ending path
203+
bot.follower.reset()
198204
state = State.COLLECT
199205
}
200206
}
@@ -212,6 +218,7 @@ class GoalSideAuto : BaseOpMode() {
212218
}
213219

214220
if (bot.follower.done) { // Ending path
221+
bot.follower.reset()
215222
state = State.GOTO_SHOOT
216223
bot.intake?.stop()
217224
bot.flywheel?.velocity = targetVelocity
@@ -226,6 +233,7 @@ class GoalSideAuto : BaseOpMode() {
226233

227234
private fun state_stop() {
228235
if (bot.follower.done) {
236+
bot.follower.reset()
229237
requestOpModeStop()
230238
}
231239
}

‎TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ColorSensorGain.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp
77
import pioneer.decode.Artifact
88
import pioneer.hardware.RevColorSensor
99

10-
@Disabled
10+
//@Disabled
1111
@TeleOp(name = "Color Sensor Gain Calibration")
1212
class ColorSensorGain : OpMode() {
1313
private lateinit var sensor: RevColorSensor

‎TeamCode/src/main/kotlin/pioneer/opmodes/calibration/SpindexerPIDTuning.kt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package pioneer.opmodes.calibration
22

3+
import com.acmerobotics.dashboard.FtcDashboard
34
import com.qualcomm.robotcore.eventloop.opmode.OpMode
45
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
56
import pioneer.hardware.Spindexer
@@ -40,5 +41,13 @@ class SpindexerPIDTuning : OpMode() {
4041
telemetry.addData("Target Position", spindexer.targetMotorPosition)
4142
telemetry.addData("Reached Target", spindexer.reachedTarget)
4243
telemetry.update()
44+
45+
FtcDashboard.getInstance().telemetry.apply {
46+
addData("Spindexer Target", targetPosition)
47+
addData("Spindexer Motor Position", spindexer.currentMotorPosition)
48+
addData("Target Position", spindexer.targetMotorPosition)
49+
addData("Reached Target", spindexer.reachedTarget)
50+
update()
51+
}
4352
}
4453
}

‎TeamCode/src/main/kotlin/pioneer/pathing/follower/Follower.kt‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -120,11 +120,11 @@ class Follower(
120120
// }
121121
// }
122122

123-
if (done) {
124-
reset()
125-
drive.stop()
126-
return true
127-
}
123+
// if (done) {
124+
// reset()
125+
// drive.stop()
126+
// return true
127+
// }
128128

129129
val state = profile[t] // MotionState(x = s)
130130
val s = state.x
@@ -160,6 +160,8 @@ class Follower(
160160
// convert position errors to robot frame
161161
val (errorX, errorY) = MathUtils.rotateVector(errorFieldX, errorFieldY, -localizer.pose.theta)
162162

163+
// FileLogger.debug("Follower", "Position Error: $errorX, $errorY")
164+
163165
// PID corrections
164166
val vxCorrect = pidVX.update(errorX, chrono.dt)
165167
val vyCorrect = pidVY.update(errorY, chrono.dt)
@@ -175,7 +177,7 @@ class Follower(
175177
alpha = 0.0,
176178
)
177179

178-
FileLogger.debug("Follower", "Drive Command: $driveCommand")
180+
// FileLogger.debug("Follower", "Drive Command: $driveCommand")
179181
drive.setDriveVA(driveCommand)
180182

181183
return false // path not complete

0 commit comments

Comments
 (0)