autonFunctions.cpp File Reference

#include "main.h"
#include "globals.h"
#include "pros/misc.h"
#include "lemlib/api.hpp"
#include "lemlib/util.hpp"
#include "pros/motors.h"
#include "pros/rtos.hpp"
#include <cmath>
#include "colorSort.h"

Go to the source code of this file.

Functions
void	intake (int power=12000)
void	outtake (int power=12000)
void	score_bottomgoal (int power=12000)
void	score_longgoal (int power=12000, Color allianceColor=Color::RED)
void	intake_stop ()
void	score_midgoal (int power=12000)
void	score_midgoal_auton (int power=12000, Color allianceColor=Color::RED, int time=-1)
void	score_longgoal_auton (int power=12000, Color allianceColor=Color::RED, int time=-1)
void	intake_to_basket ()
void	resting_state (bool trapDoor_commanded=false)
void	matchload_state (bool state)
void	matchload_wiggle (int time=1000, int speed=100)
void	relativeMotion (float expected_x, float expected_y, float expected_theta, float distance, int timeout_ms, bool forw=true, float earlyExit=0)
void	matchload_counter (int balls, int time_ms)
void	alignToGoal (double targetAngle)
double	calculateAngle (double robotHeading)

Function Documentation

alignToGoal()

void alignToGoal

(

double

targetAngle

)

Definition at line 256 of file autonFunctions.cpp.

                                     {
 
    pros::Task([=]() {
 
        double currentTheta = chassis.getPose().theta;
        double error = std::remainder(targetAngle - currentTheta, 360.0);
        if (std::abs(error) > 5.0) {
            std::cout << "Error big enough to activate alignment " << error << std::endl;
            if (error > 0) {
                leftMotors.move(-100);
                rightMotors.move(0);
            } else {
                leftMotors.move(0);
                rightMotors.move(-100);
            }
            pros::delay(std::clamp((int)error * 100, 300, 1000)); 
        }
 
        leftMotors.brake();
        rightMotors.brake();
        chassis.tank(0,0);
        pros::Task::current().remove();
    });
}

References chassis, leftMotors, and rightMotors.

calculateAngle()

double calculateAngle

(

double

robotHeading

)

Definition at line 281 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(), and frontDistance2().

intake()

void intake

(

int

power = 12000

)

Definition at line 11 of file autonFunctions.cpp.

{
    intakeFunnel.extend();
    intakeMotor.move_voltage(power);
    topMotor.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
    topMotor.brake();
}

References intakeFunnel(), intakeMotor(), and topMotor().

Referenced by awp_auton(), elim_auton(), intake_to_basket(), left_auton(), opcontrol(), right_auton(), score_longgoal(), score_midgoal_auton(), and skills_auton().

intake_stop()

void intake_stop

(

)

Definition at line 56 of file autonFunctions.cpp.

{
    intakeMotor.move_voltage(0);
    topMotor.move_voltage(0);
}

References intakeMotor(), and topMotor().

Referenced by elim_auton(), left_auton(), matchload_counter(), resting_state(), right_auton(), score_longgoal_auton(), and skills_auton().

intake_to_basket()

void intake_to_basket

(

)

Definition at line 167 of file autonFunctions.cpp.

{
    intake();
    topMotor.move_voltage(-8000);
}

References intake(), and topMotor().

matchload_counter()

void matchload_counter	(	int	balls,
		int	time_ms )

Definition at line 228 of file autonFunctions.cpp.

{
    pros::Task counter_task([=]() {
        int count = 0;
        uint32_t start_time = pros::millis();
        while(pros::millis() - start_time < time_ms && count < balls)
        {
            start_time = pros::millis();
            if(frontDistance.get_object_size() <= 70)
            {
                count++;
            }
            pros::Task::delay_until(&start_time, 10);
        }
        intake_stop();
        pros::Task::current().remove();
    });
}

References frontDistance(), and intake_stop().

matchload_state()

void matchload_state

(

bool

state

)

Definition at line 181 of file autonFunctions.cpp.

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

References matchload().

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

matchload_wiggle()

void matchload_wiggle	(	int	time = 1000,
		int	speed = 100 )

Definition at line 190 of file autonFunctions.cpp.

{
    uint32_t startTime = pros::millis();
    double initialHeading = chassis.getPose().theta;
    double targetHeading = std::round(initialHeading / 90.0) * 90.0;
    bool flip = false;
 
    while ((pros::millis() - startTime) < time) {
        if (flip) {
            leftMotors.move(-speed);
            rightMotors.move(speed);
        } else {
            leftMotors.move(speed);
            rightMotors.move(-speed);
        }
        flip = !flip;
        pros::delay(100);
    }
 
    leftMotors.brake();
    rightMotors.brake();
    pros::delay(50);
    chassis.turnToHeading(targetHeading, 800);
}

References chassis, leftMotors, and rightMotors.

outtake()

void outtake

(

int

power = 12000

)

Definition at line 19 of file autonFunctions.cpp.

