diff --git a/include/stateMachine/behaviors/wings/initWings.hpp b/include/stateMachine/behaviors/wings/initWings.hpp index 5f0631c3..d436b789 100644 --- a/include/stateMachine/behaviors/wings/initWings.hpp +++ b/include/stateMachine/behaviors/wings/initWings.hpp @@ -5,13 +5,13 @@ #include "hardware/hardware.hpp" namespace Pronounce { - auto frontLeftWingIn = std::make_shared("WingIn", frontLeftSolenoid, false); - 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 leftWingIn = std::make_shared("WingIn", frontLeftSolenoid, false); + auto leftWingOut = std::make_shared("WingOut", frontLeftSolenoid, true); + auto rightWingIn = std::make_shared("WingIn", frontRightSolenoid, false); + auto rightWingOut = std::make_shared("WingOut", frontRightSolenoid, true); - auto frontLeftWingStateController = std::make_shared("FrontLeftWing", frontLeftWingIn); - auto frontRightWingStateController = std::make_shared("FrontRightWin", frontRightWingIn); + auto leftWingStateController = std::make_shared("FrontLeftWing", leftWingIn); + auto rightWingStateController = std::make_shared("FrontRightWin", rightWingIn); void initWings() { Log("Wings Init"); diff --git a/include/stateMachine/state/competition/auton.hpp b/include/stateMachine/state/competition/auton.hpp index dead25b6..7445ea03 100644 --- a/include/stateMachine/state/competition/auton.hpp +++ b/include/stateMachine/state/competition/auton.hpp @@ -19,30 +19,30 @@ namespace Pronounce { intakeStateController->sb(intakeEject); }); - pathFollower->addCommandMapping("frontLeftWingOut", [&]() -> void { - frontLeftWingStateController->sb(frontLeftWingOut); + pathFollower->addCommandMapping("leftWingOut", [&]() -> void { + leftWingStateController->sb(leftWingOut); }); - pathFollower->addCommandMapping("frontLeftWingIn", [&]() -> void { - frontLeftWingStateController->sb(frontLeftWingIn); + pathFollower->addCommandMapping("leftWingIn", [&]() -> void { + leftWingStateController->sb(leftWingIn); }); - pathFollower->addCommandMapping("frontRightWingOut", [&]() -> void { - frontRightWingStateController->sb(frontRightWingOut); + pathFollower->addCommandMapping("rightWingOut", [&]() -> void { + rightWingStateController->sb(rightWingOut); }); - pathFollower->addCommandMapping("frontRightWingIn", [&]() -> void { - frontRightWingStateController->sb(frontRightWingIn); + pathFollower->addCommandMapping("rightWingIn", [&]() -> void { + rightWingStateController->sb(rightWingIn); }); pathFollower->addCommandMapping("frontWingsOut", [&]() -> void { - frontLeftWingStateController->sb(frontLeftWingOut); - frontRightWingStateController->sb(frontRightWingOut); + leftWingStateController->sb(leftWingOut); + rightWingStateController->sb(rightWingOut); }); pathFollower->addCommandMapping("frontWingsIn", [&]() -> void { - frontLeftWingStateController->sb(frontLeftWingIn); - frontRightWingStateController->sb(frontRightWingIn); + leftWingStateController->sb(leftWingIn); + rightWingStateController->sb(rightWingIn); }); pathFollower->addCommandMapping("hangOut", [&]() -> void { diff --git a/include/stateMachine/state/competition/enabled.hpp b/include/stateMachine/state/competition/enabled.hpp index cc96f3f8..1a7afa9e 100644 --- a/include/stateMachine/state/competition/enabled.hpp +++ b/include/stateMachine/state/competition/enabled.hpp @@ -8,8 +8,8 @@ namespace Pronounce { Log("Init"); stateControllers.addBehavior(drivetrainStateController); - stateControllers.addBehavior(frontLeftWingStateController); - stateControllers.addBehavior(frontRightWingStateController); + stateControllers.addBehavior(leftWingStateController); + stateControllers.addBehavior(rightWingStateController); stateControllers.addBehavior(winchStateController); stateControllers.addBehavior(winchStateExtensionController); stateControllers.addBehavior(catapultStateController); diff --git a/include/stateMachine/state/competition/teleop.hpp b/include/stateMachine/state/competition/teleop.hpp index ce74466f..a655de32 100644 --- a/include/stateMachine/state/competition/teleop.hpp +++ b/include/stateMachine/state/competition/teleop.hpp @@ -30,14 +30,14 @@ namespace Pronounce { if (controller1.get_digital(E_CONTROLLER_DIGITAL_Y)) { winchStateExtensionController->sb(winchASequence); } else { - frontLeftWingStateController->sb(std::make_shared(frontLeftWingOut, [=, this]() -> bool { + leftWingStateController->sb(std::make_shared(leftWingOut, [=, this]() -> bool { return !controller1.get_digital(E_CONTROLLER_DIGITAL_L2); })); } }); controller1.onPressed(E_CONTROLLER_DIGITAL_L1, [&]() -> void { - frontRightWingStateController->sb(std::make_shared(frontRightWingOut, [&]() -> bool { + rightWingStateController->sb(std::make_shared(rightWingOut, [&]() -> bool { return !controller1.get_digital(E_CONTROLLER_DIGITAL_L1); })); }); @@ -83,8 +83,8 @@ namespace Pronounce { void exit() override { Log("Exit"); drivetrainStateController->setDefaultBehavior(drivetrainStopped); - frontLeftWingStateController->ud(); - frontRightWingStateController->ud(); + leftWingStateController->ud(); + rightWingStateController->ud(); drivetrainStateController->ud(); controller1.clearCallbacks(); Enabled::exit(); diff --git a/src/main.cpp b/src/main.cpp index 09f457fc..0ede439a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,6 +13,9 @@ 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(mid_6_ball_1) +SMOOTH_SPLINE_PATH_ASSET(mid_6_ball_2) +SMOOTH_SPLINE_PATH_ASSET(mid_6_ball_awp) SMOOTH_SPLINE_PATH_ASSET(skills_1) SMOOTH_SPLINE_PATH_ASSET(skills_2) SMOOTH_SPLINE_PATH_ASSET(skills_3) @@ -80,6 +83,8 @@ void far6Ball(void* args) { turnTo(-45_deg, 0.6_s, counterclockwise); + leftWingStateController->sb(leftWingOut); + intakeStateController->sb(intakeEject); pathFollower->setMotionProfile(far_6_3); @@ -112,6 +117,8 @@ void far6BallFlick(void* args) { turnTo(-45_deg, 0.6_s, counterclockwise); + leftWingStateController->sb(leftWingOut); + intakeStateController->sb(intakeEject); pathFollower->setMotionProfile(far_6_3); @@ -132,11 +139,12 @@ void far6BallFlickAWP(void* args) { void far6BallFlickElim(void* args) { far6Ball(args); - move(-15_in, defaultProfileConstraints, 0.0, -430_deg, counterclockwise); + 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(); + move(40_in, speedProfileConstraints, 0.0, -135_deg); } void far6BallElim(void* args) { @@ -147,6 +155,7 @@ void far6BallElim(void* args) { drivetrainStateController->sb(pathFollower)->wait(); pathFollower->setMotionProfile(far_6_4); drivetrainStateController->sb(pathFollower)->wait(); + move(40_in, speedProfileConstraints, 0.0, -135_deg); } void far6BallAWP(void* args) { @@ -157,6 +166,90 @@ void far6BallAWP(void* args) { drivetrainStateController->sb(pathFollower)->wait(); } +void far5BallRushMid(void *args) { + + imuOrientation.setRotation(80.7_deg); + + intakeExtensionStateController->sb(deploySequence); + rightWingStateController->sb(std::make_shared(rightWingOut, 200_ms)); + + move(50_in, speedProfileConstraints, 0.0, 80.7_deg); + + intakeExtensionStateController->ud(); + intakeStateController->sb(intakeIntaking); + + pathFollower->setMotionProfile(mid_6_ball_1); + + drivetrainStateController->sb(pathFollower)->wait(); + + turnTo(-2_deg, 550_ms); + + intakeStateController->sb(intakeIntaking); + + move(19_in, speedProfileConstraints, 0.0, 2_deg, 0.0, 0.0); + move(-16_in, speedProfileConstraints, 0.0, 2_deg, 0.0, 0.0); + + intakeStateController->sb(intakeHold); +// move(-4_in, speedProfileConstraints, 0.0, 2_deg); + turnTo(170_deg, 700_ms); + + pathFollower->setMotionProfile(mid_6_ball_2); + + drivetrainStateController->sb(pathFollower)->wait(); + + move(-12_in, speedProfileConstraints, 0.0, 110_deg); + + leftWingStateController->ud(); + turnTo(120_deg, 300_ms); + leftWingStateController->sb(leftWingOut); + drivetrain.tankSteerVoltage(12000, 12000); + pros::Task::delay(800); + drivetrain.tankSteerVoltage(0.0, 0.0); + leftWingStateController->ud(); + move(-9_in, speedProfileConstraints, 0.0, 90_deg); + turnTo(25_deg, 200_ms); + intakeStateController->sb(intakeIntaking); + move(48_in, speedProfileConstraints, 0.0, 25_deg); + + turnTo(150_deg, 550_ms); + intakeExtensionStateController->sb(outtakeSequence); + move(38_in, speedProfileConstraints, 0.0, 150_deg); +} + +void far6BallRushMid(void *args) { + far5BallRushMid(args); + + turnTo(3_deg, 550_ms); + + intakeStateController->sb(intakeIntaking); + + move(23_in, defaultProfileConstraints, 0.0, 3_deg); + + turnTo(180_deg, 550_ms); + intakeExtensionStateController->ud(); + intakeStateController->sb(intakeEject); + leftWingStateController->sb(leftWingOut); + rightWingStateController->sb(rightWingOut); + move(35_in, speedProfileConstraints, 0.0, 180_deg); + move(-10_in, speedProfileConstraints, 0.0, 180_deg); + turnTo(0_deg, 3_s); +} + +void far5BallAWP(void *args) { + far5BallRushMid(args); + + move(-5_in, speedProfileConstraints, 0.0, 0_deg); + + turnTo(-90_deg, 600_ms); + + pathFollower->setMotionProfile(mid_6_ball_awp); + + drivetrainStateController->sb(pathFollower)->wait(); + + drivetrain.tankSteerVoltage(3000, 2000); + pros::Task::delay(5000); +} + void skills(void *args) { imuOrientation.setRotation(135_deg); @@ -181,7 +274,7 @@ void skills(void *args) { pathFollower->setMotionProfile(skills_2); drivetrainStateController->sb(pathFollower)->wait(); - frontRightWingStateController->ud(); + rightWingStateController->ud(); turnTo(170.0_deg, 400_ms, clockwise); @@ -198,8 +291,8 @@ void skills(void *args) { move(-8_in, speedProfileConstraints, 0.0, -80_deg); - frontRightWingStateController->ud(); - frontLeftWingStateController->ud(); + rightWingStateController->ud(); + leftWingStateController->ud(); turnTo(-170_deg, 0.4_s, closest); @@ -294,7 +387,7 @@ void safeCloseAWPDelay(void *args) { void closeRushMidAwp(void *args) { imuOrientation.setRotation(-75.7_deg); - frontLeftWingStateController->sb(std::make_shared(frontLeftWingOut, 300_ms)); + leftWingStateController->sb(std::make_shared(leftWingOut, 300_ms)); intakeExtensionStateController->sb(deploySequence); @@ -309,7 +402,7 @@ void closeRushMidAwp(void *args) { void closeRushMidElim(void *args) { imuOrientation.setRotation(-75.7_deg); - frontLeftWingStateController->sb(std::make_shared(frontLeftWingOut, 300_ms)); + leftWingStateController->sb(std::make_shared(leftWingOut, 300_ms)); intakeExtensionStateController->sb(deploySequence); diff --git a/static/close_rush_mid_awp.json b/static/close_rush_mid_awp.json index 3d4126d5..1aaee134 100644 --- a/static/close_rush_mid_awp.json +++ b/static/close_rush_mid_awp.json @@ -15,12 +15,12 @@ "y": 1.175 }, { - "x": 2.585, - "y": 0.95 + "x": 2.595, + "y": 0.939 }, { - "x": 3.542, - "y": 0.967 + "x": 3.422, + "y": 0.964 } ], "constraints": { @@ -41,12 +41,12 @@ "y": 0.561 }, { - "x": 3.146, - "y": 0.277 + "x": 3.241, + "y": 0.386 }, { - "x": 2.746, - "y": 0.297 + "x": 2.841, + "y": 0.406 } ], "constraints": { @@ -59,12 +59,12 @@ "stopEnd": false, "paths": [ { - "x": 2.746, - "y": 0.297 + "x": 2.918, + "y": 0.37 }, { - "x": 3.321, - "y": 0.303 + "x": 3.254, + "y": 0.41 }, { "x": 3.302, @@ -93,12 +93,12 @@ "y": 0.933 }, { - "x": 3.396, - "y": 1.477 + "x": 3.392, + "y": 1.376 }, { - "x": 3.383, - "y": 1.819 + "x": 3.379, + "y": 1.718 } ], "constraints": { diff --git a/static/close_rush_mid_no_triball.json b/static/close_rush_mid_no_triball.json index cdc8ffea..b3342b11 100644 --- a/static/close_rush_mid_no_triball.json +++ b/static/close_rush_mid_no_triball.json @@ -41,11 +41,11 @@ "y": 1.575 }, { - "x": 2.154, - "y": 0.977 + "x": 2.226, + "y": 0.971 }, { - "x": 3.401, + "x": 3.353, "y": 1.0 } ], @@ -63,16 +63,16 @@ "y": 0.897 }, { - "x": 2.991, - "y": 0.794 + "x": 3.121, + "y": 0.81 }, { - "x": 3.024, - "y": 0.329 + "x": 2.975, + "y": 0.429 }, { - "x": 2.727, - "y": 0.276 + "x": 2.678, + "y": 0.376 } ], "constraints": { @@ -85,16 +85,16 @@ "stopEnd": false, "paths": [ { - "x": 2.727, - "y": 0.276 + "x": 2.813, + "y": 0.521 }, { - "x": 3.258, - "y": 0.39 + "x": 3.109, + "y": 0.586 }, { - "x": 3.269, - "y": 0.757 + "x": 3.203, + "y": 0.744 }, { "x": 2.933, @@ -137,6 +137,10 @@ { "t": 2.52, "name": "frontLeftWingOut" + }, + { + "t": 4.19, + "name": "outtake" } ] } \ No newline at end of file diff --git a/static/far_6_2.json b/static/far_6_2.json index c1def9c9..8994732d 100644 --- a/static/far_6_2.json +++ b/static/far_6_2.json @@ -33,6 +33,10 @@ { "t": 0.25, "name": "frontLeftWingOut" + }, + { + "t": 0.86, + "name": "frontLeftWingIn" } ] } \ No newline at end of file diff --git a/static/mid_6_ball_1.json b/static/mid_6_ball_1.json new file mode 100644 index 00000000..e5a86ee8 --- /dev/null +++ b/static/mid_6_ball_1.json @@ -0,0 +1 @@ +{"segments":[{"inverted":true,"paths":[{"x":1.827,"y":1.233},{"x":1.431,"y":1.127},{"x":0.556,"y":0.771},{"x":0.335,"y":1.113}],"constraints":{"velocity":71,"accel":200}}],"commands":[{"t":0.15,"name":"intakeHold"},{"t":0.8,"name":"outtake"}]} \ No newline at end of file diff --git a/static/mid_6_ball_2.json b/static/mid_6_ball_2.json new file mode 100644 index 00000000..0a3818c1 --- /dev/null +++ b/static/mid_6_ball_2.json @@ -0,0 +1 @@ +{"segments":[{"inverted":false,"paths":[{"x":0.246,"y":0.913},{"x":0.3,"y":0.642},{"x":0.467,"y":0.368},{"x":0.651,"y":0.481}],"constraints":{"velocity":71,"accel":145}}],"commands":[{"t":0.0,"name":"leftWingOut"},{"t":0.06,"name":"awpOut"},{"t":0.06,"name":"rightWingOut"},{"t":0.09,"name":"rightWingIn"},{"t":0.09,"name":"intakeHold"},{"t":0.35,"name":"outtake"},{"t":0.85,"name":"leftWingIn"},{"t":0.93,"name":"awpIn"}]} \ No newline at end of file diff --git a/static/mid_6_ball_awp.json b/static/mid_6_ball_awp.json new file mode 100644 index 00000000..915c13fa --- /dev/null +++ b/static/mid_6_ball_awp.json @@ -0,0 +1 @@ +{"segments":[{"inverted":false,"paths":[{"x":1.592,"y":0.969},{"x":1.278,"y":1.072},{"x":1.002,"y":1.426},{"x":0.956,"y":1.652}],"constraints":{"velocity":44,"accel":170}}],"commands":[]} \ No newline at end of file