crossBarrierDetection.cpp File Reference

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

Go to the source code of this file.

Functions
void	crossBarrier (int times=2, bool reverse=false, bool fullyDrop=true)

Variables
double	PITCH_CLIMB_THRESHOLD = 8
double	PITCH_LEVEL_THRESHOLD = 1
const double	CROSSING_TIMEOUT = 1200
const double	DRIVE_SPEED = 127
const double	HEADING_KP = 6.0
const int	POST_LANDING_TIME = 200

Function Documentation

crossBarrier()

void crossBarrier	(	int	times = 2,
		bool	reverse = false,
		bool	fullyDrop = true )

Definition at line 12 of file crossBarrierDetection.cpp.

                                                                        {
    double targetHeading = imu.get_heading();
    int dir = reverse ? -1 : 1;
    if(reverse)
    {
        PITCH_CLIMB_THRESHOLD = 8.5;
        PITCH_LEVEL_THRESHOLD = 0.5;
    }
 
    for(int i = 0; i < times; i++) {
        bool hasClimbed = false;
        bool hasDropped = false;
        
        uint32_t startTime = pros::millis();
        uint32_t previousTime = pros::millis();
 
        while (true) {
 
            if (pros::millis() - startTime > CROSSING_TIMEOUT) {
                chassis.tank(0,0);
                leftMotors.brake();
                rightMotors.brake();
                return;
            }
 
            double currentPitch = imu.get_roll() * dir; 
            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 * dir) + turnOffset,
                         (DRIVE_SPEED * dir) - turnOffset);
 
            // Detect climb
            if (!hasClimbed && currentPitch > PITCH_CLIMB_THRESHOLD) {
                hasClimbed = true;
            }
 
            // Detect crest
            if (hasClimbed && !hasDropped && currentPitch < PITCH_LEVEL_THRESHOLD) {
                hasDropped = true;
            }
 
            // --- LANDING LOGIC ---
            if (hasClimbed && hasDropped) {
                if(i == 1 && times > 1)
                    matchload.retract();
 
                // FULLY DROP MODE
                if (fullyDrop && std::abs(currentPitch) < PITCH_LEVEL_THRESHOLD) {
 
                    uint32_t clearStartTime = pros::millis();
                    while (pros::millis() - clearStartTime < POST_LANDING_TIME) {
 
                        currentHeading = imu.get_heading();
                        error = targetHeading - currentHeading;
                        while (error > 180) error -= 360;
                        while (error < -180) error += 360;
 
                        turnOffset = error * HEADING_KP;
 
                        chassis.tank((DRIVE_SPEED * dir) + turnOffset,
                                     (DRIVE_SPEED * dir) - turnOffset);
 
                        pros::delay(10);
                    }
 
                    chassis.tank(0, 0);
                    leftMotors.brake();
                    rightMotors.brake();
                    break;
                }
 
                // ANGLED PARK MODE
                if (!fullyDrop && currentPitch < -PITCH_LEVEL_THRESHOLD) {
 
                    chassis.tank(0, 0);
 
                    // use hold instead of brake to prevent rocking forward
                    leftMotors.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
                    rightMotors.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
 
                    break;
                }
            }
 
            pros::Task::delay_until(&previousTime, 10);
        }
 
        if (i < times - 1)
            pros::delay(200);
    }
}

References chassis, CROSSING_TIMEOUT, DRIVE_SPEED, HEADING_KP, imu, leftMotors, matchload(), PITCH_CLIMB_THRESHOLD, PITCH_LEVEL_THRESHOLD, POST_LANDING_TIME, and rightMotors.

Referenced by skills_auton().

Variable Documentation

CROSSING_TIMEOUT

const double CROSSING_TIMEOUT = 1200

Definition at line 7 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

DRIVE_SPEED

const double DRIVE_SPEED = 127

Definition at line 8 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

HEADING_KP

const double HEADING_KP = 6.0

Definition at line 9 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

PITCH_CLIMB_THRESHOLD

double PITCH_CLIMB_THRESHOLD = 8

Definition at line 5 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

PITCH_LEVEL_THRESHOLD

double PITCH_LEVEL_THRESHOLD = 1

Definition at line 6 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().

POST_LANDING_TIME

const int POST_LANDING_TIME = 200

Definition at line 10 of file crossBarrierDetection.cpp.

Referenced by crossBarrier().