autonFunctions.cpp File Reference

#include "autonFunctions.h"
#include "globals.h"
#include "lemlib/util.hpp"
#include "pros/misc.h"
#include "pros/rtos.hpp"
#include <cmath>
#include <sys/types.h>
#include "colorSort.h"
#include "IntakeAntiJam.h"
#include "ltv.h"

Go to the source code of this file.

Functions
void	antiJamTask ()
void	intake (int power)
void	outtake (int power)
void	score_longgoal (int power, Color allianceColor)
void	intake_stop (bool hood_state)
void	score_midgoal (int power)
void	score_midgoal_auton (int power, Color allianceColor, int time)
void	score_longgoal_auton (int power, Color allianceColor, int time)
void	intake_to_basket ()
void	matchload_state (bool state)
void	relativeMotion (float expected_x, float expected_y, float expected_theta, float distance, int timeout_ms, bool forw, float earlyExit)
double	calculateAngle (double robotHeading)

Variables
const VelocityControllerConfig	config

Function Documentation

antiJamTask()

void antiJamTask

(

)

Definition at line 26 of file autonFunctions.cpp.

                   {
    while (true) {
        jamManager.update();
        pros::delay(10);
    }
}

References jamManager().

Referenced by initialize().

calculateAngle()

double calculateAngle

(

double

robotHeading

)

Definition at line 134 of file autonFunctions.cpp.

                                           {
    double d1 = frontDistance.get();
    double d2 = frontDistance2.get() + 10;
 
    if (d1 > 1800 || d2 > 1800) return -1;
    double wallAngleDeg = atan2((d1 - d2) * 0.0393701, 10.75) * 180.0 / M_PI;
 
    if ((robotHeading >= 315 && robotHeading < 360) || (robotHeading >= 0 && robotHeading < 45)) {
        return wallAngleDeg;
    }
    else if (robotHeading >= 45 && robotHeading < 135) {
        return 90 + wallAngleDeg;
    }
    else if (robotHeading >= 135 && robotHeading < 225) {
        return 180 + wallAngleDeg;
    }
    else if (robotHeading >= 225 && robotHeading < 315) {
        return 270 + wallAngleDeg;
    }
 
    return -1;
}

References frontDistance().

intake()

void intake

(

int

power

)

Definition at line 33 of file autonFunctions.cpp.

{
    if(hood.is_extended()) { hood.retract(); }
    jamManager.set_velocities((int)power/3, -(int)power/3, power);
}

References hood(), and jamManager().

Referenced by awp_auton(), left_auton_split(), left_rush(), opcontrol(), right_7_wing(), right_auton_split(), right_rush(), skills_auton(), and test_auton().

intake_stop()

void intake_stop

(

bool

hood_state

)

Definition at line 51 of file autonFunctions.cpp.

{
    jamManager.stop();
    if(!hood_state) hood.retract();
    else hood.extend();
    
    if(intake_lift.is_extended()) { intake_lift.retract(); }
}

References hood(), intake_lift(), and jamManager().

Referenced by awp_auton(), left_auton_split(), opcontrol(), right_auton_split(), score_longgoal_auton(), score_midgoal_auton(), skills_auton(), and test_auton().

intake_to_basket()

void intake_to_basket

(

)

Definition at line 105 of file autonFunctions.cpp.

{
 
}

matchload_state()

void matchload_state

(

bool

state

)

Definition at line 110 of file autonFunctions.cpp.

{
    if(state)
    {
        if(!matchloader.is_extended()) {matchloader.extend();}
    }
    else
    {
        if(matchloader.is_extended()) {matchloader.retract();}
    }
}

References matchloader().

Referenced by awp_auton(), left_auton_split(), left_rush(), right_7_wing(), right_auton_split(), right_rush(), skills_auton(), and test_auton().

outtake()

void outtake

(

int

power

)

Definition at line 39 of file autonFunctions.cpp.

