Merge pull request #248 from alexDickhans/skills-4-9

Skills 4-9
alexDickhans · Apr 10, 2024 · f96fa63 · f96fa63
2 parents d580ef3 + af14edf
commit f96fa63
Show file tree

Hide file tree

Showing 14 changed files with 174 additions and 166 deletions.
diff --git a/include/auton.h b/include/auton.h
@@ -1 +1 @@
-#define AUTON 5
+#define AUTON 0
diff --git a/include/hardware/hardware.hpp b/include/hardware/hardware.hpp
@@ -93,7 +93,14 @@ namespace Pronounce {
 		}
 
 		if (portsReport.empty()) {
-			master.getController().set_text(0, 0, "All good");
+			Log(std::to_string(pros::battery::get_capacity()))
+			if (pros::battery::get_capacity() < 50.0) {
+				master.getController().rumble("........");
+				pros::Task::delay(50);
+				master.getController().set_text(0, 0, string_format("Battery at: %.2f", pros::battery::get_capacity()));
+			} else {
+				master.getController().set_text(0, 0, "All good");
+			}
 		} else {
 			master.getController().set_text(0, 0, portsReport.c_str());
 			pros::Task::delay(50);

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/include/stateMachine/state/competition/teleop.hpp b/include/stateMachine/state/competition/teleop.hpp
@@ -64,6 +64,17 @@ namespace Pronounce {
 				}));
 			});
 
+			controller1.onPressed(E_CONTROLLER_DIGITAL_Y, [&]() -> void {
+					if (controller1.get_digital(E_CONTROLLER_DIGITAL_RIGHT)) {
+						winchStateController->sb(winchUp);
+					}
+			});
+
+			controller1.onPressed(E_CONTROLLER_DIGITAL_RIGHT, [&]() -> void {
+				if (controller1.get_digital(E_CONTROLLER_DIGITAL_Y)) {
+					winchStateController->sb(winchUp);
+				}
+			});
 
 			Enabled::initialize();
 		}

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);
 
@@ -94,6 +94,10 @@ void far6Ball(void* args) {
 	drivetrainStateController->sb(pathFollower)->wait();
 }
 
