autonRoutines.cpp File Reference

#include "globals.h"
#include "auton/autonFunctions.h"
#include "ltv.h"
#include "pros/rtos.hpp"
#include "velocityController.h"
#include "paths.h"
#include "ramsete.h"
#include "distanceReset.h"
#include "MCL.h"
#include "colorSort.h"
#include "crossBarrierDetection.h"

Go to the source code of this file.

Functions
RamsetePathFollower	ramsete (config, 2.5, 0.9)
LTVPathFollower	ltv (config)
void	precompute_auton_paths ()
void	right_auton ()
void	carry_auton ()
void	left_auton ()
void	elim_auton ()
void	awp_auton ()
void	skills_auton ()

Variables
const int	longgoal_delay = 1100
const int	midgoal_delay = 1000
const int	matchload_delay = 500
const int	mid_triball_delay = 500
const int	longgoal_offset = 6
const int	midgoal_offset = 11.7
const int	matchload_offset = 11.9
const int	triball_delay = 500
const int	dual_ball_delay = 500
const VelocityControllerConfig	config

Function Documentation

awp_auton()

void awp_auton

(

)

Definition at line 174 of file autonRoutines.cpp.

                 {
    /*
    MCL::StartMCL(-51.25, -18.5, 180);
    pros::Task mclTask(MCL::MonteCarlo);
    enable_fused_odometry(true);
    */
    //colorSort(Color::RED);
    //FIRST LONGGOAL
    /*
    MCL::StartMCL(-51.25, -18.5, 180);
    pros::Task mclTask(MCL::MonteCarlo);
    enable_fused_odometry(true);
    */
    //colorSort(Color::RED);
 
    //FIRST LONGGOAL
    descore.extend();
    chassis.setPose(-51.25, -18.5, 180);
    intake();
    chassis.moveToPoint(-51.25, -51, 1000);
    matchload_state(true);
    chassis.waitUntilDone();
 
    chassis.turnToPoint(-72, -51, 1000);
    chassis.waitUntilDone();
    
    chassis.moveToPoint(-72 + matchload_offset, -51, 1000, {.minSpeed = 20});
    chassis.waitUntilDone();
    pros::delay(50);
    
    /*
    matchload_state(true);
    intake();
    ramsete.followPath(awp_1, {.end_correction = true};
    */
 
    //pros::delay(matchload_delay);
    chassis.turnToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false});
    chassis.moveToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false, .minSpeed=35});
    chassis.waitUntilDone();
    resting_state();
    matchload_state(false);
    score_longgoal_auton();
    distanceReset(true);
    pros::delay(longgoal_delay + 150);
    resting_state();
    /*
    relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, 4, 1000, true);
 
    //MIDDLE AUTON
    //chassis.swingToPoint(-20 , -24, lemlib::DriveSide::RIGHT, 800);
    chassis.turnToPoint(-24, -24, 1000);
 
    intake();
    chassis.moveToPoint(-24 , -24, 1000, {.maxSpeed = 85, .minSpeed = 20});
    chassis.waitUntil(17);
    */
    ramsete.followPath(awp_1, {.end_correction = true});
    ramsete.waitUntilDone();
    matchload_state(true);
    chassis.waitUntilDone();
 
    chassis.turnToPoint(-24, 24, 1000);
    chassis.waitUntilDone();
    chassis.moveToPoint(-24, 24, 1500, {.maxSpeed = 85});
    chassis.waitUntil(5);
    matchload_state(false);
    chassis.waitUntil(39);
    matchload_state(true);
    chassis.waitUntilDone();
    chassis.turnToPoint(-12.3, 14.2, 1000, {.forwards = false});
    chassis.waitUntilDone();
    //relativeMotion(-20, 20, 133, 11.5, 1000, false);
    //chassis.moveToPoint(-10, 11.7, 1000, {.forwards = false});
    chassis.moveToPoint(-12.3, 14.2, 1000, {.forwards = false});
    //chassis.moveToPose(-11, 12, 133, 850, {.forwards = false, .minSpeed = 20});
    chassis.waitUntilDone();
    resting_state();
    score_midgoal();
    pros::delay(midgoal_delay + 150);
    intake_stop();
    matchload_state(true);
    intake();
    chassis.turnToPoint(-45, 54, 2000);
    chassis.waitUntilDone();
    chassis.moveToPoint(-45, 54, 1200);
    chassis.waitUntilDone();
    trapDoor.retract();
    matchload_state(true);
    matchload.extend();
    chassis.waitUntilDone();
    chassis.turnToHeading(270, 1000);
    chassis.waitUntilDone();
    pros::delay(5);
    distancePose pose = distanceReset(true);
    std::cout << "x: " << pose.x << " y: " << pose.y << std::endl;
    chassis.moveToPoint(-72 + matchload_offset - 2, 47.5, 1500, {.maxSpeed = 100, .minSpeed = 20});
    chassis.waitUntilDone();
    pros::delay(250);
    
    chassis.turnToPoint(-22 - longgoal_offset - 2.5, 47.5, 1000, {.forwards = false});
 
    chassis.moveToPoint(-22 - longgoal_offset - 2.5, 47.5, 2000, {.forwards = false, .minSpeed = 35});
    chassis.waitUntil(10);
    matchload_state(false);
    chassis.waitUntilDone();
    score_longgoal_auton();
}

