diff --git a/build.gradle b/build.gradle index 9ff5369..bbdead3 100644 --- a/build.gradle +++ b/build.gradle @@ -32,7 +32,7 @@ dependencies { } group = 'org.team5499' -version = '2.1.2' +version = '2.2.0' task sourcesJar(type: Jar) { from sourceSets.main.allJava diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/physics/DCMotorTransmission.kt b/src/main/kotlin/org/team5499/monkeyLib/math/physics/DCMotorTransmission.kt new file mode 100644 index 0000000..638f613 --- /dev/null +++ b/src/main/kotlin/org/team5499/monkeyLib/math/physics/DCMotorTransmission.kt @@ -0,0 +1,90 @@ +package org.team5499.monkeyLib.math.physics + +/** + * Model of a DC motor rotating a shaft. All parameters refer to the output (e.g. should already consider gearing + * and efficiency losses). The motor is assumed to be symmetric forward/reverse. + * + * @property speedPerVolt kV, or rad/s per V (no load) + * @property torquePerVolt N•m per V (stall) + * @property frictionVoltage the voltage needed to overcome static + */ +class DCMotorTransmission( + val speedPerVolt: Double, + val torquePerVolt: Double, + val frictionVoltage: Double +) { + // TODO add electrical constants? (e.g. current) + + /** + * Returns the idle speed of the motor at this voltage + * + * @param voltage The voltage across the motor + * @return The theoretical speed in rad/s + */ + @Suppress("ReturnCount") + fun freeSpeedAtVoltage(voltage: Double): Double { + if (voltage > 0.0) { + return Math.max(0.0, voltage - frictionVoltage) * speedPerVolt + } else if (0.0 > voltage) { + return Math.min(0.0, voltage + frictionVoltage) * speedPerVolt + } else { + return 0.0 + } + } + + /** + * Get the theoretical torque applied by the motor at a given speed and voltage + * + * @param outputSpeed The speed of the motor in rad/s + * @param voltage The voltage across the motor + * @return The theoretical torque in N•m + */ + fun getTorqueForVoltage(outputSpeed: Double, voltage: Double): Double { + var effectiveVoltage = voltage + if (outputSpeed > 0.0) { + // Forward motion, rolling friction. + effectiveVoltage -= frictionVoltage + } else if (0.0 > outputSpeed) { + // Reverse motion, rolling friction. + effectiveVoltage += frictionVoltage + } else if (voltage > 0.0) { + // System is static, forward torque. + effectiveVoltage = Math.max(0.0, voltage - frictionVoltage) + } else if (0.0 > voltage) { + // System is static, reverse torque. + effectiveVoltage = Math.min(0.0, voltage + frictionVoltage) + } else { + // System is idle. + return 0.0 + } + return torquePerVolt * (-outputSpeed / speedPerVolt + effectiveVoltage) + } + + /** + * Get the required voltage for the requested torque at a speed. + * + * @param outputSpeed The output speed of the motor in rad/s + * @param torque The output torque of the motor in N•m + * @return The theoretical voltage for the requested torque and speed + */ + fun getVoltageForTorque(outputSpeed: Double, torque: Double): Double { + var modifiedFrictionVoltage: Double = 0.0 + if (outputSpeed > 0.0) { + // Forward motion, rolling friction. + modifiedFrictionVoltage = frictionVoltage + } else if (0.0 > outputSpeed) { + // Reverse motion, rolling friction. + modifiedFrictionVoltage = -frictionVoltage + } else if (torque > 0.0) { + // System is static, forward torque. + modifiedFrictionVoltage = frictionVoltage + } else if (0.0 > torque) { + // System is static, reverse torque. + modifiedFrictionVoltage = -frictionVoltage + } else { + // System is idle. + return 0.0 + } + return torque / torquePerVolt + outputSpeed / speedPerVolt + modifiedFrictionVoltage + } +} diff --git a/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt b/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt new file mode 100644 index 0000000..095f546 --- /dev/null +++ b/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt @@ -0,0 +1,34 @@ +package tests.math.physics + +import org.junit.Test +import org.junit.Assert.assertEquals + +import org.team5499.monkeyLib.math.physics.DCMotorTransmission + +class DCMotorTransmissionTest { + val motor = DCMotorTransmission(1000.0, 0.5, 1.0) + val stallTorque = 5.5 // N•m + val freeSpeed = 11000.0 // rad/s + val epsilon = 0.1 + + @Test + fun stallTorqueTest() { + val calculatedStallTorque = motor.getTorqueForVoltage(0.0, 12.0) + println("Expected $stallTorque, but got $calculatedStallTorque.") + assertEquals(stallTorque, calculatedStallTorque, epsilon) + } + + @Test + fun freeSpeedTest() { + val calculatedFreeSpeed = motor.freeSpeedAtVoltage(12.0) + println("Expected $freeSpeed, but got $calculatedFreeSpeed.") + assertEquals(freeSpeed, calculatedFreeSpeed, epsilon) + } + + @Test + fun constantsTest() { + assertEquals(1000.0, motor.speedPerVolt, 0.0) + assertEquals(0.5, motor.torquePerVolt, 0.0) + assertEquals(1.0, motor.frictionVoltage, 0.0) + } +}