/*************************************
 *     TIME:2022.1.24
 *     Development Team: Zhiyi Technology Co., Ltd.
 *     OBSTACLE AVOIDANCE CAR
 *     Program function: ultrasonic obstacle avoidance
 *     Obstacle avoidance distance setting: Avoidance(X) 
 in the loop() function call; adjust the X parameter in 
 the function to adjust the obstacle avoidance distance

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

#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 function variable definition
  float high_speed=0;  
  float direction_bit = 0 ; 

//Default speed. The value ranges from 1 to 255
 //int speed_A = 150;
 //int speed_B = 150;

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
};

 /**********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 left()  //The left function
{
  digitalWrite(MOTOAR1,LOW);
  digitalWrite(MOTOAR2,HIGH); 
  
  digitalWrite(MOTOBL1,HIGH);
  digitalWrite(MOTOBL2,LOW); 
  
  digitalWrite(LEFT_LED, HIGH);
  digitalWrite(RIGHT_LED, LOW);
 
  Serial.println("Left");
}

void right() //Turn right function
{

  digitalWrite(MOTOAR1,HIGH);
  digitalWrite(MOTOAR2,LOW); 
  
  digitalWrite(MOTOBL1,LOW);
  digitalWrite(MOTOBL2,HIGH); 

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

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

  digitalWrite(LEFT_LED, LOW);
  digitalWrite(RIGHT_LED,LOW);
  Serial.println("Stop");
 }
 
 //initialize 
 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
}

/*
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;  //Return range value
}

void loop()
{ 
  avoidance(15);//Execute following function
}

//Obstacle avoidance function
void avoidance(int set_dis)  
{
  int distance;
  distance = GetDistance();  
  //The obtained ultrasonic data values are assigned to the distance
  //Serial.println(direction_bit);
  if(distance>=35)
  {
    forward();  
  }

  if((distance <= set_dis)&&(direction_bit<=5))//Turn left 4 times
  {
    stop();
    delay(80);
    back();
    delay(500);
    stop();
    delay(80);
    left();
    delay(500);
    direction_bit ++;
  }
  else if((distance <= set_dis)&&(direction_bit>=5))//Right turn 5 times
  {
    stop();
    delay(80);
    back();
    delay(500);
    stop();
    delay(80);
    right();
    delay(500);
    direction_bit ++;
    if(direction_bit>9)
    {
      direction_bit = 0;
    }
  }
}
