#include "globals.h"
#include "lemlib/util.hpp"
#include "pros/distance.hpp"
#include <numeric>

Classes
struct	SensorConfig
struct	SensorReadings
struct	distancePose

Functions
distancePose	calculateGlobalPosition (const SensorReadings &front_data, const SensorReadings &left_data, const SensorReadings &right_data, const SensorReadings &back_data, double heading_deg, float heading_tolerance=40.0)
distancePose	distanceReset (bool setPose=false, bool filter=true, float filter_range=3.5, float heading_tolerance=40.0)
distancePose	distanceReset (bool left_use, bool right_use, bool front_use, bool back_use, bool setPose)

Variables
const double	MM_TO_IN = 0.0393701
const double	FIELD_WIDTH = 3566.668 * MM_TO_IN
const double	FIELD_HEIGHT = 3566.668 * MM_TO_IN
const double	HALF_WIDTH = FIELD_WIDTH / 2.0
const double	HALF_HEIGHT = FIELD_HEIGHT / 2.0
const double	MAX_SENSOR_RANGE = 2000 * MM_TO_IN
const double	MIN_SENSOR_RANGE = 0 * MM_TO_IN
bool	controller_screen_avilable
const SensorConfig	front_sensor_cfg = {4.067 + 0.2, 5.756, 0}
const SensorConfig	left_sensor_cfg = {-0.5, -6.039 + 0.69, 90}
const SensorConfig	right_sensor_cfg = {0.5, 6.039 - 1, -90}
const SensorConfig	back_sensor_cfg = {-2.691 + 2.7, 3.879, 180}

Function Documentation

◆ calculateGlobalPosition()

distancePose calculateGlobalPosition	(	const SensorReadings &	front_data,
		const SensorReadings &	left_data,
		const SensorReadings &	right_data,
		const SensorReadings &	back_data,
		double	heading_deg,
		float	heading_tolerance = 40.0 )

Definition at line 44 of file distanceReset.cpp.

