Merge pull request #100 from ad101-lab/ad-closed-loop-orientation-con…

…trol-2

Closed loop orientation control

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,39 @@ namespace Pronounce {
 			this->inverted = inverted;
 		}
 
+		PID* getTurnPid() {
+			return turnPid;
+		}
+
+		void setTurnPid(PID* turnPid) {
+			this->turnPid = turnPid;
+			this->turnPid->setTurnPid(true);
+		}
+
+		double getTargetOrientation() {
+			return this->turnPid->getTarget();
+		}
+
+		void setTargetOrientation(double orientation) {
+			this->turnPid->setTarget(orientation);
+		}
+
+		bool getOrientationControl() {
+			return orientationControl;
+		}
+
+		void setOrientationControl(bool orientationControl) {
+			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

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

pathTest.cpp

@@ -51,6 +51,7 @@ int testPathIndex;
 int leftAllianceToRightHomeZoneIndex;
 int rightHomeZoneToRightAllianceIndex;
 int rightAllianceToRightRingsIndex;
+int rightRingsToRightHomeZoneIndex;
 
 int fps = 60;
 double playbackMultiplier = 1;
@@ -296,6 +297,7 @@ int main() {
 	double trackWidth = drivetrain.getTrackWidth(); 
 
 	purePursuit.setSpeed(1.0);
+	purePursuit.setTurnPid(new PID(1, 0, 0));
 
 	// Test path
 	/*
@@ -335,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
@@ -349,9 +351,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);
 
@@ -362,14 +363,9 @@ int main() {
 
 	purePursuit.setEnabled(true);
 
-	double curvature = 0.1;
-
 	// Loop through paths
 	for (int i = 0; i < purePursuit.getPaths().size(); i++) {
 
-		std::vector<Point> pathVector = purePursuit.getPath(i).getPath();
-		Path path = purePursuit.getPath(i);
-
 		purePursuit.setCurrentPathIndex(i);
 		purePursuit.setFollowing(true);
 
@@ -409,7 +405,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 >= 4*fps * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) {
 				break;
 			}
 		}

src/motionControl/tankPurePursuit.cpp

@@ -3,14 +3,26 @@
 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() {
@@ -23,6 +35,18 @@ namespace Pronounce {
 			return;
 		}
 
+		if (orientationControl) {
+			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;
+		}
+
 		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());

src/pid/pid.cpp

@@ -10,29 +10,36 @@ 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;
 		}
 
 		totalError = abs(totalError) > maxIntegral ? signum_c(totalError) * maxIntegral : totalError;
 
 		this->power = error * kP + derivitive * kD + totalError * kI;
 
-		return this->power; 
+		return this->power;
 	}
 
 	PID::~PID() {

src/utils/utils.cpp

@@ -10,19 +10,20 @@ 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) {
+        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;
+    }
+
     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)) {