Tuning skills

alexDickhans · Apr 17, 2024 · 347a3a0 · 347a3a0
1 parent 7745835
commit 347a3a0
Show file tree

Hide file tree

Showing 14 changed files with 73 additions and 170 deletions.
diff --git a/include/hardware/hardware.hpp b/include/hardware/hardware.hpp
@@ -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
@@ -78,7 +78,6 @@ void far6Ball(void* args) {
 	pathFollower->setMotionProfile(far_6_2);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	backRightWingStateController->ud();
 	turnTo(-45_deg, 0.6_s, counterclockwise);
 
 	frontLeftWingStateController->sb(frontLeftWingOut);
@@ -119,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
@@ -133,26 +131,25 @@ 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);
 
-	move(20_in, speedProfileConstraints, 0.0, -80_deg);
-	backRightWingStateController->ud();
-	backLeftWingStateController->ud();
+	move(-20_in, speedProfileConstraints, 0.0, -80_deg);
+	frontRightWingStateController->ud();
+	frontLeftWingStateController->ud();
 
 	turnTo(-70_deg, 1.0_s, closest, 12000);
 
+	move(-8_in, speedProfileConstraints, 0.0, -80_deg);
+
 	turnTo(-170_deg, 0.4_s, closest);
 
 	pathFollower->setMotionProfile(skills_5);
@@ -177,15 +174,15 @@ void skills(void *args) {
 
 	move(-13_in, speedProfileConstraints, 0.0);
 
-	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, (70_in).Convert(millimetre)) * 1_mm, 30_in, 70_in);
 
 	turnTo(55_deg, 0.4_s, closest);
 
@@ -199,33 +196,29 @@ void skills(void *args) {
 					                                  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);
+	move(-20_in, speedProfileConstraints, 0.0, 90_deg);
 
-	turnTo(80_deg, 1.0_s, closest, 12000);
+	turnTo(75_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(80_deg, 1.0_s, closest, 12000);
+	turnTo(75_deg, 1.0_s, closest, 12000);
 
 	pathFollower->setMotionProfile(skills_9);
 	drivetrainStateController->sb(pathFollower)->wait();
 
 	pros::Task::delay(3000);
-
-	backLeftWingStateController->ud();
 }
 
 void safeCloseAWP(void *args) {

diff --git a/static/skills_3.json b/static/skills_3.json
@@ -7,20 +7,20 @@
       "stopEnd": false,
       "paths": [
         {
-          "x": 2.551,
+          "x": 2.481,
           "y": 1.441
         },
         {
-          "x": 2.629,
+          "x": 2.559,
           "y": 0.994
         },
         {
-          "x": 2.778,
-          "y": 0.492
+          "x": 2.67,
+          "y": 0.569
         },
         {
-          "x": 3.001,
-          "y": 0.475
+          "x": 2.893,
+          "y": 0.552
         }
       ],
       "constraints": {
@@ -33,20 +33,20 @@
       "stopEnd": false,
       "paths": [
         {
-          "x": 2.999,
-          "y": 0.476
+          "x": 2.896,
+          "y": 0.553
         },
         {
-          "x": 3.243,
-          "y": 0.458
+          "x": 3.078,
+          "y": 0.543
         },
         {
-          "x": 3.341,
-          "y": 0.98
+          "x": 3.328,
+          "y": 0.982
         },
         {
-          "x": 3.391,
-          "y": 1.543
+          "x": 3.386,
+          "y": 1.546
         }
       ],
       "constraints": {
@@ -59,20 +59,20 @@
       "stopEnd": false,
       "paths": [
         {
-          "x": 3.39,
-          "y": 1.543
+          "x": 3.371,
+          "y": 1.402
         },
         {
-          "x": 3.537,
-          "y": 3.364
+          "x": 3.51,
+          "y": 3.223
         },
         {
-          "x": 3.162,
-          "y": 3.642
+          "x": 3.152,
+          "y": 3.613
         },
         {
-          "x": 2.262,
-          "y": 3.54
+          "x": 2.257,
+          "y": 3.607
         }
       ],
       "constraints": {

diff --git a/static/skills_5.json b/static/skills_5.json
@@ -15,8 +15,8 @@
           "y": 2.961
         },
         {
-          "x": 2.669,
-          "y": 2.227
+          "x": 2.707,
+          "y": 2.238
         },
         {
           "x": 2.459,
@@ -57,7 +57,7 @@
   ],
   "commands": [
     {
-      "t": 0.0,
+      "t": 0.36,
       "name": "frontRightWingOut"
     }
   ]

diff --git a/static/skills_6.json b/static/skills_6.json
@@ -67,8 +67,8 @@
           "y": 2.175
         },
         {
-          "x": 1.807,
-          "y": 2.104
+          "x": 1.826,
+          "y": 2.129
         },
         {
           "x": 1.738,
@@ -89,16 +89,16 @@
           "y": 2.394
         },
         {
-          "x": 1.701,
-          "y": 2.573
+          "x": 1.68,
+          "y": 2.57
         },
         {
-          "x": 1.698,
-          "y": 2.925
+          "x": 1.65,
+          "y": 2.924
         },
         {
-          "x": 1.7,
-          "y": 3.233
+          "x": 1.652,
+          "y": 3.232
         }
       ],
       "constraints": {

diff --git a/static/skills_6_5.json b/static/skills_6_5.json
@@ -93,12 +93,12 @@
           "y": 2.722
         },
         {
-          "x": 1.902,
-          "y": 2.912
+          "x": 1.816,
+          "y": 2.916
         },
         {
-          "x": 2.005,
-          "y": 3.202
+          "x": 1.851,
+          "y": 3.213
         }
       ],
       "constraints": {

diff --git a/static/skills_7.json b/static/skills_7.json
@@ -67,8 +67,8 @@
           "y": 2.06
         },
         {
-          "x": 1.666,
-          "y": 2.067
+          "x": 1.608,
+          "y": 1.993
         },
         {
           "x": 1.712,