{
    lemlib::Pose current_pose = chassis.getPose();
    double est_x = current_pose.x;
    double est_y = current_pose.y;
    bool use_pos_x_wall = (est_x >= 0);
    bool use_pos_y_wall = (est_y >= 0);
 
    bool using_odom_x = true;
    bool using_odom_y = true;
 
    struct SensorData { SensorConfig cfg; double dist_in; int confidence;};
    const SensorData sensors[] = {
        {front_sensor_cfg, front_data.dist_mm * MM_TO_IN, frontDistance.get_confidence()},
        {left_sensor_cfg,  left_data.dist_mm * MM_TO_IN, leftDistance.get_confidence()},
        {right_sensor_cfg, right_data.dist_mm * MM_TO_IN, rightDistance.get_confidence()},
        {back_sensor_cfg,  back_data.dist_mm * MM_TO_IN, backDistance.get_confidence()}
    };
 
    auto is_valid = [&](int i) {
        return (sensors[i].dist_in < MAX_SENSOR_RANGE && 
                sensors[i].dist_in >= MIN_SENSOR_RANGE);
    };
 
    double norm_heading = std::fmod(heading_deg, 360.0);
    if (norm_heading < 0) norm_heading += 360.0;
    const double TOLERANCE = heading_tolerance; 
    
    std::vector<double> x_cands;
    std::vector<double> y_cands;
 
    auto get_global_offsets = [&](const SensorConfig& cfg, double heading_rad) {
        double cos_h = std::cos(heading_rad);
        double sin_h = std::sin(heading_rad);
        double global_offset_x = (cfg.forward_offset * sin_h) + (cfg.strafe_offset * cos_h);
        double global_offset_y = (cfg.forward_offset * cos_h) - (cfg.strafe_offset * sin_h);
        return std::make_pair(global_offset_x, global_offset_y);
    };
 
 
    if (norm_heading <= TOLERANCE || norm_heading >= 360.0 - TOLERANCE) {
        double angle_off_rad = (norm_heading <= TOLERANCE) ? 
            lemlib::degToRad(norm_heading) : lemlib::degToRad(norm_heading - 360.0);
        double heading_rad = angle_off_rad;
        double perp_dist_0 = sensors[0].dist_in * std::cos(angle_off_rad);
        double perp_dist_3 = sensors[3].dist_in * std::cos(angle_off_rad);
        double perp_dist_1 = sensors[1].dist_in * std::cos(angle_off_rad);
        double perp_dist_2 = sensors[2].dist_in * std::cos(angle_off_rad);
 
        auto offset_0 = get_global_offsets(sensors[0].cfg, heading_rad);
        auto offset_1 = get_global_offsets(sensors[1].cfg, heading_rad);
        auto offset_2 = get_global_offsets(sensors[2].cfg, heading_rad);
        auto offset_3 = get_global_offsets(sensors[3].cfg, heading_rad);
 
        if (is_valid(0) && use_pos_y_wall)  { y_cands.push_back(HALF_HEIGHT - perp_dist_0 - offset_0.second); }
        if (is_valid(3) && !use_pos_y_wall) { y_cands.push_back(-HALF_HEIGHT + perp_dist_3 - offset_3.second); }
        if (is_valid(1) && !use_pos_x_wall) { x_cands.push_back(-HALF_WIDTH + perp_dist_1 - offset_1.first); }
        if (is_valid(2) && use_pos_x_wall)  { x_cands.push_back(HALF_WIDTH - perp_dist_2 - offset_2.first); }
    }
    else if (std::fabs(norm_heading - 180.0) <= TOLERANCE) {
        double angle_off_rad = lemlib::degToRad(norm_heading - 180.0);
        double heading_rad = lemlib::degToRad(norm_heading);
 
        double perp_dist_0 = sensors[0].dist_in * std::cos(angle_off_rad);
        double perp_dist_3 = sensors[3].dist_in * std::cos(angle_off_rad);
        double perp_dist_1 = sensors[1].dist_in * std::cos(angle_off_rad);
        double perp_dist_2 = sensors[2].dist_in * std::cos(angle_off_rad);
 
        auto offset_0 = get_global_offsets(sensors[0].cfg, heading_rad);
        auto offset_1 = get_global_offsets(sensors[1].cfg, heading_rad);
        auto offset_2 = get_global_offsets(sensors[2].cfg, heading_rad);
        auto offset_3 = get_global_offsets(sensors[3].cfg, heading_rad);
 
 
        if (is_valid(0) && !use_pos_y_wall) { y_cands.push_back(-HALF_HEIGHT + perp_dist_0 - offset_0.second); }
        if (is_valid(3) && use_pos_y_wall)  { y_cands.push_back(HALF_HEIGHT - perp_dist_3 - offset_3.second); }
        if (is_valid(1) && use_pos_x_wall)  { x_cands.push_back(HALF_WIDTH - perp_dist_1 - offset_1.first); }
        if (is_valid(2) && !use_pos_x_wall) { x_cands.push_back(-HALF_WIDTH + perp_dist_2 - offset_2.first); }
    }
 
    else if (std::fabs(norm_heading - 90.0) <= TOLERANCE) {
        double angle_off_rad = lemlib::degToRad(norm_heading - 90.0);
        double heading_rad = lemlib::degToRad(norm_heading);
        
        double perp_dist_0 = sensors[0].dist_in * std::cos(angle_off_rad);
        double perp_dist_3 = sensors[3].dist_in * std::cos(angle_off_rad);
        double perp_dist_1 = sensors[1].dist_in * std::cos(angle_off_rad);
        double perp_dist_2 = sensors[2].dist_in * std::cos(angle_off_rad);
 
        auto offset_0 = get_global_offsets(sensors[0].cfg, heading_rad);
        auto offset_1 = get_global_offsets(sensors[1].cfg, heading_rad);
        auto offset_2 = get_global_offsets(sensors[2].cfg, heading_rad);
        auto offset_3 = get_global_offsets(sensors[3].cfg, heading_rad);
 
 
        if (is_valid(0) && use_pos_x_wall)  { x_cands.push_back(HALF_WIDTH - perp_dist_0 - offset_0.first); }
        if (is_valid(3) && !use_pos_x_wall) { x_cands.push_back(-HALF_WIDTH + perp_dist_3 - offset_3.first); }
        if (is_valid(1) && use_pos_y_wall)  { y_cands.push_back(HALF_HEIGHT - perp_dist_1 - offset_1.second); }
        if (is_valid(2) && !use_pos_y_wall) { y_cands.push_back(-HALF_HEIGHT + perp_dist_2 - offset_2.second); }
    }
 
    else if (std::fabs(norm_heading - 270.0) <= TOLERANCE) {
        double angle_off_rad = lemlib::degToRad(norm_heading - 270.0);
        double heading_rad = lemlib::degToRad(norm_heading);
 
        double perp_dist_0 = sensors[0].dist_in * std::cos(angle_off_rad);
        double perp_dist_3 = sensors[3].dist_in * std::cos(angle_off_rad);
        double perp_dist_1 = sensors[1].dist_in * std::cos(angle_off_rad);
        double perp_dist_2 = sensors[2].dist_in * std::cos(angle_off_rad);
        
        auto offset_0 = get_global_offsets(sensors[0].cfg, heading_rad);
        auto offset_1 = get_global_offsets(sensors[1].cfg, heading_rad);
        auto offset_2 = get_global_offsets(sensors[2].cfg, heading_rad);
        auto offset_3 = get_global_offsets(sensors[3].cfg, heading_rad);
 
        if (is_valid(0) && !use_pos_x_wall) { x_cands.push_back(-HALF_WIDTH + perp_dist_0 - offset_0.first); }
        if (is_valid(3) && use_pos_x_wall)  { x_cands.push_back(HALF_WIDTH - perp_dist_3 - offset_3.first); }
        if (is_valid(1) && !use_pos_y_wall) { y_cands.push_back(-HALF_HEIGHT + perp_dist_1 - offset_1.second); }
        if (is_valid(2) && use_pos_y_wall)  { y_cands.push_back(HALF_HEIGHT - perp_dist_2 - offset_2.second); }
    }
    
    if (!x_cands.empty()) {
    est_x = std::accumulate(x_cands.begin(), x_cands.end(), 0.0) / x_cands.size();
    using_odom_x = false;
    }
 
    if (!y_cands.empty()) {
        est_y = std::accumulate(y_cands.begin(), y_cands.end(), 0.0) / y_cands.size();
        using_odom_y = false;
    }
 
 
    distancePose pose;
    pose.x = est_x;
    pose.y = est_y;
    pose.using_odom_x = using_odom_x;
    pose.using_odom_y = using_odom_y;
    return pose;
}

