From 71e2f3d070a1e5ca8876836a15ff8a1ba9816012 Mon Sep 17 00:00:00 2001 From: ad101-lab <55064247+ad101-lab@users.noreply.github.com> Date: Thu, 13 Jan 2022 15:32:05 -0700 Subject: [PATCH 1/5] Add closed loop orientation control to pure pursuit Still need to test in sim, the current enviroment makes it hard to do these tests so I might revise it to be very similar to the end enviroment --- include/motionControl/tankPurePursuit.hpp | 20 ++++++++++++++++++++ pathTest.cpp | 3 +-- src/motionControl/tankPurePursuit.cpp | 18 ++++++++++++++++++ 3 files changed, 39 insertions(+), 2 deletions(-) diff --git a/include/motionControl/tankPurePursuit.hpp b/include/motionControl/tankPurePursuit.hpp index 19ef9af5..c064ece6 100644 --- a/include/motionControl/tankPurePursuit.hpp +++ b/include/motionControl/tankPurePursuit.hpp @@ -11,10 +11,14 @@ namespace Pronounce { double speed = 100; bool inverted = false; + + bool orientationControl = false; + PID* turnPid; public: TankPurePursuit(AbstractTankDrivetrain* drivetrain); TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance); TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance); + TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, PID* turnPid, double lookaheadDistance); void updateDrivetrain(); @@ -44,6 +48,22 @@ namespace Pronounce { this->inverted = inverted; } + PID* getTurnPid() { + return turnPid; + } + + void setTurnPid(PID* turnPid) { + this->turnPid = turnPid; + } + + double getTargetOrientation() { + return this->turnPid->getTarget(); + } + + void setTargetOrientation(double orientation) { + this->turnPid->setTarget(orientation); + } + ~TankPurePursuit(); }; } // namespace Pronounce diff --git a/pathTest.cpp b/pathTest.cpp index 979e5bc9..70c9fc72 100644 --- a/pathTest.cpp +++ b/pathTest.cpp @@ -349,9 +349,8 @@ int main() { setcolor(DARKGRAY); delay(1000); - //printPath(path); #endif - Position startingPosition(30.0, 11.4, -M_PI_2); + Position startingPosition(30.0, 11.4, M_PI_2); drivetrain.reset(&startingPosition); diff --git a/src/motionControl/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp index 37116e1f..8eecc7c7 100644 --- a/src/motionControl/tankPurePursuit.cpp +++ b/src/motionControl/tankPurePursuit.cpp @@ -3,14 +3,22 @@ namespace Pronounce { TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain) : PurePursuit() { this->drivetrain = drivetrain; + this->turnPid = new PID(); } TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance) : PurePursuit(lookaheadDistance) { this->drivetrain = drivetrain; + this->turnPid = new PID(); } TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance) : PurePursuit(odometry, lookaheadDistance) { this->drivetrain = drivetrain; + this->turnPid = new PID(); + } + + TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, PID* turnPid, double lookaheadDistance) : PurePursuit(odometry, lookaheadDistance) { + this->drivetrain = drivetrain; + this->turnPid = turnPid; } void TankPurePursuit::updateDrivetrain() { @@ -23,6 +31,16 @@ namespace Pronounce { return; } + if (orientationControl) { + double currentOrientation = 0; + this->turnPid->setPosition(angleDifference(0, currentOrientation)); + double spinSpeed = this->turnPid->update(); + + this->drivetrain->skidSteerVelocity(0, spinSpeed * speed); + + return; + } + PurePursuitPointData pointData = this->getPointData(); double side = sqrt(abs(clamp(pointData.localLookaheadVector.getCartesian().getY() / this->getLookahead(), -1.0, 1.0))) * signum_c(pointData.localLookaheadVector.getCartesian().getY()); From a3964396698d37d7c93a6e4a17291aa36cd5c4cb Mon Sep 17 00:00:00 2001 From: ad101-lab <55064247+ad101-lab@users.noreply.github.com> Date: Fri, 14 Jan 2022 22:46:10 -0700 Subject: [PATCH 2/5] Add angles to paths and improve the angleDifference logic --- include/motionControl/tankPurePursuit.hpp | 8 ++++++++ pathTest.cpp | 8 +++----- src/utils/utils.cpp | 13 ++++++------- 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/include/motionControl/tankPurePursuit.hpp b/include/motionControl/tankPurePursuit.hpp index c064ece6..4bf19898 100644 --- a/include/motionControl/tankPurePursuit.hpp +++ b/include/motionControl/tankPurePursuit.hpp @@ -64,6 +64,14 @@ namespace Pronounce { this->turnPid->setTarget(orientation); } + bool getOrientationControl() { + return orientationControl; + } + + void setOrientationControl(bool orientationControl) { + this->orientationControl = orientationControl; + } + ~TankPurePursuit(); }; } // namespace Pronounce diff --git a/pathTest.cpp b/pathTest.cpp index 70c9fc72..0b76f738 100644 --- a/pathTest.cpp +++ b/pathTest.cpp @@ -296,6 +296,7 @@ int main() { double trackWidth = drivetrain.getTrackWidth(); purePursuit.setSpeed(1.0); + purePursuit.setTurnPid(new PID(1, 0, 0)); // Test path /* @@ -360,15 +361,12 @@ int main() { #endif purePursuit.setEnabled(true); - - double curvature = 0.1; + purePursuit.setOrientationControl(true); + purePursuit.setTargetOrientation(3.14); // Loop through paths for (int i = 0; i < purePursuit.getPaths().size(); i++) { - std::vector pathVector = purePursuit.getPath(i).getPath(); - Path path = purePursuit.getPath(i); - purePursuit.setCurrentPathIndex(i); purePursuit.setFollowing(true); diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp index 84d6edff..3453e3c8 100644 --- a/src/utils/utils.cpp +++ b/src/utils/utils.cpp @@ -10,19 +10,18 @@ namespace Pronounce return radians * 180 / M_PI; } - double angleDifference(double angle1, double angle2) { - double difference = angle1 - angle2; - while (difference < -M_PI) difference += M_PI * 2.0; - while (difference > M_PI) difference -= M_PI * 2.0; - return difference; - } - double signum_c(double x) { if (x > 0.0) return 1.0; if (x < 0.0) return -1.0; return x; } + double angleDifference(double angle1, double angle2) { + double difference = angle1 - angle2; + while (abs(difference) > M_PI) difference += M_PI * 2.0 * -signum_c(difference); + return difference; + } + double map(double value, double start1, double stop1, double start2, double stop2) { double result = start2 + (stop2 - start2) * ((value - start1) / (stop1 - start1)); if (std::isnan(result)) { From 9013d93d35d848a1b943ed12b06cc419f6e6b262 Mon Sep 17 00:00:00 2001 From: ad101-lab <55064247+ad101-lab@users.noreply.github.com> Date: Fri, 14 Jan 2022 23:36:39 -0700 Subject: [PATCH 3/5] Tested closed loop orientation in sim --- include/motionControl/tankPurePursuit.hpp | 9 +++++++++ include/pid/pid.hpp | 12 +++++++++++- pathTest.cpp | 2 +- src/motionControl/tankPurePursuit.cpp | 8 +++++++- src/pid/pid.cpp | 15 +++++++++++---- src/utils/utils.cpp | 2 ++ 6 files changed, 41 insertions(+), 7 deletions(-) diff --git a/include/motionControl/tankPurePursuit.hpp b/include/motionControl/tankPurePursuit.hpp index 4bf19898..095bfbe6 100644 --- a/include/motionControl/tankPurePursuit.hpp +++ b/include/motionControl/tankPurePursuit.hpp @@ -54,6 +54,7 @@ namespace Pronounce { void setTurnPid(PID* turnPid) { this->turnPid = turnPid; + this->turnPid->setTurnPid(true); } double getTargetOrientation() { @@ -72,6 +73,14 @@ namespace Pronounce { this->orientationControl = orientationControl; } + bool isDone(double maxDistance) { + return maxDistance > this->getOdometry()->getPosition()->distance(this->getPath(this->getCurrentPathIndex()).getPoint(this->getPath(this->getCurrentPathIndex()).getPath().size() - 1)); + } + + bool isDoneOrientation(double maxDifference) { + return maxDifference > angleDifference(this->getOdometry()->getPosition()->getTheta(), this->getTurnPid()->getTarget()); + } + ~TankPurePursuit(); }; } // namespace Pronounce diff --git a/include/pid/pid.hpp b/include/pid/pid.hpp index bd08ede2..4625a4e0 100644 --- a/include/pid/pid.hpp +++ b/include/pid/pid.hpp @@ -26,9 +26,11 @@ namespace Pronounce { double maxIntegral = 0.3; double power; + + bool turnPid = false; public: PID(); - PID(double kP, double kI, double kD, double target = 0, double position = 0); + PID(double kP, double kI, double kD, double target = 0, double position = 0, bool turnPid = false); double update(); @@ -120,6 +122,14 @@ namespace Pronounce { this->maxIntegral = maxIntegral; } + bool getTurnPid() { + return turnPid; + } + + void setTurnPid(bool turnPid) { + this->turnPid = turnPid; + } + ~PID(); }; } // namespace Pronounce diff --git a/pathTest.cpp b/pathTest.cpp index 0b76f738..66ee221c 100644 --- a/pathTest.cpp +++ b/pathTest.cpp @@ -406,7 +406,7 @@ int main() { robotPositions.emplace_back(Point(odometry.getPosition()->getX(), odometry.getPosition()->getY())); #endif - if (loopcount >= 5*fps * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) { + if (loopcount >= 20 * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) { break; } } diff --git a/src/motionControl/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp index 8eecc7c7..03cfa492 100644 --- a/src/motionControl/tankPurePursuit.cpp +++ b/src/motionControl/tankPurePursuit.cpp @@ -4,21 +4,25 @@ namespace Pronounce { TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain) : PurePursuit() { this->drivetrain = drivetrain; this->turnPid = new PID(); + this->turnPid->setTurnPid(true); } TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance) : PurePursuit(lookaheadDistance) { this->drivetrain = drivetrain; this->turnPid = new PID(); + this->turnPid->setTurnPid(true); } TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance) : PurePursuit(odometry, lookaheadDistance) { this->drivetrain = drivetrain; this->turnPid = new PID(); + this->turnPid->setTurnPid(true); } TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, PID* turnPid, double lookaheadDistance) : PurePursuit(odometry, lookaheadDistance) { this->drivetrain = drivetrain; this->turnPid = turnPid; + this->turnPid->setTurnPid(true); } void TankPurePursuit::updateDrivetrain() { @@ -32,10 +36,12 @@ namespace Pronounce { } if (orientationControl) { - double currentOrientation = 0; + double currentOrientation = this->getOdometry()->getPosition()->getTheta(); this->turnPid->setPosition(angleDifference(0, currentOrientation)); double spinSpeed = this->turnPid->update(); + std::cout << "Angle difference: " << this->turnPid->getError() << std::endl; + this->drivetrain->skidSteerVelocity(0, spinSpeed * speed); return; diff --git a/src/pid/pid.cpp b/src/pid/pid.cpp index 71f347fb..0d559093 100644 --- a/src/pid/pid.cpp +++ b/src/pid/pid.cpp @@ -10,21 +10,28 @@ namespace Pronounce { this->position = 0.0; } - PID::PID(double kP, double kI, double kD, double target, double position) { + PID::PID(double kP, double kI, double kD, double target, double position, bool turnPid) { this->kP = kP; this->kI = kI; this->kD = kD; this->target = target; this->position = position; + this->turnPid = turnPid; } double PID::update() { - this->error = target - position; + if (turnPid) { + this->error = angleDifference(target, position); + } + else { + this->error = target - position; + } this->derivitive = error - prevError; if (abs(error) < integralBound) { totalError += error; - } else { + } + else { totalError = 0; } @@ -32,7 +39,7 @@ namespace Pronounce { this->power = error * kP + derivitive * kD + totalError * kI; - return this->power; + return this->power; } PID::~PID() { diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp index 3453e3c8..5257acb7 100644 --- a/src/utils/utils.cpp +++ b/src/utils/utils.cpp @@ -17,6 +17,8 @@ namespace Pronounce } double angleDifference(double angle1, double angle2) { + while (abs(angle1) > M_PI) angle1 += M_PI * 2.0 * -signum_c(angle1); + while (abs(angle2) > M_PI) angle2 += M_PI * 2.0 * -signum_c(angle2); double difference = angle1 - angle2; while (abs(difference) > M_PI) difference += M_PI * 2.0 * -signum_c(difference); return difference; From 6dea17c9336b57ad645e393d15ed7ea30a476e40 Mon Sep 17 00:00:00 2001 From: ad101-lab <55064247+ad101-lab@users.noreply.github.com> Date: Fri, 14 Jan 2022 23:37:08 -0700 Subject: [PATCH 4/5] Tested closed loop orientation in sim --- pathTest.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/pathTest.cpp b/pathTest.cpp index 66ee221c..2b1032ae 100644 --- a/pathTest.cpp +++ b/pathTest.cpp @@ -361,8 +361,6 @@ int main() { #endif purePursuit.setEnabled(true); - purePursuit.setOrientationControl(true); - purePursuit.setTargetOrientation(3.14); // Loop through paths for (int i = 0; i < purePursuit.getPaths().size(); i++) { @@ -406,7 +404,7 @@ int main() { robotPositions.emplace_back(Point(odometry.getPosition()->getX(), odometry.getPosition()->getY())); #endif - if (loopcount >= 20 * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) { + if (loopcount >= 4*fps * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) { break; } } From 7dfc6be0428de8ffa7c40767af5e93c25ade4b37 Mon Sep 17 00:00:00 2001 From: ad101-lab <55064247+ad101-lab@users.noreply.github.com> Date: Sat, 15 Jan 2022 11:52:05 -0700 Subject: [PATCH 5/5] Fix path --- pathTest.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/pathTest.cpp b/pathTest.cpp index 2b1032ae..fdb1b3d1 100644 --- a/pathTest.cpp +++ b/pathTest.cpp @@ -51,6 +51,7 @@ int testPathIndex; int leftAllianceToRightHomeZoneIndex; int rightHomeZoneToRightAllianceIndex; int rightAllianceToRightRingsIndex; +int rightRingsToRightHomeZoneIndex; int fps = 60; double playbackMultiplier = 1; @@ -336,10 +337,10 @@ int main() { QuadraticSplinePath rightRingsToRightHomeZone = QuadraticSplinePath(); - rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, M_PI))); - rightAllianceToRightRings.addPoint(SplinePoint(Point(85, 36), Vector(20.0, M_PI_2))); + rightRingsToRightHomeZone.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, M_PI))); + rightRingsToRightHomeZone.addPoint(SplinePoint(Point(85, 36), Vector(20.0, M_PI_2))); - rightAllianceToRightRingsIndex = purePursuit.addPath(rightAllianceToRightRings.getPath(0.1)); + rightRingsToRightHomeZoneIndex = purePursuit.addPath(rightRingsToRightHomeZone.getPath(0.1)); #if GRAPH