References awp_1, chassis, descore(), distanceReset(), intake(), intake_stop(), longgoal_delay, longgoal_offset, matchload(), matchload_offset, matchload_state(), midgoal_delay, ramsete(), resting_state(), score_longgoal_auton(), score_midgoal(), trapDoor(), distancePose::x, and distancePose::y.

Referenced by selector().

carry_auton()

void carry_auton

(

)

Definition at line 110 of file autonRoutines.cpp.

                   {
    chassis.setPose(-51.25, -18.5, 180);
    distanceReset(true);
    intake();
    ramsete.followPath(awp_1, {.end_correction = true});
    ramsete.waitUntil(5);
    matchload_state(true);
    ramsete.waitUntilDone();
    pros::delay(matchload_delay);
    chassis.turnToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false});
    chassis.moveToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false, .minSpeed=35}, false);
    intake_stop();
    score_longgoal_auton();
    pros::delay(longgoal_delay + 150);
    matchload_state(false);
    distanceReset(true);
 
    
}

References awp_1, chassis, distanceReset(), intake(), intake_stop(), longgoal_delay, longgoal_offset, matchload_delay, matchload_state(), ramsete(), and score_longgoal_auton().

Referenced by selector().

elim_auton()

void elim_auton

(

)

Definition at line 170 of file autonRoutines.cpp.

                  {
    ltv.followPath(skills_test, {.test = true});
}

References ltv(), and skills_test.

Referenced by autonomous(), and selector().

left_auton()

void left_auton

(

)

Definition at line 130 of file autonRoutines.cpp.

                  {
 
    chassis.setPose(-47, 12, 60);
    intake();
    chassis.turnToPoint(-22, 22, 1000, {});
    chassis.moveToPoint(-22, 22 , 1000, {});
    chassis.waitUntil(20);
    matchload_state(true);
    chassis.waitUntilDone();
    pros::delay(100);
    matchload_state(false);
 
    ramsete.followPath(left_1, {.end_correction = true});
    matchload_state(true);
    ramsete.followPath(left_2, {.backwards = true});
    
    chassis.turnToPoint(-72 + matchload_offset, 47, 1000);
    distanceReset(true);
    chassis.moveToPoint(-72 + matchload_offset, 47, 1500, {.forwards = true}, false);
    pros::delay(matchload_delay);
 
    chassis.turnToPoint(-22 - longgoal_offset, 47, 1000, {.forwards = false});
    chassis.moveToPoint(-22 - longgoal_offset, 47, 2000, {.forwards = false, .minSpeed = 35});
    chassis.waitUntil(10);
    intake_stop();
    chassis.waitUntilDone();
    score_longgoal_auton();
    pros::delay(longgoal_delay + 150);
    resting_state();
 
    ramsete.followPath(left_3, {.end_correction = true});
    descore.retract();
    leftMotors.move(100);
    rightMotors.move(100);
    pros::delay(1500);
    leftMotors.brake();
    rightMotors.brake();
 
}

References chassis, descore(), distanceReset(), intake(), intake_stop(), left_1, left_2, left_3, leftMotors, longgoal_delay, longgoal_offset, matchload_delay, matchload_offset, matchload_state(), ramsete(), resting_state(), rightMotors, and score_longgoal_auton().

Referenced by selector().

ltv()

LTVPathFollower ltv

(

config

)

References config.

Referenced by elim_auton().

precompute_auton_paths()

void precompute_auton_paths

(

)

Definition at line 55 of file autonRoutines.cpp.

                              {
    std::vector<std::string> paths = {};
    ramsete.precompute_paths(paths);
}

References ramsete().

ramsete()

RamsetePathFollower ramsete	(	config	,
		2.	5,
		0.	9 )

References config.

Referenced by awp_auton(), carry_auton(), left_auton(), precompute_auton_paths(), and skills_auton().

right_auton()

void right_auton

(

)

Definition at line 60 of file autonRoutines.cpp.

