QDAC-firmware/app/main.c

/*********************************************************************************************************************
 * CH32V307VCT6 Opensourec Library <EFBFBD><EFBFBD><EFBFBD><EFBFBD>CH32V307VCT6 <EFBFBD><EFBFBD>Դ<EFBFBD>⣩<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹٷ<EFBFBD> SDK <EFBFBD>ӿڵĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD>
 * Copyright (c) 2022 SEEKFREE <EFBFBD><EFBFBD><EFBFBD>ɿƼ<EFBFBD>
 *
 * <EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>CH32V307VCT6 <EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 *
 * CH32V307VCT6 <EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ը<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᷢ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> GPL<EFBFBD><EFBFBD>GNU General Public License<EFBFBD><EFBFBD><EFBFBD><EFBFBD> GNUͨ<EFBFBD>ù<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 * <EFBFBD><EFBFBD> GPL <EFBFBD>ĵ<EFBFBD>3<EFBFBD>棨<EFBFBD><EFBFBD> GPL3.0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>κκ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>İ汾<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<EFBFBD><EFBFBD><EFBFBD>޸<EFBFBD><EFBFBD><EFBFBD>
 *
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κεı<EFBFBD>֤
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ի<EFBFBD><EFBFBD>ʺ<EFBFBD><EFBFBD>ض<EFBFBD><EFBFBD><EFBFBD>;<EFBFBD>ı<EFBFBD>֤
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>μ<EFBFBD> GPL
 *
 * <EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬʱ<EFBFBD>յ<EFBFBD>һ<EFBFBD><EFBFBD> GPL <EFBFBD>ĸ<EFBFBD><EFBFBD><EFBFBD>
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><https://www.gnu.org/licenses/>
 *
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD> GPL3.0 <EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤Э<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>İ汾
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӣ<EFBFBD>İ<EFBFBD><EFBFBD><EFBFBD> libraries/doc <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD> GPL3_permission_statement.txt <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> libraries <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD> LICENSE <EFBFBD>ļ<EFBFBD>
 * <EFBFBD><EFBFBD>ӭ<EFBFBD><EFBFBD>λʹ<EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD>޸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>뱣<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɿƼ<EFBFBD><EFBFBD>İ<EFBFBD>Ȩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 *
 * <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>          main
 * <EFBFBD><EFBFBD>˾<EFBFBD><EFBFBD><EFBFBD><EFBFBD>          <EFBFBD>ɶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɿƼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>޹<EFBFBD>˾
 * <EFBFBD>汾<EFBFBD><EFBFBD>Ϣ          <EFBFBD>鿴 libraries/doc <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> version <EFBFBD>ļ<EFBFBD> <EFBFBD>汾˵<EFBFBD><EFBFBD>
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>          MounRiver Studio V1.8.1
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ̨          CH32V307VCT6
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>          https://seekfree.taobao.com/
 *
 * <EFBFBD>޸ļ<EFBFBD>¼
 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>                                      <EFBFBD><EFBFBD><EFBFBD><EFBFBD>                             <EFBFBD><EFBFBD>ע
 * 2022-09-15        <EFBFBD><EFBFBD>W            first version
 ********************************************************************************************************************/
#include "zf_common_headfile.h"
#include "gl_headfile.h"

uint8 (*Img_Gray)[MT9V03X_W]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MT9V03X_W <20>е<EFBFBD> uint8 <20><><EFBFBD>͵Ķ<CDB5>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//uint8 *mt9v03x_image_copy[0];               // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8 <20><><EFBFBD>͵<EFBFBD>һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2];
sint32 pts_left_count, pts_right_count;
float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2];
sint32 pts_inv_l_count, pts_inv_r_count;
float32 pts_filter_l[PT_MAXLEN][2], pts_filter_r[PT_MAXLEN][2];
sint32 pts_filter_l_count, pts_filter_r_count;
float32 pts_resample_left[PT_MAXLEN][2], pts_resample_right[PT_MAXLEN][2];
sint32 pts_resample_left_count, pts_resample_right_count;
float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
sint32 mid_left_count, mid_right_count;



//<2F><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>+<2B>Ǽ<EFBFBD><C7BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
float angle_new_left[PT_MAXLEN];
float angle_new_right[PT_MAXLEN];
int angle_new_left_num, angle_new_right_num;
uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W];

// <20><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>
float angle_left[PT_MAXLEN];
float angle_right[PT_MAXLEN];
int angle_left_num, angle_right_num;

// L<>ǵ<EFBFBD>
int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
bool Lpt0_found, Lpt1_found;
int Lpt1[2],Lpt0[2];

int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id;
bool Lpt_in0_found, Lpt_in1_found;
int Lpt_in1[2], Lpt_in0[2];

// <20><>ֱ<EFBFBD><D6B1>
bool is_straight0, is_straight1;

// <20><><EFBFBD><EFBFBD>
bool is_turn0, is_turn1;

// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float rptsn[PT_MAXLEN][2];
int rptsn_num;
float aim_distance;

enum track_type_e track_type = TRACK_RIGHT;


int frame_count = 0;

void img_processing();
void get_corners();
void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value);
void threshold(uint8_t *img_data, uint8_t *output_data, int width, int height, int thres);

int main(void)
{
    clock_init(SYSTEM_CLOCK_120M); // <20><>ʼ<EFBFBD><CABC>оƬʱ<C6AC><CAB1> <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>Ϊ 120MHz
    debug_init();                  // <20><><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC>MPU ʱ<><CAB1> <20><><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD>
    ips114_init();
    mt9v03x_init();

    while (1) {
        //while (frame_count < 20) {
            // if (mt9v03x_finish_flag) {
            //     memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
            //     adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8);
            //     //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110);
            //     ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
            //     mt9v03x_finish_flag = 0;
            //     frame_count++;
            // }

        //}
        if (mt9v03x_finish_flag) {
            //ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0);
            memcpy(mt9v03x_image_copy[0], mt9v03x_image[0],(sizeof(mt9v03x_image_copy)/sizeof(uint8_t)));
            //Img_Gray = mt9v03x_image;
            //mt9v03x_image_copy[0] = Img_Gray[0];
            mt9v03x_finish_flag = 0;


            state_type = COMMON_STATE;
            img_processing();

            get_corners();

            aim_distance = COMMON_AIM;

            tracking();

            ElementJudge();

            ElementRun();

            MidLineTrack();

                      
        }

        
    }
}


/*
void threshold(uint8_t *img_data, uint8_t *output_data, int width, int height, int thres){
  for(int y=0; y<height; y++){
    for(int x=0; x<width; x++){
      output_data[x+y*width] = img_data[x+y*width]>thres ? 255 : 0;
    }
  }
}


void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value){
  int half_block = block / 2;
  for(int y=half_block; y<height-half_block; y++){
    for(int x=half_block; x<width-half_block; x++){
      // <20><><EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD>ֵ
      int thres = 0;
      for(int dy=-half_block; dy<=half_block; dy++){
        for(int dx=-half_block; dx<=half_block; dx++){
          thres += img_data[(x+dx)+(y+dy)*width];
        }
      }
      thres = thres / (block * block) - clip_value;
      // <20><><EFBFBD>ж<EFBFBD>ֵ<EFBFBD><D6B5>
      output_data[x+y*width] = img_data[x+y*width]>thres ? 255 : 0;
    }
  }
}*/