From 2ac9b30221b7d2947cc5c1a7f79bb18acc18191d Mon Sep 17 00:00:00 2001 From: taimorioka Date: Tue, 10 Dec 2024 18:43:18 -0800 Subject: [PATCH 1/3] fix: all of the rev swerve units issues --- ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat | Bin 0 -> 2048 bytes simgui-ds.json | 97 ++++++++++++++++++ simgui.json | 23 +++++ src/main/java/BuildConstants.java | 17 +++ .../frc2024/subsystems/drivetrain/Module.kt | 63 +++++++----- 7 files changed, 174 insertions(+), 26 deletions(-) create mode 100644 ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/BuildConstants.java diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..c20292e361ef5b600250315b2dc279022938171c GIT binary patch literal 2048 zcmdT_Pe{{o6n}0GGYc1`cxp%yBw^E)iDeEc=#VfW!lQI3XqBTs5El6hjIv9@Km;M` z645~?33+RQV2;X+4j!}yh6EiFV_}wC-}im*xb#@)j-BA z$*A`jljFS__iFr>J8isKF=3l=Rb7z}J35(PqKICzE7l`CIgkD3gdBH79zEX$4^M0SDflQ4Q^&ke z&oDe6=5Uk422tVKMZ7y2zPQ`CgG+)RbgVE$JqzIz*(Kg#4!K-Ln6e{C&_oAEAxZ5}d#{QJnw!2=033Fio<@EPnV@0an`&)*vQA{ceCM|eUDCGzpeMx z%l&@N!EHI!-juA^oZ&sx`6d6E%_e#^ApIuVst4|yy*Eb0&c{3UQ}%aw&ydVlCciTo i`>mP3=e`n)54?op#i`8t9nFuW#U8yOdcqg8c)tO70ZaP; literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..cc54e1820aeaec5920ea0e8bfd3cbead3779351b GIT binary patch literal 2048 zcmdT^O-Phs5FU3G69X4?t&=H5kVI)KCH>IHQWR0J5p*p&RA|^12_Zq>!N@F3S}0*5 zGDtevN*?Nz2SJg_A38+1va!*jOHnQUly398>xvXumtN5MX6BihXXc%E_!0y@ z!zx7s@B!3(%QNlYN_Vg2TV)?BkdyHKzve#*fz(3L3L4jt*RVbH^Dp8(nkU&?H9m6{ zAbZ(sSvO|2`VrpI(@Yy-aT~mmBGN;7A`KtVkId^lJ2S%9AjONVr1xa;qXFU5aL1m+EeD1HWIJhvhekVW;d7h1QgzDktIG=6U+&mcI`~P0-|t zr-Of%-=xi}qD$B=_qefO_Pm#c$H->*{4+4*rn&u@V0{`jO_hhIn?*Z=?k literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..a7c2dd412624ff20d1f81e2969aef364fc27dba0 GIT binary patch literal 2048 zcmdT^Pe{~J5T5<*ikU(yfq9733Ly{v+br8!T%v9*tkj{SbP3wbr9U7R_zMiO(}EGk zLL`wyhe#~wmWE+TVMd6D(1i^Nx`<(EVB4EF^X;xe1lFZ@ncvKO-@I?;z4x1=*+Qa) z!(`Vhwc-)^iR!<4;Qd?cwxHf7d~CqZ(%gTof0RN?&gKm?u9O!PT=M+|-_bhhM^#hH z-9)*=F1J3EHRwlt|KKIQ5#a6i_7$-o^@s&NpdZoN35#zvgd<2A3qE-1nu6*Tv%g;x z8LxoBFEB3W6PovH{+joviRP9`TSTXZ;h;z2(s3UeU=oj++~9*Ob=XqQ21| z;Qc-*9(+ynCJyJ{E%pW39&8*cV{1y<#d%=a#-6Pn;(RML+3*ETjyNNBK18AmbB;U_ zJi}P0TQ+3pOxib*_+yUPo~4dHo^JwmOC+9BT^i0B;I3;VCsuUyev^7dnjasA9@Syu ztP6aGaewH;OD;P^zO{pRe>8S}j|qpDrGA((c?6rUC_GWKh(z@pr7c$M}AM>PX;{z}!ow!GAD&LbboJz(CjnAgr}nHToyS&@5#``V`a zG*=C5L%;d8OY$7v2fT#VLHg50;RDZ>>%j*e&+lsRvb~R}hdS-IEB%S>;zdriwS*&0 z_I&r%3m!P|!Wsz0Wq + var velocity: Measure> } class DrivingTalon(id: CTREDeviceId) : DrivingMotor { @@ -121,8 +129,7 @@ class DrivingTalon(id: CTREDeviceId) : DrivingMotor { TorqueCurrentConfigs().apply { withPeakForwardTorqueCurrent(DRIVING_CURRENT_LIMIT) withPeakReverseTorqueCurrent(-DRIVING_CURRENT_LIMIT) - } - ) + }) } @@ -130,17 +137,17 @@ class DrivingTalon(id: CTREDeviceId) : DrivingMotor { Robot.statusSignals[id.name] = inner.version } - override val position: Double - get() = inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE + override val position: Measure + get() = Meters.of(inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE) - override var velocity: Double - get() = inner.velocity.value + override var velocity: Measure> + get() = MetersPerSecond.of(inner.velocity.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE) set(value) { - inner.setControl(VelocityTorqueCurrentFOC(value / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE).withSlot(0)) + inner.setControl(VelocityTorqueCurrentFOC(value.`in`(MetersPerSecond) / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE)) } } -class DrivingSparkMAX(id: REVMotorControllerId) : DrivingMotor { +class DrivingSparkMAX(val id: REVMotorControllerId) : DrivingMotor { private val inner = CANSparkMax(id, CANSparkLowLevel.MotorType.kBrushless).apply { restoreFactoryDefaults() @@ -153,8 +160,8 @@ class DrivingSparkMAX(id: REVMotorControllerId) : DrivingMotor { init { inner.encoder.apply { // convert native units of rotations and RPM to meters and meters per second - positionConversionFactor = DRIVING_GEAR_RATIO_NEO * WHEEL_CIRCUMFERENCE - velocityConversionFactor = DRIVING_GEAR_RATIO_NEO * WHEEL_CIRCUMFERENCE / 60 + positionConversionFactor = WHEEL_CIRCUMFERENCE / DRIVING_GEAR_RATIO_NEO + velocityConversionFactor = WHEEL_CIRCUMFERENCE / DRIVING_GEAR_RATIO_NEO / 60 } inner.pidController.apply { @@ -165,13 +172,14 @@ class DrivingSparkMAX(id: REVMotorControllerId) : DrivingMotor { } } - override val position: Double - get() = inner.encoder.position + override val position: Measure + get() = Meters.of(inner.encoder.position) - override var velocity: Double - get() = inner.encoder.velocity + override var velocity: Measure> + get() = MetersPerSecond.of(inner.encoder.velocity) set(value) { - inner.set(value) + Logger.recordOutput("/Drivetrain/$id/OutputVel", value) + inner.pidController.setReference(value.`in`(MetersPerSecond), CANSparkBase.ControlType.kVelocity) } } @@ -220,20 +228,23 @@ class SimSwerveModule : SwerveModule { // take the known wheel diameter, divide it by two to get the radius, then get the // circumference -internal val WHEEL_RADIUS = Units.inchesToMeters(3.0) / 2 -internal val WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * TAU +internal val WHEEL_RADIUS = Inches.of(3.0).`in`(Meters) / 2 +internal val WHEEL_CIRCUMFERENCE = Meters.of(WHEEL_RADIUS * TAU) -internal const val NEO_FREE_SPEED_RPM = 5676.0 -internal const val NEO_FREE_SPEED_RPS: Double = NEO_FREE_SPEED_RPM / 60 +internal val NEO_FREE_SPEED = RPM.of(5676.0) + +private const val DRIVING_MOTOR_PINION_TEETH = 14 internal const val DRIVING_GEAR_RATIO_TALON = 1.0 / 3.56 -internal const val DRIVING_GEAR_RATIO_NEO = 0.0 +internal const val DRIVING_GEAR_RATIO_NEO = (45.0 * 22.0) / (DRIVING_MOTOR_PINION_TEETH * 15.0) + +internal val NEO_DRIVING_FREE_SPEED = (NEO_FREE_SPEED.`in`(RotationsPerSecond) * WHEEL_CIRCUMFERENCE.`in`(Meters)) / DRIVING_GEAR_RATIO_NEO internal val DRIVING_PID_GAINS_TALON: PIDGains = PIDGains(4.0, 0.0, 0.1) internal val DRIVING_PID_GAINS_NEO: PIDGains = PIDGains(0.04, 0.0, 0.0) internal val DRIVING_FF_GAINS_TALON: MotorFFGains = MotorFFGains(5.75, 0.0) internal val DRIVING_FF_GAINS_NEO: MotorFFGains = - MotorFFGains(0.0, 1 / NEO_FREE_SPEED_RPS, 0.0) // TODO: ensure this is right + MotorFFGains(0.0, 1 / NEO_DRIVING_FREE_SPEED, 0.0) // TODO: ensure this is right internal val TURNING_PID_GAINS: PIDGains = PIDGains(1.7, 0.0, 0.125) internal const val DRIVING_CURRENT_LIMIT = 35.0 From e8301f29ed950f4647ff194aef33bf68432ee7ce Mon Sep 17 00:00:00 2001 From: Gavin Niederman Date: Tue, 10 Dec 2024 18:54:41 -0800 Subject: [PATCH 2/3] fix: remove old build constatns --- src/main/java/BuildConstants.java | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 src/main/java/BuildConstants.java diff --git a/src/main/java/BuildConstants.java b/src/main/java/BuildConstants.java deleted file mode 100644 index 888b298..0000000 --- a/src/main/java/BuildConstants.java +++ /dev/null @@ -1,17 +0,0 @@ -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "bunnybots-2024"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 43; - public static final String GIT_SHA = "6a97221d7d42a8b0b17af79a831dc75ee3d9e173"; - public static final String GIT_DATE = "2024-11-20 16:28:24 PST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2024-11-20 16:50:15 PST"; - public static final long BUILD_UNIX_TIME = 1732150215462L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} From d3d22d66f62902bbd65c7e795d335ae2eb40443b Mon Sep 17 00:00:00 2001 From: Gavin Niederman Date: Tue, 10 Dec 2024 18:59:06 -0800 Subject: [PATCH 3/3] fix: goop? --- .../frc2024/subsystems/drivetrain/Module.kt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt index 97dcf82..b11f8e3 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt @@ -138,12 +138,12 @@ class DrivingTalon(id: CTREDeviceId) : DrivingMotor { } override val position: Measure - get() = Meters.of(inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE) + get() = Meters.of(inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters)) override var velocity: Measure> - get() = MetersPerSecond.of(inner.velocity.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE) + get() = MetersPerSecond.of(inner.velocity.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters)) set(value) { - inner.setControl(VelocityTorqueCurrentFOC(value.`in`(MetersPerSecond) / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE)) + inner.setControl(VelocityTorqueCurrentFOC(value.`in`(MetersPerSecond) / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE.`in`(Meters))) } } @@ -160,8 +160,8 @@ class DrivingSparkMAX(val id: REVMotorControllerId) : DrivingMotor { init { inner.encoder.apply { // convert native units of rotations and RPM to meters and meters per second - positionConversionFactor = WHEEL_CIRCUMFERENCE / DRIVING_GEAR_RATIO_NEO - velocityConversionFactor = WHEEL_CIRCUMFERENCE / DRIVING_GEAR_RATIO_NEO / 60 + positionConversionFactor = WHEEL_CIRCUMFERENCE.`in`(Meters) / DRIVING_GEAR_RATIO_NEO + velocityConversionFactor = WHEEL_CIRCUMFERENCE.`in`(Meters) / DRIVING_GEAR_RATIO_NEO / 60 } inner.pidController.apply {