Add rotation changes for clockwise and counterclockwise

alexDickhans · Apr 9, 2024 · 744e724 · 744e724
1 parent a84ba0b
commit 744e724
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 7 deletions.
diff --git a/include/motionControl/rotationController.hpp b/include/motionControl/rotationController.hpp
@@ -9,8 +9,14 @@
 #include "pros/rtos.hpp"
 #include "hardware/hardware.hpp"
 
-namespace Pronounce
-{
+namespace Pronounce {
+	enum RotationOptimizer {
+		none,
+		closest,
+		clockwise,
+		counterclockwise
+	};
+
 	class RotationController : public Behavior {
 	private:
 		PID rotationPID;
@@ -22,15 +28,42 @@ namespace Pronounce
 
 		double idleSpeed = 0.0;
 
+		RotationOptimizer rotationOptimizer;
+
 	public:
-		RotationController(std::string name, TankDrivetrain& drivetrain, std::function<Angle()> angleFunction, PID rotationPID, Angle target, double idleSpeed = 0.0) : drivetrain(drivetrain), rotationPID(rotationPID), angleFunction(std::move(angleFunction)), Behavior(name) {
+		RotationController(std::string name, TankDrivetrain& drivetrain, std::function<Angle()> angleFunction, PID rotationPID, Angle target, double idleSpeed = 0.0, RotationOptimizer rotationOptimizer = RotationOptimizer::none) : drivetrain(drivetrain), rotationPID(rotationPID), angleFunction(std::move(angleFunction)), Behavior(name), rotationOptimizer(rotationOptimizer) {
 			rotationPID.setTarget(target.Convert(radian));
 			this->idleSpeed = idleSpeed;
 			this->target = target;
 		}
 
 		void initialize() {
 
+			Angle currentPosition = angleFunction();
+
+			switch (rotationOptimizer) {
+				case closest:
+					target = currentPosition + angleDifference(target.getValue(), currentPosition.getValue()) * 1_rad;
+					break;
+				case clockwise:
+					target = angleDifference(target.getValue(), currentPosition.getValue()) * 1_rad;
+					if (target.Convert(radian) < 0.0) {
+						target += 360_deg;
+					}
+					target += currentPosition;
+					break;
+				case counterclockwise:
+					target = angleDifference(target.getValue(), currentPosition.getValue()) * 1_rad;
+					if (target.Convert(radian) > 0.0) {
+						target -= 360_deg;
+					}
+					target += currentPosition;
+					break;
+				case none:
+				default:
+					break;
+			}
+
 			rotationPID.reset();
 			rotationPID.setTarget(target.Convert(radian));
 

diff --git a/include/stateMachine/behaviors/catapult/initCatapult.hpp b/include/stateMachine/behaviors/catapult/initCatapult.hpp
@@ -8,6 +8,7 @@ namespace Pronounce {
 	PID cataPID(9.0, 0.0, 12.0, 0.0);
 	auto catapultFire = std::make_shared<Catapult>("CatapultFire", catapultMotors, 1.0);
 	auto catapultHold = std::make_shared<CatapultHold>("CatapultHold", catapultMotors, 1.15714285714, &cataPID);
+	auto catapultHoldHigh = std::make_shared<CatapultHold>("CatapultHold", catapultMotors, 0.7, &cataPID);
 
 	auto catapultStateController = std::make_shared<StateController>("CatapultStateController", catapultHold);
 

diff --git a/src/main.cpp b/src/main.cpp
@@ -23,10 +23,10 @@ SMOOTH_SPLINE_PATH_ASSET(skills_10);
 SMOOTH_SPLINE_PATH_ASSET(close_mid_rush_elim);
 SMOOTH_SPLINE_PATH_ASSET(close_rush_mid_2);
 
-void turnTo(Angle angle, QTime waitTimeMS, double idleSpeed = 0.0) {
+void turnTo(Angle angle, QTime waitTimeMS, RotationOptimizer rotationOptimizer = none, double idleSpeed = 0.0) {
 	auto angleRotation = std::make_shared<RotationController>("AngleTurn", drivetrain,
 	                                                          [&]() -> Angle { return imuOrientation.getAngle(); },
-	                                                          turningPid, angle, idleSpeed);
+	                                                          turningPid, angle, idleSpeed, rotationOptimizer);
 
 	drivetrainStateController->sb(angleRotation);
 
@@ -129,6 +129,8 @@ void skills(void *args) {
 
 	turnTo(-15.0_deg, 300_ms);
 
+	catapultStateController->sb(catapultHoldHigh);
+
 	pathFollower->setMotionProfile(skills_3);
 	drivetrainStateController->sb(pathFollower)->wait();
 
@@ -174,7 +176,7 @@ void skills(void *args) {
 	drivetrainStateController->sb(pathFollower)->wait();
 
 	drivetrainStateController->sb(
-			std::make_shared<RotationController>("MatchloadRotationController", drivetrain, [&]() -> auto { return imuOrientation.getAngle(); }, turningPid, 0_deg));
+			std::make_shared<RotationController>("MatchloadRotationController", drivetrain, [&]() -> auto { return imuOrientation.getAngle(); }, turningPid, 0_deg, 0.0, closest));
 
 	QLength wallDistance = getDistanceSensorMedian(wallDistanceSensor, 3, (70_in).Convert(millimetre)) * 1_mm;
 
@@ -195,7 +197,7 @@ void skills(void *args) {
 	turningPid.setTurnPid(true);
 	movingTurnPid.setTurnPid(true);
 
-	turnTo(135_deg, 0.4_s);
+	turnTo(135_deg, 0.4_s, closest);
 
 	pathFollower->setMotionProfile(skills_8);
 	drivetrainStateController->sb(pathFollower)->wait();