Skip to content

Commit a2baa06

Browse files
tervaytervay
committed
Fix DCMotor equations to use freeCurrent
1 parent 95cb38e commit a2baa06

File tree

8 files changed

+274
-35
lines changed

8 files changed

+274
-35
lines changed

‎wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ public DCMotor(
6868
this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
6969
this.KvRadPerSecPerVolt =
7070
freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
71-
this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
71+
this.KtNMPerAmp = this.stallTorqueNewtonMeters / (this.stallCurrentAmps - this.freeCurrentAmps);
7272
}
7373

7474
/**
@@ -89,7 +89,7 @@ public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
8989
* @return The current drawn by the motor.
9090
*/
9191
public double getCurrent(double torqueNm) {
92-
return torqueNm / KtNMPerAmp;
92+
return torqueNm / KtNMPerAmp + freeCurrentAmps;
9393
}
9494

9595
/**
@@ -99,7 +99,7 @@ public double getCurrent(double torqueNm) {
9999
* @return The torque output.
100100
*/
101101
public double getTorque(double currentAmpere) {
102-
return currentAmpere * KtNMPerAmp;
102+
return (currentAmpere - freeCurrentAmps) * KtNMPerAmp;
103103
}
104104

105105
/**
@@ -110,7 +110,7 @@ public double getTorque(double currentAmpere) {
110110
* @return The voltage of the motor.
111111
*/
112112
public double getVoltage(double torqueNm, double speedRadiansPerSec) {
113-
return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm;
113+
return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm + freeCurrentAmps * rOhms;
114114
}
115115

116116
/**
@@ -122,7 +122,7 @@ public double getVoltage(double torqueNm, double speedRadiansPerSec) {
122122
*/
123123
public double getSpeed(double torqueNm, double voltageInputVolts) {
124124
return voltageInputVolts * KvRadPerSecPerVolt
125-
- 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
125+
- 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt - KvRadPerSecPerVolt * freeCurrentAmps * rOhms;
126126
}
127127

128128
/**

‎wpimath/src/main/native/include/frc/system/plant/DCMotor.h‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,7 @@ class WPILIB_DLLEXPORT DCMotor {
7171
freeSpeed(freeSpeed),
7272
R(nominalVoltage / this->stallCurrent),
7373
Kv(freeSpeed / (nominalVoltage - R * this->freeCurrent)),
74-
Kt(this->stallTorque / this->stallCurrent) {}
74+
Kt(this->stallTorque / (this->stallCurrent - this->freeCurrent)) {}
7575

7676
/**
7777
* Returns current drawn by motor with given speed and input voltage.
@@ -90,7 +90,7 @@ class WPILIB_DLLEXPORT DCMotor {
9090
* @param torque The torque produced by the motor.
9191
*/
9292
constexpr units::ampere_t Current(units::newton_meter_t torque) const {
93-
return torque / Kt;
93+
return torque / Kt + freeCurrent;
9494
}
9595

9696
/**
@@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT DCMotor {
9999
* @param current The current drawn by the motor.
100100
*/
101101
constexpr units::newton_meter_t Torque(units::ampere_t current) const {
102-
return current * Kt;
102+
return (current - freeCurrent) * Kt;
103103
}
104104

105105
/**
@@ -111,7 +111,7 @@ class WPILIB_DLLEXPORT DCMotor {
111111
*/
112112
constexpr units::volt_t Voltage(units::newton_meter_t torque,
113113
units::radians_per_second_t speed) const {
114-
return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
114+
return 1.0 / Kv * speed + 1.0 / Kt * R * torque + freeCurrent * R;
115115
}
116116

117117
/**
@@ -123,7 +123,8 @@ class WPILIB_DLLEXPORT DCMotor {
123123
*/
124124
constexpr units::radians_per_second_t Speed(
125125
units::newton_meter_t torque, units::volt_t inputVoltage) const {
126-
return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
126+
return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv -
127+
Kv * freeCurrent * R;
127128
}
128129