+void far6BallAWP(void* args) {
+	far6Ball(args);
+}
+
 void skills(void *args) {
 
 	imuOrientation.setRotation(135_deg);
@@ -123,9 +127,9 @@ void skills(void *args) {
 	frontRightWingStateController->sb(frontRightWingIn);
 	frontLeftWingStateController->sb(frontLeftWingIn);
 
-	turnTo(90_deg, 300_ms, 6000);
+	turnTo(-15.0_deg, 300_ms);
 
-	turnTo(-15.0_deg, 600_ms);
+	catapultStateController->sb(catapultHoldHigh);
 
 	pathFollower->setMotionProfile(skills_3);
 	drivetrainStateController->sb(pathFollower)->wait();
@@ -172,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;
 
@@ -193,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();
@@ -387,7 +391,7 @@ void initialize() {
 #if AUTON == 0
 	auton->setAuton(far6Ball);
 #elif AUTON == 1
-	auton->setAuton(far5BallAWP);
+	auton->setAuton(far6BallAWP);
 #elif AUTON == 2
 	auton->setAuton(safeCloseAWP);
 #elif AUTON == 3

diff --git a/static/close_mid_rush.json b/static/close_mid_rush.json
@@ -1,106 +1,6 @@
 {
   "startSpeed": 0.0,
   "endSpeed": 0.0,
-  "segments": [
-    {
-      "inverted": false,
-      "stopEnd": false,
-      "paths": [
-        {
-          "x": 2.646,
-          "y": 1.199
-        },
-        {
-          "x": 2.492,
-          "y": 1.434
-        },
-        {
-          "x": 2.231,
-          "y": 1.574
-        },
-        {
-          "x": 1.929,
-          "y": 1.661
-        }
-      ],
-      "constraints": {
-        "velocity": 76,
-        "accel": 120
-      }
-    },
-    {
-      "inverted": true,
-      "stopEnd": false,
-      "paths": [
-        {
-          "x": 1.929,
-          "y": 1.661
-        },
-        {
-          "x": 2.378,
-          "y": 1.429
-        },
-        {
-          "x": 2.244,
-          "y": 0.491
-        },
-        {
-          "x": 2.144,
-          "y": 0.044
-        }
-      ],
-      "constraints": {
-        "velocity": 76,
-        "accel": 120
-      }
-    },
-    {
-      "inverted": false,
-      "stopEnd": false,
-      "paths": [
-        {
-          "x": 2.782,
-          "y": 0.318
-        },
-        {
-          "x": 3.031,
-          "y": 0.313
-        },
-        {
-          "x": 3.401,
-          "y": 0.422
-        },
-        {
-          "x": 3.309,
-          "y": 0.7
-        }
-      ],
-      "constraints": {
-        "velocity": 76,
-        "accel": 120
-      }
-    }
-  ],
-  "commands": [
-    {
-      "t": 0.0,
-      "name": "intake"
-    },
-    {
-      "t": 2.0,
-      "name": "rightWingOut"
-    },
-    {
-      "t": 2.0,
-      "name": "awpOut"
-    },
-    {
-      "t": 2.06,
-      "name": "rightWingIn"
-    },
-    {
-      "t": 2.94,
-      "name": "awpIn"
-    }
-  ]
+  "segments": [],
+  "commands": []
 }
diff --git a/static/close_rush_mid_2.json b/static/close_rush_mid_2.json
@@ -1 +1,42 @@
-{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":3.016,"y":0.45},{"x":3.247,"y":0.555},{"x":3.429,"y":1.221},{"x":3.43,"y":1.485}],"constraints":{"velocity":35,"accel":120}}],"commands":[{"t":0.0,"name":"leftWingOut"},{"t":0.77,"name":"outtake"}]}
+{
+  "startSpeed": 0.0,
+  "endSpeed": 0.0,
+  "segments": [
+    {
+      "inverted": false,
+      "stopEnd": false,
+      "paths": [
+        {
+          "x": 3.016,
+          "y": 0.45
+        },
+        {
+          "x": 3.247,
+          "y": 0.555
+        },
+        {
+          "x": 3.429,
+          "y": 1.221
+        },
+        {
+          "x": 3.43,
+          "y": 1.485
+        }
+      ],
+      "constraints": {
+        "velocity": 35,
+        "accel": 120
+      }
+    }
+  ],
+  "commands": [
+    {
+      "t": 0.0,
+      "name": "leftWingOut"
+    },
+    {
+      "t": 0.77,
+      "name": "outtake"
+    }
+  ]
+}
diff --git a/static/skills_3.json b/static/skills_3.json
@@ -67,12 +67,12 @@
           "y": 3.356
         },
         {
-          "x": 3.161,
-          "y": 3.633
+          "x": 3.182,
+          "y": 3.641
         },
         {
-          "x": 2.286,
-          "y": 3.468
+          "x": 2.282,
+          "y": 3.539
         }
       ],
       "constraints": {

diff --git a/static/skills_5.json b/static/skills_5.json
@@ -15,12 +15,12 @@
           "y": 3.331
         },
         {
-          "x": 2.835,
-          "y": 2.338
+          "x": 2.846,
+          "y": 2.239
         },
         {
-          "x": 2.448,
-          "y": 2.29
+          "x": 2.459,
+          "y": 2.191
         }
       ],
       "constraints": {
@@ -33,12 +33,12 @@
       "stopEnd": false,
       "paths": [
         {
-          "x": 2.448,
-          "y": 2.293
+          "x": 2.459,
+          "y": 2.191
         },
         {
-          "x": 2.155,
-          "y": 2.238
+          "x": 2.166,
+          "y": 2.136
         },
         {
           "x": 1.992,
@@ -65,7 +65,7 @@
       "name": "frontLeftWingOut"
     },
     {
-      "t": 1.15,
+      "t": 1.13,
       "name": "frontLeftWingIn"
     }
   ]