Skip to content

Commit 17efec0

Browse files
Ryan4253Ryan4253
committed
Switch Rotation to using au and tighten API
1 parent 73119f1 commit 17efec0

File tree

7 files changed

+176
-300
lines changed

7 files changed

+176
-300
lines changed

‎CMakeLists.txt‎

Lines changed: 54 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -123,71 +123,71 @@ add_executable(RaidZeroLib
123123
include/okapi/api/odometry/stateMode.hpp
124124
include/okapi/api/odometry/odomState.hpp
125125

126-
include/RaidZeroLib/api/Control/Async/AsyncVelBangBangController.hpp
127-
include/RaidZeroLib/api/Control/Async/AsyncVelTBHController.hpp
128-
include/RaidZeroLib/api/Control/Iterative/IterativeVelTBHController.hpp
129-
include/RaidZeroLib/api/Control/Iterative/IterativeVelBangBangController.hpp
130-
include/RaidZeroLib/api/Control/Feedforward/FeedforwardController.hpp
131-
include/RaidZeroLib/api/Control/Feedforward/SimpleMotorFeedforward.hpp
132-
include/RaidZeroLib/api/Chassis/AdaptivePurePursuitController.hpp
133-
include/RaidZeroLib/api/Utility/StateMachine.hpp
134-
include/RaidZeroLib/api/Utility/Util.hpp
135-
include/RaidZeroLib/api/Utility/CrossPlatformThread.hpp
136-
include/RaidZeroLib/api/Utility/CrossPlatformMutex.hpp
126+
# include/RaidZeroLib/api/Control/Async/AsyncVelBangBangController.hpp
127+
# include/RaidZeroLib/api/Control/Async/AsyncVelTBHController.hpp
128+
# include/RaidZeroLib/api/Control/Iterative/IterativeVelTBHController.hpp
129+
# include/RaidZeroLib/api/Control/Iterative/IterativeVelBangBangController.hpp
130+
# include/RaidZeroLib/api/Control/Feedforward/FeedforwardController.hpp
131+
# include/RaidZeroLib/api/Control/Feedforward/SimpleMotorFeedforward.hpp
132+
# include/RaidZeroLib/api/Chassis/AdaptivePurePursuitController.hpp
133+
# include/RaidZeroLib/api/Utility/StateMachine.hpp
134+
# include/RaidZeroLib/api/Utility/Util.hpp
135+
# include/RaidZeroLib/api/Utility/CrossPlatformThread.hpp
136+
# include/RaidZeroLib/api/Utility/CrossPlatformMutex.hpp
137137
include/RaidZeroLib/api/Utility/Math.hpp
138-
include/RaidZeroLib/api/Pathing/DiscretePath.hpp
139-
include/RaidZeroLib/api/Pathing/ParametricPath.hpp
140-
include/RaidZeroLib/api/Pathing/CubicBezier.hpp
141-
include/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.hpp
142-
include/RaidZeroLib/api/Trajectory/MotionProfile/TrapezoidalMotionProfile.hpp
143-
include/RaidZeroLib/api/Trajectory/MotionProfile/MotionProfile.hpp
144-
include/RaidZeroLib/api/Filter/SlewRate.hpp
145-
include/RaidZeroLib/api/Geometry/Point.hpp
138+
# include/RaidZeroLib/api/Pathing/DiscretePath.hpp
139+
# include/RaidZeroLib/api/Pathing/ParametricPath.hpp
140+
# include/RaidZeroLib/api/Pathing/CubicBezier.hpp
141+
# include/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.hpp
142+
# include/RaidZeroLib/api/Trajectory/MotionProfile/TrapezoidalMotionProfile.hpp
143+
# include/RaidZeroLib/api/Trajectory/MotionProfile/MotionProfile.hpp
144+
# include/RaidZeroLib/api/Filter/SlewRate.hpp
145+
# include/RaidZeroLib/api/Geometry/Point.hpp
146146
include/RaidZeroLib/api/Geometry/Rotation.hpp
147-
include/RaidZeroLib/api/Geometry/Transform.hpp
148-
include/RaidZeroLib/api/Geometry/Twist.hpp
149-
include/RaidZeroLib/api/Geometry/Pose.hpp
150-
include/RaidZeroLib/api/Units/Units.hpp
151-
include/RaidZeroLib/api.hpp
147+
# include/RaidZeroLib/api/Geometry/Transform.hpp
148+
# include/RaidZeroLib/api/Geometry/Twist.hpp
149+
# include/RaidZeroLib/api/Geometry/Pose.hpp
150+
# include/RaidZeroLib/api/Units/Units.hpp
151+
# include/RaidZeroLib/api.hpp
152152

153-
src/RaidZeroLib/api/Control/Async/AsyncVelTBHController.cpp
154-
src/RaidZeroLib/api/Control/Async/AsyncVelBangBangController.cpp
155-
src/RaidZeroLib/api/Control/Iterative/IterativeVelTBHController.cpp
156-
src/RaidZeroLib/api/Control/Iterative/IterativeVelBangBangController.cpp
157-
src/RaidZeroLib/api/Control/Feedforward/SimpleMotorFeedforward.cpp
158-
src/RaidZeroLib/api/Chassis/AdaptivePurePursuitController.cpp
159-
src/RaidZeroLib/api/Utility/CrossPlatformMutex.cpp
153+
# src/RaidZeroLib/api/Control/Async/AsyncVelTBHController.cpp
154+
# src/RaidZeroLib/api/Control/Async/AsyncVelBangBangController.cpp
155+
# src/RaidZeroLib/api/Control/Iterative/IterativeVelTBHController.cpp
156+
# src/RaidZeroLib/api/Control/Iterative/IterativeVelBangBangController.cpp
157+
# src/RaidZeroLib/api/Control/Feedforward/SimpleMotorFeedforward.cpp
158+
# src/RaidZeroLib/api/Chassis/AdaptivePurePursuitController.cpp
159+
# src/RaidZeroLib/api/Utility/CrossPlatformMutex.cpp
160160
src/RaidZeroLib/api/Utility/Math.cpp
161-
src/RaidZeroLib/api/Utility/CrossPlatformThread.cpp
162-
src/RaidZeroLib/api/Pathing/ParametricPath.cpp
163-
src/RaidZeroLib/api/Pathing/DiscretePath.cpp
164-
src/RaidZeroLib/api/Pathing/CubicBezier.cpp
165-
src/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.cpp
166-
src/RaidZeroLib/api/Filter/SlewRate.cpp
167-
src/RaidZeroLib/api/Geometry/Twist.cpp
168-
src/RaidZeroLib/api/Geometry/Point.cpp
169-
src/RaidZeroLib/api/Geometry/Transform.cpp
161+
# src/RaidZeroLib/api/Utility/CrossPlatformThread.cpp
162+
# src/RaidZeroLib/api/Pathing/ParametricPath.cpp
163+
# src/RaidZeroLib/api/Pathing/DiscretePath.cpp
164+
# src/RaidZeroLib/api/Pathing/CubicBezier.cpp
165+
# src/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.cpp
166+
# src/RaidZeroLib/api/Filter/SlewRate.cpp
167+
# src/RaidZeroLib/api/Geometry/Twist.cpp
168+
# src/RaidZeroLib/api/Geometry/Point.cpp
169+
# src/RaidZeroLib/api/Geometry/Transform.cpp
170170
src/RaidZeroLib/api/Geometry/Rotation.cpp
171-
src/RaidZeroLib/api/Geometry/Pose.cpp
171+
# src/RaidZeroLib/api/Geometry/Pose.cpp
172172

173-
test/test/Control/Feedforward/SimpleMotorFeedforwardTest.cpp
174-
test/test/Control/Iterative/IterativeVelBangBangControllerTest.cpp
173+
# test/test/Control/Feedforward/SimpleMotorFeedforwardTest.cpp
174+
# test/test/Control/Iterative/IterativeVelBangBangControllerTest.cpp
175175

176176
test/test/Geometry/RotationTest.cpp
177-
test/test/Geometry/PointTest.cpp
178-
test/test/Geometry/TwistTest.cpp
179-
test/test/Geometry/TransformTest.cpp
180-
test/test/Geometry/PoseTest.cpp
177+
# test/test/Geometry/PointTest.cpp
178+
# test/test/Geometry/TwistTest.cpp
179+
# test/test/Geometry/TransformTest.cpp
180+
# test/test/Geometry/PoseTest.cpp
181181

182-
test/test/Pathing/DiscretePathTest.cpp
183-
test/test/Pathing/ParametricPathTest.cpp
184-
test/test/Pathing/CubicBezierTest.cpp
185-
test/test/Pathing/PiecewiseCubicBezierTest.cpp
182+
# test/test/Pathing/DiscretePathTest.cpp
183+
# test/test/Pathing/ParametricPathTest.cpp
184+
# test/test/Pathing/CubicBezierTest.cpp
185+
# test/test/Pathing/PiecewiseCubicBezierTest.cpp
186186

187-
test/test/Units/UnitsTest.cpp
187+
# test/test/Units/UnitsTest.cpp
188188

189-
test/test/Utility/MathTest.cpp
190-
test/test/Utility/StateMachineTest.cpp
189+
# test/test/Utility/MathTest.cpp
190+
# test/test/Utility/StateMachineTest.cpp
191191

192192
test/include/mocks.hpp
193193
test/src/mocks.cpp)

