Merge pull request #253 from alexDickhans/autons-4-16

More worlds autons
alexDickhans · Apr 18, 2024 · fc8c96e · fc8c96e
2 parents 6f2ad44 + 13c6ebc
commit fc8c96e
Show file tree

Hide file tree

Showing 22 changed files with 198 additions and 286 deletions.
diff --git a/include/auton.h b/include/auton.h
@@ -1 +1 @@
-#define AUTON 2
+#define AUTON 6
diff --git a/include/hardware/hardware.hpp b/include/hardware/hardware.hpp
@@ -24,7 +24,7 @@ namespace Pronounce {
 
 	pros::Mutex robotMutex;
 
-    #define IS_SKILLS (AUTON == 5)
+    #define IS_SKILLS (AUTON == 6)
 
 	pros::Controller masterController(pros::E_CONTROLLER_MASTER);
 	RobotJoystick master = RobotJoystick(masterController);
@@ -43,8 +43,6 @@ namespace Pronounce {
 
 	pros::adi::DigitalOut frontLeftSolenoid('G', false);
 	pros::adi::DigitalOut frontRightSolenoid('F', false);
-	pros::adi::DigitalOut backLeftSolenoid('E', false);
-	pros::adi::DigitalOut backRightSolenoid('D', false);
 
 	pros::MotorGroup intakeMotors({17}, pros::MotorGears::blue, pros::MotorEncoderUnits::rotations);
 	// Inertial Measurement Unit

diff --git a/include/stateMachine/behaviors/catapult/initCatapult.hpp b/include/stateMachine/behaviors/catapult/initCatapult.hpp
@@ -8,7 +8,6 @@ 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.5, &cataPID);
 
 	auto catapultStateController = std::make_shared<StateController>("CatapultStateController", catapultHold);
 

diff --git a/include/stateMachine/behaviors/wings/initWings.hpp b/include/stateMachine/behaviors/wings/initWings.hpp
@@ -9,15 +9,9 @@ namespace Pronounce {
 	auto frontLeftWingOut = std::make_shared<Wing>("WingOut", frontLeftSolenoid, true);
 	auto frontRightWingIn = std::make_shared<Wing>("WingIn", frontRightSolenoid, false);
 	auto frontRightWingOut = std::make_shared<Wing>("WingOut", frontRightSolenoid, true);
-	auto backLeftWingIn = std::make_shared<Wing>("WingIn", backLeftSolenoid, false);
-	auto backLeftWingOut = std::make_shared<Wing>("WingOut", backLeftSolenoid, true);
-	auto backRightWingIn = std::make_shared<Wing>("WingIn", backRightSolenoid, false);
-	auto backRightWingOut = std::make_shared<Wing>("WingOut", backRightSolenoid, true);
 
 	auto frontLeftWingStateController = std::make_shared<StateController>("FrontLeftWing", frontLeftWingIn);
 	auto frontRightWingStateController = std::make_shared<StateController>("FrontRightWin", frontRightWingIn);
-	auto backLeftWingStateController = std::make_shared<StateController>("BackLeftWing", backLeftWingIn);
-	auto backRightWingStateController = std::make_shared<StateController>("BackRightWing", backRightWingIn);
 
 	void initWings() {
 		Log("Wings Init");

diff --git a/include/stateMachine/state/competition/auton.hpp b/include/stateMachine/state/competition/auton.hpp
@@ -35,22 +35,6 @@ namespace Pronounce {
 			frontRightWingStateController->sb(frontRightWingIn);
 		});
 
-		pathFollower->addCommandMapping("backRightWingOut", [&]() -> void {
-			backRightWingStateController->sb(backRightWingOut);
-		});
-
-		pathFollower->addCommandMapping("backRightWingIn", [&]() -> void {
-			backRightWingStateController->sb(backRightWingIn);
-		});
-
-		pathFollower->addCommandMapping("backLeftWingOut", [&]() -> void {
-			backLeftWingStateController->sb(backLeftWingOut);
-		});
-
-		pathFollower->addCommandMapping("backLeftWingIn", [&]() -> void {
-			backLeftWingStateController->sb(backLeftWingIn);
-		});
-
 		pathFollower->addCommandMapping("frontWingsOut", [&]() -> void {
 			frontLeftWingStateController->sb(frontLeftWingOut);
 			frontRightWingStateController->sb(frontRightWingOut);
@@ -61,16 +45,6 @@ namespace Pronounce {
 			frontRightWingStateController->sb(frontRightWingIn);
 		});
 
-		pathFollower->addCommandMapping("backWingsOut", [&]() -> void {
-			backLeftWingStateController->sb(backLeftWingOut);
-			backRightWingStateController->sb(backRightWingOut);
-		});
-
-		pathFollower->addCommandMapping("backWingsIn", [&]() -> void {
-			backLeftWingStateController->sb(backLeftWingIn);
-			backRightWingStateController->sb(backRightWingIn);
-		});
-
 		pathFollower->addCommandMapping("hangOut", [&]() -> void {
 			winchStateController->sb(winchUp);
 		});

diff --git a/include/stateMachine/state/competition/enabled.hpp b/include/stateMachine/state/competition/enabled.hpp
@@ -10,8 +10,6 @@ namespace Pronounce {
 		stateControllers.addBehavior(drivetrainStateController);
 		stateControllers.addBehavior(frontLeftWingStateController);
 		stateControllers.addBehavior(frontRightWingStateController);
-		stateControllers.addBehavior(backLeftWingStateController);
-		stateControllers.addBehavior(backRightWingStateController);
 		stateControllers.addBehavior(winchStateController);
 		stateControllers.addBehavior(winchStateExtensionController);
 		stateControllers.addBehavior(catapultStateController);
@@ -48,7 +46,7 @@ namespace Pronounce {
 			// See if the distance sensor detects a new object within 1 inch of the sensor
 			if (catapultDistance.get() * 1_mm <
 			    0.75_in // see if an object is detected by the distance sensor on the catapult
-			    && lastDistance > 0.75_in && pros::millis() * 1_ms - lastCount >
+			    && lastDistance > 1.5_in && pros::millis() * 1_ms - lastCount >
 			                                 0.25_s) { // If the last distance sensor reading was greater than an inch indicates that the
 				// triball is moving closer to the sensor, meaning that there is a new triball
 

diff --git a/include/stateMachine/state/competition/teleop.hpp b/include/stateMachine/state/competition/teleop.hpp
@@ -39,15 +39,6 @@ namespace Pronounce {
 				}
 			});
 
-			controller1.onPressed(E_CONTROLLER_DIGITAL_L1, [&]() -> void {
-				backLeftWingStateController->sb(std::make_shared<Until>(backLeftWingOut, [=, this]() -> bool {
-					return !controller1.get_digital(E_CONTROLLER_DIGITAL_L1);
-				}));
-				backRightWingStateController->sb(std::make_shared<Until>(backRightWingOut, [&]() -> bool {
-					return !controller1.get_digital(E_CONTROLLER_DIGITAL_L1);
-				}));
-			});
-
 			controller1.onPressed(E_CONTROLLER_DIGITAL_R2, [&]() -> void {
 				if (controller1.get_digital(E_CONTROLLER_DIGITAL_Y)) {
 					winchStateExtensionController->sb(winchCSequence);
@@ -89,8 +80,6 @@ namespace Pronounce {
 		void exit() override {
 			Log("Exit");
 			drivetrainStateController->setDefaultBehavior(drivetrainStopped);
-			backLeftWingStateController->ud();
-			backRightWingStateController->ud();
 			frontLeftWingStateController->ud();
 			frontRightWingStateController->ud();
 			drivetrainStateController->ud();

diff --git a/src/main.cpp b/src/main.cpp
@@ -12,6 +12,7 @@ SMOOTH_SPLINE_PATH_ASSET(far_6_1)
 SMOOTH_SPLINE_PATH_ASSET(far_6_2)
 SMOOTH_SPLINE_PATH_ASSET(far_6_3)
 SMOOTH_SPLINE_PATH_ASSET(far_6_4)
+SMOOTH_SPLINE_PATH_ASSET(far_6_5)
 SMOOTH_SPLINE_PATH_ASSET(skills_1)
 SMOOTH_SPLINE_PATH_ASSET(skills_2)
 SMOOTH_SPLINE_PATH_ASSET(skills_3)
@@ -77,8 +78,6 @@ void far6Ball(void* args) {
 	pathFollower->setMotionProfile(far_6_2);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	backRightWingStateController->ud();
-	move(15_in, speedProfileConstraints, 0.0, -250_deg);
 	turnTo(-45_deg, 0.6_s, counterclockwise);
 
 	frontLeftWingStateController->sb(frontLeftWingOut);
@@ -87,25 +86,24 @@ void far6Ball(void* args) {
 
 	pathFollower->setMotionProfile(far_6_3);
 	drivetrainStateController->sb(pathFollower)->wait();
-
-	move(-15_in, defaultProfileConstraints, 0.0, -430_deg);
-
-//	frontLeftWingStateController->ud();
-
 }
 
 void far6BallElim(void* args) {
 	far6Ball(args);
 
+	move(-15_in, defaultProfileConstraints, 0.0, -430_deg);
 	pathFollower->setMotionProfile(far_6_3);
 	drivetrainStateController->sb(pathFollower)->wait();
 	pathFollower->setMotionProfile(far_6_4);
 	drivetrainStateController->sb(pathFollower)->wait();
 }
 
 void far6BallAWP(void* args) {
+	far6Ball(args);
 	pathFollower->setMotionProfile(far_6_4);
 	drivetrainStateController->sb(pathFollower)->wait();
+	pathFollower->setMotionProfile(far_6_5);
+	drivetrainStateController->sb(pathFollower)->wait();
 }
 
 void skills(void *args) {
@@ -120,7 +118,6 @@ void skills(void *args) {
 			                                     21.0_deg, -800.0));
 	auton->resetTriballs();
 	pros::Task::delay(100);
-	backLeftWingStateController->sb(backLeftWingOut);
 	pros::Task::delay(900);
 
 	// Wait until the catapult triballs shot has increased to 44 triballs
@@ -134,25 +131,24 @@ void skills(void *args) {
 	pathFollower->setMotionProfile(skills_2);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	frontRightWingStateController->sb(frontRightWingIn);
-	frontLeftWingStateController->sb(frontLeftWingIn);
+	frontRightWingStateController->ud();
 
-	turnTo(180.0_deg, 600_ms, clockwise);
-
-	catapultStateController->sb(catapultHoldHigh);
+	turnTo(180.0_deg, 400_ms, clockwise);
 
 	pathFollower->setMotionProfile(skills_3);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(20_in, speedProfileConstraints, 0.0, -80_deg);
+	move(-20_in, speedProfileConstraints, 0.0, -80_deg);
+
+	turnTo(-70_deg, 1.0_s, closest, 12000);
 
-	turnTo(-70_deg, 1.0_s, closest, -12000);
+	move(-20_in, speedProfileConstraints, 0.0, -80_deg);
+	frontRightWingStateController->ud();
+	frontLeftWingStateController->ud();
 
-	move(20_in, speedProfileConstraints, 0.0, -80_deg);
-	backRightWingStateController->ud();
-	backLeftWingStateController->ud();
+	turnTo(-70_deg, 1.0_s, closest, 12000);
 
-	turnTo(-70_deg, 1.0_s, closest, -12000);
+	move(-8_in, speedProfileConstraints, 0.0, -80_deg);
 
 	turnTo(-170_deg, 0.4_s, closest);
 
@@ -176,17 +172,17 @@ void skills(void *args) {
 	pathFollower->setMotionProfile(skills_7);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-13_in, speedProfileConstraints, 0.0);
+	move(-10_in, speedProfileConstraints, 0.0, 0.0_deg);
 
-	move(18_in, speedProfileConstraints, 0.0, 0.0_deg);
+	move(18_in, speedProfileConstraints, 0.0, 0.0_deg, 0.0, 70_in/second);
 
 	pathFollower->setMotionProfile(skills_7_5);
 	drivetrainStateController->sb(pathFollower)->wait();
 
 	drivetrainStateController->sb(
 			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;
+	QLength wallDistance = std::clamp(getDistanceSensorMedian(wallDistanceSensor, 3, (80_in).Convert(millimetre)) * 1_mm, 30_in, 80_in);
 
 	turnTo(55_deg, 0.4_s, closest);
 
@@ -195,38 +191,36 @@ void skills(void *args) {
 					{PathPlanner::BezierSegment(PathPlanner::Point(
 							                                  wallDistance, 76_in),
 					                                  PathPlanner::Point(
-							                                  wallDistance - 14_in,
+							                                  0.95 * wallDistance - 14_in,
 							                                  66_in),
 					                                  PathPlanner::Point(
 							                                  15_in, 52_in),
 					                                  PathPlanner::Point(
-							                                  16_in, 32_in), true, true,
+							                                  20_in, 32_in), true, true,
 					                                  pushingProfileConstraints)}));
+
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	turnTo(-55_deg, 0.4_s, closest);
+	turnTo(-45_deg, 0.4_s, closest);
 
 	pathFollower->setMotionProfile(skills_8);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(20_in, speedProfileConstraints, 0.0, 90_deg);
-
-	turnTo(80_deg, 1.0_s, closest, -12000);
-
-	winchStateController->sb(winchUp);
-
-	move(20_in, speedProfileConstraints, 0.0, 90_deg);
+	move(-20_in, speedProfileConstraints, 0.0, 90_deg);
 
-	backLeftWingStateController->ud();
+	turnTo(75_deg, 1.0_s, closest, 12000);
 
-	turnTo(80_deg, 1.0_s, closest, -12000);
+	move(-20_in, speedProfileConstraints, 0.0, 90_deg);
 
 	pathFollower->setMotionProfile(skills_9);
+
+	turnTo(75_deg, 1.0_s, closest, 12000);
+
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	pros::Task::delay(3000);
+	winchStateController->sb(winchC);
 
-	backLeftWingStateController->ud();
+	pros::Task::delay(3000);
 }
 
 void safeCloseAWP(void *args) {
@@ -397,7 +391,7 @@ void initialize() {
 	pros::Task display(updateDisplay, TASK_PRIORITY_MIN + 1, TASK_STACK_DEPTH_DEFAULT, "updateDisplay");
 
 #if AUTON == 0
-	auton->setAuton(far6Ball);
+	auton->setAuton(far6BallElim);
 #elif AUTON == 1
 	auton->setAuton(far6BallAWP);
 #elif AUTON == 2
@@ -481,7 +475,7 @@ void autonomous() {
 void opcontrol() {
 
 	// Causes the programming skills code to only run during skills
-#if AUTON == 5
+#if AUTON == 6
 	robotMutex.lock();
 	auton->setAuton(skills);
 	competitionController->sb(std::make_shared<Until>(auton, [=]() -> auto {
@@ -496,4 +490,4 @@ void opcontrol() {
 	robotMutex.unlock();
 }
 
-// !SECTION
+// !SECTION
diff --git a/static/close_rush_mid_awp.json b/static/close_rush_mid_awp.json
@@ -7,16 +7,16 @@
       "stopEnd": false,
       "paths": [
         {
-          "x": 1.946,
-          "y": 1.237
+          "x": 2.097,
+          "y": 1.218
         },
         {
-          "x": 2.307,
-          "y": 1.248
+          "x": 2.458,
+          "y": 1.17
         },
         {
-          "x": 2.238,
-          "y": 1.006
+          "x": 2.369,
+          "y": 0.98
         },
         {
           "x": 3.558,
@@ -63,16 +63,16 @@
           "y": 0.297
         },
         {
-          "x": 3.33,
-          "y": 0.405
+          "x": 3.321,
+          "y": 0.303
         },
         {
-          "x": 3.316,
-          "y": 0.826
+          "x": 3.302,
+          "y": 0.692
         },
         {
-          "x": 2.981,
-          "y": 0.892
+          "x": 3.052,
+          "y": 0.736
         }
       ],
       "constraints": {
@@ -85,20 +85,20 @@
       "stopEnd": false,
       "paths": [
         {
-          "x": 2.981,
-          "y": 0.892
+          "x": 3.092,
+          "y": 0.888
         },
         {
-          "x": 3.271,
-          "y": 0.967
+          "x": 3.35,
+          "y": 0.933
         },
         {
-          "x": 3.36,
-          "y": 1.184
+          "x": 3.377,
+          "y": 1.18
         },
         {
-          "x": 3.347,
-          "y": 1.711
+          "x": 3.354,
+          "y": 1.522
         }
       ],
       "constraints": {