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

Adafruit_8x16matrix matrix = Adafruit_8x16matrix();


//Define L298N Dual H-Bridge Motor Controller Pins
//Define L9110S motor drive pins

#define MOTOAR1 6    //Right front wheel
#define MOTOAR2 7

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

/*Definition and wiring description of left and right obstacle avoidance sensors*/


/*From left to right, connect to D3,A1-A3 ,D10*/
#define LFSensor_0 11  //OLD D3
#define LFSensor_1 12
#define LFSensor_2 13
#define LFSensor_3 14
#define LFSensor_4 15  //OLD D10

//LED indicator definition
 #define LEFT_LED  17 //left LED
 #define RIGHT_LED 10 //right LED

//Default speed. The value ranges from 1 to 255
//  int speedA = 180;
//  int speedB = 180;

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 control*/
void forward()  //Forward
{ 
 digitalWrite(MOTOAR1,LOW);//
 digitalWrite(MOTOAR2,HIGH); 

 digitalWrite(MOTOBL1,LOW);  
 digitalWrite(MOTOBL2,HIGH); 
 
 digitalWrite(LEFT_LED, HIGH);
 digitalWrite(RIGHT_LED, HIGH);
 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 left()
{

  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()
{
 
  digitalWrite(MOTOAR1,HIGH); 
  digitalWrite(MOTOAR2,LOW);  
  
  digitalWrite(MOTOBL1,LOW); 
  digitalWrite(MOTOBL2,HIGH);  

  digitalWrite(LEFT_LED, LOW);
  digitalWrite(RIGHT_LED, HIGH);

  Serial.println("Right");
}


void back()
{                      

  digitalWrite(MOTOAR1,HIGH);//
  digitalWrite(MOTOAR2,LOW); //Right wheel level
  
  digitalWrite(MOTOBL1,HIGH); //Left wheel level
  digitalWrite(MOTOBL2,LOW); //
  digitalWrite(LEFT_LED, HIGH);
  digitalWrite(RIGHT_LED, HIGH);

  Serial.println("Back");
}

void stop()    //Stop
{
  digitalWrite(MOTOAR1,LOW); //Right wheel level
  digitalWrite(MOTOAR2,LOW); //
  
  digitalWrite(MOTOBL1,LOW); //Left wheel level
  digitalWrite(MOTOBL2,LOW); //

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


void setup()
{
   
 
  pinMode(MOTOAR1,OUTPUT);//before useing io pin, pin mode must be set first 
  pinMode(MOTOAR2,OUTPUT);
  pinMode(MOTOBL1,OUTPUT);
  pinMode(MOTOBL2,OUTPUT);
  stop();//stop move  
  pinMode(LEFT_LED ,OUTPUT);
  pinMode(RIGHT_LED ,OUTPUT);
  matrix.begin(0x70);  // pass in the address
  Serial.begin(9600);     //open serial and set the baudrate
  

}


boolean flag=false;

void loop()
{ 
 auto_tracking();
} //end of loop
 
char sensor[5];
 /*read sensor value string, 1 stands for black, 0 starnds for white, i.e 10000 means the first sensor(from left) detect black line, other 4 sensors detected white ground */
String read_sensor_values()
{  
  int sensorvalue=32;
  sensor[0]=digitalRead(LFSensor_0);
 
  sensor[1]=digitalRead(LFSensor_1);
 
  sensor[2]=digitalRead(LFSensor_2);
 
  sensor[3]=digitalRead(LFSensor_3);
 
  sensor[4]=digitalRead(LFSensor_4);
  sensorvalue +=sensor[0]*16+sensor[1]*8+sensor[2]*4+sensor[3]*2+sensor[4];
  
  String senstr= String(sensorvalue,BIN);
  senstr=senstr.substring(1,6);

  return senstr;
}

void auto_tracking()
{
 String sensorval= read_sensor_values();
  Serial.println(sensorval);
  //delay(1000);
  if ( sensorval=="10000" )
  {
    //The black line is in the left of the car, need  left turn 
    left();  //Turn left
    delay(15);      
  }
  if ((sensorval=="10100")  || (sensorval=="01000") || (sensorval=="01100") || (sensorval=="11100")  || (sensorval=="10010") || (sensorval=="11010"))
  {
    forward(); //Turn slight left
    delay(20);
    left();
    delay(10);
  }

  if ( sensorval=="00001" ){ //The black line is on the right of the car, need  right turn 
    right();  //Turn right
    delay(15);   // set_Motorspeed(FAST_SPEED,FAST_SPEED);
  }

  if ((sensorval=="00011" )|| (sensorval=="00010")  || (sensorval=="00101") || (sensorval=="00110") || (sensorval=="00111") || (sensorval=="01011") || (sensorval=="01111")   || (sensorval=="01011")  || (sensorval=="01001"))
  {
    forward();   //Turn slight right
    delay(20);
    right();
    delay(10);
  }

  if (sensorval=="00100"){
     forward();   //The car front touch stop line, need stop
    //set_Motorspeed(0,0);
  }

  if (sensorval=="11111"){
     stop();   //The car front touch stop line, need stop
    //set_Motorspeed(0,0);
  }
}
