Merge pull request #134 from ad101-lab/autons

autons
alexDickhans · May 3, 2022 · 165fcc8 · 165fcc8
2 parents ee4c2a5 + 5d30d5e
commit 165fcc8
Show file tree

Hide file tree

Showing 11 changed files with 914 additions and 572 deletions.
diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
diff --git a/include/chassis/abstractTankDrivetrain.hpp b/include/chassis/abstractTankDrivetrain.hpp
@@ -37,10 +37,27 @@ namespace Pronounce {
 			this->tankSteerVelocity(leftSpeed, rightSpeed);
 		}
 
+		void driveCurvatureVoltage(double speed, double curvature) {			
+			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));
+
+			if (maxSpeed > abs(speed)) {
+				double multiplier = abs(speed) / maxSpeed;
+				leftSpeed *= multiplier;
+				rightSpeed *= multiplier;
+			}
+
+			this->tankSteerVoltage(leftSpeed, rightSpeed);
+		}
+
 		virtual void skidSteerVelocity(double speed, double turn) {}
 
 		virtual void tankSteerVelocity(double leftSpeed, double rightSpeed) {}
 
+		virtual void tankSteerVoltage(double leftSpeed, double rightSpeed) {}
+
 		~AbstractTankDrivetrain();
 	};
 } // namespace Pronounce

diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp
@@ -24,11 +24,21 @@ namespace Pronounce {
 			this->getRightMotors().move_velocity(speed - turn);
 		}
 
+		void skidSteerVoltage(double speed, double turn) {
+			this->getLeftMotors().move_voltage(speed + turn);
+			this->getRightMotors().move_voltage(speed - turn);
+		}
+
 		void tankSteerVelocity(double leftSpeed, double rightSpeed) {
 			this->getLeftMotors().move_velocity(leftSpeed);
 			this->getRightMotors().move_velocity(rightSpeed);
 		}
 
+		void tankSteerVoltage(double leftSpeed, double rightSpeed) {
+			this->getLeftMotors().move_voltage(leftSpeed);
+			this->getRightMotors().move_voltage(rightSpeed);
+		}
+
 		~TankDrivetrain();
 	};
 } // namespace Pronounce

diff --git a/include/feedbackControllers/bangBang.hpp b/include/feedbackControllers/bangBang.hpp
@@ -24,6 +24,10 @@ namespace Pronounce {
 		 */
 		double update(double input);
 
+		double getLastInput() {
+			return lastInput;
+		}
+
 		double getSetPoint() {
 			return setPoint;
 		}

diff --git a/include/motionControl/purePursuit.hpp b/include/motionControl/purePursuit.hpp
@@ -24,6 +24,7 @@ namespace Pronounce {
 		Vector localLookaheadVector;
 		Vector normalizedLookaheadVector;
 		double curvature;
+		double distanceFromEnd;
 	};
 
 	/**

diff --git a/include/motionControl/tankPurePursuit.hpp b/include/motionControl/tankPurePursuit.hpp
@@ -17,6 +17,8 @@ namespace Pronounce {
 		PID* turnPid;
 
 		double lastUpdateTime = 0;
+
+		bool useVoltage = false;
 	public:
 		TankPurePursuit(AbstractTankDrivetrain* drivetrain);
 		TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance);
@@ -27,6 +29,14 @@ namespace Pronounce {
 
 		void stop();
 
+		bool getUseVoltage() {
+			return useVoltage;
+		}
+
+		void setUseVoltage(bool useVoltage) {
+			this->useVoltage = useVoltage;
+		}
+
 		AbstractTankDrivetrain* getDrivetrain() {
 			return drivetrain;
 		}

diff --git a/pathTest.cpp b/pathTest.cpp
@@ -276,12 +276,14 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
+		QuadraticSplinePath rightNeutralGoalToNearRIghtPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(60, 70), Vector(20, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(75, 70), Vector(20, 0)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(105.7, 80), Vector(25, M_PI)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(80, 60), Vector(20, M_PI_2)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(25, 60), Vector(20, M_PI_2)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(25, 36), Vector(20, -M_PI_2*0.8)));
 
-	smoothPath = midGoalToFarPlatform.getPath(0.1);
+	smoothPath = rightNeutralGoalToNearRIghtPlatform.getPath(0.1);
 
 	paths.emplace_back(smoothPath);