Automatic following mode code analysis

Posted by XiaoR GEEKTechnology Co.,Ltd on

Automatic following mode code analysis

I. Function Principle

Installed 3 forward infrared obstacle avoidance module in the front of the car , each one sets at an angle of about 45 degrees. The middle infrared obstacle avoidance module is used to judge the distance between the controller in front, to prevent the distance too close to hit people. The left and right two infrared obstacle avoidance modules are actually the "reverse application" of "obstacle avoidance", equivalent to "obstacle finding", and are responsible for detecting the two legs of the controller. When the controller is in front of the right side of the car, the robot's sensor can determine that the signal on the right side is 0 and the signal on the left side is 1, so it executes the right-turn logic to correct the direction of the car. If the middle sensor is not triggered, it means that the distance between the robot and the operator is sufficient, and the straight line logic can be executed at this time.

II. The hardware connection

As shown in the figure, we choose the main driver board P21, P33, P37 three IO as the left, middle and right infrared detection module interface. Then adjust the sensitivity of the module so that the P21 trigger distance = P37 trigger distance ≈ 25cm; the middle IR module adjusts the distance of about 15cm to ensure that it will not hit the operator.

III.  The software writing

Open the lower program project with keil C V4, find the config.h configuration file, and add the IO pin configuration of the patrol line sensor in the configuration file:

  1. /* Follow mode pin configuration */
  2. sbit Input_Detect0=P3^7;          //Right probe
  3. sbit Input_Detect1=P3^3;         //Distance detection
  4. sbit Input_Detect2=P2^1;         //Left probe

copy code

Then open the motor.c file and add the code for the patrol line to the file. The first thing is that you should define a variable to save the robot's motion state so that the robot always does a direction correction action after it deviates from the course and before it returns to the correct direction.

uchar  Robots_Run_Status;

copy code

  1. Then add the logic function for the auto-follow function to achieve the logic of auto-following.
  2. //follow mode
  3. void Follow_Track(void)
  4. {
  5.                 switch(Robots_Run_Status)
  6.                {
  7.                       case 0x01:MOTOR_GO_RIGHT;     break;   
  8.                       case 0x02:MOTOR_GO_LEFT;     break;           
  9.                       case 0x03:MOTOR_GO_FORWARD;  break;           
  10.                       case 0x04:MOTOR_GO_STOP;  break;        
  11.                }
  13.            if(Input_Detect1 == 1)        //This is a major premise, if the middle sensor is not triggered, indicating that the robot is still 15cm away from the controller, which is a safe distance, otherwise it will directly execute the stop command
  14.                 {
  16.                          if((Input_Detect0 == 0)&& (Input_Detect2 == 0)) //Left and right sensors detect the controller's left and right feet at the same time
  17.                       {
  18.                                Robots_Run_Status=0x04;//Execute the stop command
  19.                       }
  21.                       if((Input_Detect0 == 0)&& (Input_Detect2 == 1))//The left sensor does not detect the controller, the right sensor detects it, indicating that the controller is on the right side of the robot, and the robot executes a right turn command to correct the car's head.                        {
  22.                               Robots_Run_Status=0x01;//turn right
  23.                       }
  25.                       if((Input_Detect0 == 1)&& (Input_Detect2 == 0))//The right sensor does not detect the controller, but the left sensor detects it, indicating that the controller is on the left side of the robot, and the robot executes the left turn command to correct the car's head                      
  26. {
  27.                               Robots_Run_Status=0x02;//turn left
  28.                       }
  30.                       if((Input_Detect0 == 1)&& (Input_Detect2 == 1))//Both left and right infrared obstacle avoidance sensors are not triggered, indicating that there is no obstacle in front of the car and can move forward                        
  31. {
  32.                               Robots_Run_Status=0x03;//go straight                        }
  33.                 }
  34.                 else
  35.                 {
  36.                            Robots_Run_Status=0x04;
  37.                 }
  38. }

Copy code

To call the line walk function which has been written.

In the main function main.c to find the while(1) dead loop and add the patrol function that you just wrotewhile(1)

  1.       {
  2. Follow_Track();//Execute line patrol
  3.       }

Copy code

This following demo has limitations, it is equivalent that it needs the people to "guide" the direction of the robot, if in the process of travel, the peoplewho is in front of the robot car run away suddenly .  Sorry, the robot will lose the target, straight ahead until the middle of the anti-collision infrared sensors are triggered so far


Leave a comment

Please note, comments must be approved before they are published