2022 Electric Competition Question C Car Tracking (Scheme 1 + Core Code)

Table of contents

foreword

For the 2022 electric competition question C car tracking, the team has made a total of two solutions:
      the first is mainly based onCamera (openmv) is the main, the rear car is tracked by identifying the QR code on the front car. This solution has more stable performance and better compatibility, and can achieve 1-4 questions.

For details, please refer to the description of the openmv official website:

Highlights:https://book.openmv.cc/image/apriltag.html

the second,Belongs to the team’s internal program, which will not be disclosed for the time being. This is simpler to implement and the material is cheap. However, the disadvantage is that the scalability is poor, and it is expected to only be able to do the third question.
     Let’s talk about the method of the first scheme, and attach the entire project at the bottom of the text, and download it if necessary (it is made with STM32, the competition question requires TI board, there is a basic download, do transplantation, and be cautious if there is no foundation. !)

1. The topic

2. Option 1

1. List of materials

When debugging, look at the PID parameters, which is convenient for debugging, but has no actual function, just debugging.

The vehicle in front can use the tracking module to track and walk, or openmv. It will be more complicated to use openmv, but the accuracy is high. The team uses openmv to track the line.
Motor drive, compared to L298N, TB6612 can adjust PID

2. Description

The time is relatively short. Here is a brief introduction to the idea. The car in front walks through the patrol line, the rear car scans the QR code at the rear of the car in front, and openmv feeds the image data to the microcontroller for tracking.

The video explains the use of QR code:
https://singtown.com/learn/49590/
First, initialize the camera to initialize the configuration, and then start to detect the QR code. In order to ensure the accurate judgment of the QR code, the initial configuration to prevent lens distortion should be added when initializing the camera, so that the edges of the image can be displayed smoothly. Then judge whether the content in the two-dimensional code is the parameter setting of the system, if so, execute its corresponding tracking function, if not, return to continue to detect the effective two-dimensional code. The tracking trolley can achieve 3 functions.

① Specify the path tracing function. According to the actual road conditions, the path control of the car can be realized by setting three parameters such as distance, turning angle and running speed;
② Guide path tracking function. The functions such as left turn, right turn, U-turn, forward and backward of the car can be realized independently with the content of the QR code. In actual operation, you can post the corresponding QR code at the intersection and turning point. When the car moves to the area, it will recognize the QR code posted there, and then execute the corresponding action according to the QR code, so as to realize the QR code to plan the car. motion path.
③ Real-time tracking function. When the content of the QR code identified by OpenMV is neither the content of the specified path nor the content of the QR code used by the path planning function, the function of tracking the QR code in real time will be executed.

After the QR code is detected, if the QR code is not in the center of the entire image, then the calculation of the center error needs to be performed. The steering of the steering gear head and the driving action of the trolley are closely related to the calculation of the center error. After obtaining the QR code target, the difference between X and Y that needs to be moved can be calculated by making a difference with the center point of the image. Then call the PID algorithm for X and Y respectively, and the result obtained is the rotation angle required by the corresponding steering gear and the steering angle of the car. In this paper, the digital position PID controller adopted by the PID controller makes the tracking more stable and can follow the QR code autonomously. In addition, when the car is running, the PID algorithm also provides a basis for the speed regulation of the car. When the QR code is in the area directly in front of the image, the trolley executes the forward command. If the QR code is closer to the camera, the imaging area of ​​the QR code will be larger. At this time, the trolley is too close to the QR code, and the speed of the trolley should be appropriately reduced. . When the car is farther away from the QR code, the image will be smaller, and the speed of the car should be increased appropriately.

  1. [Schematic]

3. Core code

#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "beep.h"
#include "timer.h"
#include "oled.h"
#include "MOTIR.h"
#include "apiltag.h"
#include "math.h"

u16 led0pwmval=0;
u16 led0pwmval_l=0;
u16 led0pwmval_r=0;
u8 car_z; 
u8 car_x;
u8 car_pwm;
u8 zd_flag;
u16  pwm_max;
u16  pwm_small;  

extern u8 time3;

extern u8 TIM8CH1_CAPTURE_STA;       //input capture status                          
extern u16 TIM8CH1_CAPTURE_VAL;  //input capture value  

extern float Tx_num;
extern float Ty_num;
extern float Tz_num;
extern u8 apiltag_id;

extern u8 Tx_fu;
 extern u8 Ty_fu;
 extern u8 Tz_fu;
 extern u8 hccmd_flag;
 extern u8 oled_flag;
 extern u8 openmv_usart1_flag;              // 
