crossBarrierDetection.cpp File Reference

#include "globals.h"
#include <sys/types.h>

Go to the source code of this file.

Functions
void	crossBarrier ()

Variables
const double	PITCH_CLIMB_THRESHOLD = 15.0
const double	PITCH_LEVEL_THRESHOLD = 5.0
const double	CROSSING_TIMEOUT = 3000
const double	DRIVE_SPEED = 90
const double	HEADING_KP = 2.0

Function Documentation

crossBarrier()

void crossBarrier

(

)

Definition at line 11 of file crossBarrierDetection.cpp.

                    {
    double targetHeading = imu.get_heading();
    
    bool hasClimbed = false;
    bool hasDropped = false;
    
    uint32_t startTime = pros::millis();
    while (true) {
        u_int32_t currentTime = pros::millis();
        if (pros::millis() - startTime > CROSSING_TIMEOUT) {
            chassis.tank(-127, -127);
            pros::delay(300);
            chassis.tank(0, 0);
            break; 
        }
 
        double currentPitch = imu.get_pitch(); 
        double currentHeading = imu.get_heading();
        double error = targetHeading - currentHeading;
        
        while (error > 180) error -= 360;
        while (error < -180) error += 360;
 
        double turnOffset = error * HEADING_KP;
 
        chassis.tank(DRIVE_SPEED + turnOffset, DRIVE_SPEED - turnOffset);
        if (!hasClimbed && currentPitch > PITCH_CLIMB_THRESHOLD) {
            hasClimbed = true;
            std::cout << "Status: CLIMBING" << std::endl;
        }
 
        if (hasClimbed && !hasDropped && currentPitch < PITCH_LEVEL_THRESHOLD) {
            hasDropped = true;
            std::cout << "Status: DROPPING" << std::endl;
        }
 
        if (hasClimbed && hasDropped && std::abs(currentPitch) < PITCH_LEVEL_THRESHOLD) {
            std::cout << "Status: LANDED" << std::endl;
            
            pros::delay(200); 
            chassis.tank(0, 0);
            break; 
        }
        pros::Task::delay_until(&currentTime, 10);
    }
}

References chassis, CROSSING_TIMEOUT, DRIVE_SPEED, HEADING_KP, imu, PITCH_CLIMB_THRESHOLD, and PITCH_LEVEL_THRESHOLD.

Variable Documentation

CROSSING_TIMEOUT

const double CROSSING_TIMEOUT = 3000

Definition at line 7 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

DRIVE_SPEED

const double DRIVE_SPEED = 90

Definition at line 8 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

HEADING_KP

const double HEADING_KP = 2.0

Definition at line 9 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

PITCH_CLIMB_THRESHOLD

const double PITCH_CLIMB_THRESHOLD = 15.0

Definition at line 5 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

PITCH_LEVEL_THRESHOLD

const double PITCH_LEVEL_THRESHOLD = 5.0

Definition at line 6 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().