Skip to content

Commit b2879cf

Browse files
Braedeng0Braedeng0
committed
Add position PID coefficients (untested) and work on full auto
1 parent dd6e48a commit b2879cf

File tree

4 files changed

+64
-22
lines changed

4 files changed

+64
-22
lines changed

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

Lines changed: 18 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -137,6 +137,20 @@ object Constants {
137137
@JvmField var THETA_KP = 5.0
138138
@JvmField var THETA_KI = 0.0
139139
@JvmField var THETA_KD = 0.0
140+
141+
// Position PID coefficients for final pose correction
142+
@JvmField var POS_X_KP = 0.0
143+
@JvmField var POS_X_KI = 0.0
144+
@JvmField var POS_X_KD = 0.0
145+
146+
@JvmField var POS_Y_KP = 0.0
147+
@JvmField var POS_Y_KI = 0.0
148+
@JvmField var POS_Y_KD = 0.0
149+
150+
@JvmField var POS_THETA_KP = 0.0
151+
@JvmField var POS_THETA_KI = 0.0
152+
@JvmField var POS_THETA_KD = 0.0
153+
140154
}
141155

142156
object Camera {
@@ -159,18 +173,17 @@ object Constants {
159173

160174
@Config
161175
object Spindexer {
162-
@JvmField var KP = 0.00045
163-
176+
@JvmField var KP = 0.000425
164177
@JvmField var KI = 0.0
165-
166-
@JvmField var KD = 0.04
178+
@JvmField var KD = 0.02
167179

168180
@JvmField var KS_START = 0.05
169181
@JvmField var KS_STEP = 0.01
170182

171183
@JvmField var MAX_POWER_RATE = 5.0
172184

173185
const val POSITION_TOLERANCE_TICKS = 100
186+
const val VELOCITY_TOLERANCE_TPS = 125
174187
const val TICKS_PER_REV = 8192
175188

176189
// TODO: Tune these values when we test on the real hardware
@@ -191,7 +204,7 @@ object Constants {
191204
}
192205

193206
object ServoPositions {
194-
const val LAUNCHER_REST = 0.067
207+
const val LAUNCHER_REST = 0.065
195208
const val LAUNCHER_TRIGGERED = 0.315
196209
}
197210

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -116,7 +116,7 @@ class Spindexer(
116116
get() {
117117
// Compute circular error
118118
val error = wrapTicks(targetMotorPosition - currentMotorPosition)
119-
return abs(error) < Constants.Spindexer.POSITION_TOLERANCE_TICKS
119+
return abs(error) < Constants.Spindexer.POSITION_TOLERANCE_TICKS && (motor.velocity < Constants.Spindexer.VELOCITY_TOLERANCE_TPS)
120120
}
121121

122122
// Getters for artifact storage status
@@ -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()
148+
private val ticksPerRadian: Int = -(Constants.Spindexer.TICKS_PER_REV / (2 * PI)).toInt() // FIXME: Negative
149149
private val motorPID =
150150
PIDController(
151151
Constants.Spindexer.KP,
@@ -390,7 +390,7 @@ class Spindexer(
390390
private fun detectArtifact(sensor: RevColorSensor): Artifact? {
391391
// Determine artifact based on hue thresholds
392392
return when {
393-
sensor.distance > 15.0 -> null
393+
sensor.distance > 8.0 -> null
394394
sensor.hue < 170 && sensor.hue > 140 -> Artifact.GREEN
395395
sensor.hue < 250 && sensor.hue > 170 -> Artifact.PURPLE
396396
else -> null

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ import pioneer.decode.Motif
1010
import pioneer.decode.Obelisk
1111
import pioneer.decode.Points
1212
import pioneer.general.AllianceColor
13+
import pioneer.helpers.FileLogger
1314
import pioneer.helpers.Toggle
1415
import pioneer.helpers.next
1516
import pioneer.opmodes.BaseOpMode
@@ -111,6 +112,7 @@ class GoalSideAuto : BaseOpMode() {
111112
if (!bot.follower.isFollowing) { // Starting path
112113
bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact)
113114
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")
114116
bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, endPose))
115117
}
116118
if (lookForTag) {

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

Lines changed: 41 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,6 @@ import pioneer.helpers.PIDController
1010
import pioneer.helpers.Pose
1111
import pioneer.localization.Localizer
1212
import pioneer.pathing.motionprofile.*
13-
import pioneer.pathing.motionprofile.constraints.AccelerationConstraint
14-
import pioneer.pathing.motionprofile.constraints.VelocityConstraint
1513
import pioneer.pathing.paths.Path
1614
import kotlin.math.*
1715
import kotlin.time.DurationUnit
@@ -23,9 +21,14 @@ class Follower(
2321
private val timer = ElapsedTime()
2422
private val chrono = Chrono(autoUpdate = false, units = DurationUnit.MILLISECONDS)
2523

26-
private val pidX = PIDController(Constants.Follower.X_KP, Constants.Follower.X_KI, Constants.Follower.X_KD)
27-
private val pidY = PIDController(Constants.Follower.Y_KP, Constants.Follower.Y_KI, Constants.Follower.Y_KD)
28-
private val pidHeading = PIDController(Constants.Follower.THETA_KP, Constants.Follower.THETA_KI, Constants.Follower.THETA_KD)
24+
private val pidVX = PIDController(Constants.Follower.X_KP, Constants.Follower.X_KI, Constants.Follower.X_KD)
25+
private val pidVY = PIDController(Constants.Follower.Y_KP, Constants.Follower.Y_KI, Constants.Follower.Y_KD)
26+
private val pidVHeading = PIDController(Constants.Follower.THETA_KP, Constants.Follower.THETA_KI, Constants.Follower.THETA_KD)
27+
28+
// Second set of PIDs for direct position control to be used when path is done (if needed)
29+
private val pidXPos = PIDController(Constants.Follower.POS_X_KP, Constants.Follower.POS_X_KI, Constants.Follower.POS_X_KD)
30+
private val pidYPos = PIDController(Constants.Follower.POS_Y_KP, Constants.Follower.POS_Y_KI, Constants.Follower.POS_Y_KD)
31+
private val pidHeadingPos = PIDController(Constants.Follower.POS_THETA_KP, Constants.Follower.POS_THETA_KI, Constants.Follower.POS_THETA_KD)
2932

3033
private var path: Path? = null
3134
private var profile: MotionProfile? = null
@@ -68,9 +71,9 @@ class Follower(
6871
this.path = path
6972
this.profile = calculateMotionProfile(path)
7073
// Reset PID controllers and timer
71-
pidX.reset()
72-
pidY.reset()
73-
pidHeading.reset()
74+
pidVX.reset()
75+
pidVY.reset()
76+
pidVHeading.reset()
7477
timer.reset()
7578

7679
FileLogger.debug("Follower", "Profile duration=${profile?.duration()}")
@@ -79,9 +82,9 @@ class Follower(
7982
fun reset() {
8083
path = null
8184
profile = null
82-
pidX.reset()
83-
pidY.reset()
84-
pidHeading.reset()
85+
pidVX.reset()
86+
pidVY.reset()
87+
pidVHeading.reset()
8588
drive.stop()
8689
}
8790

@@ -93,6 +96,30 @@ class Follower(
9396
val profile = profile ?: return true
9497
val t = timer.seconds()
9598

99+
// if (timer.seconds() > t) { // Untested
100+
// // If path is done but not within tolerances
101+
// if (!done) {
102+
// // Switch to position PID control to correct final pose
103+
// val errorX = path.endPose.x - localizer.pose.x
104+
// val errorY = path.endPose.y - localizer.pose.y
105+
// val errorHeading = MathUtils.normalizeRadians(path.endPose.theta - localizer.pose.theta)
106+
// val vxCorrect = pidXPos.update(errorX, chrono.dt)
107+
// val vyCorrect = pidYPos.update(errorY, chrono.dt)
108+
// val omegaCorrect = pidHeadingPos.update(errorHeading, chrono.dt)
109+
// val driveCommand = Pose(
110+
// vx = vxCorrect,
111+
// vy = vyCorrect,
112+
// omega = omegaCorrect,
113+
// ax = 0.0,
114+
// ay = 0.0,
115+
// alpha = 0.0,
116+
// )
117+
// FileLogger.debug("Follower", "Final Pose Correction Drive Command: $driveCommand")
118+
// drive.setDriveVA(driveCommand)
119+
// return false
120+
// }
121+
// }
122+
96123
if (done) {
97124
reset()
98125
drive.stop()
@@ -134,9 +161,9 @@ class Follower(
134161
val (errorX, errorY) = MathUtils.rotateVector(errorFieldX, errorFieldY, -localizer.pose.theta)
135162

136163
// PID corrections
137-
val vxCorrect = pidX.update(errorX, chrono.dt)
138-
val vyCorrect = pidY.update(errorY, chrono.dt)
139-
val omegaCorrect = pidHeading.update(errorHeading, chrono.dt)
164+
val vxCorrect = pidVX.update(errorX, chrono.dt)
165+
val vyCorrect = pidVY.update(errorY, chrono.dt)
166+
val omegaCorrect = pidVHeading.update(errorHeading, chrono.dt)
140167

141168
// final drive command
142169
val driveCommand = Pose(

0 commit comments

Comments
 (0)