extern u8 openmv_flag      ;

float KP = 50;
float KI = 0;
float KD = 0;
float distance = 0;

float camera_distance=0;
float error = 0;
float last_error = 0;
float Output = 0;

float x_KP = 20;
float x_KI = 0;
float x_KD = 0;
float x_distance = 0;

float x_camera_distance=0;
float x_error = 0;
float x_last_error = 0;
float x_Output = 0;

float PID(float KP,float KI,float KD,float distance,float camera_distance)
{

    error = camera_distance - distance;
    Output = KP * (error) + KI * (error + last_error) + KD * (error - last_error);
    last_error = error;
    return Output;
}

float x_PID(float x_KP,float x_KI,float x_KD,float x_distance,float x_camera_distance)
{

    x_error = x_camera_distance - x_distance;
    x_Output = x_KP * (x_error) + x_KI * (x_error + x_last_error) + x_KD * (x_error - x_last_error);
    x_last_error = x_error;
    return x_Output;
}

 int main(void)
 {      
float qh_num=0;
float X_num=0;   
float mm;
u16 cm;
u32 temp=0;
u16 num_mold;

u8 id= 0 ;
u8 id_flag=0;
u8 SRF05_flag=0; 

u8 oled_hccmd=0;
u8 PWMzkb;        
vu16 HC_cmd;            //The variable prefix vu16 is to call the return value of the variable in the library function 
    MOTIR_Init();         //motor initialization 
    delay_init();             //delay function initialization      
    LED_Init();              //initialize the trigger IO of HY_SRF_04 Replace the original LED initialization 
    BEEP_Init();             //Initialize the buzzer port 
    delay_init();             //Initialize the delay function     



    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
     //Set NVIC interrupt group 2: 2-bit preemption priority, 2-bit response priority 
    uart4_init( 115200 );    //The initial baud rate of serial port 4 is 115200 
    uart3_init( 115200 );     //The initial baud rate of serial port 3 is 115200 
    LED_Init ();                  //Initialize the LED port      
    KEY_Init();           //Initialize the hardware interface connected to the button. Commenting out here will cause the car to not move 
    TIM1_PWM_Init( 899 , 3 );    //The main frequency of timer 1 is 36M, and the calculation is 36000000 /((899+1)*(3+1))=10KHZ 
  TIM3_Int_Init( 49 , 7199 );     //10Khz count frequency, counting to 500 is 500ms //Do interrupt count 
  TIM2_Int_Init( 99 , 7199 );     //10Khz Counting frequency, counting to 500 is 500ms //openmv interrupt reminder 
    TIM8_Cap_Init( 0XFFFF, 36 -1 );  // count at 1Mhz   

     OLED initialization settings
        OLED_Clear();      //OLED clears 
     OLED_Init();             //OLED initializes 
     OLED_ColorTurn( 0 ); //0 normal display, 1 reverse color display 
  OLED_DisplayTurn( 0 ); //0 normal display 1 screen flip display 
     //Recommended minimum The empty ratio is 30%. Since the steering code of this program changes the duty ratio of the two motors on both sides, the difference is 40%, and the minimum 20% can make the car move forward, and 20% will make the car completely move the wheels when turning, friction Sassafras The force is huge and affects the operation of the car, so the recommended duty cycle is 30%, and the calculation is 270/(899+1)=30%


    pwm_max= 720 ;      // used to limit the maximum duty cycle 
    pwm_small= 270 ;    // used to limit the minimum value of the duty cycle 
    led0pwmval=pwm_small;      // initialize the speed to the minimum value 
    while ( 1 )
    {
        id=apiltag_id;
        PWMzkb=led0pwmval/ 9 ;         //Define PWM duty cycle variable for OLED display 
        oled_exe();                 //Display Chinese function   
   OLED_ShowNum( 32 , 0 ,Tx_num, 2 , 16 );
        OLED_ShowNum( 80 , 0 ,PWMzkb, 2 , 16 );
        OLED_ShowString(100,0,"id",16);
        OLED_ShowNum(104,16,apiltag_id,1,16);
        OLED_ShowNum( 32 , 16 ,Ty_num, 2 , 16 );
        OLED_ShowString(52,16,"Ry",16);
    //OLED_ShowNum(80,16,Ry_num,2,16);
        OLED_ShowNum(32,32,Tz_num,2,16);
        OLED_ShowString(52,32,"flag",16);
        OLED_ShowNum(88,32,SRF05_flag,1,16);
        OLED_ShowNum(32,48,openmv_usart1_flag,3,16);  
        OLED_ShowString(0,48,"fps",16);

        OLED_ShowNum(88,48,oled_hccmd,2,16);
        OLED_ShowString(60,48,"cmd",16);
        OLED_Refresh();     //OLED refresh display

if(hccmd_flag==1)
{
        HC_cmd=hc_cmd();       //It's not normal here hc_cmd() always returns a value instead of once, variable, but always sent
oled_hccmd=HC_cmd;
}hccmd_flag=0;

        ///Key input here returns K, led0pwmval value

  //KEY_EXE();  

/       

  UART4_EXE();       //Bluetooth serial port driver function

 USART3_EXE();     //openmv data processing





/       


if(HC_cmd==1|HC_cmd==2)
{
    zd_flag=1;
}
else if (HC_cmd!=1&HC_cmd!=0&HC_cmd!=2)
zd_flag=0;

if(HC_cmd==2)
{

 printf ( "\r\nUltrasonic detection distance: %d " ,cm);
   printf ( "\r\nThe current speed is: %d " ,led0pwmval/ 9 );        //Keep the current moving speed 
    printf ( "\ r\nThe current Z coordinate is: %f " ,Tz_num);
     printf ( "\r\nThe current X coordinate is: %f " ,Tx_num);
     printf ( "\r\nThe output PWM: %d " ,led0pwmval );
     printf ( "\r\nOutput duty cycle: %d " ,led0pwmval/ 9 );
     if (qh_num< 0 ) 
         printf ( "\r\nPID: - %f.2 " ,qh_num);
     if ( qh_num>0)
        printf("\r\nPID:   %f.2 ",qh_num);

}
if (HC_cmd== 3 )     //before
{
car_z=1;
car_x=0;
}
    if (HC_cmd== 4 )     //return
    {
    car_z=2;
car_x=0;
    }
        car_x=1;

            if (HC_cmd== 6 )    //right 
                car_x= 2 ;
                 if (HC_cmd== 7 )     //brake
                {   
                car_z=3;
                    car_x=0;
                }
                if(HC_cmd==8)
                car_pwm=1;
                    if(HC_cmd==9)
       car_pwm=2;
                if(HC_cmd==10)
       led0pwmval=pwm_small;
                    if(HC_cmd==11)
        led0pwmval=pwm_max;
                    if(HC_cmd==12)
                    {       
                    id_flag= 1 ;
                     printf ( "\r\nFollow ID1 license plate id: %d " ,id_flag);
                    }   
                        if(HC_cmd==13)
          {       
                    id_flag= 2 ;
                     printf ( "\r\nFollow ID1 license plate id: %d " ,id_flag);
                    }   
                    if(HC_cmd==14)
                    {       
                    id_flag= 3 ;
                     printf ( "\r\nFollow ID1 license plate id: %d " ,id_flag);
                    }   
                    if(HC_cmd==15)
                    {       
                    id_flag= 4 ;
                     printf ( "\r\nFollow ID1 license plate id: %d " ,id_flag);
                    }   

                        HC_cmd=0;

            // command judgment

    if(id==id_flag)
            {
                    if(openmv_flag==1)
                    {
     if(zd_flag==1)
      {

    qh_num=PID(KP,KI,KD,30.5,Tz_num);
        if(qh_num<-pwm_max)
            qh_num=-pwm_max;
        if(qh_num>pwm_max)
            qh_num=pwm_max;

    X_num=x_PID(x_KP,x_KI,x_KD,2,Tx_num);
        if(X_num<-pwm_small)
            X_num=-pwm_small;
        if(X_num>pwm_small)
            X_num=pwm_small;

        if(qh_num>0)
        {
            num_pwm= (int)qh_num;


            MotorA1= 0 ;    // Left wheel forward 
            MotorA2= 1 ;
            MotorC1= 1 ;
            MotorC2=0;

            TIM_SetCompare1(TIM1,num_pwm+X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 here is the timer PWM output channel 1 
            TIM_SetCompare3(TIM1,num_pwm+X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 here is Timer PWM output channel 3
        }

        if(qh_num<0)
        {   
            num_pwm= (int)-qh_num;
            MotorA1= 1 ;    // left wheel back 
            MotorA2= 0 ;
            EngineC1= 0 ;
            MotorC2=1;

            TIM_SetCompare1(TIM1,num_pwm-X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 here is the timer PWM output channel 1 
            TIM_SetCompare3(TIM1,num_pwm-X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 here is Timer PWM output channel 3
        }

        if(qh_num>0)
        {
            num_pwm= (int)qh_num;
            MotorB1= 0 ;     //Right wheel forward 
            MotorB2= 1 ;
            MotorD1= 1 ;
            MotorD2=0;

            TIM_SetCompare2(TIM1,num_pwm-X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 Here is the timer PWM output channel 1 
            TIM_SetCompare4(TIM1,num_pwm-X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 Here is Timer PWM output channel 3
        }

        if(qh_num<0)
        {
            num_pwm= (int)-qh_num;
            MotorB1= 1 ;     //The right wheel goes backward 
            MotorB2= 0 ;
            MotorD1= 0 ;
            MotorD2=1;

            TIM_SetCompare2(TIM1,num_pwm+X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 here is the timer PWM output channel 1 
            TIM_SetCompare4(TIM1,num_pwm+X_num);                 //The occupancy ratio is equal to led0pwmval/arr+1 here is Timer PWM output channel 3
        }

       }
       }

            if(openmv_flag==0)
            {
                MotorA1= 0 ;   
              MotorA2= 0 ;
              EngineC1= 0 ;
              MotorC2=0;
                MotorB1= 0 ;   
              MotorB2=0;
              MotorD1= 0 ;
              MotorD2=0;
            }
        }
            else if(id!=id_flag)
            {
                MotorA1= 0 ;   
              MotorA2= 0 ;
              EngineC1= 0 ;
              MotorC2=0;
                MotorB1= 0 ;   
              MotorB2=0;
              MotorD1= 0 ;
              MotorD2=0;
            }
    /*
Motor drive function
    car_z controls forward and backward
    car_x controls left and right turns
    car_pwm controls acceleration and deceleration
    */
    /
if(zd_flag==0)
{
Motor_EXE(car_z,car_x,car_pwm,pwm_max,pwm_small);     
TIM_SetCompare1(TIM1,led0pwmval_l);              //The occupancy ratio is equal to led0pwmval/arr+1 Here is the timer PWM output channel 1 
TIM_SetCompare3(TIM1,led0pwmval_l);              //The occupancy ratio is equal to led0pwmval/arr+1 Here is the timer PWM output Channel 3

TIM_SetCompare2(TIM1,led0pwmval_r);              //The occupancy ratio is equal to led0pwmval/arr+1 Here is the timer PWM output channel 2 
TIM_SetCompare4(TIM1,led0pwmval_r);              //The occupancy ratio is equal to led0pwmval/arr+1 Here is the timer PWM output Channel 4
}

///             
    PCout(7)=1;
    delay_us(10);
    PCout(7)=0;


        if (TIM8CH1_CAPTURE_STA& 0X80 ) //A rising edge was successfully captured
        {
            temp=TIM8CH1_CAPTURE_STA&0X3F;
            temp*= 65536 ; //Total overflow time 
            temp+=TIM8CH1_CAPTURE_VAL; //Get the total high level time

            mm=temp* 0.34 / 4 ;            //The unit is mm, the ultrasonic ranging range is 2cm~450cm, and the highest precision unit is 3mm 
            cm=mm/ 10 ;

            if(mm>20&&cm<4500)
            {
        if(mm<150)
        { SRF05_flag= 1 ;
             if (time3== 5 )    //Wait for 500ms, still means no misjudgment
            {
    printf ( "\r\nWarning: The detected object is close to %d cm" ,cm);
        time3= 0 ;
            }
        }
        if(mm>150)
            SRF05_flag=0;
        }
            TIM8CH1_CAPTURE_STA= 0 ; //Start the next capture
        }



    }
}

4. Project acquisition

20 years of Guangdong Electric Competition Open Question: The car tracking done by this team just fits the 2022 Electric Competition title. All the information (completed projects + schematic diagrams, etc.) are concentrated here. Time is running out, it has not been sorted out, don’t download it if you mind .
STM32F103+openmv4+ encoder motor (basic can be transplanted to TI board)

Link: https://pan.baidu.com/s/1Hof4heUnRhtKbP4Xq-0n_g?pwd=FBMZ
Extraction code: FBMZ
– sharing from Baidu Netdisk Super Member V5

Leave a Comment

Your email address will not be published. Required fields are marked *