‎include/RaidZeroLib/api/Geometry/Rotation.hpp‎

Lines changed: 15 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,55 +1,38 @@
11
#pragma once
2-
#include "RaidZeroLib/api/Utility/Math.hpp"
3-
#include "okapi/api/units/QAngle.hpp"
4-
#include "okapi/api/units/QLength.hpp"
2+
#include "au/au.hpp"
53

64
namespace rz {
7-
using namespace okapi;
85

96
class Rotation {
107
public:
11-
constexpr Rotation() = default;
8+
constexpr Rotation() noexcept = default;
129

13-
Rotation(QAngle iTheta);
10+
explicit Rotation(au::QuantityD<au::Radians> theta) noexcept;
1411

15-
Rotation(QLength iX, QLength iY);
12+
Rotation(au::QuantityD<au::Meters> x, au::QuantityD<au::Meters> y) noexcept;
1613

17-
Rotation(double iX, double iY);
14+
[[nodiscard]] au::QuantityD<au::Radians> Theta() const noexcept;
1815

19-
Rotation(const Rotation& rhs);
16+
[[nodiscard]] double Sin() const noexcept;
2017

21-
~Rotation() = default;
18+
[[nodiscard]] double Cos() const noexcept;
2219

23-
QAngle Theta() const;
20+
[[nodiscard]] double Tan() const noexcept;
2421

25-
double Sin() const;
22+
[[nodiscard]] Rotation operator+(const Rotation& rhs) const noexcept;
2623

27-
double Cos() const;
24+
[[nodiscard]] Rotation operator-(const Rotation& rhs) const noexcept;
2825

29-
double Tan() const;
26+
[[nodiscard]] Rotation operator-() const noexcept;
3027

31-
Rotation operator+(const Rotation& rhs) const;
28+
[[nodiscard]] Rotation operator*(double scalar) const noexcept;
3229

33-
Rotation operator-(const Rotation& rhs) const;
30+
[[nodiscard]] Rotation operator/(double scalar) const noexcept;
3431

35-
Rotation operator-() const;
36-
37-
Rotation operator*(double scalar) const;
38-
39-
Rotation operator/(double scalar) const;
40-
41-
bool operator==(const Rotation& rhs) const;
42-
43-
bool operator!=(const Rotation& rhs) const;
44-
45-
void operator=(const Rotation& rhs);
46-
47-
Rotation rotateBy(const Rotation& rhs) const;
32+
[[nodiscard]] bool isApprox(const Rotation& rhs, au::QuantityD<au::Radians> tol = au::radians(1e-12)) const noexcept;
4833

4934
private:
50-
QAngle theta{0.0};
51-
double cosine{1.0};
52-
double sine{0.0};
35+
au::QuantityD<au::Radians> theta = au::ZERO;
5336
};
5437

5538
} // namespace rz

