El Toro Grande: Self-Driving Car Using Machine Learning

#include <Wire.h>
#include <TimerOne.h>
#define tpLEFT 3
#define epLEFT 2
#define tpBACK 6
#define epBACK 5
#define tpRIGHT 9
#define epRIGHT 8
#define tpFRONT 12
#define epFRONT 11

void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission();
  Wire.requestFrom(Address, Nbytes);
  uint8_t index = 0;
  while (Wire.available())
    Data[index++] = Wire.read();
}
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}
long int ti;
volatile bool intFlag = false;
void setup()
{
  delay(100);
  pinMode(LED_BUILTIN, OUTPUT);
  Wire.begin();
  Serial.begin(115200);

  I2CwriteByte(0x68, 29, 0x06);
  I2CwriteByte(0x68, 26, 0x06);
  I2CwriteByte(0x68, 27, 0x10);
  I2CwriteByte(0x68, 28, 0x08);
  I2CwriteByte(0x68, 0x37, 0x02);
  I2CwriteByte(0x0C, 0x0A, 0x16);

  pinMode(13, OUTPUT);
  Timer1.initialize(10000);         
  Timer1.attachInterrupt(callback); 

  ti = millis();

  pinMode(tpLEFT, OUTPUT);
  pinMode(epLEFT, INPUT);
  pinMode(tpBACK, OUTPUT);
  pinMode(epBACK, INPUT);
  pinMode(tpRIGHT, OUTPUT);
  pinMode(epRIGHT, INPUT);
  pinMode(tpFRONT, OUTPUT);
  pinMode(epFRONT, INPUT);
//  delay(100);
}

long int cpt = 0;

void callback()
{
  intFlag = true;
  digitalWrite(13, digitalRead(13) ^ 1);
}
int count = 0;


int ringRing;



void loop()
{
//  while(millis()<10000)
//  {
//    count++;

//  ringRing = ' ';
//  if(Serial.available() > 0) {
//    ringRing = Serial.read();
//    Serial.print("RINGRING: "+ringRing);
//  }
//  if(ringRing == 122) {
//    readData();
//  }

readData();

  

//}
//Serial.print("COUNT: ");
//Serial.print(count);
}

void readData()
{
  
  delay(20);
  int left = getDistance(tpLEFT, epLEFT);
  int right = getDistance(tpRIGHT, epRIGHT);
  int front = getDistance(tpFRONT, epFRONT);
  int back = getDistance(tpBACK, epBACK);
  int threshold = 50;
  Serial.print(" st ");
  if (left < threshold && left != 0)
  {
    Serial.print(" left ");
  }
  if (right < threshold && right != 0)
  {
    Serial.print(" right ");
  }
  if (front < 10 && front != 0)
  {
    Serial.print(" front ");
  }
  if (back < threshold && back != 0)
  {
    Serial.print(" back ");
  }
  Serial.print("et ");

  while (!intFlag);
  intFlag = false;
  uint8_t Buf[14];
  I2Cread(0x68, 0x3B, 14, Buf);
  //acc
  int16_t ax = -(Buf[0] << 8 | Buf[1]);
  int16_t ay = -(Buf[2] << 8 | Buf[3]);
  int16_t az = Buf[4] << 8 | Buf[5];
  // gyr
  int16_t gx = -(Buf[8] << 8 | Buf[9]);
  int16_t gy = -(Buf[10] << 8 | Buf[11]);
  int16_t gz = Buf[12] << 8 | Buf[13];
  // acc
  Serial.print(ax, DEC);
    Serial.print(" ");
  Serial.print(ay, DEC);
      Serial.print(" ");
  Serial.print(az, DEC);
      Serial.print(" ea ");

  // gyr
  Serial.print(gx, DEC);
      Serial.print(" ");

  Serial.print(gy, DEC);
      Serial.print(" ");

  Serial.print(gz, DEC);
      Serial.print(" END");

  uint8_t ST1;
  do
  {
    I2Cread(0x0C, 0x02, 1, &ST1);
  }
  while (!(ST1 & 0x01));
  Serial.println();
}
int getDistance(int trigPin, int echoPin)
{
  long duration, distance;
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, 20000); 
  if (duration == 0) 
  {
    pinMode(echoPin, OUTPUT); 
    digitalWrite(echoPin, LOW);
    delayMicroseconds(200);
    pinMode(echoPin, INPUT); 
  }
  distance = (duration / 2) / 29.1; 
  return distance; 
}