{
    if(intakeFunnel.is_extended())
    {
        intakeFunnel.retract();
        pros::delay(100);
        intakeMotor.move_voltage(12000);
        pros::delay(300);
    }
    topMotor.move_voltage(-12000);
    if(low_ramp_down_time >= 1000)
        intakeMotor.move_voltage( -12000);
    else
        intakeMotor.move_voltage(-power);
}

References intakeFunnel(), intakeMotor(), low_ramp_down_time, and topMotor().

Referenced by opcontrol(), and right_auton().

relativeMotion()

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

Definition at line 217 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 skills_auton().

resting_state()

void resting_state

(

bool

trapDoor_commanded = false

)

Definition at line 173 of file autonFunctions.cpp.

{
    intake_stop();
    if(!trapDoor_commanded)
        trapDoor.retract();
    topMotor.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
}

References intake_stop(), topMotor(), and trapDoor().

Referenced by opcontrol().

score_bottomgoal()

void score_bottomgoal

(

int

power = 12000

)

Definition at line 35 of file autonFunctions.cpp.

{
    intakeMotor.move_voltage(-power);
}

References intakeMotor().

score_longgoal()

void score_longgoal	(	int	power = 12000,
		Color	allianceColor = Color::RED )

Definition at line 40 of file autonFunctions.cpp.

{
    intake(power);
    if(get_color() != allianceColor && get_color() != Color::NONE && color_sort_enable)
    {
        topMotor.move_voltage(-8000);
        std::cout << "Color Rejected" << std::endl;
    }
    else
    {
        topMotor.move_voltage(power);
        std::cout << "Color Accepted" << std::endl;
    }
 
}

References allianceColor, color_sort_enable, get_color(), intake(), and topMotor().

Referenced by opcontrol().

score_longgoal_auton()

void score_longgoal_auton	(	int	power = 12000,
		Color	allianceColor = Color::RED,
		int	time = -1 )

Definition at line 133 of file autonFunctions.cpp.

{
    leftMotors.move(-50);
    rightMotors.move(-50);
    u_int32_t startTime = pros::millis();
    if(color_sort_enable)
    {
        blockBlocker.extend();
        scoringBand.extend();
    }
    if(time != -1)
    {
        while(pros::millis() - startTime < time)
        {
            if(get_color() != allianceColor && get_color() != Color::NONE && color_sort_enable)
            {
                topMotor.move_voltage(-10000);
                intakeMotor.move_voltage(12000);      
            }
            else
            {
                topMotor.move_voltage(power);
                intakeMotor.move_voltage(power);
            }
        }
    }
    intake_stop();
    chassis.tank(0,0);
    if(blockBlocker.is_extended()) blockBlocker.retract();
    if(scoringBand.is_extended()) scoringBand.retract();
}

References allianceColor, blockBlocker(), chassis, color_sort_enable, get_color(), intake_stop(), intakeMotor(), leftMotors, rightMotors, scoringBand(), and topMotor().

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

score_midgoal()

void score_midgoal

(

int

power = 12000

)

Definition at line 62 of file autonFunctions.cpp.

{
    if(midgoal_first)
    {
        if(!color_sort_enable)
        {
            midgoal_first = false;
            trapDoor.extend();
            intakeMotor.move_voltage(12000);
            topMotor.move_voltage(6000);
        }
        else if(color_sort_enable && get_color() == allianceColor)
        {
            midgoal_first = false;
            trapDoor.extend();
            intakeMotor.move_voltage(12000);
            topMotor.move_voltage(6000);
        }
    }
    if(get_color() != allianceColor && get_color() != Color::NONE && color_sort_enable)
    {
        topMotor.move_voltage(-8000);
    }
    else {
        if (ramp_up_time >= 1200)
        {
            topMotor.move_voltage(8000);
            intakeMotor.move_voltage(12000);
        }
        if (ramp_up_time >= 800)
        {
            topMotor.move_voltage(5000); // Prev: 4000
            intakeMotor.move_voltage(12000);
        }
        else if(ramp_up_time >= 400)
        {
            topMotor.move_voltage(6000);
            intakeMotor.move_voltage(12000);
        }
        else {
            topMotor.move_voltage(10000);
            intakeMotor.move_voltage(12000);
        }
    }
    //if (ramp_up_time >= 1600)
    //{
    //    topMotor.move_voltage(4000);
    //}
    //topMotor.move_voltage(10000);
    
}

References allianceColor, color_sort_enable, get_color(), intakeMotor(), midgoal_first, ramp_up_time, topMotor(), and trapDoor().

Referenced by opcontrol().

score_midgoal_auton()

void score_midgoal_auton	(	int	power = 12000,
		Color	allianceColor = Color::RED,
		int	time = -1 )

Definition at line 114 of file autonFunctions.cpp.

{
    intake(12000);
    chassis.tank(-35, -35);
    topMotor.move_velocity(330);
    pros::delay(250);
    topMotor.move_velocity(300);
    pros::delay(400);
    topMotor.move_velocity(300);
    pros::delay(400);
    topMotor.move_velocity(270);
    pros::delay(550);
    topMotor.move_velocity(270);
    pros::delay(400);
    topMotor.move_velocity(240);
    pros::delay(1400);
    
}

References allianceColor, chassis, intake(), and topMotor().

Referenced by skills_auton().