From 347a3a0841d52ab5da34f59738efdb91c1a79f79 Mon Sep 17 00:00:00 2001 From: Alex Dickhans <55064247+alexDickhans@users.noreply.github.com> Date: Tue, 16 Apr 2024 18:49:40 -0600 Subject: [PATCH] Tuning skills --- include/hardware/hardware.hpp | 2 - .../behaviors/catapult/initCatapult.hpp | 1 - .../behaviors/wings/initWings.hpp | 6 -- .../stateMachine/state/competition/auton.hpp | 26 -------- .../state/competition/enabled.hpp | 4 +- .../stateMachine/state/competition/teleop.hpp | 11 ---- src/main.cpp | 39 +++++------- static/skills_3.json | 44 +++++++------- static/skills_5.json | 6 +- static/skills_6.json | 16 ++--- static/skills_6_5.json | 8 +-- static/skills_7.json | 4 +- static/skills_8.json | 60 +++---------------- static/skills_9.json | 16 ++--- 14 files changed, 73 insertions(+), 170 deletions(-) diff --git a/include/hardware/hardware.hpp b/include/hardware/hardware.hpp index 16504bde..b67f2f12 100755 --- 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 index 4a9c943c..4e06b489 100644 --- 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("CatapultFire", catapultMotors, 1.0); auto catapultHold = std::make_shared("CatapultHold", catapultMotors, 1.15714285714, &cataPID); - auto catapultHoldHigh = std::make_shared("CatapultHold", catapultMotors, 0.5, &cataPID); auto catapultStateController = std::make_shared("CatapultStateController", catapultHold); diff --git a/include/stateMachine/behaviors/wings/initWings.hpp b/include/stateMachine/behaviors/wings/initWings.hpp index 2dfd4ee0..5f0631c3 100644 --- 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("WingOut", frontLeftSolenoid, true); auto frontRightWingIn = std::make_shared("WingIn", frontRightSolenoid, false); auto frontRightWingOut = std::make_shared("WingOut", frontRightSolenoid, true); - auto backLeftWingIn = std::make_shared("WingIn", backLeftSolenoid, false); - auto backLeftWingOut = std::make_shared("WingOut", backLeftSolenoid, true); - auto backRightWingIn = std::make_shared("WingIn", backRightSolenoid, false); - auto backRightWingOut = std::make_shared("WingOut", backRightSolenoid, true); auto frontLeftWingStateController = std::make_shared("FrontLeftWing", frontLeftWingIn); auto frontRightWingStateController = std::make_shared("FrontRightWin", frontRightWingIn); - auto backLeftWingStateController = std::make_shared("BackLeftWing", backLeftWingIn); - auto backRightWingStateController = std::make_shared("BackRightWing", backRightWingIn); void initWings() { Log("Wings Init"); diff --git a/include/stateMachine/state/competition/auton.hpp b/include/stateMachine/state/competition/auton.hpp index 0f0d58b1..dead25b6 100644 --- 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 index 9c170194..0d473400 100644 --- 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 index ef98a66b..c8196189 100644 --- 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(backLeftWingOut, [=, this]() -> bool { - return !controller1.get_digital(E_CONTROLLER_DIGITAL_L1); - })); - backRightWingStateController->sb(std::make_shared(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 index 99bfc759..ffd36d59 100644 --- 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,7 +174,7 @@ 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(); @@ -185,7 +182,7 @@ void skills(void *args) { drivetrainStateController->sb( std::make_shared("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 index b6335ee2..c3a413f2 100644 --- 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 index fe9ae637..22c75fbd 100644 --- 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 index cfebc454..cee9d392 100644 --- 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 index 6b660b77..3e248e57 100644 --- 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 index 6d348dc0..89c10209 100644 --- 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, diff --git a/static/skills_8.json b/static/skills_8.json index dc0fa744..17a46f93 100644 --- a/static/skills_8.json +++ b/static/skills_8.json @@ -7,42 +7,16 @@ "stopEnd": false, "paths": [ { - "x": 0.626, - "y": 0.209 + "x": 0.567, + "y": 0.291 }, { - "x": 0.408, - "y": 0.328 + "x": 0.161, + "y": 0.674 }, { - "x": 0.379, - "y": 0.541 - }, - { - "x": 0.285, - "y": 0.935 - } - ], - "constraints": { - "velocity": 71, - "accel": 195 - } - }, - { - "inverted": false, - "stopEnd": false, - "paths": [ - { - "x": 0.286, - "y": 0.936 - }, - { - "x": 0.195, - "y": 1.416 - }, - { - "x": 0.138, - "y": 1.749 + "x": 0.125, + "y": 1.644 }, { "x": 0.114, @@ -63,8 +37,8 @@ "y": 2.346 }, { - "x": 0.097, - "y": 2.879 + "x": 0.113, + "y": 2.932 }, { "x": 0.575, @@ -84,23 +58,7 @@ "commands": [ { "t": 0.0, - "name": "backLeftWingOut" - }, - { - "t": 0.0, - "name": "backRightWingOut" - }, - { - "t": 0.28, - "name": "backRightWingIn" - }, - { - "t": 2.0, - "name": "backRightWingOut" - }, - { - "t": 2.4, - "name": "backRightWingIn" + "name": "frontRightWingOut" } ] } \ No newline at end of file diff --git a/static/skills_9.json b/static/skills_9.json index c43e658e..f44c85ba 100644 --- a/static/skills_9.json +++ b/static/skills_9.json @@ -41,12 +41,12 @@ "y": 3.23 }, { - "x": 0.275, - "y": 3.17 + "x": 0.336, + "y": 3.167 }, { - "x": 0.262, - "y": 2.626 + "x": 0.323, + "y": 2.623 } ], "constraints": { @@ -59,12 +59,12 @@ "stopEnd": false, "paths": [ { - "x": 0.262, - "y": 2.626 + "x": 0.323, + "y": 2.621 }, { - "x": 0.25, - "y": 2.319 + "x": 0.311, + "y": 2.314 }, { "x": 0.387,