Skip to content

Commit

Permalink
Add old 6 ball path back just in case
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexDickhans
alexDickhans committed Apr 24, 2024
1 parent d1ae1d1 commit cf63712
Show file tree
Hide file tree
Showing 11 changed files with 165 additions and 61 deletions.
12 changes: 6 additions & 6 deletions include/stateMachine/behaviors/wings/initWings.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,13 @@
#include "hardware/hardware.hpp"

namespace Pronounce {
auto frontLeftWingIn = std::make_shared<Wing>("WingIn", frontLeftSolenoid, false);
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 leftWingIn = std::make_shared<Wing>("WingIn", frontLeftSolenoid, false);
auto leftWingOut = std::make_shared<Wing>("WingOut", frontLeftSolenoid, true);
auto rightWingIn = std::make_shared<Wing>("WingIn", frontRightSolenoid, false);
auto rightWingOut = std::make_shared<Wing>("WingOut", frontRightSolenoid, true);

auto frontLeftWingStateController = std::make_shared<StateController>("FrontLeftWing", frontLeftWingIn);
auto frontRightWingStateController = std::make_shared<StateController>("FrontRightWin", frontRightWingIn);
auto leftWingStateController = std::make_shared<StateController>("FrontLeftWing", leftWingIn);
auto rightWingStateController = std::make_shared<StateController>("FrontRightWin", rightWingIn);

void initWings() {
Log("Wings Init");
Expand Down
24 changes: 12 additions & 12 deletions include/stateMachine/state/competition/auton.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
4 changes: 2 additions & 2 deletions include/stateMachine/state/competition/enabled.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
8 changes: 4 additions & 4 deletions include/stateMachine/state/competition/teleop.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,14 +30,14 @@ namespace Pronounce {
if (controller1.get_digital(E_CONTROLLER_DIGITAL_Y)) {
winchStateExtensionController->sb(winchASequence);
} else {
frontLeftWingStateController->sb(std::make_shared<Until>(frontLeftWingOut, [=, this]() -> bool {
leftWingStateController->sb(std::make_shared<Until>(leftWingOut, [=, this]() -> bool {
return !controller1.get_digital(E_CONTROLLER_DIGITAL_L2);
}));
}
});

controller1.onPressed(E_CONTROLLER_DIGITAL_L1, [&]() -> void {
frontRightWingStateController->sb(std::make_shared<Until>(frontRightWingOut, [&]() -> bool {
rightWingStateController->sb(std::make_shared<Until>(rightWingOut, [&]() -> bool {
return !controller1.get_digital(E_CONTROLLER_DIGITAL_L1);
}));
});
Expand Down Expand Up @@ -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();
Expand Down
105 changes: 99 additions & 6 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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<Wait>(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);
Expand All @@ -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);

Expand All @@ -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);

Expand Down Expand Up @@ -294,7 +387,7 @@ void safeCloseAWPDelay(void *args) {
void closeRushMidAwp(void *args) {
imuOrientation.setRotation(-75.7_deg);

frontLeftWingStateController->sb(std::make_shared<Wait>(frontLeftWingOut, 300_ms));
leftWingStateController->sb(std::make_shared<Wait>(leftWingOut, 300_ms));

intakeExtensionStateController->sb(deploySequence);

Expand All @@ -309,7 +402,7 @@ void closeRushMidAwp(void *args) {
void closeRushMidElim(void *args) {
imuOrientation.setRotation(-75.7_deg);

frontLeftWingStateController->sb(std::make_shared<Wait>(frontLeftWingOut, 300_ms));
leftWingStateController->sb(std::make_shared<Wait>(leftWingOut, 300_ms));

intakeExtensionStateController->sb(deploySequence);

Expand Down
32 changes: 16 additions & 16 deletions static/close_rush_mid_awp.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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": {
Expand All @@ -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": {
Expand All @@ -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,
Expand Down Expand Up @@ -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": {
Expand Down
Loading

0 comments on commit cf63712

Please sign in to comment.