El Toro Grande: Self-Driving Car Using Machine Learning

ETG is an autonomous RC car that utilizes a RPi 3 and Arduino to localize itself in the environment and avoid colliding into other bots.

Jul 26, 2018

•

17455 views

•

18 respects

self driving car

autonomous car

machine-learning

Devices & Components

Arduino Uno Rev3

PCA9685

Ultrasonic Sensor - HC-SR04 (Generic)

Raspberry Pi 3 Model B

SparkFun IMU Breakout - MPU-9250

Hardware & Tools

3D Printer (generic)

Project description

Code

Arduino Sensor Polling

arduino

1#include <Wire.h>
2#include <TimerOne.h>
3#define tpLEFT 3
4#define   epLEFT 2
5#define tpBACK 6
6#define epBACK 5
7#define tpRIGHT 9
8#define   epRIGHT 8
9#define tpFRONT 12
10#define epFRONT 11
11
12void I2Cread(uint8_t   Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
13{
14  Wire.beginTransmission(Address);
15   Wire.write(Register);
16  Wire.endTransmission();
17  Wire.requestFrom(Address,   Nbytes);
18  uint8_t index = 0;
19  while (Wire.available())
20    Data[index++]   = Wire.read();
21}
22void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t   Data)
23{
24  Wire.beginTransmission(Address);
25  Wire.write(Register);
26   Wire.write(Data);
27  Wire.endTransmission();
28}
29long int ti;
30volatile   bool intFlag = false;
31void setup()
32{
33  delay(100);
34  pinMode(LED_BUILTIN,   OUTPUT);
35  Wire.begin();
36  Serial.begin(115200);
37
38  I2CwriteByte(0x68,   29, 0x06);
39  I2CwriteByte(0x68, 26, 0x06);
40  I2CwriteByte(0x68, 27, 0x10);
41   I2CwriteByte(0x68, 28, 0x08);
42  I2CwriteByte(0x68, 0x37, 0x02);
43  I2CwriteByte(0x0C,   0x0A, 0x16);
44
45  pinMode(13, OUTPUT);
46  Timer1.initialize(10000);         
47   Timer1.attachInterrupt(callback); 
48
49  ti = millis();
50
51  pinMode(tpLEFT,   OUTPUT);
52  pinMode(epLEFT, INPUT);
53  pinMode(tpBACK, OUTPUT);
54  pinMode(epBACK,   INPUT);
55  pinMode(tpRIGHT, OUTPUT);
56  pinMode(epRIGHT, INPUT);
57  pinMode(tpFRONT,   OUTPUT);
58  pinMode(epFRONT, INPUT);
59//  delay(100);
60}
61
62long int cpt   = 0;
63
64void callback()
65{
66  intFlag = true;
67  digitalWrite(13, digitalRead(13)   ^ 1);
68}
69int count = 0;
70
71
72int ringRing;
73
74
75
76void loop()
77{
78//   while(millis()<10000)
79//  {
80//    count++;
81
82//  ringRing = ' ';
83//   if(Serial.available() > 0) {
84//    ringRing = Serial.read();
85//    Serial.print("RINGRING:   "+ringRing);
86//  }
87//  if(ringRing == 122) {
88//    readData();
89//  }
90
91readData();
92
93   
94
95//}
96//Serial.print("COUNT: ");
97//Serial.print(count);
98}
99
100void   readData()
101{
102  
103  delay(20);
104  int left = getDistance(tpLEFT, epLEFT);
105   int right = getDistance(tpRIGHT, epRIGHT);
106  int front = getDistance(tpFRONT,   epFRONT);
107  int back = getDistance(tpBACK, epBACK);
108  int threshold = 50;
109   Serial.print(" st ");
110  if (left < threshold && left != 0)
111  {
112    Serial.print("   left ");
113  }
114  if (right < threshold && right != 0)
115  {
116    Serial.print("   right ");
117  }
118  if (front < 10 && front != 0)
119  {
120    Serial.print("   front ");
121  }
122  if (back < threshold && back != 0)
123  {
124    Serial.print("   back ");
125  }
126  Serial.print("et ");
127
128  while (!intFlag);
129  intFlag   = false;
130  uint8_t Buf[14];
131  I2Cread(0x68, 0x3B, 14, Buf);
132  //acc
133   int16_t ax = -(Buf[0] << 8 | Buf[1]);
134  int16_t ay = -(Buf[2] << 8 | Buf[3]);
135   int16_t az = Buf[4] << 8 | Buf[5];
136  // gyr
137  int16_t gx = -(Buf[8] << 8   | Buf[9]);
138  int16_t gy = -(Buf[10] << 8 | Buf[11]);
139  int16_t gz = Buf[12]   << 8 | Buf[13];
140  // acc
141  Serial.print(ax, DEC);
142    Serial.print(" ");
143   Serial.print(ay, DEC);
144      Serial.print(" ");
145  Serial.print(az, DEC);
146       Serial.print(" ea ");
147
148  // gyr
149  Serial.print(gx, DEC);
150      Serial.print("   ");
151
152  Serial.print(gy, DEC);
153      Serial.print(" ");
154
155  Serial.print(gz,   DEC);
156      Serial.print(" END");
157
158  uint8_t ST1;
159  do
160  {
161     I2Cread(0x0C, 0x02, 1, &ST1);
162  }
163  while (!(ST1 & 0x01));
164  Serial.println();
165}
166int   getDistance(int trigPin, int echoPin)
167{
168  long duration, distance;
169  digitalWrite(trigPin,   HIGH); 
170  delayMicroseconds(10);
171  digitalWrite(trigPin, LOW);
172
173  duration   = pulseIn(echoPin, HIGH, 20000); 
174  if (duration == 0) 
175  {
176    pinMode(echoPin,   OUTPUT); 
177    digitalWrite(echoPin, LOW);
178    delayMicroseconds(200);
179     pinMode(echoPin, INPUT); 
180  }
181  distance = (duration / 2) / 29.1; 
182   return distance; 
183}
184
185
186

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

