Skip to content

Commit 2022db4

Browse files
committed
Update position test and reports
1 parent c5e9f9b commit 2022db4

File tree

5 files changed

+47
-15
lines changed

5 files changed

+47
-15
lines changed

src/main/kotlin/frc/team2767/deepspace/command/HealthCheckCommand.kt

Lines changed: 21 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,6 @@ class HealthCheckCommand : Command() {
4646
}
4747

4848
timedTest {
49-
warmUp = 0.5
50-
duration = 2.0
5149
percentOutput = 0.75
5250
currentRange = 0.6..1.2
5351
speedRange = 1000..1100
@@ -91,9 +89,13 @@ class HealthCheckCommand : Command() {
9189

9290
positionTest {
9391
percentOutput = 0.2
94-
encoderTarget = 10_000
92+
93+
encoderChangeTarget = 10_000
94+
encoderGoodEnough = 500
9595
encoderTimeOutCount = 5000
96-
encoderGoodEnough = 1500
96+
97+
currentRange = 0.0..0.5
98+
speedRange = 500..600
9799
}
98100

99101
positionTalon {
@@ -109,9 +111,13 @@ class HealthCheckCommand : Command() {
109111

110112
positionTest {
111113
percentOutput = 0.2
114+
115+
encoderChangeTarget = 2500
112116
encoderGoodEnough = 200
113-
encoderTarget = 2500
114117
encoderTimeOutCount = 500
118+
119+
currentRange = 0.0..0.5
120+
speedRange = 500..600
115121
}
116122
}
117123

@@ -121,16 +127,24 @@ class HealthCheckCommand : Command() {
121127

122128
positionTest {
123129
percentOutput = 0.2
130+
131+
encoderChangeTarget = 3000
124132
encoderGoodEnough = 50
125-
encoderTarget = 3000
126133
encoderTimeOutCount = 500
134+
135+
currentRange = 0.0..0.5
136+
speedRange = 500..600
127137
}
128138

129139
positionTest {
130140
percentOutput = -0.2
141+
142+
encoderChangeTarget = 3000
131143
encoderGoodEnough = 50
132-
encoderTarget = 3000
133144
encoderTimeOutCount = 250
145+
146+
currentRange = 0.0..0.5
147+
speedRange = 500..600
134148
}
135149
}
136150

src/main/kotlin/frc/team2767/deepspace/health/TalonPositionTest.kt

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,12 @@ private val logger = KotlinLogging.logger {}
1717
class TalonPositionTest(private val group: TalonGroup) : Test, Reportable {
1818
override var name = "position test"
1919
var percentOutput = 0.0
20+
var currentRange = 0.0..0.0
21+
var speedRange = 0..0
2022
var warmUp = 0.25
2123
var peakVoltage = 12.0
22-
var encoderTarget = 0
24+
25+
var encoderChangeTarget = 0
2326
var encoderGoodEnough = 5
2427
var encoderTimeOutCount = 0
2528

@@ -58,8 +61,8 @@ class TalonPositionTest(private val group: TalonGroup) : Test, Reportable {
5861
currents.add(talon.outputCurrent)
5962
speeds.add(talon.selectedSensorVelocity)
6063

61-
if ((talon.selectedSensorPosition - startingPosition).absoluteValue > encoderTarget) {
62-
logger.info { "reached encoder target $encoderTarget" }
64+
if ((talon.selectedSensorPosition - startingPosition).absoluteValue > encoderChangeTarget) {
65+
logger.info { "reached encoder target $encoderChangeTarget" }
6366
talon.set(PercentOutput, 0.0)
6467
state = STOPPED
6568
return
@@ -93,12 +96,15 @@ class TalonPositionTest(private val group: TalonGroup) : Test, Reportable {
9396
}
9497

9598
override fun reportRows(tagConsumer: TagConsumer<Appendable>) {
99+
val current = currents.average()
100+
val speed = speeds.average().toInt()
101+
96102
tagConsumer.tr {
97103
td { +"${talon.deviceID}" }
98104
td { +"%.1f".format(percentOutput * peakVoltage) }
99-
td { +"$encoderTarget" }
100-
td { +"%.2f".format(currents.average()) }
101-
td { +"%.2f".format(speeds.average()) }
105+
td { +"$encoderChangeTarget" }
106+
td(classes = currentRange.statusOf(current)) { +"%.2f".format(current) }
107+
td(classes = speedRange.statusOf(speed)) { +"$speed" }
102108
}
103109
}
104110

src/main/kotlin/frc/team2767/deepspace/health/TalonTimedTest.kt

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -86,12 +86,14 @@ class TalonTimedTest(private val group: TalonGroup) : Test, Reportable {
8686

8787
override fun reportRows(tagConsumer: TagConsumer<Appendable>) {
8888
group.talons.forEach {
89+
val current = talonCurrents[it]?.average() ?: 0.0
90+
val speed = talonSpeeds[it]?.average()?.toInt() ?: 0
8991
tagConsumer.tr {
9092
td { +"${it.deviceID}" }
9193
td { +"%.2f".format(percentOutput * 12.0) }
9294
td { +"%.2f".format(duration) }
93-
td { +"%.2f".format(talonCurrents[it]?.average()) }
94-
td { +"%.2f".format(talonSpeeds[it]?.average()) }
95+
td(classes = currentRange.statusOf(current)) { +"%.2f".format(current) }
96+
td(classes = speedRange.statusOf(speed)) { +"$speed" }
9597
}
9698
}
9799
}

src/main/kotlin/frc/team2767/deepspace/health/Test.kt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@ package frc.team2767.deepspace.health
22

33
import kotlinx.html.TagConsumer
44

5+
fun <T : Comparable<T>> ClosedRange<T>.statusOf(value: T): String = if (this.contains(value)) "pass" else "fail"
6+
57
interface Test {
68
var name: String
79
fun execute()

src/main/resources/healthcheck.css

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,4 +17,12 @@ th {
1717

1818
td {
1919
text-align: right;
20+
}
21+
22+
td.pass {
23+
background-color: white
24+
}
25+
26+
td.fail {
27+
background-color: #ffb6c1
2028
}

0 commit comments

Comments
 (0)