{
    if(!intake_lift.is_extended()) { intake_lift.extend(); }
    jamManager.set_velocities(-((int)power/3), ((int)power/3), -power);
}

References intake_lift(), and jamManager().

Referenced by opcontrol(), and right_auton_split().

relativeMotion()

void relativeMotion	(	float	expected_x,
		float	expected_y,
		float	expected_theta,
		float	distance,
		int	timeout_ms,
		bool	forw,
		float	earlyExit )

Definition at line 123 of file autonFunctions.cpp.

{
    lemlib::Pose targetPose(
        expected_x + distance * std::sin(lemlib::degToRad(expected_theta)),
        expected_y + distance * std::cos(lemlib::degToRad(expected_theta)),
        expected_theta
    );
 
    chassis.moveToPoint(targetPose.x, targetPose.y, timeout_ms, {.forwards = forw, .earlyExitRange = earlyExit});
}

References chassis().

Referenced by left_auton_split(), and skills_auton().

score_longgoal()

void score_longgoal	(	int	power,
		Color	allianceColor )

Definition at line 45 of file autonFunctions.cpp.

{
    if(!hood.is_extended()) { hood.extend(); }
    jamManager.set_velocities((int)power/3, -(int)power/3, power);
}

References allianceColor, hood(), and jamManager().

Referenced by awp_auton(), opcontrol(), and skills_auton().

score_longgoal_auton()

void score_longgoal_auton	(	int	power,
		Color	allianceColor,
		int	time )

Definition at line 79 of file autonFunctions.cpp.

{
    u_int32_t start_time = pros::millis();
    if(!hood.is_extended()) {hood.extend();}
    if(time != -1) {   
        while(pros::millis() - start_time < (u_int32_t)time) {
            if(allianceColor != Color::NONE)
            {
                Color detected = get_color();
                if(detected != Color::NONE && detected != allianceColor && color_sort_enable)
                {
                    intake_stop();
                    jamManager.set_velocities(-200, 600, -200);
                    pros::delay(100);
                    intake_stop();
                    break;
                }
            }
            jamManager.set_velocities((int)power/3, -(int)power/3, power);
            chassis.tank(-20, -20);
            pros::delay(10); 
        }
    }
    chassis.brake();
    intake_stop();
}

References allianceColor, chassis(), color_sort_enable, get_color(), hood(), intake_stop(), and jamManager().

Referenced by awp_auton(), left_auton_split(), left_rush(), right_7_wing(), right_auton_split(), right_rush(), skills_auton(), and test_auton().

score_midgoal()

void score_midgoal

(

int

power

)

Definition at line 60 of file autonFunctions.cpp.

{
    jamManager.set_velocities((int)power/3, (int)power/3, power);
}

References jamManager().

Referenced by awp_auton(), left_auton_split(), opcontrol(), skills_auton(), and test_auton().

score_midgoal_auton()

void score_midgoal_auton	(	int	power,
		Color	allianceColor,
		int	time )

Definition at line 65 of file autonFunctions.cpp.

{
    u_int32_t start_time = pros::millis();
    if(time != -1) {   
        while(pros::millis() - start_time < (u_int32_t)time) {
            jamManager.set_velocities((int)power/3, (int)power/3, power);
            chassis.tank(-20, -20);
            pros::delay(10);
        }
    }
    chassis.brake();
    intake_stop();
}

References allianceColor, chassis(), intake_stop(), and jamManager().

Variable Documentation

config

const VelocityControllerConfig config

Initial value:

{
5.09498070723,
0.23396112183,
1.22496514644,
0.964796393603,
0.522291806997,
10.933332351,
44.6788428062,
}

Definition at line 16 of file autonFunctions.cpp.

                                     {
5.09498070723,
0.23396112183,
1.22496514644,
0.964796393603,
0.522291806997,
10.933332351,
44.6788428062,
};

Referenced by awp_auton(), left_auton_split(), ltv(), right_7_wing(), right_auton_split(), skills_auton(), and test_auton().