{
    int longgoal_y_believed = -48;
 
    chassis.setPose(-51.25, -18.5, 180);
    intake();
    chassis.moveToPoint(-51.25, -51, 1000);
    matchload_state(true);
    chassis.waitUntilDone();
 
    chassis.turnToPoint(-72, -51, 1000);
    chassis.waitUntilDone();
    
    chassis.moveToPoint(-72 + matchload_offset - 1.25, -51, 1000, {.minSpeed = 20});
    chassis.waitUntilDone();
    pros::delay(175);
    chassis.turnToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false});
    chassis.moveToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false, .minSpeed=35});
    chassis.waitUntilDone();
    resting_state();
    matchload_state(false);
    score_longgoal_auton();
    distancePose pose = distanceReset(true);
    longgoal_y_believed = pose.y;
    pros::delay(longgoal_delay + 150);
    resting_state();
    relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, 4, 200, true);
    chassis.turnToPoint(-22, -22, 1000);
    intake();
    chassis.moveToPoint(-22 , -22, 1000, {.maxSpeed = 85, .minSpeed = 20});
    chassis.waitUntil(16);
    matchload_state(true);
    chassis.waitUntilDone();
 
    chassis.turnToPoint(-39, longgoal_y_believed, 1000, {.forwards = false});
    chassis.moveToPoint(-39, longgoal_y_believed, 1500, {.forwards = false});
    chassis.turnToPoint(-22 - longgoal_offset - 2, longgoal_y_believed, 1000, {.forwards = false});
    chassis.moveToPoint(-22 - longgoal_offset - 2, longgoal_y_believed, 1500, {.forwards = false}, false);
    intake_stop();
    score_longgoal_auton();
    pros::delay(1000);
    resting_state(false);
    chassis.moveToPoint(-22 - longgoal_offset - 1, longgoal_y_believed, 1000, {.forwards = false});
    chassis.moveToPoint(-22 - longgoal_offset - 1, longgoal_y_believed, 1000, {.forwards = false});
    score_longgoal_auton();
    pros::delay(1000);
 
 
}

References chassis, distanceReset(), intake(), intake_stop(), longgoal_delay, longgoal_offset, matchload_offset, matchload_state(), relativeMotion(), resting_state(), score_longgoal_auton(), and distancePose::y.

Referenced by selector().

skills_auton()

void skills_auton

(

)

Definition at line 283 of file autonRoutines.cpp.

                    {
    descore.extend();
    color_sort_enable = false;
 
    const int longgoal_delay = 1100;
    const int midgoal_delay = 1000;
    const int matchload_delay = 500;
    const int mid_triball_delay = 500;
    const int longgoal_offset = 6;
    const int midgoal_offset = 11.7;
    const int matchload_offset = 11.9;
    const int triball_delay = 500;
    const int dual_ball_delay = 500;
    /*
    chassis.setPose(-55, 0, 270);
    chassis.tank(90, 90);
    pros::delay(1200);
    leftMotors.brake();
    rightMotors.brake();
    pros::delay(1000);
    chassis.tank(-100, -100);
    pros::delay(600);
    leftMotors.brake();
    rightMotors.brake();
    chassis.turnToHeading(270, 1000);
    chassis.waitUntilDone();
    */
    chassis.setPose(-46, 0, 270);
    distancePose pose = distanceReset(true);
    std::cout << "Believed Y from parking zone" << pose.y << std::endl;
    chassis.setPose(pose.x, pose.y, 270);
    chassis.turnToPoint(-24, 24, 1000);
    chassis.moveToPoint(-24, 24, 2000);
    chassis.turnToPoint(-12.3, 14.2, 1000, {.forwards = false});
    chassis.moveToPoint(-12.3, 14.2, 1500, {.forwards = false}, false);
    
    chassis.turnToPoint(-45, 54, 1000);
    chassis.moveToPoint(-45, 54, 2000);
    chassis.turnToPoint(-72 + matchload_offset, 47.5, 1000, {}), false;
    distanceReset(true);
    chassis.moveToPoint(-72 + matchload_offset, 47.5, 1500, {}, false);
    ramsete.followPath(skills_1, {.backwards = true});
    ramsete.waitUntilDone();
    chassis.moveToPoint(22 + longgoal_offset, 62, 3000, {.forwards = false, .maxSpeed = 90}, false);
    distanceReset(true);
    chassis.turnToPoint(45, 48, 1500, {.forwards = false});
    chassis.moveToPoint(47, 48, 2000, {.forwards = false});
    chassis.turnToPoint(22 + longgoal_offset - 2, 48, 1000, {.forwards = false}, false);
    distanceReset(true);
    chassis.moveToPoint(22 + longgoal_offset - 2, 48, 1500, {.forwards = false}, false);
    distanceReset(true);
    chassis.turnToPoint(72 - matchload_offset, 47, 1000);
    chassis.moveToPoint(72 - matchload_offset, 47, 2000, {}, false);
    pros::delay(matchload_delay);
    chassis.turnToPoint(22 + longgoal_offset - 2, 48, 1000, {.forwards = false});
    chassis.moveToPoint(22 + longgoal_offset - 2, 48, 1500, {.forwards = false}, false);
    pros::delay(longgoal_delay);
    distanceReset(true);
    ramsete.followPath(skills_2, {.backwards = false, .end_correction = true});
    ramsete.waitUntilDone();
    chassis.tank(100, 100);
    pros::delay(1500);
    chassis.turnToHeading(180, 2000);
    chassis.waitUntilDone();
    distanceReset(true);
    ramsete.followPath(skills_3, {.backwards = false, .end_correction = true});
    ramsete.waitUntilDone();
    chassis.turnToPoint(72 - matchload_offset, -47, 1000);
    distanceReset(true);
    chassis.moveToPoint(72 - matchload_offset, -47, 2000, {}, false);
    pros::delay(matchload_delay);
 
    ramsete.followPath(skills_4, {.backwards = true, .end_correction = true});
    ramsete.waitUntilDone();
    chassis.moveToPoint(-22 - longgoal_offset, -62, 2000, {.forwards = true, .earlyExitRange = 2}, false);
    ramsete.followPath(skills_5, {.backwards = false, .end_correction = true});
    ramsete.waitUntilDone();
    chassis.turnToPoint(-22 - longgoal_offset - 2, -47, 1000, {.forwards = false}, false);
    distanceReset(true);
    chassis.moveToPoint(-22 - longgoal_offset - 2, -47, 1500, {.forwards = false}, false);
    pros::delay(longgoal_delay);
    distanceReset(true);
 
    chassis.turnToPoint(-72 + matchload_offset, -47, 1000);
    chassis.moveToPoint(-72 + matchload_offset, -47, 2000, {.forwards = true}, false);
    pros::delay(matchload_delay);
 
    ramsete.followPath(skills_6, {.backwards = false, .end_correction = true});
    ramsete.waitUntilDone();
    chassis.turnToPoint(-72 + 5.5, 0, 1000);
    chassis.moveToPoint(-72 + 5.5, 0, 2000, {.forwards = true});
 
}