129130
/**

‎wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,8 @@ void testLQROnElevator() {
3333

3434
var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
3535

36-
assertEquals(522.87006795347486, K.get(0, 0), 1e-6);
37-
assertEquals(38.239878385020411, K.get(0, 1), 1e-6);
36+
assertEquals(522.59, K.get(0, 0), 1e-2);
37+
assertEquals(38.17, K.get(0, 1), 1e-2);
3838
}
3939

4040
@Test
@@ -49,7 +49,7 @@ void testFourMotorElevator() {
4949
new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt)
5050
.getK();
5151

52-
assertEquals(10.381, K.get(0, 0), 1e-2);
52+
assertEquals(10.365, K.get(0, 0), 1e-2);
5353
assertEquals(0.6929, K.get(0, 1), 1e-2);
5454
}
5555

@@ -69,8 +69,8 @@ void testLQROnArm() {
6969

7070
var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
7171

72-
assertEquals(19.339349883583761, K.get(0, 0), 1e-6);
73-
assertEquals(3.3542559517421582, K.get(0, 1), 1e-6);
72+
assertEquals(19.247, K.get(0, 0), 1e-2);
73+
assertEquals(3.335, K.get(0, 1), 1e-2);
7474
}
7575

7676
/**

‎wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,14 @@ void testDrivetrainVelocitySystem() {
2424
model
2525
.getA()
2626
.isEqual(
27-
MatBuilder.fill(Nat.N2(), Nat.N2(), -10.14132, 3.06598, 3.06598, -10.14132),
27+
MatBuilder.fill(Nat.N2(), Nat.N2(), -10.3182, 3.11946, 3.11946, -10.3182),
2828
0.001));
2929

3030
assertTrue(
3131
model
3232
.getB()
3333
.isEqual(
34-
MatBuilder.fill(Nat.N2(), Nat.N2(), 4.2590, -1.28762, -1.2876, 4.2590), 0.001));
34+
MatBuilder.fill(Nat.N2(), Nat.N2(), 4.33333, -1.31008, -1.31008, 4.33333), 0.001));
3535

3636
assertTrue(
3737
model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), 0.001));
@@ -44,9 +44,9 @@ void testDrivetrainVelocitySystem() {
4444
void testElevatorSystem() {
4545
var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
4646
assertTrue(
47-
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001));
47+
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -100.782), 0.001));
4848

49-
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
49+
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 21.1628), 0.001));
5050

5151
assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), 0.001));
5252

@@ -56,9 +56,9 @@ void testElevatorSystem() {
5656
@Test
5757
void testFlywheelSystem() {
5858
var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
59-
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
59+
assertTrue(model.getA().isEqual(VecBuilder.fill(-27.339), 0.001));
6060

61-
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
61+
assertTrue(model.getB().isEqual(VecBuilder.fill(1377.785), 0.001));
6262

6363
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
6464

@@ -69,9 +69,9 @@ void testFlywheelSystem() {
6969
void testDCMotorSystem() {
7070
var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
7171
assertTrue(
72-
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001));
72+
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -27.339), 0.001));
7373

74-
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
74+
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1377.785), 0.001));
7575

7676
assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001));
7777

‎wpimath/src/test/java/edu/wpi/first/math/system/plant/DCMotorTest.java‎

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package edu.wpi.first.math.system.plant;
6+
7+
import static org.junit.jupiter.api.Assertions.assertEquals;
8+
9+
import java.util.function.Supplier;
10+
import java.util.stream.Stream;
11+
import org.junit.jupiter.params.ParameterizedTest;
12+
import org.junit.jupiter.params.provider.Arguments;
13+
import org.junit.jupiter.params.provider.MethodSource;
14+
15+
class DCMotorTest {
16+
private static final double kTolerance = 1e-12;
17+
18+
static Stream<Arguments> motors() {
19+
return Stream.of(
20+
Arguments.of("CIM", (Supplier<DCMotor>) () -> DCMotor.getCIM(1)),
21+
Arguments.of("MiniCIM", (Supplier<DCMotor>) () -> DCMotor.getMiniCIM(1)),
22+
Arguments.of("Bag", (Supplier<DCMotor>) () -> DCMotor.getBag(1)),
23+
Arguments.of("Vex775Pro", (Supplier<DCMotor>) () -> DCMotor.getVex775Pro(1)),
24+
Arguments.of("AndymarkRs775_125", (Supplier<DCMotor>) () -> DCMotor.getAndymarkRs775_125(1)),
25+
Arguments.of("BanebotsRs775", (Supplier<DCMotor>) () -> DCMotor.getBanebotsRs775(1)),
26+
Arguments.of("Andymark9015", (Supplier<DCMotor>) () -> DCMotor.getAndymark9015(1)),
27+
Arguments.of("BanebotsRs550", (Supplier<DCMotor>) () -> DCMotor.getBanebotsRs550(1)),
28+
Arguments.of("NEO", (Supplier<DCMotor>) () -> DCMotor.getNEO(1)),
29+
Arguments.of("Neo550", (Supplier<DCMotor>) () -> DCMotor.getNeo550(1)),
30+
Arguments.of("Falcon500", (Supplier<DCMotor>) () -> DCMotor.getFalcon500(1)),
31+
Arguments.of("Falcon500Foc", (Supplier<DCMotor>) () -> DCMotor.getFalcon500Foc(1)),
32+
Arguments.of("RomiBuiltIn", (Supplier<DCMotor>) () -> DCMotor.getRomiBuiltIn(1)),
33+
Arguments.of("KrakenX60", (Supplier<DCMotor>) () -> DCMotor.getKrakenX60(1)),
34+
Arguments.of("KrakenX60Foc", (Supplier<DCMotor>) () -> DCMotor.getKrakenX60Foc(1)),
35+
Arguments.of("KrakenX44", (Supplier<DCMotor>) () -> DCMotor.getKrakenX44(1)),
36+
Arguments.of("KrakenX44Foc", (Supplier<DCMotor>) () -> DCMotor.getKrakenX44Foc(1)),
37+
Arguments.of("Minion", (Supplier<DCMotor>) () -> DCMotor.getMinion(1)),
38+
Arguments.of("NeoVortex", (Supplier<DCMotor>) () -> DCMotor.getNeoVortex(1)));
39+
}
40+
41+
@ParameterizedTest(name = "{0}")
42+
@MethodSource("motors")
43+
void testCurrentAtZeroSpeedAndNominalVoltageIsStallCurrent(
44+
String name, Supplier<DCMotor> motorFactory) {
45+
var motor = motorFactory.get();
46+
assertEquals(
47+
motor.stallCurrentAmps,
48+
motor.getCurrent(0.0, motor.nominalVoltageVolts),
49+
kTolerance);
50+
}
51+
52+
@ParameterizedTest(name = "{0}")
53+
@MethodSource("motors")
54+
void testCurrentAtFreeSpeedAndNominalVoltageIsFreeCurrent(
55+
String name, Supplier<DCMotor> motorFactory) {
56+
var motor = motorFactory.get();
57+
assertEquals(
58+
motor.freeCurrentAmps,
59+
motor.getCurrent(motor.freeSpeedRadPerSec, motor.nominalVoltageVolts),
60+
kTolerance);
61+
}
62+
63+
@ParameterizedTest(name = "{0}")
64+
@MethodSource("motors")
65+
void testCurrentAtStallTorqueIsStallCurrent(String name, Supplier<DCMotor> motorFactory) {
66+
var motor = motorFactory.get();
67+
assertEquals(motor.stallCurrentAmps, motor.getCurrent(motor.stallTorqueNewtonMeters), kTolerance);
68+
}
69+
70+
@ParameterizedTest(name = "{0}")
71+
@MethodSource("motors")
72+
void testCurrentAtZeroTorqueIsFreeCurrent(String name, Supplier<DCMotor> motorFactory) {
73+
var motor = motorFactory.get();
74+
assertEquals(motor.freeCurrentAmps, motor.getCurrent(0.0), kTolerance);
75+
}
76+
77+
@ParameterizedTest(name = "{0}")
78+
@MethodSource("motors")
79+
void testTorqueAtFreeCurrentIsZero(String name, Supplier<DCMotor> motorFactory) {
80+
var motor = motorFactory.get();
81+
assertEquals(0.0, motor.getTorque(motor.freeCurrentAmps), kTolerance);
82+
}
83+
84+
@ParameterizedTest(name = "{0}")
85+
@MethodSource("motors")
86+
void testTorqueAtStallCurrentIsStallTorque(String name, Supplier<DCMotor> motorFactory) {
87+
var motor = motorFactory.get();
88+
assertEquals(
89+
motor.stallTorqueNewtonMeters, motor.getTorque(motor.stallCurrentAmps), kTolerance);
90+
}
91+
92+
@ParameterizedTest(name = "{0}")
93+
@MethodSource("motors")
94+
void testVoltageAtStallTorqueAndZeroSpeedIsNominalVoltage(
95+
String name, Supplier<DCMotor> motorFactory) {
96+
var motor = motorFactory.get();
97+
assertEquals(
98+
motor.nominalVoltageVolts,
99+
motor.getVoltage(motor.stallTorqueNewtonMeters, 0.0),
100+
kTolerance);
101+
}
102+
103+
@ParameterizedTest(name = "{0}")
104+
@MethodSource("motors")
105+
void testVoltageAtZeroTorqueAndFreeSpeedIsNominalVoltage(
106+
String name, Supplier<DCMotor> motorFactory) {
107+
var motor = motorFactory.get();
108+
assertEquals(
109+
motor.nominalVoltageVolts, motor.getVoltage(0.0, motor.freeSpeedRadPerSec), kTolerance);
110+
}
111+
112+
@ParameterizedTest(name = "{0}")
113+
@MethodSource("motors")
114+
void testSpeedAtStallTorqueAndNominalVoltageIsZero(
115+
String name, Supplier<DCMotor> motorFactory) {
116+
var motor = motorFactory.get();
117+
assertEquals(
118+
0.0, motor.getSpeed(motor.stallTorqueNewtonMeters, motor.nominalVoltageVolts), kTolerance);
119+
}
120+
121+
@ParameterizedTest(name = "{0}")
122+
@MethodSource("motors")
123+
void testSpeedAtZeroTorqueAndNominalVoltageIsFreeSpeed(
124+
String name, Supplier<DCMotor> motorFactory) {
125+
var motor = motorFactory.get();
126+
assertEquals(
127+
motor.freeSpeedRadPerSec, motor.getSpeed(0.0, motor.nominalVoltageVolts), kTolerance);
128+
}
129+
}
130+

‎wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,8 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
3333
Matrixd<1, 2> K =
3434
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K();
3535

36-
EXPECT_NEAR(522.87006795347486, K(0, 0), 1e-6);
37-
EXPECT_NEAR(38.239878385020411, K(0, 1), 1e-6);
36+
EXPECT_NEAR(522.58976745337009, K(0, 0), 1e-6);
37+
EXPECT_NEAR(38.170344262642963, K(0, 1), 1e-6);
3838
}
3939

4040
TEST(LinearQuadraticRegulatorTest, ArmGains) {
@@ -59,8 +59,8 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
5959
LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5_ms}
6060
.K();
6161

62-
EXPECT_NEAR(19.339349883583761, K(0, 0), 1e-6);
63-
EXPECT_NEAR(3.3542559517421582, K(0, 1), 1e-6);
62+
EXPECT_NEAR(19.24674874537445, K(0, 0), 1e-6);
63+
EXPECT_NEAR(3.3352730789607676, K(0, 1), 1e-6);
6464
}
6565

6666
TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {

‎wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,9 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
2121
#endif
2222

2323
ASSERT_TRUE(model.A().isApprox(
24-
frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
24+
frc::Matrixd<2, 2>{{-10.3182, 3.11946}, {3.11946, -10.3182}}, 0.001));
2525
ASSERT_TRUE(model.B().isApprox(
26-
frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
26+
frc::Matrixd<2, 2>{{4.33333, -1.31008}, {-1.31008, 4.33333}}, 0.001));
2727
ASSERT_TRUE(
2828
model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
2929
ASSERT_TRUE(
@@ -35,8 +35,8 @@ TEST(LinearSystemIDTest, ElevatorSystem) {
3535
0.05_m, 12)
3636
.Slice(0);
3737
ASSERT_TRUE(model.A().isApprox(
38-
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
39-
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));
38+
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -100.782}}, 0.001));
39+
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 21.1628}, 0.001));
4040
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001));
4141
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
4242
}
@@ -50,8 +50,8 @@ TEST(LinearSystemIDTest, FlywheelSystem) {
5050
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
5151
#endif
5252

53-
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
54-
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
53+
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-27.339}, 0.001));
54+
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1377.79}, 0.001));
5555
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
5656
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
5757
}
@@ -66,8 +66,8 @@ TEST(LinearSystemIDTest, DCMotorSystem) {
6666
#endif
6767

6868
ASSERT_TRUE(
69-
model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
70-
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
69+
model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -27.339}}, 0.001));
70+
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1377.79}, 0.001));
7171
ASSERT_TRUE(
7272
model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
7373
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001));

0 commit comments

Comments
 (0)