 
/***************Ultrasonic following************

******time:2022.1.10***************
Development team: Shenzhen Zhiyi Technology Co., LTD

***********************************/ 

#include <Wire.h>
#include <Adafruit_GFX.h>
#include "Adafruit_LEDBackpack.h"

Adafruit_8x16matrix matrix = Adafruit_8x16matrix();

//Define L9110S motor drive pins

#define MOTOAR1 6    //right-front wheel
#define MOTOAR2 7

#define MOTOBL1 9    //Left front wheel
#define MOTOBL2 8

//LED indicator definition
 #define LEFT_LED 17 //左LED
 #define RIGHT_LED 10 //右LED

 //Define ultrasonic pins
 #define  Trig 4   // Initializes the ultrasonic trigger pin
 #define  Echo 5   // Initializes the echo pin of the ultrasonic wave 
 
//Set the ultrasonic following value
 float Track_Dis = 10;       //Following distance value  int dist=0,time=0;  
 float distance = 0.0;
 
 void setup()
 {
  pinMode(MOTOAR1,OUTPUT);//before useing io pin, pin mode must be set first 
  pinMode(MOTOAR2,OUTPUT);
  pinMode(MOTOBL1,OUTPUT);
  pinMode(MOTOBL2,OUTPUT);
  pinMode(LEFT_LED ,OUTPUT);
  pinMode(RIGHT_LED ,OUTPUT);
  pinMode(Trig,OUTPUT); 
  pinMode(Echo,INPUT); 
  matrix.begin(0x70);  // pass in the address
  Serial.begin(9600);  // Begin serial monitor to note down the threshold clearance of table surface
}
static const uint8_t PROGMEM
smile_bmp[] =
{ B00111100,
  B01000010,
  B10100101,
  B10000001,
  B10100101,
  B10011001,
  B01000010,
  B00111100
},

frown_bmp[] =
{ B00111100,
  B01000010,
  B10100101,
  B10000001,
  B10011001,
  B10100101,
  B01000010,
  B00111100
};


/*
Function: obtain ultrasonic sensor ranging data
Parameters: Trig, Echo
Parameter Description: The sensor is connected to pin ports 4 and 5 on the mainboard
*/
float GetDistance()
{
    float distance;
  
  digitalWrite(Trig, LOW); 
  delayMicroseconds(2); 
  digitalWrite(Trig, HIGH); 
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);

  distance = pulseIn(Echo, HIGH) / 58.00;
     
  return distance;
}

void loop()
{ 

  track();//Execute follow function

}

//Ultrasonic following function
void track()
{
      //static int flag =  0;
    distance = GetDistance();  //Acquisition of ultrasonic distance

    if(distance < Track_Dis && distance < Track_Dis)
	{
     //If the ultrasonic detection distance is less than ultrasonic following value 10 and both meet the requirements, the car will be backed up
      back();
      
	}

    else if(distance > Track_Dis && distance < Track_Dis+15 )//If ultrasonic detection distance is greater than ultrasonic following value 10 and detection distance is less than 20; Then the trolley is carried out
     { 
       forward();  //advance
    }

    else         //Stop the trolley
    {
           stop(); //stop
      }
      
 }


/**********Motor running function*************/

void forward()  //Forward function
{ 
  
 digitalWrite(MOTOAR1,LOW);  
 digitalWrite(MOTOAR2,HIGH);   
  
 digitalWrite(MOTOBL1,LOW);  
 digitalWrite(MOTOBL2,HIGH); 
 digitalWrite(LEFT_LED, LOW);
 digitalWrite(RIGHT_LED, LOW);
 Serial.println("Forward");//Sends a message to the serial monitor
  matrix.clear();
  matrix.drawBitmap(0, 0, smile_bmp, 8, 8, LED_ON);//The left dot matrix shows a smile
  matrix.drawBitmap(0, 8, smile_bmp, 8, 8, LED_ON);//The dot bar on the right shows a smile
  matrix.writeDisplay();
  delay(100);
 }
 
 
void back()  //Backward function
{                      
  digitalWrite(MOTOAR1,HIGH);  
  digitalWrite(MOTOAR2,LOW); 
  
  digitalWrite(MOTOBL1,HIGH); 
  digitalWrite(MOTOBL2,LOW); 
  digitalWrite(LEFT_LED, HIGH);
  digitalWrite(RIGHT_LED, HIGH);
  Serial.println("Back");
  matrix.clear();
  matrix.drawBitmap(0, 0, frown_bmp, 8, 8, LED_ON); //The dot matrix on the left shows frowning
  matrix.drawBitmap(0, 8, frown_bmp, 8, 8, LED_ON); //The dot matrix on the right shows frowning
  matrix.writeDisplay();
  delay(100);
}

 void stop()//Stop function
 {
  digitalWrite(MOTOAR1,LOW); 
  digitalWrite(MOTOAR2,LOW);  
  
  digitalWrite(MOTOBL1,LOW); 
  digitalWrite(MOTOBL2,LOW);  

  digitalWrite(LEFT_LED, HIGH);
  digitalWrite(RIGHT_LED, HIGH);
  Serial.println("Stop");
 }


 