References chassis, color_sort_enable, descore(), distanceReset(), dual_ball_delay, longgoal_delay, longgoal_offset, matchload_delay, matchload_offset, mid_triball_delay, midgoal_delay, midgoal_offset, ramsete(), skills_1, skills_2, skills_3, skills_4, skills_5, skills_6, triball_delay, distancePose::x, and distancePose::y.

Referenced by selector().

Variable Documentation

config

const VelocityControllerConfig config

Initial value:

{
5.8432642308,
0.213526937516,
1.14429410811,
0.906356177095,
0.347072436421,
11.4953431776,
54.5797495382,
}

Definition at line 41 of file autonRoutines.cpp.

                                     {
5.8432642308,
0.213526937516,
1.14429410811,
0.906356177095,
0.347072436421,
11.4953431776,
54.5797495382,
};

Referenced by ltv(), and ramsete().

dual_ball_delay

const int dual_ball_delay = 500

Definition at line 39 of file autonRoutines.cpp.

Referenced by skills_auton().

longgoal_delay

const int longgoal_delay = 1100

Definition at line 31 of file autonRoutines.cpp.

Referenced by awp_auton(), carry_auton(), left_auton(), right_auton(), and skills_auton().

longgoal_offset

const int longgoal_offset = 6

Definition at line 35 of file autonRoutines.cpp.

Referenced by awp_auton(), carry_auton(), left_auton(), right_auton(), and skills_auton().

matchload_delay

const int matchload_delay = 500

Definition at line 33 of file autonRoutines.cpp.

Referenced by carry_auton(), left_auton(), and skills_auton().

matchload_offset

const int matchload_offset = 11.9

Definition at line 37 of file autonRoutines.cpp.

Referenced by awp_auton(), left_auton(), right_auton(), and skills_auton().

mid_triball_delay

const int mid_triball_delay = 500

Definition at line 34 of file autonRoutines.cpp.

Referenced by skills_auton().

midgoal_delay

const int midgoal_delay = 1000

Definition at line 32 of file autonRoutines.cpp.

Referenced by awp_auton(), and skills_auton().

midgoal_offset

const int midgoal_offset = 11.7

Definition at line 36 of file autonRoutines.cpp.

Referenced by skills_auton().

triball_delay

const int triball_delay = 500

Definition at line 38 of file autonRoutines.cpp.

Referenced by skills_auton().