Merge pull request #108 from ad101-lab/ad-berthoud-awp

Berthoud awp
alexDickhans · Jan 23, 2022 · 70ca73c · 70ca73c
2 parents e3614d6 + 6831456
commit 70ca73c
Show file tree

Hide file tree

Showing 17 changed files with 744 additions and 198 deletions.
diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
@@ -3,6 +3,8 @@
 #include "utils/pointUtil.hpp"
 #include "utils/path.hpp"
 #include "utils/splinePath.hpp"
+#include "utils/quadraticSplinePath.hpp"
+#include "utils/splinePoint.hpp"
 #include "motionControl/purePursuit.hpp"
 #include "utils/purePursuitProfile.hpp"
 #include "utils/purePursuitProfileManager.hpp"
@@ -21,7 +23,6 @@ namespace Pronounce {
 
 	// Right awp right
 	int farRightHomeZoneToRightAllianceIndex;
-	int rightAllianceToRightHomeZoneIndex;
 
 	// Left steal left
 	int leftAllianceToLeftNeutralIndex;
@@ -34,31 +35,55 @@ namespace Pronounce {
 	int nearPlatformViaLeftNeutralToFarPlatformIndex;
 	int nearPlatformToMidIndex;
 
+	// Left AWP
+	int leftAllianceToRightHomeZoneIndex;
+	int rightHomeZoneToRightAllianceIndex;
+	int rightAllianceToRightHomeZoneIndex;
+	int rightAllianceToRightRingsIndex;
+	int rightRingsToRightHomeZoneIndex;
+
+
+	// Skills
+	int leftHomeZoneToLeftNeutralGoalIndex;
+	int leftNeutralGoalToFarHomeZoneIndex;
+	int farHomeZoneToMidNeutralGoalIndex;
+	int midNeutralGoalToPlatformIndex;
+	int farPlatformToDropOffGoalIndex;
+	int goalDropOffToFarLeftAllianceIndex;
+	int farLeftAllianceToLeftRingsIndex;
+	int leftRingsToGoalDropIndex;
+	int farPlatformToEnterHomezoneIndex;
+	int rightHomeZoneToFarRightAllianceGoalIndex;
+	int farRightAllianceGoalToNearPreloadsIndex;
+	int nearPlatformAlignIndex;
+	int leftHomeZoneToPlatformIndex;
+
+	int forwardIndex = -1;
+	int backwardIndex = -1;
+
 	void autoPaths(PurePursuit* purePursuit) {
 		// Default pure pursuit profile
 		PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0);
 		purePursuit->getPurePursuitProfileManager().setDefaultProfile(defaultProfile);
 
-		// Test path
+		//SECTION: Test path
 		Path testPath = Path();
 
 		testPath.addPoint(0, 0);
-		testPath.addPoint(24, 0);
-		testPath.addPoint(24, 24);
-		testPath.addPoint(24, 48);
-		testPath.addPoint(-24, 48);
-		testPath.addPoint(0, 24);
+		testPath.addPoint(0, 100);
 
 		testPathIndex = purePursuit->addPath(testPath);
 
+		// !SECTION
+
+		//SECTION Right steal right
 		Path rightNeutralToRightHomeZone;
 
 		rightNeutralToRightHomeZone.addPoint(105.7, 60);
 		rightNeutralToRightHomeZone.addPoint(105.7, 16);
 
 		rightNeutralToRightHomeIndex = purePursuit->addPath(rightNeutralToRightHomeZone);
 
-		// Right Steal Right
 		Path rightHomeToGoalNeutral;
 
 		rightHomeToGoalNeutral.addPoint(105.7, 16);
@@ -70,7 +95,7 @@ namespace Pronounce {
 
 		rightNeutralToMidNeutral.addPoint(105.7, 62);
 		rightNeutralToMidNeutral.addPoint(75.3, 40);
-		rightNeutralToMidNeutral.addPoint(60.3, 65);
+		rightNeutralToMidNeutral.addPoint(65, 70);
 
 		rightNeutralToMidNeutralIndex = purePursuit->addPath(rightNeutralToMidNeutral.getPath(0.1));
 
@@ -95,25 +120,18 @@ namespace Pronounce {
 
 		farRightHomeZoneToRightAllianceIndex = purePursuit->addPath(farRightHomeZoneToRightAlliance);
 
-		Path rightAllianceToRightHomeZone;
-
-		rightAllianceToRightHomeZone.addPoint(127.9, 24);
-		rightAllianceToRightHomeZone.addPoint(105.7, 16);
-
-		rightAllianceToRightHomeZoneIndex = purePursuit->addPath(rightAllianceToRightHomeZone);
-
-		Path leftAllianceToLeftNeutral;
+		QuadraticSplinePath leftAllianceToLeftNeutral;
 
-		leftAllianceToLeftNeutral.addPoint(29, 11.4);
-		leftAllianceToLeftNeutral.addPoint(32, 67);
+		leftAllianceToLeftNeutral.addPoint(SplinePoint(Point(30, 11.4), Vector(10, M_PI_2)));
+		leftAllianceToLeftNeutral.addPoint(SplinePoint(Point(32, 67), Vector(10, 0)));
 
-		leftAllianceToLeftNeutralIndex = purePursuit->addPath(leftAllianceToLeftNeutral);
+		leftAllianceToLeftNeutralIndex = purePursuit->addPath(leftAllianceToLeftNeutral.getPath(0.1));
 
 		Path leftNeutralToMidNeutral;
 
 		leftNeutralToMidNeutral.addPoint(32, 67);
 		leftNeutralToMidNeutral.addPoint(65.3, 40);
-		leftNeutralToMidNeutral.addPoint(70.3, 65);
+		leftNeutralToMidNeutral.addPoint(70.3, 70.3);
 
 		leftNeutralToMidNeutralIndex = purePursuit->addPath(leftNeutralToMidNeutral);
 
@@ -153,6 +171,167 @@ namespace Pronounce {
 
 		nearPlatformToMidIndex = purePursuit->addPath(nearPlatformToMid);
 
+		QuadraticSplinePath leftAllianceToRightHomeZone = QuadraticSplinePath();
+
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(30.0, 11.4), Vector(0, M_PI_2)));
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(15.2, 35.0), Vector(0, 0.0)));
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(46.4, 46.4), Vector(0, M_PI_2)));
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(80, 46.4), Vector(0, M_PI_2)));
+
+		leftAllianceToRightHomeZoneIndex = purePursuit->addPath(leftAllianceToRightHomeZone.getPath(0.1));
+
+		QuadraticSplinePath rightHomeZoneToRightAlliance = QuadraticSplinePath();
+
+		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(80, 46.4), Vector(15.0, -M_PI_2 - M_PI_4)));
+		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(125.0, 30), Vector(15.0, -M_PI_2 - M_PI_4)));
+
+		rightHomeZoneToRightAllianceIndex = purePursuit->addPath(rightHomeZoneToRightAlliance.getPath(0.1));
+
+		QuadraticSplinePath rightAllianceToRightHomeZone = QuadraticSplinePath();
+
+		rightAllianceToRightHomeZone.addPoint(SplinePoint(Point(130.0, 30), Vector(15.0, -M_PI_2 - M_PI_4)));
+		rightAllianceToRightHomeZone.addPoint(SplinePoint(Point(80, 30), Vector(15.0, -M_PI_2 - M_PI_4)));
+
+		rightAllianceToRightHomeZoneIndex = purePursuit->addPath(rightAllianceToRightHomeZone.getPath(0.1));
+
+		QuadraticSplinePath rightAllianceToRightRings = QuadraticSplinePath();
+
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(130.0, 26), 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();
+
+		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)));
+
+		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.1));
+
+		//SECTION Skills
+
+	// Left home zone to left neutral goal
+		QuadraticSplinePath leftHomeZoneToLeftNeutralGoal;
+
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(23.2, 11.4), Vector(30, M_PI_4 * 1.5)));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(48, 46.8), Vector(20, 0)));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(35, 66), Vector(20, 0)));
+
+		int leftHomeZoneToLeftNeutralGoalIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.01));
+
+		QuadraticSplinePath leftNeutralGoalToFarHomeZone;
+
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 66), Vector(30, -M_PI_4)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(70.3, 93.9), Vector(30, 0)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(70.3, 107), Vector(20, 0)));
+
+		int leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.01));
+
+		QuadraticSplinePath farHomeZoneToMidNeutralGoal;
+
+		farHomeZoneToMidNeutralGoal.addPoint(SplinePoint(Point(70.3, 107), Vector(20, 0)));
+		farHomeZoneToMidNeutralGoal.addPoint(SplinePoint(Point(70.3, 75), Vector(30, 0)));
+
+		int farHomeZoneToMidNeutralGoalIndex = purePursuit->addPath(farHomeZoneToMidNeutralGoal.getPath(0.01));
+
+		QuadraticSplinePath midNeutralGoalToPlatform;
+
+		midNeutralGoalToPlatform.addPoint(SplinePoint(Point(70.3, 75), Vector(30, 0)));
+		midNeutralGoalToPlatform.addPoint(SplinePoint(Point(75, 107), Vector(30, 0)));
+
+		int midNeutralGoalToPlatformIndex = purePursuit->addPath(midNeutralGoalToPlatform.getPath(0.01));
+
+		QuadraticSplinePath farPlatformToDropOffGoal;
+
+		farPlatformToDropOffGoal.addPoint(SplinePoint(Point(65, 107), Vector(5, 0)));
+		farPlatformToDropOffGoal.addPoint(SplinePoint(Point(65, 80), Vector(5, 0)));
+
+		int farPlatformToDropOffGoalIndex = purePursuit->addPath(farPlatformToDropOffGoal.getPath(0.01));
+
+		QuadraticSplinePath goalDropOffToFarLeftAlliance;
+
+		goalDropOffToFarLeftAlliance.addPoint(SplinePoint(Point(65, 80), Vector(15, 0)));
+		goalDropOffToFarLeftAlliance.addPoint(SplinePoint(Point(18, 97), Vector(15, M_PI_4)));
+
+		int goalDropOffToFarLeftAllianceIndex = purePursuit->addPath(goalDropOffToFarLeftAlliance.getPath(0.01));
+
+		QuadraticSplinePath farLeftAllianceToLeftRings;
+
+		farLeftAllianceToLeftRings.addPoint(SplinePoint(Point(18, 97), Vector(15, M_PI_4 + M_PI)));
+		farLeftAllianceToLeftRings.addPoint(SplinePoint(Point(23.2, 70.3), Vector(15, M_PI)));
+
+		int farLeftAllianceToLeftRingsIndex = purePursuit->addPath(farLeftAllianceToLeftRings.getPath(0.01));
+
+		QuadraticSplinePath leftRingsToGoalDrop;
+
+		leftRingsToGoalDrop.addPoint(SplinePoint(Point(23.2, 70.3), Vector(15, 0)));
+		leftRingsToGoalDrop.addPoint(SplinePoint(Point(60, 73), Vector(2, -M_PI_4)));
+		leftRingsToGoalDrop.addPoint(SplinePoint(Point(75, 107), Vector(10, 0)));
+
+		int leftRingsToGoalDropIndex = purePursuit->addPath(leftRingsToGoalDrop.getPath(0.01));
+
+		QuadraticSplinePath farPlatformToEnterHomezone;
+
+		farPlatformToEnterHomezone.addPoint(SplinePoint(Point(75, 107), Vector(2, 0)));
+		farPlatformToEnterHomezone.addPoint(SplinePoint(Point(75, 95), Vector(2, 0)));
+
+		int farPlatformToEnterHomezoneIndex = purePursuit->addPath(farPlatformToEnterHomezone.getPath(0.01));
+
+		QuadraticSplinePath enterHomeZoneToRightHomeZone;
+
+		enterHomeZoneToRightHomeZone.addPoint(SplinePoint(Point(75, 95), Vector(10, 0)));
+		enterHomeZoneToRightHomeZone.addPoint(SplinePoint(Point(129.2, 105.7), Vector(10, -M_PI_2)));
+
+		int enterHomeZoneToRightHomeZoneIndex = purePursuit->addPath(enterHomeZoneToRightHomeZone.getPath(0.01));
+
+		QuadraticSplinePath rightHomeZoneToFarRightAllianceGoal;
+
+		rightHomeZoneToFarRightAllianceGoal.addPoint(SplinePoint(Point(129.2, 105.7), Vector(10, M_PI_2)));
+		rightHomeZoneToFarRightAllianceGoal.addPoint(SplinePoint(Point(103, 125), Vector(10, M_PI_4)));
+
+		int rightHomeZoneToFarRightAllianceGoalIndex = purePursuit->addPath(rightHomeZoneToFarRightAllianceGoal.getPath(0.01));
+
+		QuadraticSplinePath farRightAllianceGoalToNearPreloads;
+
+		farRightAllianceGoalToNearPreloads.addPoint(SplinePoint(Point(103, 125), Vector(40, M_PI_4)));
+		farRightAllianceGoalToNearPreloads.addPoint(SplinePoint(Point(129.2, 70.3), Vector(20, -M_PI)));
+		farRightAllianceGoalToNearPreloads.addPoint(SplinePoint(Point(117.5, 5), Vector(10, 0)));
+
+		int farRightAllianceGoalToNearPreloadsIndex = purePursuit->addPath(farRightAllianceGoalToNearPreloads.getPath(0.01));
+
+		SplinePath nearPlatformAlign;
+
+		nearPlatformAlign.addPoint(117.5, 5);
+		nearPlatformAlign.addPoint(117.5, 11.4);
+		nearPlatformAlign.addPoint(130, 11.4);
+
+		int nearPlatformAlignIndex = purePursuit->addPath(nearPlatformAlign.getPath(0.01));
+
+		QuadraticSplinePath leftHomeZoneToPlatform;
+
+		leftHomeZoneToPlatform.addPoint(SplinePoint(Point(130, 11.4), Vector(10, -M_PI_2)));
+		leftHomeZoneToPlatform.addPoint(SplinePoint(Point(80, 11.4), Vector(10, -M_PI_2)));
+
+		leftHomeZoneToPlatformIndex = purePursuit->addPath(leftHomeZoneToPlatform.getPath(0.01));
+
+		Path forward;
+
+		forward.addPoint(0, 16);
+		forward.addPoint(0, 60);
+
+		forwardIndex = purePursuit->addPath(forward);
+
+		Path backward;
+
+		forward.addPoint(0, 60);
+		forward.addPoint(0, 30);
+
+		backwardIndex = purePursuit->addPath(backward);
+
+		//!SECTION
+
+
 		printf("Array size: %d\n", purePursuit->getPaths().size());
 	}
 