‎include/RaidZeroLib/api/Units/Units.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010
#include "okapi/api/units/QTime.hpp"
1111
#include "okapi/api/units/RQuantity.hpp"
1212

13+
#include "au/au.hpp"
14+
1315
/*
1416
* The unit library we uses allows us to check our math during compile time
1517
* compile time using dimensional analysis. This file includes expansion

‎include/RaidZeroLib/api/Utility/Math.hpp‎

Lines changed: 13 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -5,32 +5,33 @@
55
#include <optional>
66

77
namespace rz {
8-
using namespace okapi;
98

109
template <typename T>
1110
int sgn(T val) {
1211
return (T(0.0) < val) - (val < T(0.0));
1312
}
1413

15-
QAngularSpeed linearToWheelVelocity(QSpeed velocity, QLength wheelDiameter);
14+
// QAngularSpeed linearToWheelVelocity(QSpeed velocity, QLength wheelDiameter);
1615

17-
QSpeed wheelToLinearVelocity(QAngularSpeed velocity, QLength wheelDiameter);
16+
// QSpeed wheelToLinearVelocity(QAngularSpeed velocity, QLength wheelDiameter);
1817

19-
double constrainAngle360(double iAngle);
18+
[[nodiscard]] double constrainAngle360(double angle) noexcept;
2019

21-
double constrainAngle180(double iAngle);
20+
[[nodiscard]] double constrainAngle180(double angle) noexcept;
2221

23-
QAngle constrainAngle360(QAngle iAngle);
22+
[[nodiscard]] au::QuantityD<au::Radians> constrainAngle360(au::QuantityD<au::Radians> angle) noexcept;
2423

25-
QAngle constrainAngle180(QAngle iAngle);
24+
[[nodiscard]] au::QuantityD<au::Radians> constrainAngle180(au::QuantityD<au::Radians> angle) noexcept;
2625

27-
double sinc(double x);
2826

29-
std::optional<std::pair<double, double>> quadraticFormula(double a, double b, double c);
3027

31-
std::pair<QSpeed, QSpeed> wheelForwardKinematics(QSpeed linearVelocity, QCurvature curvature, QLength wheelTrack);
28+
// double sinc(double x);
3229

33-
std::pair<QAcceleration, QAcceleration> wheelForwardKinematics(QAcceleration linearAcceleration, QCurvature curvature,
34-
QLength wheelTrack);
30+
// std::optional<std::pair<double, double>> quadraticFormula(double a, double b, double c);
31+
32+
// std::pair<QSpeed, QSpeed> wheelForwardKinematics(QSpeed linearVelocity, QCurvature curvature, QLength wheelTrack);
33+
34+
// std::pair<QAcceleration, QAcceleration> wheelForwardKinematics(QAcceleration linearAcceleration, QCurvature curvature,
35+
// QLength wheelTrack);
3536

3637
}; // namespace rz

‎src/RaidZeroLib/api/Geometry/Rotation.cpp‎

Lines changed: 20 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -1,83 +1,52 @@
11
#include "RaidZeroLib/api/Geometry/Rotation.hpp"
2+
#include "RaidZeroLib/api/Utility/Math.hpp"
23

34
namespace rz {
45

5-
Rotation::Rotation(QAngle iTheta) {
6-
theta = constrainAngle180(iTheta);
7-
sine = sin(iTheta).convert(number);
8-
cosine = cos(iTheta).convert(number);
9-
}
10-
11-
Rotation::Rotation(QLength iX, QLength iY) {
12-
const auto magnitude = hypot(iX, iY);
13-
if (magnitude > 1e-6 * meter) {
14-
sine = (iY / magnitude).convert(number);
15-
cosine = (iX / magnitude).convert(number);
16-
} else {
17-
sine = 0.0;
18-
cosine = 1.0;
19-
}
20-
theta = std::atan2(sine, cosine) * radian;
21-
}
6+
Rotation::Rotation(au::QuantityD<au::Radians> theta) noexcept : theta(constrainAngle180(theta)) {}
227

23-
Rotation::Rotation(double iX, double iY) : Rotation(iX * meter, iY * meter) {
24-
}
25-
26-
Rotation::Rotation(const Rotation& rhs) : theta(rhs.theta), sine(rhs.sine), cosine(rhs.cosine) {
27-
}
8+
Rotation::Rotation(au::QuantityD<au::Meters> x, au::QuantityD<au::Meters> y) noexcept
9+
: theta(x == au::ZERO && y == au::ZERO ? au::ZERO : au::arctan2(y, x)){}
2810

29-
QAngle Rotation::Theta() const {
11+
au::QuantityD<au::Radians> Rotation::Theta() const noexcept {
3012
return theta;
3113
}
3214

33-
double Rotation::Sin() const {
34-
return sine;
15+
double Rotation::Sin() const noexcept {
16+
return au::sin(theta);
3517
}
3618

37-
double Rotation::Cos() const {
38-
return cosine;
19+
double Rotation::Cos() const noexcept {
20+
return au::cos(theta);
3921
}
4022

41-
double Rotation::Tan() const {
42-
return sine / cosine;
23+
double Rotation::Tan() const noexcept {
24+
return au::tan(theta);
4325
}
4426

45-
Rotation Rotation::operator+(const Rotation& rhs) const {
46-
return rotateBy(rhs);
27+
Rotation Rotation::operator+(const Rotation& rhs) const noexcept {
28+
return Rotation(theta + rhs.theta);
4729
}
4830

49-
Rotation Rotation::operator-(const Rotation& rhs) const {
31+
Rotation Rotation::operator-(const Rotation& rhs) const noexcept {
5032
return *this + -rhs;
5133
}
5234

53-
Rotation Rotation::operator-() const {
35+
Rotation Rotation::operator-() const noexcept {
5436
return *this * -1;
5537
}
5638

57-
Rotation Rotation::operator*(double scalar) const {
39+
Rotation Rotation::operator*(double scalar) const noexcept {
5840
return Rotation(theta * scalar);
5941
}
6042

61-
Rotation Rotation::operator/(double scalar) const {
43+
Rotation Rotation::operator/(double scalar) const noexcept {
6244
return *this * (1.0 / scalar);
6345
}
6446

65-
bool Rotation::operator==(const Rotation& rhs) const {
66-
return std::hypot(cosine - rhs.cosine, sine - rhs.sine) < 1E-9;
67-
}
68-
69-
bool Rotation::operator!=(const Rotation& rhs) const {
70-
return !(*this == rhs);
71-
}
72-
73-
void Rotation::operator=(const Rotation& rhs) {
74-
theta = rhs.theta;
75-
sine = rhs.sine;
76-
cosine = rhs.cosine;
77-
}
78-
79-
Rotation Rotation::rotateBy(const Rotation& rhs) const {
80-
return Rotation((cosine * rhs.cosine - sine * rhs.sine) * meter, (cosine * rhs.sine + sine * rhs.cosine) * meter);
47+
bool Rotation::isApprox(const Rotation& rhs, au::QuantityD<au::Radians> tol) const noexcept {
48+
const auto diff = constrainAngle180(theta - rhs.theta);
49+
return au::abs(diff) <= tol;
8150
}
8251

8352
} // namespace rz

0 commit comments

Comments
 (0)