From f06c46c67ad80cc2f6df48c8f3d344f9cd03a19b Mon Sep 17 00:00:00 2001 From: ad101-lab <55064247+alexDickhans@users.noreply.github.com> Date: Sat, 1 Oct 2022 20:11:19 -0600 Subject: [PATCH] Tournament Champtions --- include/feedbackControllers/pid.hpp | 4 +- .../behaviors/drivetrain/initDrivetrain.hpp | 41 +++-- .../behaviors/endgame/endgame.hpp | 6 +- .../behaviors/endgame/initEndgame.hpp | 6 +- .../behaviors/intake/initIntake.hpp | 6 +- .../behaviors/launcher/initLauncher.hpp | 24 ++- .../behaviors/launcher/launcher.hpp | 2 +- include/stateMachine/sequence.hpp | 2 +- include/stateMachine/state/modeLogic.hpp | 9 +- include/stateMachine/state/robotStatus.hpp | 16 +- .../stateMachine/state/teleopModeLogic.hpp | 24 ++- src/main.cpp | 162 +++++++++++++++--- 12 files changed, 223 insertions(+), 79 deletions(-) diff --git a/include/feedbackControllers/pid.hpp b/include/feedbackControllers/pid.hpp index 3e7726fe..763c6b6e 100644 --- a/include/feedbackControllers/pid.hpp +++ b/include/feedbackControllers/pid.hpp @@ -27,8 +27,8 @@ namespace Pronounce { double prevError = 0.0; double derivitive; - double integralBound = 30.0; - double maxIntegral = 0.3; + double integralBound = 3000.0; + double maxIntegral = 30; double power; diff --git a/include/stateMachine/behaviors/drivetrain/initDrivetrain.hpp b/include/stateMachine/behaviors/drivetrain/initDrivetrain.hpp index 8b749dea..cdb11f40 100644 --- a/include/stateMachine/behaviors/drivetrain/initDrivetrain.hpp +++ b/include/stateMachine/behaviors/drivetrain/initDrivetrain.hpp @@ -78,30 +78,38 @@ namespace Pronounce { ProfileConstraints rowIntakeProfileConstraints; SinusoidalVelocityProfile testVelocityProfile(24_in, defaultProfileConstraints); - SinusoidalVelocityProfile frontRollerToAllianceStackVelocityProfile(16_in, defaultProfileConstraints); + SinusoidalVelocityProfile frontRollerToAllianceStackVelocityProfile(38_in, defaultProfileConstraints); SinusoidalVelocityProfile intakeAllianceStackVelocityProfile(24_in, stackIntakeProfileConstraints); - SinusoidalVelocityProfile allianceStackToAllianceDiscsVelocityProfile(24_in, rowIntakeProfileConstraints); + SinusoidalVelocityProfile allianceStackToAllianceDiscsVelocityProfile(112.5_in, rowIntakeProfileConstraints); SinusoidalVelocityProfile allianceDiscsToRollerVelocityProfile(24_in, defaultProfileConstraints); + SinusoidalVelocityProfile moveForward8inVelocityProfile(12_in, defaultProfileConstraints); + SinusoidalVelocityProfile moveForward24inVelocityProfile(24_in, defaultProfileConstraints); - PID turningPid(380, 0, 2000); + PID turningPid(330, 0, 2000); OmniMotionProfiling testProfile("MotionProfilingTest", testVelocityProfile, drivetrain, &turningPid, &odometry, 0.0_deg, 0_deg); - OmniMotionProfiling frontRollerToAllianceStack("FrontRollerToAllianceStack", frontRollerToAllianceStackVelocityProfile, drivetrain, &turningPid, &odometry, 45.0_deg, 0_deg); + OmniMotionProfiling frontRollerToAllianceStack("FrontRollerToAllianceStack", frontRollerToAllianceStackVelocityProfile, drivetrain, &turningPid, &odometry, 30.0_deg, 0_deg); OmniMotionProfiling intakeAllianceStack("IntakeAllianceStack", intakeAllianceStackVelocityProfile, drivetrain, &turningPid, &odometry, 0.0_deg, -45_deg); - OmniMotionProfiling allianceStackToAllianceDiscs("AllianceStackToAllianceDiscs", allianceStackToAllianceDiscsVelocityProfile, drivetrain, &turningPid, &odometry, 0.0_deg, -45_deg); - OmniMotionProfiling allianceDiscsToRoller("AllianceDiscsToRoller", allianceDiscsToRollerVelocityProfile, drivetrain, &turningPid, &odometry, -45.0_deg, 180_deg); + OmniMotionProfiling allianceStackToAllianceDiscs("AllianceStackToAllianceDiscs", allianceStackToAllianceDiscsVelocityProfile, drivetrain, &turningPid, &odometry, 6.0_deg, -45_deg); + OmniMotionProfiling allianceDiscsToRoller("AllianceDiscsToRoller", allianceDiscsToRollerVelocityProfile, drivetrain, &turningPid, &odometry, -45.0_deg, -90_deg); + OmniMotionProfiling moveForward8in("MoveForward8In", moveForward8inVelocityProfile, drivetrain, &turningPid, &odometry, 90.0_deg, -45_deg); + OmniMotionProfiling moveForward8in0deg("MoveForward8In", moveForward8inVelocityProfile, drivetrain, &turningPid, &odometry, 90.0_deg, 0_deg); + OmniMotionProfiling moveBackward8in0deg("MoveBackward8In", moveForward8inVelocityProfile, drivetrain, &turningPid, &odometry, -90.0_deg, 0_deg); + OmniMotionProfiling moveForward24in180("MoveForward24In180", moveForward24inVelocityProfile, drivetrain, &turningPid, &odometry, 180.0_deg, -90_deg); // BezierPath testPath; // PurePursuitProfile defaultPurePursuitProfile = {10.0_in, testVelocityProfile}; // OmniPurePursuit testPurePursuit(&drivetrain, &odometry, defaultPurePursuitProfile, testPath); - RotationController turnTo90("TurnTo90", &drivetrain, &odometry, &turningPid, 90_deg); - Wait turnTo905s(&turnTo90, 3000); + RotationController turnTo90("TurnTo90", &drivetrain, &odometry, &turningPid, -90_deg); + Wait turnTo905s(&turnTo90, 1.5_s); RotationController turnTo315("TurnTo315", &drivetrain, &odometry, &turningPid, 315_deg); - Wait turnTo3155s(&turnTo315, 1500); - RotationController turnTo180("TurnTo180", &drivetrain, &odometry, &turningPid, 180_deg); - Wait turnTo1805s(&turnTo180, 1500); + Wait turnTo3155s(&turnTo315, 1.5_s); + RotationController turnTo180("TurnTo180", &drivetrain, &odometry, &turningPid, 270_deg); + Wait turnTo1805s(&turnTo180, 1.5_s); + RotationController turnTo_135("turnTo_135", &drivetrain, &odometry, &turningPid, -135_deg); + Wait turnTo_1355s(&turnTo_135, 1.5_s); OmniMovement drivetrainRoller("DrivetrainRoller", &drivetrain, 50, -90_deg); - Wait drivetrainRollerWait(&drivetrainRoller, 600); + Wait drivetrainRollerWait(&drivetrainRoller, 600_ms); StateController drivetrainStateController("DrivetrainStateController", &drivetrainStopped); @@ -115,7 +123,7 @@ namespace Pronounce { stackIntakeProfileConstraints.maxAcceleration = 60 * inchs2; stackIntakeProfileConstraints.maxJerk = 3.0; - rowIntakeProfileConstraints.maxVelocity = 20_in/1_s; + rowIntakeProfileConstraints.maxVelocity = 30_in/1_s; rowIntakeProfileConstraints.maxAcceleration = 60 * inchs2; rowIntakeProfileConstraints.maxJerk = 3.0; @@ -124,18 +132,25 @@ namespace Pronounce { intakeAllianceStackVelocityProfile.setProfileConstraints(stackIntakeProfileConstraints); allianceStackToAllianceDiscsVelocityProfile.setProfileConstraints(rowIntakeProfileConstraints); allianceDiscsToRollerVelocityProfile.setProfileConstraints(defaultProfileConstraints); + moveForward8inVelocityProfile.setProfileConstraints(defaultProfileConstraints); + moveForward24inVelocityProfile.setProfileConstraints(defaultProfileConstraints); testVelocityProfile.calculate(100); frontRollerToAllianceStackVelocityProfile.calculate(100); intakeAllianceStackVelocityProfile.calculate(100); allianceStackToAllianceDiscsVelocityProfile.calculate(100); allianceDiscsToRollerVelocityProfile.calculate(100); + moveForward8inVelocityProfile.calculate(100); testProfile.setVelocityProfile(testVelocityProfile); frontRollerToAllianceStack.setVelocityProfile(frontRollerToAllianceStackVelocityProfile); intakeAllianceStack.setVelocityProfile(intakeAllianceStackVelocityProfile); allianceStackToAllianceDiscs.setVelocityProfile(allianceStackToAllianceDiscsVelocityProfile); allianceDiscsToRoller.setVelocityProfile(allianceDiscsToRollerVelocityProfile); + moveForward8in.setVelocityProfile(moveForward8inVelocityProfile); + moveForward8in0deg.setVelocityProfile(moveForward8inVelocityProfile); + moveForward24in180.setVelocityProfile(moveForward24inVelocityProfile); + // Motor brake modes frontLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); diff --git a/include/stateMachine/behaviors/endgame/endgame.hpp b/include/stateMachine/behaviors/endgame/endgame.hpp index d145766b..aa5ad349 100644 --- a/include/stateMachine/behaviors/endgame/endgame.hpp +++ b/include/stateMachine/behaviors/endgame/endgame.hpp @@ -7,10 +7,10 @@ namespace Pronounce { class Endgame : public Behavior { private: - ADIDigitalOutGroup& digitalOutputGroup; + pros::ADIDigitalOut& digitalOutputGroup; bool enabled; public: - Endgame(std::string name, ADIDigitalOutGroup& digitalOutputGroup, bool enabled); + Endgame(std::string name, pros::ADIDigitalOut& digitalOutputGroup, bool enabled); void initialize() { digitalOutputGroup.set_value(enabled); @@ -31,7 +31,7 @@ namespace Pronounce { ~Endgame(); }; - Endgame::Endgame(std::string name, ADIDigitalOutGroup& digitalOutputGroup, bool enabled) : digitalOutputGroup(digitalOutputGroup), enabled(enabled), Behavior(name) { + Endgame::Endgame(std::string name, pros::ADIDigitalOut& digitalOutputGroup, bool enabled) : digitalOutputGroup(digitalOutputGroup), enabled(enabled), Behavior(name) { } Endgame::~Endgame() { diff --git a/include/stateMachine/behaviors/endgame/initEndgame.hpp b/include/stateMachine/behaviors/endgame/initEndgame.hpp index 820886ea..6f8acfd7 100644 --- a/include/stateMachine/behaviors/endgame/initEndgame.hpp +++ b/include/stateMachine/behaviors/endgame/initEndgame.hpp @@ -7,15 +7,13 @@ #include "stateMachine/wait.hpp" namespace Pronounce { - ADIDigitalOutGroup endgameDigitalOutputs; + pros::ADIDigitalOut endgameDigitalOutputs(7, false); Endgame endgameDisabled("EndgameDisabled", endgameDigitalOutputs, false); Endgame endgameEnabled("EndgameEnabled", endgameDigitalOutputs, true); - StateController endgameStateController("EndgameStateController", &endgameEnabled); + StateController endgameStateController("EndgameStateController", &endgameDisabled); void initEndgame() { - endgameDigitalOutputs.addDigitalOutput(new pros::ADIDigitalOut(8, false)); - endgameDigitalOutputs.addDigitalOutput(new pros::ADIDigitalOut(7, false)); } } \ No newline at end of file diff --git a/include/stateMachine/behaviors/intake/initIntake.hpp b/include/stateMachine/behaviors/intake/initIntake.hpp index e80f4e07..32339d4b 100644 --- a/include/stateMachine/behaviors/intake/initIntake.hpp +++ b/include/stateMachine/behaviors/intake/initIntake.hpp @@ -28,10 +28,10 @@ namespace Pronounce { StateController intakeStateController("IntakeStateController", &intakeIntaking); StateController intakeStateExtensionController("IntakeStateExtensionController", new Behavior()); - Wait intakeDejam1(&intakeDejam, 500); - Wait intakeDejam2(&intakeIntaking, 500); + Wait intakeDejam1(&intakeDejam, 500_ms); + Wait intakeDejam2(&intakeIntaking, 500_ms); - Wait intakeRoller1(&intakeEjecting, 1000); + Wait intakeRoller1(&intakeEjecting, 1_s); Sequence intakeRoller("IntakeRollerSequence"); diff --git a/include/stateMachine/behaviors/launcher/initLauncher.hpp b/include/stateMachine/behaviors/launcher/initLauncher.hpp index cded49f1..83d1ea2c 100644 --- a/include/stateMachine/behaviors/launcher/initLauncher.hpp +++ b/include/stateMachine/behaviors/launcher/initLauncher.hpp @@ -15,7 +15,7 @@ namespace Pronounce { - pros::Motor flywheel1(11, pros::E_MOTOR_GEARSET_36, true); + pros::Motor flywheel1(16, pros::E_MOTOR_GEARSET_36, true); pros::Motor turretMotor(3, pros::E_MOTOR_GEARSET_06, false); @@ -24,7 +24,7 @@ namespace Pronounce { pros::ADIDigitalOut indexer(1, false); FlywheelPID flywheelPID(4.0, 0.1, 0.0, 3.55); - PID turretPID(30000.0, 0.0, 35000.0); + PID turretPID(30000.0, 0.0, 120000.0); pros::Rotation turretRotation(4); @@ -36,8 +36,8 @@ namespace Pronounce { StateController launcherStateController("LauncherStateController", &launcherIdle); StateController launcherStateExtensionController("LauncherStateExtensionController", new Behavior()); - Wait launchDisc1(&launcherLaunching, 300); - Wait launchDisc2(&launcherFullSpeed, 700); + Wait launchDisc1(&launcherLaunching, 300_ms); + Wait launchDisc2(&launcherFullSpeed, 1500_ms); Sequence launchDisc("LaunchDisc"); @@ -58,23 +58,19 @@ namespace Pronounce { launchDisc.addState(&launcherStateController, &launcherFullSpeed); launchDisc.addState(&launcherStateController, &launchDisc1); - launch2Disc.addState(&launcherStateController, &launcherFullSpeed); + launch2Disc.addState(&launcherStateController, &launchDisc2); launch2Disc.addState(&launcherStateController, &launchDisc1); - launch2Disc.addState(&launcherStateController, &launcherFullSpeed); + launch2Disc.addState(&launcherStateController, &launchDisc2); launch2Disc.addState(&launcherStateController, &launchDisc1); - launch3Disc.addState(&launcherStateController, &launcherFullSpeed); - launch3Disc.addState(&launcherStateController, &launchDisc1); - launch3Disc.addState(&launcherStateController, &launcherFullSpeed); - launch3Disc.addState(&launcherStateController, &launchDisc1); - launch3Disc.addState(&launcherStateController, &launcherFullSpeed); launch3Disc.addState(&launcherStateController, &launchDisc1); + launch3Disc.addState(&launcherStateController, &launchDisc2); turretMotor.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); - pros::vision_signature_s_t redGoal = pros::Vision::signature_from_utility(RED_GOAL, 4979, 7501, 6240, -601, 307, -147, 3.000, 0); - turretVision.set_signature(RED_GOAL, &redGoal); - turretVision.set_exposure(95); + // pros::vision_signature_s_t redGoal = pros::Vision::signature_from_utility(RED_GOAL, 4979, 7501, 6240, -601, 307, -147, 3.000, 0); + // turretVision.set_signature(RED_GOAL, &redGoal); + // turretVision.set_exposure(95); flywheelRPM.add(38.0, 2335.0); flywheelRPM.add(62.0, 2400.0); diff --git a/include/stateMachine/behaviors/launcher/launcher.hpp b/include/stateMachine/behaviors/launcher/launcher.hpp index 4a3ca0b1..f4e7119c 100644 --- a/include/stateMachine/behaviors/launcher/launcher.hpp +++ b/include/stateMachine/behaviors/launcher/launcher.hpp @@ -86,7 +86,7 @@ namespace Pronounce { bool isDone() { if (useIsDone) { - return abs(this->flywheelSpeed - this->getFlywheelSpeed()) < 20; + return fabs(this->flywheelSpeed - this->getFlywheelSpeed()) < 20.0; } else { return false; } diff --git a/include/stateMachine/sequence.hpp b/include/stateMachine/sequence.hpp index eb77426a..1d8bf5f7 100644 --- a/include/stateMachine/sequence.hpp +++ b/include/stateMachine/sequence.hpp @@ -35,7 +35,7 @@ namespace Pronounce { stateControllers.at(currentIndex)->setCurrentBehavior(behaviors.at(currentIndex)); } else { - std::cout << this->getname() << "Ending" << std::endl; + std::cout << this->getName() << "Ending" << std::endl; stateControllers.at(currentIndex)->useDefaultBehavior(); currentIndex++; // Done diff --git a/include/stateMachine/state/modeLogic.hpp b/include/stateMachine/state/modeLogic.hpp index a430307b..0efd74ce 100644 --- a/include/stateMachine/state/modeLogic.hpp +++ b/include/stateMachine/state/modeLogic.hpp @@ -24,6 +24,7 @@ namespace Pronounce { stateControllers.addBehavior(&launcherStateExtensionController); stateControllers.addBehavior(&launcherStateController); stateControllers.addBehavior(&drivetrainStateController); + stateControllers.addBehavior(&endgameStateController); stateControllers.addBehavior(&teleopController); } @@ -50,7 +51,13 @@ namespace Pronounce { launcherLaunching.setFlywheelSpeed(robotStatus->getFlywheelTarget()); launcherFullSpeed.setFlywheelSpeed(robotStatus->getFlywheelTarget()); - double turretAngle = clamp(robotStatus->getTurretAngle().getValue(), -M_PI_2, M_PI_2); + double turretAngle = 0; + + if (false) { + turretAngle = clamp(angleDifference(robotStatus->getTurretAngle().Convert(radian), 0), -M_PI_2, M_PI_2); + } else { + turretAngle = clamp(angleDifference((robotStatus->getTurretAngle() - odometry.getAngle()).getValue(), 0), -M_PI_2, M_PI_2); + } launcherStopped.setTurretAngle(turretAngle); launcherIdle.setTurretAngle(turretAngle); diff --git a/include/stateMachine/state/robotStatus.hpp b/include/stateMachine/state/robotStatus.hpp index c81e9558..880286c0 100644 --- a/include/stateMachine/state/robotStatus.hpp +++ b/include/stateMachine/state/robotStatus.hpp @@ -18,11 +18,11 @@ namespace Pronounce { // const GameMode gameMode = GameMode::Red; double flywheelAdjustment = 0; - Angle turretAdjustment = 0.0; + Angle turretAdjustment = 90_deg; + double currentFlywheelRPM = 2300; class RobotStatus : public Behavior { private: - double flywheelRPM = 2200; Angle turretAngle = 0.0; FlywheelController allianceGoal; @@ -44,8 +44,10 @@ namespace Pronounce { FlywheelValue flywheelValues = this->allianceGoal.getFlywheelValue(*odometry.getPosition(), odometry.getCurrentVelocity()); - // this->turretAngle = flywheelValues.turretAngle - odometry.getAngle(); - // this->flywheelRPM = flywheelValues.flywheelSpeed; + if (pros::competition::is_autonomous()) { + // this->turretAngle = flywheelValues.turretAngle - odometry.getAngle(); + // currentFlywheelRPM = flywheelValues.flywheelSpeed; + } } void exit() { @@ -53,15 +55,15 @@ namespace Pronounce { } double getFlywheelTarget() { - return flywheelRPM + flywheelAdjustment; + return currentFlywheelRPM + flywheelAdjustment; } double getActualFlywheelRpm() { return launcherIdle.getFlywheelSpeed(); } - void setFlywheelRPM(double flywheelRPM) { - this->flywheelRPM = flywheelRPM; + void setFlywheelRPM(double flywheelRPM1) { + currentFlywheelRPM = flywheelRPM1; } Angle getTurretAngle() { diff --git a/include/stateMachine/state/teleopModeLogic.hpp b/include/stateMachine/state/teleopModeLogic.hpp index 2ebbe7eb..abcd1656 100644 --- a/include/stateMachine/state/teleopModeLogic.hpp +++ b/include/stateMachine/state/teleopModeLogic.hpp @@ -65,19 +65,25 @@ namespace Pronounce { } if (controller2->is_connected()) { - turretAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 500.0 : 0; - flywheelAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 10.0 : 0; + turretAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 3000.0 : 0; + flywheelAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 20.0 : 0; - if (controller2->get_digital_new_press(DIGITAL_L2)) - robotStatus.setFlywheelRPM(2200); + if (controller2->get_digital_new_press(DIGITAL_L2)) { + robotStatus->setFlywheelRPM(2300); + flywheelAdjustment = 0.0; + } if (controller2->get_digital_new_press(DIGITAL_L1)) - robotStatus.setFlywheelRPM(3000); + robotStatus->setFlywheelRPM(2500); - if (controller2->get_digital_new_press(DIGITAL_R1) && controller2.get_digital_new_press(R2)) - endgameStateController.setCurrentBehavior(engameEnabled); + if (controller2->get_digital(DIGITAL_X) && controller1->get_digital(DIGITAL_X)) + endgameStateController.setCurrentBehavior(&endgameEnabled); + + if (pros::millis() % 200 < 15) { + controller2->clear(); + } else { + controller2->set_text(0, 0, std::to_string(this->robotStatus->getFlywheelTarget())); + } } else { - if (controller1->get_digital_new_press(DIGITAL_R1) && controller1.get_digital_new_press(R2)) - endgameStateController.setCurrentBehavior(engameEnabled); } } diff --git a/src/main.cpp b/src/main.cpp index 1f4b9130..70fe463b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,8 +7,6 @@ RobotStatus robotStatus; ModeLogic modeLogic(&robotStatus); TeleopModeLogic teleopModeLogic(new pros::Controller(CONTROLLER_MASTER), new pros::Controller(CONTROLLER_PARTNER)); -pros::Task modeLogicTask; - // SECTION Auton /** @@ -45,19 +43,60 @@ int autonTemplate() { return 0; } +int closeHalfAWP() { + robotStatus.setFlywheelRPM(2970); + turretAdjustment = -5.0_deg; + + pros::Task::delay(50); + + drivetrainStateController.setCurrentBehavior(&drivetrainRollerWait); + intakeStateController.setCurrentBehavior(&intakeEjecting); + + pros::Task::delay(700); + + launcherStateExtensionController.setCurrentBehavior(&launch2Disc); + + intakeStateController.setCurrentBehavior(&intakeStopped); + + pros::Task::delay(2000); + + robotStatus.setFlywheelRPM(2900); + turretAdjustment = -5.0_deg; + + while (!launcherStateExtensionController.isDone()) { + pros::Task::delay(50); + } + + pros::Task::delay(3000); + + return 0; +} + int closeFullAWP() { + robotStatus.setFlywheelRPM(2970); + turretAdjustment = -5.0_deg; pros::Task::delay(50); drivetrainStateController.setCurrentBehavior(&drivetrainRollerWait); intakeStateController.setCurrentBehavior(&intakeEjecting); + + pros::Task::delay(700); + launcherStateExtensionController.setCurrentBehavior(&launch2Disc); pros::Task::delay(700); intakeStateController.setCurrentBehavior(&intakeStopped); - pros::Task::delay(2500); + pros::Task::delay(2000); + + robotStatus.setFlywheelRPM(2900); + turretAdjustment = -5.0_deg; + + while (!launcherStateExtensionController.isDone()) { + pros::Task::delay(50); + } drivetrainStateController.setCurrentBehavior(&frontRollerToAllianceStack); @@ -67,9 +106,18 @@ int closeFullAWP() { pros::Task::delay(50); } - intakeStateController.setCurrentBehavior(&intakeIntaking); + drivetrainStateController.setCurrentBehavior(&moveForward8in); + + pros::Task::delay(50); - drivetrainStateController.setCurrentBehavior(&turnTo3155s); + while (!drivetrainStateController.isDone()) { + pros::Task::delay(50); + } + + drivetrainStateController.setCurrentBehavior(&turnTo315); + robotStatus.setFlywheelRPM(2700); + + intakeStateController.setCurrentBehavior(&intakeIntaking); pros::Task::delay(50); @@ -77,9 +125,11 @@ int closeFullAWP() { pros::Task::delay(50); } - pros::Task::delay(500); + pros::Task::delay(200); - drivetrainStateController.setCurrentBehavior(&intakeAllianceStack); + drivetrainStateController.setCurrentBehavior(&allianceStackToAllianceDiscs); + + turretAdjustment = -87_deg; pros::Task::delay(50); @@ -87,15 +137,17 @@ int closeFullAWP() { pros::Task::delay(50); } + intakeStateController.setCurrentBehavior(&intakeEjecting); + // drivetrainStateController.setCurrentBehavior(&turnTo90); launcherStateExtensionController.setCurrentBehavior(&launch3Disc); pros::Task::delay(50); - while (!launcherStateExtensionController.isDone()) { + while (!drivetrainStateController.isDone()) { pros::Task::delay(50); } -/* - drivetrainStateController.setCurrentBehavior(&allianceStackToAllianceDiscs); + + drivetrainStateController.setCurrentBehavior(&allianceDiscsToRoller); pros::Task::delay(50); @@ -103,15 +155,40 @@ int closeFullAWP() { pros::Task::delay(50); } - launcherStateExtensionController.setCurrentBehavior(&launch3Disc); + drivetrainStateController.setCurrentBehavior(&drivetrainRollerWait); + + pros::Task::delay(3000); + + return 0; +} + +int skills() { + robotStatus.setFlywheelRPM(2970); + turretAdjustment = -5.0_deg; pros::Task::delay(50); + drivetrainStateController.setCurrentBehavior(&drivetrainRollerWait); + intakeStateController.setCurrentBehavior(&intakeEjecting); + + pros::Task::delay(700); + + launcherStateExtensionController.setCurrentBehavior(&launch2Disc); + + pros::Task::delay(700); + + intakeStateController.setCurrentBehavior(&intakeStopped); + + pros::Task::delay(1300); + + robotStatus.setFlywheelRPM(2900); + turretAdjustment = -5.0_deg; + while (!launcherStateExtensionController.isDone()) { pros::Task::delay(50); } - drivetrainStateController.setCurrentBehavior(&turnTo180); + drivetrainStateController.setCurrentBehavior(&frontRollerToAllianceStack); pros::Task::delay(50); @@ -119,7 +196,7 @@ int closeFullAWP() { pros::Task::delay(50); } - drivetrainStateController.setCurrentBehavior(&allianceDiscsToRoller); + drivetrainStateController.setCurrentBehavior(&moveForward8in); pros::Task::delay(50); @@ -127,11 +204,54 @@ int closeFullAWP() { pros::Task::delay(50); } - drivetrainStateController.setCurrentBehavior(&drivetrainRollerWait); - intakeStateController.setCurrentBehavior(&intakeRoller); - launcherStateController.setCurrentBehavior(&launch3Disc); -*/ - pros::Task::delay(3000); + drivetrainStateController.setCurrentBehavior(&turnTo315); + robotStatus.setFlywheelRPM(2700); + + intakeStateController.setCurrentBehavior(&intakeIntaking); + + pros::Task::delay(50); + + while (!drivetrainStateController.isDone()) { + pros::Task::delay(50); + } + + pros::Task::delay(200); + + drivetrainStateController.setCurrentBehavior(&allianceStackToAllianceDiscs); + + turretAdjustment = -87_deg; + + pros::Task::delay(50); + + while (!drivetrainStateController.isDone()) { + pros::Task::delay(50); + } + + drivetrainStateController.setCurrentBehavior(&turnTo_135); + + pros::Task::delay(50); + + while (!drivetrainStateController.isDone()) { + pros::Task::delay(50); + } + + pros::Task::delay(2000); + + endgameStateController.setCurrentBehavior(&endgameEnabled); + + pros::Task::delay(500); + + return 0; +} + +int rightSideAWP() { + + pros::Task::delay(1000); + + turretAdjustment = 0_deg; + robotStatus.setFlywheelRPM(3000); + + launcherStateExtensionController.setCurrentBehavior(&launch2Disc); return 0; } @@ -244,7 +364,7 @@ void initialize() { modeLogic.initialize(); - modeLogicTask = pros::Task(update); + pros::Task modeLogicTask = pros::Task(update); } // !SECTION @@ -256,6 +376,7 @@ void initialize() { */ void disabled() { teleopController.useDefaultBehavior(); + launcherStateController.useDefaultBehavior(); // Create a label lv_obj_t* disabledLabel = lv_label_create(lv_scr_act(), NULL); @@ -308,8 +429,7 @@ void autonomous() { */ void opcontrol() { pros::delay(10); - - drivetrainStateController.setCurrentBehavior(&fieldRelativeJoystick); + teleopController.setCurrentBehavior(&teleopModeLogic); // Driver Control Loop