diff --git a/include/chassis/abstractTankDrivetrain.hpp b/include/chassis/abstractTankDrivetrain.hpp
@@ -2,6 +2,7 @@
 
 #include "abstractDrivetrain.hpp"
 #include "utils/utils.hpp"
+#include <iostream>
 
 namespace Pronounce {
 	class AbstractTankDrivetrain : public AbstractDrivetrain {
@@ -20,8 +21,10 @@ namespace Pronounce {
 		}
 
 		void driveCurvature(double speed, double curvature) {
-			double leftSpeed = speed * (2.0 - curvature * trackWidth) / 2.0;
-			double rightSpeed = speed * (2.0 + curvature * trackWidth) / 2.0;
+			std::cout << "Speed: " << speed << " Curvature: " << curvature << std::endl;
+
+			double leftSpeed = speed * (2.0 + curvature * trackWidth) / 2.0;
+			double rightSpeed = speed * (2.0 - curvature * trackWidth) / 2.0;
 
 			double maxSpeed = max(abs(leftSpeed), abs(rightSpeed));
 

diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp
@@ -21,6 +21,7 @@ namespace Pronounce {
 		}
 
 		void tankSteerVelocity(double leftSpeed, double rightSpeed) {
+			// std::cout << "2Left speed: " << leftSpeed << " Right speed: " << rightSpeed << std::endl;
 			this->getLeftMotors().move_velocity(leftSpeed);
 			this->getRightMotors().move_velocity(rightSpeed);
 		}

diff --git a/include/driver/motorButton.hpp b/include/driver/motorButton.hpp
@@ -27,11 +27,18 @@ namespace Pronounce {
 
         int autonomousAuthority = 0;
 
+		bool goToHeight = false;
+		double height;
+		double length;
+
+		double gearRatio;
+
         int min = 0;
         int max = 0;
         pros::Motor* motor;
 
     public:
+        MotorButton();
         MotorButton(pros::Controller* controller, pros::Motor* motor,
         pros::controller_digital_e_t positiveButton = pros::E_CONTROLLER_DIGITAL_L1,
         pros::controller_digital_e_t negativeButton = pros::E_CONTROLLER_DIGITAL_L2,
@@ -40,10 +47,13 @@ namespace Pronounce {
         int negativeAuthority = 0,
         int min = 0,
         int max = 0);
-        MotorButton();
 
         void updateActuator();
 
+		void resetPosition(double currentPosition) {
+			this->motor->set_zero_position(-currentPosition);
+		}
+
         bool getGoToImmediately() {
             return this->goToImmediately;
         }
@@ -68,6 +78,38 @@ namespace Pronounce {
             this->autonomousPosition = autonomousPosition;
         }
 
+		bool getGoToHeight() {
+			return this->goToHeight;
+		}
+
+		void setGoToHeight(bool goToHeight) {
+			this->goToHeight = goToHeight;
+		}
+
+		double getLength(double length) {
+			return this->length;
+		}
+
+		void setLength(double length) {
+			this->length = length;
+		}
+
+		double getHeight() {
+			return height;
+		}
+
+		void setHeight(double height) {
+			this->height = height;
+		}
+
+		double getGearRatio() {
+			return gearRatio;
+		}
+
+		void setGearRatio(double gearRatio) {
+			this->gearRatio = gearRatio;
+		}
+
 		bool getJammed() {
 			return this->jammed;
 		}
@@ -116,4 +158,3 @@ namespace Pronounce {
     };
 
 } // namespace Pronounce
-