References back_sensor_cfg, backDistance(), chassis(), SensorReadings::dist_mm, SensorConfig::forward_offset, front_sensor_cfg, frontDistance(), HALF_HEIGHT, HALF_WIDTH, left_sensor_cfg, leftDistance(), MAX_SENSOR_RANGE, MIN_SENSOR_RANGE, MM_TO_IN, right_sensor_cfg, rightDistance(), SensorConfig::strafe_offset, distancePose::using_odom_x, distancePose::using_odom_y, distancePose::x, and distancePose::y.

Referenced by distanceReset(), and distanceReset().

◆ distanceReset() [1/2]

distancePose distanceReset	(	bool	left_use,
		bool	right_use,
		bool	front_use,
		bool	back_use,
		bool	setPose )

Definition at line 225 of file distanceReset.cpp.

                                                                                                       {
    double heading_deg = chassis.getPose().theta; 
 
    const int invalid_dist_mm = 10000;
    const int invalid_confidence = 0; 
 
    // Direct readings based on boolean flags
    SensorReadings front_data = front_use
        ? SensorReadings{(double)frontDistance.get_distance(), frontDistance.get_object_size(), frontDistance.get_confidence()}
        : SensorReadings{invalid_dist_mm, 0, invalid_confidence};
    
    SensorReadings left_data = left_use
        ? SensorReadings{(double)leftDistance.get_distance(), leftDistance.get_object_size(), leftDistance.get_confidence()}
        : SensorReadings{invalid_dist_mm, 0, invalid_confidence};
 
    SensorReadings right_data = right_use
        ? SensorReadings{(double)rightDistance.get_distance(), rightDistance.get_object_size(), rightDistance.get_confidence()}
        : SensorReadings{invalid_dist_mm, 0, invalid_confidence};
 
    SensorReadings back_data = back_use
        ? SensorReadings{(double)backDistance.get_distance(), backDistance.get_object_size(), backDistance.get_confidence()}
        : SensorReadings{invalid_dist_mm, 0, invalid_confidence};
    
    distancePose pose = calculateGlobalPosition(front_data, left_data, right_data, back_data, heading_deg);
    
    if(setPose) {
        chassis.setPose(pose.x, pose.y, chassis.getPose().theta);
    }
 
    return pose;
}

References backDistance(), calculateGlobalPosition(), chassis(), frontDistance(), leftDistance(), rightDistance(), distancePose::x, and distancePose::y.

◆ distanceReset() [2/2]

distancePose distanceReset	(	bool	setPose = false,
		bool	filter = true,
		float	filter_range = 3.5,
		float	heading_tolerance = 40.0 )

Definition at line 191 of file distanceReset.cpp.

                                                                                                                       {
    double heading_deg = chassis.getPose().theta;
 
    const SensorReadings front_data = {(double)frontDistance.get_distance(), frontDistance.get_object_size(), frontDistance.get_confidence()};
    const SensorReadings left_data  = {(double)leftDistance.get_distance(),  leftDistance.get_object_size(),  leftDistance.get_confidence()};
    const SensorReadings right_data = {(double)rightDistance.get_distance(), rightDistance.get_object_size(), rightDistance.get_confidence()};
    const SensorReadings back_data  = {(double)backDistance.get_distance(),  backDistance.get_object_size(),  backDistance.get_confidence()};
 
    distancePose pose = calculateGlobalPosition(front_data, left_data, right_data, back_data, heading_deg, heading_tolerance);
    if(filter)
    {
        if(std::abs(pose.x - chassis.getPose().x) > filter_range)
        {
            pose.x = chassis.getPose().x;
            pose.using_odom_x = true;
            std::cout << "Filtered X" << std::endl;
        }
        if(std::abs(pose.y - chassis.getPose().y) > filter_range)
        {
            pose.y = chassis.getPose().y;
            pose.using_odom_y = true;
            std::cout << "Filtered Y" << std::endl;
        }
        
    }
    if(setPose) {
        chassis.setPose(pose.x, pose.y, chassis.getPose().theta);
        pros::delay(2);
    }
 
    std::cout << "Distance Reset Pose: " << pose.x << ", " << pose.y << ", using_odom_x: " << pose.using_odom_x << ", using_odom_y: " << pose.using_odom_y << std::endl;
    return pose;
}