diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp new file mode 100644 index 00000000..52ec17a6 --- /dev/null +++ b/include/autoPaths.hpp @@ -0,0 +1,157 @@ +#pragma once + +#include "utils/pointUtil.hpp" +#include "utils/path.hpp" +#include "utils/splinePath.hpp" +#include "motionControl/purePursuit.hpp" +#include "utils/purePursuitProfile.hpp" +#include "utils/purePursuitProfileManager.hpp" + +namespace Pronounce { + + // Test path + int testPathIndex; + + // Right steal right + int rightHomeToGoalNeutralIndex; + int rightNeutralToMidNeutralIndex; + int midNeutralToRightAllianceIndex; + int midNeutralToMidHomeZoneIndex; + int rightNeutralToRightHomeIndex; + + // Right awp right + int farRightHomeZoneToRightAllianceIndex; + int rightAllianceToRightHomeZoneIndex; + + // Left steal left + int leftAllianceToLeftNeutralIndex; + int leftNeutralToMidNeutralIndex; + int midNeutralToLeftHomeZoneIndex; + + // Skills + int rightNeutralToFarPlatformIndex; + int farPlatformToNearPlatformIndex; + int nearPlatformViaLeftNeutralToFarPlatformIndex; + int nearPlatformToMidIndex; + + void autoPaths(PurePursuit purePursuit) { + // Default pure pursuit profile + PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0); + purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile); + + // Test path + Path testPath = Path(); + + testPath.addPoint(0, 0); + testPath.addPoint(24, 0); + testPath.addPoint(24, 24); + testPath.addPoint(24, 48); + testPath.addPoint(-24, 48); + testPath.addPoint(0, 24); + + testPathIndex = purePursuit.addPath(testPath); + + Path rightNeutralToRightHomeZone; + + rightNeutralToRightHomeZone.addPoint(105.7, 60); + rightNeutralToRightHomeZone.addPoint(105.7, 16); + + rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone); + + // Right Steal Right + Path rightHomeToGoalNeutral; + + rightHomeToGoalNeutral.addPoint(105.7, 16); + rightHomeToGoalNeutral.addPoint(105.7, 61); + + rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral); + + Path rightNeutralToMidNeutral; + + rightNeutralToMidNeutral.addPoint(105.7, 62); + rightNeutralToMidNeutral.addPoint(75.3, 40); + rightNeutralToMidNeutral.addPoint(60.3, 65); + + rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral); + + Path midNeutralToRightAlliance; + + midNeutralToRightAlliance.addPoint(70.3, 65); + midNeutralToRightAlliance.addPoint(120.1, 28); + + midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance); + + Path midNeutralToMidHomeZone; + + midNeutralToMidHomeZone.addPoint(70.3, 70.3); + midNeutralToMidHomeZone.addPoint(70.3, 36); + + midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone); + + Path farRightHomeZoneToRightAlliance; + + farRightHomeZoneToRightAlliance.addPoint(127.9, 16); + farRightHomeZoneToRightAlliance.addPoint(127.9, 24); + + farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance); + + Path rightAllianceToRightHomeZone; + + rightAllianceToRightHomeZone.addPoint(127.9, 24); + rightAllianceToRightHomeZone.addPoint(105.7, 16); + + rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone); + + Path leftAllianceToLeftNeutral; + + leftAllianceToLeftNeutral.addPoint(29, 11.4); + leftAllianceToLeftNeutral.addPoint(32, 67); + + leftAllianceToLeftNeutralIndex = purePursuit.addPath(leftAllianceToLeftNeutral); + + Path leftNeutralToMidNeutral; + + leftNeutralToMidNeutral.addPoint(32, 67); + leftNeutralToMidNeutral.addPoint(65.3, 40); + leftNeutralToMidNeutral.addPoint(70.3, 65); + + leftNeutralToMidNeutralIndex = purePursuit.addPath(leftNeutralToMidNeutral); + + // mid neutral to mid home zone + + Path rightNeutralToFarPlatform; + + rightNeutralToFarPlatform.addPoint(105.7, 61); + rightNeutralToFarPlatform.addPoint(75, 76.5); + rightNeutralToFarPlatform.addPoint(75, 100); + rightNeutralToFarPlatform.addPoint(60.3, 115); + + rightNeutralToFarPlatformIndex = purePursuit.addPath(rightNeutralToFarPlatform); + + Path farPlatformToNearPlatform; + + farPlatformToNearPlatform.addPoint(70.3, 107); + farPlatformToNearPlatform.addPoint(60, 70.3); + farPlatformToNearPlatform.addPoint(58.6, 64.1); + farPlatformToNearPlatform.addPoint(58.6, 45); + farPlatformToNearPlatform.addPoint(70.3, 30.7); + + farPlatformToNearPlatformIndex = purePursuit.addPath(farPlatformToNearPlatform); + + Path nearPlatformViaLeftNeutralToFarPlatform; + + nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 30.7); + nearPlatformViaLeftNeutralToFarPlatform.addPoint(35, 61); + nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 115); + + nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit.addPath(nearPlatformViaLeftNeutralToFarPlatform); + + Path nearPlatformToMid; + + nearPlatformToMid.addPoint(70.3, 115); + nearPlatformToMid.addPoint(70.3, 70.3); + + nearPlatformToMidIndex = purePursuit.addPath(nearPlatformToMid); + } + +} // Namespace Prononce diff --git a/include/main.h b/include/main.h index d2e3c5eb..47978fd9 100644 --- a/include/main.h +++ b/include/main.h @@ -47,6 +47,7 @@ * User defined imports */ +#include "autoPaths.hpp" #include "defines.h" // Auton diff --git a/src/main.cpp b/src/main.cpp index d6a607bf..b1c9cde6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -55,30 +55,7 @@ bool preDriverTasksDone = false; int driverMode = 0; -// Test path -int testPathIndex; - -// Right steal right -int rightHomeToGoalNeutralIndex; -int rightNeutralToMidNeutralIndex; -int midNeutralToRightAllianceIndex; -int midNeutralToMidHomeZoneIndex; -int rightNeutralToRightHomeIndex; - -// Right awp right -int farRightHomeZoneToRightAllianceIndex; -int rightAllianceToRightHomeZoneIndex; - -// Left steal left -int leftAllianceToLeftNeutralIndex; -int leftNeutralToMidNeutralIndex; -int midNeutralToLeftHomeZoneIndex; - -// Skills -int rightNeutralToFarPlatformIndex; -int farPlatformToNearPlatformIndex; -int nearPlatformViaLeftNeutralToFarPlatformIndex; -int nearPlatformToMidIndex; +// SECTION Auton void flipOut() { intakeButton.setButtonStatus(ButtonStatus::NEGATIVE); @@ -345,7 +322,9 @@ int postAuton() { return 0; } +// !SECTION +// SECTION INIT /** * Render thread to update items on the controller */ @@ -475,6 +454,7 @@ void initSelector() { * Initialize the logger for data collection after the match */ void initLogger() { + Logger::setDefaultLogger( std::make_shared( TimeUtilFactory::createDefault().getTimer(), @@ -485,127 +465,6 @@ void initLogger() { Logger::getDefaultLogger()->debug("LOGGER: Logger initialized"); } -void autoPaths() { - // Default pure pursuit profile - PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0); - purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile); - - // Test path - Path testPath = Path(); - - testPath.addPoint(0, 0); - testPath.addPoint(24, 0); - testPath.addPoint(24, 24); - testPath.addPoint(24, 48); - testPath.addPoint(-24, 48); - testPath.addPoint(0, 24); - - testPathIndex = purePursuit.addPath(testPath); - - Path rightNeutralToRightHomeZone; - - rightNeutralToRightHomeZone.addPoint(105.7, 60); - rightNeutralToRightHomeZone.addPoint(105.7, 16); - - rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone); - - // Right Steal Right - Path rightHomeToGoalNeutral; - - rightHomeToGoalNeutral.addPoint(105.7, 16); - rightHomeToGoalNeutral.addPoint(105.7, 61); - - rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral); - - Path rightNeutralToMidNeutral; - - rightNeutralToMidNeutral.addPoint(105.7, 62); - rightNeutralToMidNeutral.addPoint(75.3, 40); - rightNeutralToMidNeutral.addPoint(60.3, 65); - - rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral); - - Path midNeutralToRightAlliance; - - midNeutralToRightAlliance.addPoint(70.3, 65); - midNeutralToRightAlliance.addPoint(120.1, 28); - - midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance); - - Path midNeutralToMidHomeZone; - - midNeutralToMidHomeZone.addPoint(70.3, 70.3); - midNeutralToMidHomeZone.addPoint(70.3, 36); - - midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone); - - Path farRightHomeZoneToRightAlliance; - - farRightHomeZoneToRightAlliance.addPoint(127.9, 16); - farRightHomeZoneToRightAlliance.addPoint(127.9, 24); - - farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance); - - Path rightAllianceToRightHomeZone; - - rightAllianceToRightHomeZone.addPoint(127.9, 24); - rightAllianceToRightHomeZone.addPoint(105.7, 16); - - rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone); - - Path leftAllianceToLeftNeutral; - - leftAllianceToLeftNeutral.addPoint(29, 11.4); - leftAllianceToLeftNeutral.addPoint(32, 67); - - leftAllianceToLeftNeutralIndex = purePursuit.addPath(leftAllianceToLeftNeutral); - - Path leftNeutralToMidNeutral; - - leftNeutralToMidNeutral.addPoint(32, 67); - leftNeutralToMidNeutral.addPoint(65.3, 40); - leftNeutralToMidNeutral.addPoint(70.3, 65); - - leftNeutralToMidNeutralIndex = purePursuit.addPath(leftNeutralToMidNeutral); - - // mid neutral to mid home zone - - Path rightNeutralToFarPlatform; - - rightNeutralToFarPlatform.addPoint(105.7, 61); - rightNeutralToFarPlatform.addPoint(75, 76.5); - rightNeutralToFarPlatform.addPoint(75, 100); - rightNeutralToFarPlatform.addPoint(60.3, 115); - - rightNeutralToFarPlatformIndex = purePursuit.addPath(rightNeutralToFarPlatform); - - Path farPlatformToNearPlatform; - - farPlatformToNearPlatform.addPoint(70.3, 107); - farPlatformToNearPlatform.addPoint(60, 70.3); - farPlatformToNearPlatform.addPoint(58.6, 64.1); - farPlatformToNearPlatform.addPoint(58.6, 45); - farPlatformToNearPlatform.addPoint(70.3, 30.7); - - farPlatformToNearPlatformIndex = purePursuit.addPath(farPlatformToNearPlatform); - - Path nearPlatformViaLeftNeutralToFarPlatform; - - nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 30.7); - nearPlatformViaLeftNeutralToFarPlatform.addPoint(35, 61); - nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 115); - - nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit.addPath(nearPlatformViaLeftNeutralToFarPlatform); - - Path nearPlatformToMid; - - nearPlatformToMid.addPoint(70.3, 115); - nearPlatformToMid.addPoint(70.3, 70.3); - - nearPlatformToMidIndex = purePursuit.addPath(nearPlatformToMid); - -} - /** * Filter and apply the quadratic function. */ @@ -637,12 +496,14 @@ void initialize() { initSensors(); initMotors(); initDrivetrain(); - autoPaths(); + autoPaths(purePursuit); initController(); initLogger(); // initSelector(); } +// !SECTION +// SECTION Disabled /** * Runs while the robot is disabled i.e. before and after match, between auton * and teleop period @@ -663,6 +524,10 @@ void disabled() { } +// !SECTION + +// SECTION Competition Initialize + /** * Starts when connected to the field */ @@ -671,6 +536,10 @@ void competition_initialize() { } +// !SECTION + +// SECTION Auton + /** * Runs during the autonomous. NO user control */ @@ -683,6 +552,9 @@ void autonomous() { postAuton(); } +// !SECTION + +// SECTION Operator Control /** * Runs during operator/teleop control @@ -734,3 +606,5 @@ void opcontrol() { pros::delay(10); } } + +// !SECTION