QDAC-firmware/app/gl_barrier.c

#include "zf_common_headfile.h"
#include "gl_headfile.h"

enum barrier_type_e barrier_type = BARRIER_NONE;
enum s_type_e s_type             = S_NONE;
enum shield_type_e shield_type   = SHIELD_NONE;
float (*mid_track_s)[2];
int32_t mid_track_count_s;
int temp_min_s;
int temp_max_s;
int barrier_begin_flag = 0;
int barrier_count      = 0;
float barrirer_time=500.f;
void CheckBarrier()
{
    if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) {
        barrier_type = BARRIER_LEFT_BEGIN;
    }
    if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) {
        barrier_type = BARRIER_RIGHT_BEGIN;
    }
}

void RunBarrier()
{
    if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
        track_type = TRACK_RIGHT;
        if (barrier_type == BARRIER_LEFT_BEGIN)
        {
            timer_clear(TIM_2);
            timer_start(TIM_2);
            barrier_type = BARRIER_LEFT_RUNNING;
        }
        if (barrier_type == BARRIER_LEFT_RUNNING)
        {
            if (timer_get(TIM_2) >= barrirer_time)
            {
                barrier_type = BARRIER_NONE;
                timer_stop(TIM_2);
                timer_clear(TIM_2);
            }
            else{
                barrier_type = BARRIER_LEFT_RUNNING;
            }
        }
        
        
        //     if (barrier_begin_flag == 0) {
        //         timer_clear(TIM_3);
        //         timer_start(TIM_3);
        //         barrier_begin_flag = 1;
        //     }
        // }
        // if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
        //     if (barrier_begin_flag == 1) {
        //         barrier_type = BARRIER_LEFT_BEGIN;
        //         time_barrier = timer_get(TIM_3);
        //         if (time_barrier >= 300 && time_barrier < 600) {
        //             barrier_type = BARRIER_LEFT_RUNNING;
        //         } else if (time_barrier >= 600) {
        //             barrier_type       = BARRIER_NONE;
        //             barrier_count = 1;
        //             barrier_begin_flag = 0;
        //             timer_stop(TIM_3);
        //             timer_clear(TIM_3);
        //         }
        //     }
        // }
        // if (pts_resample_left_count >= 5) {
        //     if (Lpt0_found) {
        //         Lpt0_found_count++;
        //     }
        //     if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
        //         Lpt0_found_count = 0;
        //         barrier_type     = BARRIER_LEFT_RUNNING;
        //         track_type       = TRACK_RIGHT;
        //         timer_clear(TIM_2);
        //         timer_start(TIM_2);
        //     } else if (barrier_type == BARRIER_LEFT_RUNNING) {
        //         track_type = TRACK_RIGHT;
        //         if (timer_get(TIM_2) >= 500) {
        //             barrier_type = BARRIER_NONE;
        //             track_type   = TRACK_RIGHT;
        //             timer_stop(TIM_2);
        //             timer_clear(TIM_2);
        //         }
        //     }
        // } else {
        //     track_type   = TRACK_RIGHT;
        //     barrier_type = BARRIER_NONE;
        // }

        // if (barrier_type == BARRIER_LEFT_RUNNING) {
        //     if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) {
        //         barrier_type = BARRIER_NONE;
        //     }
        // }

        // if (Lpt0_found) {
        //     Lpt0_found_count++;
        // }
        // if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
        //     Lpt0_found_count = 0;
        //     barrier_type     = BARRIER_LEFT_RUNNING;
        //     track_type       = TRACK_RIGHT;
        //     timer_clear(TIM_2);
        //     timer_start(TIM_2);
        // } else if (barrier_type == BARRIER_LEFT_RUNNING) {
        //     track_type   = TRACK_RIGHT;
        //     if (timer_get(TIM_2) >= 500) {
        //         barrier_type = BARRIER_NONE;
        //         track_type   = TRACK_RIGHT;
        //         timer_stop(TIM_2);
        //         timer_clear(TIM_2);
        //     }
        // }
    }

    if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
        track_type = TRACK_LEFT;
        if (barrier_type == BARRIER_RIGHT_BEGIN)
        {
            timer_clear(TIM_2);
            timer_start(TIM_2);
            barrier_type = BARRIER_RIGHT_RUNNING;
        }
        if (barrier_type == BARRIER_RIGHT_RUNNING)
        {
            if (timer_get(TIM_2) >= barrirer_time)
            {
                barrier_type = BARRIER_NONE;
                timer_stop(TIM_2);
                timer_clear(TIM_2);
            }
            else{
                barrier_type = BARRIER_RIGHT_RUNNING;
            }
        }
        //     if (barrier_begin_flag == 0) {
        //         timer_clear(TIM_3);
        //         timer_start(TIM_3);
        //         barrier_begin_flag = 1;
        //     }
        // }
        // if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
        //     if (barrier_begin_flag == 1) {
        //         barrier_type = BARRIER_RIGHT_BEGIN;
        //         time_barrier = timer_get(TIM_3);
        //         if (time_barrier >= 300 && time_barrier < 600) {
        //             barrier_type = BARRIER_RIGHT_RUNNING;
        //         } else if (time_barrier >= 600) {
        //             barrier_type       = BARRIER_NONE;
        //             barrier_count = 1;
        //             barrier_begin_flag = 0;
        //             timer_stop(TIM_3);
        //             timer_clear(TIM_3);
        //         }
        //     }
        // }
        // if (pts_resample_right_count >= 5) {
        //     if (Lpt1_found) {
        //         Lpt1_found_count++;
        //     }
        //     if (Lpt1_found_count >= 1 && barrier_type == BARRIER_RIGHT_BEGIN) {
        //         Lpt1_found_count = 0;
        //         barrier_type     = BARRIER_RIGHT_RUNNING;
        //         track_type       = TRACK_LEFT;
        //         timer_clear(TIM_2);
        //         timer_start(TIM_2);
        //     } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
        //         track_type = TRACK_LEFT;
        //         if (timer_get(TIM_2) >= 500) {
        //             timer_stop(TIM_2);
        //             timer_clear(TIM_2);
        //             barrier_type = BARRIER_NONE;
        //             track_type   = TRACK_LEFT;
        //         }
        //     }
        // } else {
        //     track_type   = TRACK_LEFT;
        //     barrier_type = BARRIER_NONE;
        // }

        // if (barrier_type == BARRIER_RIGHT_RUNNING) {
        //     if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) {
        //         barrier_type = BARRIER_NONE;
        //     }
        // }
    }
}

void Check_s()
{
    if (track_type == TRACK_LEFT) {
        mid_track_s       = mid_left;
        mid_track_count_s = mid_left_count;
    } else {
        mid_track_s       = mid_right;
        mid_track_count_s = mid_right_count;
    }

    int min_s = 1000;
    int max_s = 1;

    for (int i = 0; i < mid_track_count_s; i++) {
        if (mid_track_s[i][1] < min_s) {
            min_s      = mid_track_s[i][1];
            temp_min_s = min_s;
        }
        if (mid_track_s[i][1] > max_s) {
            max_s      = mid_track_s[i][1];
            temp_max_s = max_s;
        }
    }

    if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5) {
        s_type = S_BEGIN;
    }
}

void RunS()
{

    if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) {
        s_type = S_NONE;
    }
}

// void Check_shield(){
//     if (begin_flag == 1)
//     {
//         shield_type = SHIELD_BEGIN;
//         timer_clear(TIM_3);
//         timer_start(TIM_3);
//         begin_flag = 0;
//     }
//     if (timer_get(TIM_3) > 1000 && shield_type == SHIELD_BEGIN)
//     {
//         shield_type = SHIELD_NONE;
//         timer_stop(TIM_3);
//         timer_clear(TIM_3);
//     }

// }