Skip to content

Commit

Permalink
update Phoenix6 and fix mechanism units
Browse files Browse the repository at this point in the history
  • Loading branch information
MatthewChoulas committed Jan 8, 2025
1 parent b2a558d commit 6968c57
Show file tree
Hide file tree
Showing 7 changed files with 150 additions and 92 deletions.
6 changes: 5 additions & 1 deletion src/main/kotlin/org/team4099/lib/units/Derivatives.kt
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
package org.team4099.lib.units

import org.team4099.lib.units.base.*
import org.team4099.lib.units.base.METERS_PER_FOOT
import org.team4099.lib.units.base.METERS_PER_INCH
import org.team4099.lib.units.base.Meter
import org.team4099.lib.units.base.SECONDS_PER_MINUTE
import org.team4099.lib.units.base.Second
import org.team4099.lib.units.derived.Radian
import kotlin.math.PI

Expand Down
16 changes: 8 additions & 8 deletions src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt
Original file line number Diff line number Diff line change
Expand Up @@ -281,8 +281,8 @@ fun ctreAngularMechanismSensor(
Timescale.CTRE,
1023.0,
compensationVoltage,
{ controller.rotorVelocity.value.toDouble() },
{ controller.rotorPosition.value.toDouble() }
{ controller.rotorVelocity.valueAsDouble },
{ controller.rotorPosition.valueAsDouble }
)
}

Expand All @@ -299,8 +299,8 @@ fun ctreLinearMechanismSensor(
Timescale.CTRE,
1023.0,
compensationVoltage,
{ controller.rotorVelocity.value.toDouble() },
{ controller.rotorPosition.value.toDouble() }
{ controller.rotorVelocity.valueAsDouble },
{ controller.rotorPosition.valueAsDouble }
)
}

Expand All @@ -319,8 +319,8 @@ fun ctreLinearMechanismSensor(
Timescale.PHOENIX_PRO,
1.0,
compensationVoltage,
{ velocitySignal.value },
{ positionSignal.value },
{ velocitySignal.valueAsDouble },
{ positionSignal.valueAsDouble },
)
}

Expand All @@ -337,8 +337,8 @@ fun ctreAngularMechanismSensor(
Timescale.PHOENIX_PRO,
1.0,
compensationVoltage,
{ velocitySignal.value },
{ positionSignal.value },
{ velocitySignal.valueAsDouble },
{ positionSignal.valueAsDouble },
)
}

Expand Down
9 changes: 5 additions & 4 deletions src/main/kotlin/org/team4099/lib/units/base/Frequency.kt
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,15 @@ package org.team4099.lib.units.base
import org.team4099.lib.units.UnitKey
import org.team4099.lib.units.Value

object Hertz: UnitKey
object Hertz : UnitKey

typealias Frequency = Value<Hertz>

inline val Double.hertz: Frequency
get() = Frequency(this)
get() = Frequency(this)

inline val Number.hertz: Frequency
get() = toDouble().hertz
get() = toDouble().hertz

inline val Frequency.inHertz: Double
get() = value
get() = value
52 changes: 26 additions & 26 deletions src/main/kotlin/org/team4099/lib/units/derived/Controller.kt
Original file line number Diff line number Diff line change
Expand Up @@ -273,15 +273,15 @@ inline val ProportionalGain<Radian, Velocity<Radian>>.inRadiansPerSecondPerRotat
get() = inRadiansPerSecondPerRadian * RADIANS_PER_ROTATION

inline val ProportionalGain<Velocity<Meter>, Velocity<Radian>>.inRadiansPerSecondPerMetersPerSecond:
Double
get() = value
Double
get() = value

inline val ProportionalGain<
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRadiansPerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRadiansPerSecond: Double
get() = value

inline val ProportionalGain<
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRotationPerMinute: Double
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRotationPerMinute: Double
get() = inRadiansPerSecondPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE

inline val IntegralGain<Meter, Velocity<Radian>>.inRadiansPerSecondPerMeterSeconds: Double
Expand Down Expand Up @@ -324,7 +324,7 @@ inline val DerivativeGain<Meter, Velocity<Radian>>.inRadiansPerSecondPerFootPerS
get() = inRadiansPerSecondPerMeterPerSecond * METERS_PER_FOOT

inline val DerivativeGain<
Velocity<Meter>, Velocity<Radian>>.inRadiansPerSecondPerMetersPerSecondPerSecond: Double
Velocity<Meter>, Velocity<Radian>>.inRadiansPerSecondPerMetersPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<Radian, Velocity<Radian>>.inRadiansPerSecondPerRadianPerSecond: Double
Expand All @@ -337,15 +337,15 @@ inline val DerivativeGain<Radian, Velocity<Radian>>.inRadiansPerSecondPerRotatio
get() = inRadiansPerSecondPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val DerivativeGain<
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRadiansPerSecondPerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRadiansPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerDegreesPerSecondPerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerDegreesPerSecondPerSecond: Double
get() = inRadiansPerSecondPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES

inline val DerivativeGain<
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRotationsPerMinutePerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inRadiansPerSecondPerRotationsPerMinutePerSecond: Double
get() = inRadiansPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val Double.metersPerSecondPerMetersPerSecond
Expand All @@ -370,15 +370,15 @@ inline val ProportionalGain<Radian, Velocity<Meter>>.inMetersPerSecondPerRotatio
get() = inMetersPerSecondPerRadian * RADIANS_PER_ROTATION

