autonFunctions.cpp File Reference

#include "main.h"
#include "globals.h"
#include "pros/misc.h"
#include "lemlib/api.hpp"
#include "lemlib/util.hpp"
#include "MCL.h"
#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_longgoal_auton (int power=12000, Color allianceColor=Color::RED)
void	intake_to_basket ()
void	resting_state (bool trapDoor_commanded=false)
void	matchload_state (bool state)
void	longgoal_prep ()
void	reset_odometry ()
void	matchload_wiggle (int time=1000, int speed=100)
void	MCL_reset (bool x=true, bool y=true)
void	fusion_loop_fn (void *ignore)
void	enable_fused_odometry (bool enable)
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)

Variables
pros::Task *	fusionTask = nullptr

Function Documentation

enable_fused_odometry()

void enable_fused_odometry

(

bool

enable

)

Definition at line 170 of file autonFunctions.cpp.

                                        {
    if (enable) {
        if (fusionTask == nullptr) {
            fusionTask = new pros::Task(fusion_loop_fn, NULL, "MCL_Fusion");
        }
    } else {
        if (fusionTask != nullptr) {
            fusionTask->remove(); 
            delete fusionTask;  
            fusionTask = nullptr; 
        }
    }
}

References fusion_loop_fn(), and fusionTask.

fusion_loop_fn()

void fusion_loop_fn

(

void *

ignore

)

Definition at line 148 of file autonFunctions.cpp.

                                  {
    const uint32_t LOOP_DELAY_MS = 10;
    uint32_t start_time = pros::millis();
 
    const float MCL_GAIN = 0.05f; 
 
    while (true) {
        lemlib::Pose odomPose = chassis.getPose(true);
        
        MCL::particle_mutex.take();
        float target_x = MCL::X;
        float target_y = MCL::Y;
        MCL::particle_mutex.give();
 
        float new_x = odomPose.x + (target_x - odomPose.x) * MCL_GAIN;
        float new_y = odomPose.y + (target_y - odomPose.y) * MCL_GAIN;
        chassis.setPose(new_x, new_y, odomPose.theta);
 
        pros::Task::delay_until(&start_time, LOOP_DELAY_MS);
    }
}

References chassis, MCL::particle_mutex, MCL::X, and MCL::Y.

Referenced by enable_fused_odometry().

intake()

void intake

(

int

power = 12000

)

Definition at line 12 of file autonFunctions.cpp.

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

References intakeMotor(), and topMotor().

Referenced by awp_auton(), carry_auton(), intake_to_basket(), left_auton(), opcontrol(), right_auton(), score_longgoal(), and score_midgoal().

intake_stop()

void intake_stop

(

)

Definition at line 46 of file autonFunctions.cpp.

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

References intakeMotor(), and topMotor().

Referenced by awp_auton(), carry_auton(), left_auton(), matchload_counter(), resting_state(), and right_auton().

intake_to_basket()

void intake_to_basket

(

)

Definition at line 80 of file autonFunctions.cpp.

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

References intake(), and topMotor().

Referenced by opcontrol().

longgoal_prep()

void longgoal_prep

(

)

Definition at line 112 of file autonFunctions.cpp.

{
 
}

matchload_counter()

void matchload_counter	(	int	balls,
		int	time_ms )

Definition at line 195 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 97 of file autonFunctions.cpp.

{
    if(state && !matchload.is_extended())
    {
        matchload.extend();
    }
    else if(matchload.is_extended())
    {
        matchload.retract();
    }
    else {
        std::cout << "Matchload state unchanged\n" << std::endl;
    }
}

References matchload().

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

matchload_wiggle()

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

Definition at line 122 of file autonFunctions.cpp.

{
    u_int32_t start_time = pros::millis();
    int sign = 1;
   while(pros::millis() - start_time < time)
   {
        leftMotors.move_voltage(3000 * sign);
        rightMotors.move_voltage(3000 * sign);
        sign *= -1;
        pros::delay(speed);
   }
   leftMotors.brake();
   rightMotors.brake();
}

References leftMotors, and rightMotors.

MCL_reset()

void MCL_reset	(	bool	x = true,
		bool	y = true )

Definition at line 137 of file autonFunctions.cpp.

{
    MCL::particle_mutex.take();
    float X = x ? MCL::X : chassis.getPose().x;
    float Y = y ? MCL::Y : chassis.getPose().y;
    MCL::particle_mutex.give();
    chassis.setPose(X, Y, chassis.getPose().theta);
}

References chassis, MCL::particle_mutex, MCL::X, and MCL::Y.

outtake()

void outtake

(

int

power = 12000

)

Definition at line 19 of file autonFunctions.cpp.

{
    intakeMotor.move_voltage(-power);
}

References intakeMotor().

Referenced by opcontrol(), and score_midgoal().

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 184 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 right_auton().

reset_odometry()

void reset_odometry

(

)

Definition at line 117 of file autonFunctions.cpp.

{
 
}

resting_state()

void resting_state

(

bool

trapDoor_commanded = false

)

Definition at line 88 of file autonFunctions.cpp.

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

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

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

score_bottomgoal()

void score_bottomgoal

(

int

power = 12000

)

Definition at line 24 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 29 of file autonFunctions.cpp.

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

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

Referenced by opcontrol(), and score_longgoal_auton().

score_longgoal_auton()

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

Definition at line 73 of file autonFunctions.cpp.

{
    leftMotors.move(-20);
    rightMotors.move(-20);
    score_longgoal(power, allianceColor);
}

References allianceColor, leftMotors, rightMotors, and score_longgoal().

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

score_midgoal()

void score_midgoal

(

int

power = 12000

)

Definition at line 52 of file autonFunctions.cpp.

{
    if(get_color() != allianceColor && get_color() != Color::NONE && color_sort_enable)
    {
        topMotor.move_voltage(-8000);
    }
    else {
        topMotor.move_voltage(12000);
    }
    if(midgoal_first)
    {
        outtake(8000);
        topMotor.move_voltage(-12000);
        pros::delay(300);
        midgoal_first = false;
    }
    intake(power);
    topMotor.move_voltage(power);
    if(!trapDoor.is_extended()) trapDoor.extend();
}

References allianceColor, color_sort_enable, get_color(), intake(), midgoal_first, outtake(), topMotor(), and trapDoor().

Referenced by awp_auton(), and opcontrol().

Variable Documentation

fusionTask

pros::Task* fusionTask = nullptr

Definition at line 146 of file autonFunctions.cpp.

Referenced by enable_fused_odometry().