diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 5f05d8c7..65e3dc08 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -60,7 +60,7 @@ tasks.register("setupRustLibs", Exec) { doFirst { println "Executing task before tests..." println "$projectDir/src/main/rust/rerun" - } + } doLast { copy { @@ -130,6 +130,19 @@ dependencies { implementation(name: 'ftclib-2.1.1', ext: 'aar') testImplementation 'junit:junit:4.13.2' + + def lwjglVersion = "3.3.3" + def lwjglNatives = "natives-macos" + testImplementation "org.lwjgl:lwjgl:$lwjglVersion" + testImplementation "org.lwjgl:lwjgl-glfw:$lwjglVersion" + testRuntimeOnly "org.lwjgl:lwjgl:$lwjglVersion:$lwjglNatives" + testRuntimeOnly "org.lwjgl:lwjgl-glfw:$lwjglVersion:$lwjglNatives" +} + +tasks.withType(Test) { + testLogging { + showStandardStreams = true + } } // ./gradlew cargoBuild needs to run every time the rust library updates diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/Limelight.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/Limelight.kt index 9f293c96..362369e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/Limelight.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/Limelight.kt @@ -134,7 +134,9 @@ class Limelight(val robot: Robot): SubsystemBase() { if (x != null && y != null) { telemetry.addData("LL Raw Pos", "(${String.format("%.2f", x*39.37)}in, ${String.format("%.2f", y*39.37)}in)", TelemetryGroups.LIMELIGHT) } - telemetry.addData("LL Given Angle", (hardware as HardwareIO).limelightIO.currentYaw, TelemetryGroups.LIMELIGHT) + if (hardware is HardwareIO) { + telemetry.addData("LL Given Angle", (hardware as HardwareIO).limelightIO.currentYaw, TelemetryGroups.LIMELIGHT) + } telemetry.addData("LL Latency", "${String.format("%.1f", hardware.limelightLatencyMs())}ms", TelemetryGroups.LIMELIGHT) val acceptRate = if (totalMeasurements > 0) (acceptedMeasurements * 100 / totalMeasurements) else 0 telemetry.addData("LL Accept Rate", "$acceptRate% ($acceptedMeasurements/$totalMeasurements)", TelemetryGroups.LIMELIGHT) diff --git a/TeamCode/src/main/jniLibs/librerun.dylib b/TeamCode/src/main/jniLibs/librerun.dylib index 742f5f26..ef480b7e 100755 Binary files a/TeamCode/src/main/jniLibs/librerun.dylib and b/TeamCode/src/main/jniLibs/librerun.dylib differ diff --git a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/Mecanum.kt b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/Mecanum.kt index b70b4ec8..e7ef64a7 100644 --- a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/Mecanum.kt +++ b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/Mecanum.kt @@ -35,7 +35,8 @@ class Mecanum(val simIO: SimIO, val mecanumParams: MecanumParams) { body.mass = bodyMass body.setPosition(0.0, 0.0, mecanumParams.robotHeight / 2.0) body.gravityMode = false - body.linearDamping = 0.035 + body.linearDamping = 0.05 + body.angularDamping = 0.5 bodyGeom.body = body bodyGeom.setPosition(body.position) } @@ -46,69 +47,83 @@ class Mecanum(val simIO: SimIO, val mecanumParams: MecanumParams) { var heading = 0.0; fun physicsTick(frontRight: Double, frontLeft: Double, backRight: Double, backLeft: Double) { - val frontRightForce = calcMotorForce(frontRight) - val frontLeftForce = calcMotorForce(frontLeft) - val backRightForce = calcMotorForce(backRight) - val backLeftForce = calcMotorForce(backLeft) - - var x_force = (frontLeftForce - frontRightForce - backLeftForce + backRightForce) * (sqrt(2.0) /2.0) - var y_force = (frontLeftForce + frontRightForce + backLeftForce + backRightForce) * (sqrt(2.0) /2.0) - - // todo use forces for rotation - val rotation_power = -frontLeftForce + frontRightForce - backLeftForce + backRightForce - - - // todo actually calculate friction -// val frictionForce = params.mass * 9.81 * params.frictionCoeff -// if (posVel.x != 0.0) { -// x_force += (if (posVel.x > 0) { -frictionForce } else { frictionForce }) -// if (posVel.x.absoluteValue < 1e-2) { -// x_force = 0.0 -// posVel.x = 0.0 -// } -// } -// if (posVel.y != 0.0) { -// y_force += (if (posVel.y > 0) { -frictionForce } else { frictionForce }) -// if (posVel.y.absoluteValue < 1e-2) { -// y_force = 0.0 -// posVel.y = 0.0 -// } -// } - + // Get robot-local velocity from world velocity + val worldVx = body.linearVel.get0() + val worldVy = body.linearVel.get1() + val localVx = worldVx * cos(-heading) - worldVy * sin(-heading) + val localVy = worldVx * sin(-heading) + worldVy * cos(-heading) + val angVel = body.angularVel.get2() + + // Half of wheelbase dimensions for wheel position offsets + val halfWidth = params.wheelBase.x / 2.0 + val halfDepth = params.wheelBase.y / 2.0 + + // Compute each wheel's ground speed along its roller direction + // Mecanum: wheel ground speed = (forward +/- strafe +/- turn contribution) projected onto roller axis + val flSpeed = (localVy + localVx + (halfWidth + halfDepth) * angVel) + val frSpeed = (localVy - localVx - (halfWidth + halfDepth) * angVel) + val blSpeed = (localVy - localVx + (halfWidth + halfDepth) * angVel) + val brSpeed = (localVy + localVx - (halfWidth + halfDepth) * angVel) + + val frontLeftForce = calcMotorForce(frontLeft, flSpeed) + val frontRightForce = calcMotorForce(frontRight, frSpeed) + val backLeftForce = calcMotorForce(backLeft, blSpeed) + val backRightForce = calcMotorForce(backRight, brSpeed) + + val x_force = (frontLeftForce - frontRightForce - backLeftForce + backRightForce) * (sqrt(2.0) / 2.0) + val y_force = (frontLeftForce + frontRightForce + backLeftForce + backRightForce) * (sqrt(2.0) / 2.0) + val torque = (-frontLeftForce + frontRightForce - backLeftForce + backRightForce) * (halfWidth + halfDepth) * (sqrt(2.0) / 2.0) body.addForce( x_force * cos(heading) - y_force * sin(heading), x_force * sin(heading) + y_force * cos(heading), 0.0 - ); - body.setAngularVel(0.0, 0.0, rotation_power * params.frictionCoeff) + ) + body.addTorque(0.0, 0.0, torque) body.setPosition(body.position.get0(), body.position.get1(), mecanumParams.robotHeight / 2.0) } - fun calcMotorForce(power: Double): Double { + fun calcMotorForce(power: Double, wheelGroundSpeed: Double): Double { val torquePerAmp = params.stallTorque / params.stallCurrent - // back-emf (V per rad/s) - val backEmf = 12.0 / params.freeSpeed + val freeSpeedRadPerSec = params.freeSpeed * 2.0 * PI / 60.0 // RPM -> rad/s + val backEmfConstant = 12.0 / freeSpeedRadPerSec // V per rad/s at motor shaft val windingResistance = 12.0 / params.stallCurrent - // TODO: properly simulate back EMF -// val amps = (power*12.0 - backEmf * params.freeSpeed) / windingResistance - val amps = (power*12.0) / windingResistance - val torque = torquePerAmp * amps - - val motorForce = torque / params.wheelRadius + val wheelSpeed = wheelGroundSpeed / params.wheelRadius // wheel rad/s + val motorSpeed = wheelSpeed * params.gearRatio + val amps = ((power * 12.0 - backEmfConstant * motorSpeed) / windingResistance) + .coerceIn(-params.stallCurrent, params.stallCurrent) // clamp to physical limits + val motorTorque = torquePerAmp * amps + val wheelTorque = motorTorque * params.gearRatio - return motorForce + return wheelTorque / params.wheelRadius // N } + private var tickCount = 0L + private var lastVx = 0.0 + private var lastVy = 0.0 + fun tick(dt: Double): Double { // TODO actually set encoder stuff correctly - pos.x = body.position.get0() * Drivetrain.DriveConstants.TICKS_PER_METER - pos.y = body.position.get1() * Drivetrain.DriveConstants.TICKS_PER_METER + pos.x = body.position.get0() // meters + pos.y = body.position.get1() // meters val lastHeading = heading; heading = body.quaternion.toEuler().get2() + if (tickCount++ % 100 == 0L) { + val vx = body.linearVel.get0() + val vy = body.linearVel.get1() + val speed = sqrt(vx * vx + vy * vy) + val ax = if (dt > 0) (vx - lastVx) / dt else 0.0 + val ay = if (dt > 0) (vy - lastVy) / dt else 0.0 + val accel = sqrt(ax * ax + ay * ay) + println("Mecanum: pos=(%.2f, %.2f)m vel=(%.2f, %.2f)m/s speed=%.2fm/s accel=%.2fm/s² angVel=%.2frad/s" + .format(body.position.get0(), body.position.get1(), vx, vy, speed, accel, body.angularVel.get2())) + lastVx = vx + lastVy = vy + } + return heading - lastHeading } diff --git a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/MecanumParams.kt b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/MecanumParams.kt index f0516b2d..0f9d7fa2 100644 --- a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/MecanumParams.kt +++ b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/Drivetrain/MecanumParams.kt @@ -9,8 +9,9 @@ data class MecanumParams( val robotHeight: Double, val wheelRadius: Double, val wheelBase: Vector2d, - val stallTorque: Double, - val stallCurrent: Double, - val freeSpeed: Double, + val stallTorque: Double, // N·m (motor output, before external gearing) + val stallCurrent: Double, // Amps + val freeSpeed: Double, // RPM (motor output, before external gearing) + val gearRatio: Double = 1.0, // driven/driving (>1 = reduction) val frictionCoeff: Double = 0.05 ) \ No newline at end of file diff --git a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimGamepad.kt b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimGamepad.kt index 007ef182..82ea1d83 100644 --- a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimGamepad.kt +++ b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimGamepad.kt @@ -3,9 +3,7 @@ package org.firstinspires.ftc.test.SimCode import org.firstinspires.ftc.teamcode.io.ButtonType import org.firstinspires.ftc.teamcode.io.GamepadIO import org.firstinspires.ftc.teamcode.io.Input -import org.firstinspires.ftc.teamcode.io.TriggerType import org.lwjgl.glfw.GLFW -import org.lwjgl.glfw.GLFWGamepadState class SimGamepad: GamepadIO() { @@ -14,58 +12,77 @@ class SimGamepad: GamepadIO() { init { check(GLFW.glfwInit()) { "Unable to initialize GLFW" } + GLFW.glfwPollEvents() + + var found = false for (jid in GLFW.GLFW_JOYSTICK_1..GLFW.GLFW_JOYSTICK_LAST) { if (GLFW.glfwJoystickPresent(jid)) { - println("Joystick connected: " + GLFW.glfwGetJoystickName(jid)) - this.jid = jid; - break; + println("SimGamepad: Joystick $jid connected: ${GLFW.glfwGetJoystickName(jid)}") + val axes = GLFW.glfwGetJoystickAxes(jid) + val buttons = GLFW.glfwGetJoystickButtons(jid) + println("SimGamepad: Axes=${axes?.limit() ?: 0}, Buttons=${buttons?.limit() ?: 0}") + this.jid = jid + found = true + break } } + if (!found) { + println("SimGamepad: No joystick detected!") + } } private fun isControllerConnected(): Boolean { return GLFW.glfwJoystickPresent(jid) } -// -// + + // Logitech Dual Action (D-mode / DirectInput) mapping + // 4 axes: 0=LX, 1=LY, 2=RX, 3=RY + // 16 buttons: 0=X, 1=A, 2=B, 3=Y, 4=LB, 5=RB, 6=LT, 7=RT, + // 8=Back, 9=Start, 10=LS, 11=RS, 12=DUp, 13=DDown, 14=DLeft, 15=DRight fun buttonToButtonValue(button: ButtonType): Boolean { return GLFW.glfwGetJoystickButtons(jid)!!.get( when (button) { - ButtonType.A -> 0 - ButtonType.B -> 1 - ButtonType.X -> 2 + ButtonType.A -> 1 + ButtonType.B -> 2 + ButtonType.X -> 0 ButtonType.Y -> 3 - ButtonType.DPadUp -> 11 - ButtonType.DPadRight -> 12 + ButtonType.DPadUp -> 12 + ButtonType.DPadRight -> 15 ButtonType.DPadDown -> 13 ButtonType.DPadLeft -> 14 - ButtonType.Back -> 6 - ButtonType.Start -> 7 - ButtonType.Guide -> 10 + ButtonType.Back -> 8 + ButtonType.Start -> 9 + ButtonType.Guide -> 8 // no guide button in D-mode, map to back ButtonType.LeftBumper -> 4 ButtonType.RightBumper -> 5 - ButtonType.LeftStick -> 8 - ButtonType.RightStick -> 9 + ButtonType.LeftStick -> 10 + ButtonType.RightStick -> 11 } ).toInt() == 1 } fun inputToInputValue(input: Input): Double { - return GLFW.glfwGetJoystickAxes(jid)!!.get( - when (input) { - Input.LeftTrigger -> 2 - Input.RightTrigger -> 5 - Input.LeftStickX -> 0 - Input.LeftStickY -> 1 - Input.RightStickX -> 3 - Input.RightStickY -> 4 - } - ).toDouble() + val axes = GLFW.glfwGetJoystickAxes(jid)!! + val buttons = GLFW.glfwGetJoystickButtons(jid)!! + return when (input) { + // Triggers are buttons in D-mode, return 0.0 or 1.0 + Input.LeftTrigger -> buttons.get(6).toDouble() + Input.RightTrigger -> buttons.get(7).toDouble() + Input.LeftStickX -> axes.get(0).toDouble() + Input.LeftStickY -> axes.get(1).toDouble() + Input.RightStickX -> axes.get(2).toDouble() + Input.RightStickY -> axes.get(3).toDouble() + } } + private var tickCount = 0L + override fun tick() { + GLFW.glfwPollEvents() + // don't try to access the controller if it isn't connected if (!isControllerConnected()) { + if (tickCount++ % 100 == 0L) println("SimGamepad: Controller not connected (tick $tickCount)") return; } diff --git a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimIO.kt b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimIO.kt index 04bc987f..5fe879c1 100644 --- a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimIO.kt +++ b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimCode/SimIO.kt @@ -32,7 +32,8 @@ class SimIO: RobotIO { Vector2d(0.285, 0.335), 1.83, 9.2, - 435.0 + 435.0, + 16.0 / 24.0 // 24t driving 16t driven = step-up, wheel spins faster ) ) diff --git a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimTest.kt b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimTest.kt index 3953e6fa..a629a2b3 100644 --- a/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimTest.kt +++ b/TeamCode/src/test/kotlin/org/firstinspires/ftc/test/SimTest.kt @@ -19,18 +19,18 @@ class SimTest { val simIo = SimIO() val robot = Robot( simIo, - DummyGamepad(), SimGamepad(), + DummyGamepad(), SimTelemetry(), OpModeType.TELEOP - ); + ) RerunLogging.connect("sim", "rerun+http://127.0.0.1:9876/proxy").use { - while (true) { robot.run() it.logState(robot, simIo.field, simIo.mecanumSim.body.position.get2(), simIo) + Thread.sleep(10) // ~50Hz, similar to FTC loop rate } }