Add backwards driving functionality, tested and working AWP path too

include/chassis/abstractTankDrivetrain.hpp

@@ -20,13 +20,13 @@ namespace Pronounce {
 		}
 
 		void driveCurvature(double speed, double curvature) {
-			double leftSpeed = speed * (2 - curvature * trackWidth) / 2;
-			double rightSpeed = speed * (2 + curvature * trackWidth) / 2;
+			double leftSpeed = speed * (2.0 - curvature * trackWidth) / 2.0;
+			double rightSpeed = speed * (2.0 + curvature * trackWidth) / 2.0;
 
-			double maxSpeed = max(leftSpeed, rightSpeed);
+			double maxSpeed = max(abs(leftSpeed), abs(rightSpeed));
 
-			if (maxSpeed > speed) {
-				double multiplier = speed / maxSpeed;
+			if (maxSpeed > abs(speed)) {
+				double multiplier = abs(speed) / maxSpeed;
 				leftSpeed *= multiplier;
 				rightSpeed *= multiplier;
 			}

include/chassis/simDrivetrain.hpp

@@ -11,6 +11,7 @@ namespace Pronounce {
 	class SimDrivetrain {
 	private:
 		Position* position;
+		double resetOrientation = 0.0;
 	public:
 		SimDrivetrain();
 		SimDrivetrain(Position* position);
@@ -23,6 +24,19 @@ namespace Pronounce {
 			this->position = position;
 		}
 
+		double getResetOrientation() {
+			return resetOrientation;
+		}
+
+		void setResetOrientation(double resetPosition) {
+			this->resetOrientation = resetOrientation;
+		}
+
+		void reset(Position* position) {
+			this->position->operator=(position);
+			this->resetOrientation = position->getTheta();
+		}
+
 		virtual void update() {}
 
 		~SimDrivetrain();

pathTest.cpp

@@ -48,6 +48,9 @@
 using namespace Pronounce;
 
 int testPathIndex;
+int leftAllianceToRightHomeZoneIndex;
+int rightHomeZoneToRightAllianceIndex;
+int rightAllianceToRightRingsIndex;
 
 int fps = 60;
 double playbackMultiplier = 0.5;
@@ -305,13 +308,37 @@ int main() {
 	testPathIndex = purePursuit.addPath(testPath.getPath(0.1));
 	*/
 
-	QuadraticSplinePath quadraticSplineTestPath = QuadraticSplinePath();
 
-	quadraticSplineTestPath.addPoint(SplinePoint(Point(24.0, 24.0), Vector(30.0, 0.0)));
-	quadraticSplineTestPath.addPoint(SplinePoint(Point(70.3, 70.3), Vector(30.0, 0.0)));
-	quadraticSplineTestPath.addPoint(SplinePoint(Point(24, 120.3), Vector(30.0, 0.0)));
+	QuadraticSplinePath leftAllianceToRightHomeZone = QuadraticSplinePath();
 
-	testPathIndex = purePursuit.addPath(quadraticSplineTestPath.getPath(0.1));
+	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(30.0, 11.4), Vector(15.0, M_PI_2)));
+	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(15.2, 35.0), Vector(15.0, 0.0)));
+	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(46.4, 46.4), Vector(15.0, M_PI_2)));
+	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(93.9, 46.4), Vector(15.0, M_PI_2)));
+
+	leftAllianceToRightHomeZoneIndex = purePursuit.addPath(leftAllianceToRightHomeZone.getPath(0.1));
+
+	QuadraticSplinePath rightHomeZoneToRightAlliance = QuadraticSplinePath();
+
+	rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(93.9, 46.4), Vector(15.0, -M_PI_2)));
+	rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123.0, 40.0), Vector(15.0, - M_PI_2 - M_PI_4)));
+
+	rightHomeZoneToRightAllianceIndex = purePursuit.addPath(rightHomeZoneToRightAlliance.getPath(0.1));
+
+	QuadraticSplinePath rightAllianceToRightRings = QuadraticSplinePath();
+
+	rightAllianceToRightRings.addPoint(SplinePoint(Point(123.0, 40.0), Vector(15.0, - M_PI_2 - M_PI_4)));
+	rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 46.8), Vector(0.0, 0.0)));
+	rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, 0.0)));
+
+	rightAllianceToRightRingsIndex = purePursuit.addPath(rightAllianceToRightRings.getPath(0.1));
+
+	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)));
+
+	rightAllianceToRightRingsIndex = purePursuit.addPath(rightAllianceToRightRings.getPath(0.1));
 
 
 #if GRAPH
@@ -324,9 +351,9 @@ int main() {
 	delay(1000);
 	//printPath(path);
 #endif
-	Position startingPosition(24, 24, 0);
+	Position startingPosition(30.0, 11.4, -M_PI_2);
 
-	drivetrain.setPosition(&startingPosition);
+	drivetrain.reset(&startingPosition);
 
 #if GRAPH
 	std::vector<Point> robotPositions;
@@ -351,7 +378,7 @@ int main() {
 			loopcount++;
 
 			odometry.update();
-			//drivetrain.driveCurvature(1, (1.0 / 24.0));
+			// drivetrain.driveCurvature(-1.0, (1.0 / 24.0));
 			purePursuit.update();
 			drivetrain.update();
 
@@ -381,7 +408,8 @@ int main() {
 
 			robotPositions.emplace_back(Point(odometry.getPosition()->getX(), odometry.getPosition()->getY()));
 #endif
-			if (loopcount > 5*fps * purePursuit.getPaths().size()) {
+
+			if (loopcount >= 5*fps * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) {
 				break;
 			}
 		}

src/chassis/simTankDrivetrain.cpp

@@ -31,7 +31,7 @@ namespace Pronounce {
 		leftDistance += leftVelocity;
 		rightDistance += rightVelocity;
 
-		double relativeAngle = (leftDistance - rightDistance) / this->getTrackWidth();
+		double relativeAngle = ((leftDistance - rightDistance) / this->getTrackWidth()) + this->getResetOrientation();
 
 		// Calculate a vector
 		Vector localOffset(offset, relativeAngle+M_PI_2);

src/motionControl/tankPurePursuit.cpp

@@ -27,7 +27,7 @@ namespace Pronounce {
 
 		double side = sqrt(abs(clamp(pointData.localLookaheadVector.getCartesian().getY() / this->getLookahead(), -1.0, 1.0))) * signum_c(pointData.localLookaheadVector.getCartesian().getY());
 
-		side = side < 0.5 ? 1.0 : side;
+		side = abs(side) < 0.5 ? signum_c(side) : side;
 
 		// Redundant scalar, could be used in the future
 		// Found that using the Y values got the same affect when this was wanted and a better affect when it wasn't
@@ -40,11 +40,6 @@ namespace Pronounce {
 
 		double speed = clamp(this->getSpeed() * side * scalar, -this->getSpeed(), this->getSpeed());
 
-		// Drive backwards
-		if (speed < 0) {
-			pointData.curvature = -pointData.curvature;
-		}
-
 		drivetrain->driveCurvature(speed, pointData.curvature);
 	}