inline val ProportionalGain<Velocity<Meter>, Velocity<Meter>>.inMetersPerSecondPerMetersPerSecond:
Double
get() = value
Double
get() = value

inline val ProportionalGain<Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRadiansPerSecond:
Double
get() = value
Double
get() = value

inline val ProportionalGain<
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRotationPerMinute: Double
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRotationPerMinute: Double
get() = inMetersPerSecondPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE

inline val IntegralGain<Meter, Velocity<Meter>>.inMetersPerSecondPerMeterSeconds: Double
Expand Down Expand Up @@ -421,7 +421,7 @@ inline val DerivativeGain<Meter, Velocity<Meter>>.inMetersPerSecondPerFootPerSec
get() = inMetersPerSecondPerMeterPerSecond * METERS_PER_FOOT

inline val DerivativeGain<
Velocity<Meter>, Velocity<Meter>>.inMetersPerSecondPerMetersPerSecondPerSecond: Double
Velocity<Meter>, Velocity<Meter>>.inMetersPerSecondPerMetersPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<Radian, Velocity<Meter>>.inMetersPerSecondPerRadianPerSecond: Double
Expand All @@ -434,15 +434,15 @@ inline val DerivativeGain<Radian, Velocity<Meter>>.inMetersPerSecondPerRotations
get() = inMetersPerSecondPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val DerivativeGain<
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRadiansPerSecondPerSecond: Double
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRadiansPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerDegreesPerSecondPerSecond: Double
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerDegreesPerSecondPerSecond: Double
get() = inMetersPerSecondPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES

inline val DerivativeGain<
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRotationsPerMinutePerSecond: Double
Velocity<Radian>, Velocity<Meter>>.inMetersPerSecondPerRotationsPerMinutePerSecond: Double
get() = inMetersPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val Double.MetersPerMeters
Expand Down Expand Up @@ -557,15 +557,15 @@ inline val ProportionalGain<Radian, Velocity<Radian>>.inDegreesPerSecondPerRotat
get() = inDegreesPerSecondPerRadian * RADIANS_PER_ROTATION

inline val ProportionalGain<Velocity<Meter>, Velocity<Radian>>.inDegreesPerSecondPerMetersPerSecond:
Double
get() = value
Double
get() = value

inline val ProportionalGain<
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRadiansPerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRadiansPerSecond: Double
get() = value

inline val ProportionalGain<
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRotationPerMinute: Double
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRotationPerMinute: Double
get() = inDegreesPerSecondPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE

inline val IntegralGain<Meter, Velocity<Radian>>.inDegreesPerSecondPerMeterSeconds: Double
Expand Down Expand Up @@ -608,7 +608,7 @@ inline val DerivativeGain<Meter, Velocity<Radian>>.inDegreesPerSecondPerFootPerS
get() = inDegreesPerSecondPerMeterPerSecond * METERS_PER_FOOT

inline val DerivativeGain<
Velocity<Meter>, Velocity<Radian>>.inDegreesPerSecondPerMetersPerSecondPerSecond: Double
Velocity<Meter>, Velocity<Radian>>.inDegreesPerSecondPerMetersPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<Radian, Velocity<Radian>>.inDegreesPerSecondPerRadianPerSecond: Double
Expand All @@ -621,13 +621,13 @@ inline val DerivativeGain<Radian, Velocity<Radian>>.inDegreesPerSecondPerRotatio
get() = inDegreesPerSecondPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val DerivativeGain<
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRadiansPerSecondPerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRadiansPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerDegreesPerSecondPerSecond: Double
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerDegreesPerSecondPerSecond: Double
get() = inDegreesPerSecondPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES

inline val DerivativeGain<
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRotationsPerMinutePerSecond: Double
get() = inDegreesPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
Velocity<Radian>, Velocity<Radian>>.inDegreesPerSecondPerRotationsPerMinutePerSecond: Double
get() = inDegreesPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
4 changes: 0 additions & 4 deletions src/test/kotlin/team4099/controller/PIDControllerTest.kt
Original file line number Diff line number Diff line change
@@ -1,22 +1,18 @@
package com.team4099.controller

import org.junit.jupiter.api.Test
import org.team4099.lib.controller.PIDController
import org.team4099.lib.controller.ProfiledPIDController
import org.team4099.lib.controller.SimpleMotorFeedforward
import org.team4099.lib.controller.TrapezoidProfile
import org.team4099.lib.units.Acceleration
import org.team4099.lib.units.Fraction
import org.team4099.lib.units.LinearMechanismSensor
import org.team4099.lib.units.Timescale
import org.team4099.lib.units.Value
import org.team4099.lib.units.base.Meter
import org.team4099.lib.units.base.Second
import org.team4099.lib.units.base.amps
import org.team4099.lib.units.base.inches
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.base.seconds
import org.team4099.lib.units.derived.ProportionalGain
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inVoltsPerInch
import org.team4099.lib.units.derived.inVoltsPerRotation
Expand Down
10 changes: 5 additions & 5 deletions vendordeps/PathPlanner.json
Original file line number Diff line number Diff line change
@@ -1,26 +1,26 @@
{
"fileName": "PathplannerLib.json",
"fileName": "PathplannerLib2024.json",
"name": "PathplannerLib",
"version": "2024.2.1",
"version": "2024.2.8",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2024",
"mavenUrls": [
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
],
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib2024.json",
"javaDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
"version": "2024.2.1"
"version": "2024.2.8"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
"version": "2024.2.1",
"version": "2024.2.8",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand Down
Loading

0 comments on commit 6968c57

Please sign in to comment.