Arduino Sensor Polling

arduino

1#include <Wire.h>
2#include <TimerOne.h>
3#define tpLEFT 3
4#define  epLEFT 2
5#define tpBACK 6
6#define epBACK 5
7#define tpRIGHT 9
8#define  epRIGHT 8
9#define tpFRONT 12
10#define epFRONT 11
11
12void I2Cread(uint8_t  Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
13{
14  Wire.beginTransmission(Address);
15  Wire.write(Register);
16  Wire.endTransmission();
17  Wire.requestFrom(Address,  Nbytes);
18  uint8_t index = 0;
19  while (Wire.available())
20    Data[index++]  = Wire.read();
21}
22void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t  Data)
23{
24  Wire.beginTransmission(Address);
25  Wire.write(Register);
26  Wire.write(Data);
27  Wire.endTransmission();
28}
29long int ti;
30volatile  bool intFlag = false;
31void setup()
32{
33  delay(100);
34  pinMode(LED_BUILTIN,  OUTPUT);
35  Wire.begin();
36  Serial.begin(115200);
37
38  I2CwriteByte(0x68,  29, 0x06);
39  I2CwriteByte(0x68, 26, 0x06);
40  I2CwriteByte(0x68, 27, 0x10);
41  I2CwriteByte(0x68, 28, 0x08);
42  I2CwriteByte(0x68, 0x37, 0x02);
43  I2CwriteByte(0x0C,  0x0A, 0x16);
44
45  pinMode(13, OUTPUT);
46  Timer1.initialize(10000);         
47  Timer1.attachInterrupt(callback); 
48
49  ti = millis();
50
51  pinMode(tpLEFT,  OUTPUT);
52  pinMode(epLEFT, INPUT);
53  pinMode(tpBACK, OUTPUT);
54  pinMode(epBACK,  INPUT);
55  pinMode(tpRIGHT, OUTPUT);
56  pinMode(epRIGHT, INPUT);
57  pinMode(tpFRONT,  OUTPUT);
58  pinMode(epFRONT, INPUT);
59//  delay(100);
60}
61
62long int cpt  = 0;
63
64void callback()
65{
66  intFlag = true;
67  digitalWrite(13, digitalRead(13)  ^ 1);
68}
69int count = 0;
70
71
72int ringRing;
73
74
75
76void loop()
77{
78//  while(millis()<10000)
79//  {
80//    count++;
81
82//  ringRing = ' ';
83//  if(Serial.available() > 0) {
84//    ringRing = Serial.read();
85//    Serial.print("RINGRING:  "+ringRing);
86//  }
87//  if(ringRing == 122) {
88//    readData();
89//  }
90
91readData();
92
93  
94
95//}
96//Serial.print("COUNT: ");
97//Serial.print(count);
98}
99
100void  readData()
101{
102  
103  delay(20);
104  int left = getDistance(tpLEFT, epLEFT);
105  int right = getDistance(tpRIGHT, epRIGHT);
106  int front = getDistance(tpFRONT,  epFRONT);
107  int back = getDistance(tpBACK, epBACK);
108  int threshold = 50;
109  Serial.print(" st ");
110  if (left < threshold && left != 0)
111  {
112    Serial.print("  left ");
113  }
114  if (right < threshold && right != 0)
115  {
116    Serial.print("  right ");
117  }
118  if (front < 10 && front != 0)
119  {
120    Serial.print("  front ");
121  }
122  if (back < threshold && back != 0)
123  {
124    Serial.print("  back ");
125  }
126  Serial.print("et ");
127
128  while (!intFlag);
129  intFlag  = false;
130  uint8_t Buf[14];
131  I2Cread(0x68, 0x3B, 14, Buf);
132  //acc
133  int16_t ax = -(Buf[0] << 8 | Buf[1]);
134  int16_t ay = -(Buf[2] << 8 | Buf[3]);
135  int16_t az = Buf[4] << 8 | Buf[5];
136  // gyr
137  int16_t gx = -(Buf[8] << 8  | Buf[9]);
138  int16_t gy = -(Buf[10] << 8 | Buf[11]);
139  int16_t gz = Buf[12]  << 8 | Buf[13];
140  // acc
141  Serial.print(ax, DEC);
142    Serial.print(" ");
143  Serial.print(ay, DEC);
144      Serial.print(" ");
145  Serial.print(az, DEC);
146      Serial.print(" ea ");
147
148  // gyr
149  Serial.print(gx, DEC);
150      Serial.print("  ");
151
152  Serial.print(gy, DEC);
153      Serial.print(" ");
154
155  Serial.print(gz,  DEC);
156      Serial.print(" END");
157
158  uint8_t ST1;
159  do
160  {
161    I2Cread(0x0C, 0x02, 1, &ST1);
162  }
163  while (!(ST1 & 0x01));
164  Serial.println();
165}
166int  getDistance(int trigPin, int echoPin)
167{
168  long duration, distance;
169  digitalWrite(trigPin,  HIGH); 
170  delayMicroseconds(10);
171  digitalWrite(trigPin, LOW);
172
173  duration  = pulseIn(echoPin, HIGH, 20000); 
174  if (duration == 0) 
175  {
176    pinMode(echoPin,  OUTPUT); 
177    digitalWrite(echoPin, LOW);
178    delayMicroseconds(200);
179    pinMode(echoPin, INPUT); 
180  }
181  distance = (duration / 2) / 29.1; 
182  return distance; 
183}
184
185
186

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

Downloadable files

Sensor Network