Description:
For my project, I am building a mini-Tesla car that will feature both autopilot mode and manual mode. When the autopilot mode is enabled, the car will be able to move around and avoid all obstacles. Whereas for the manual mode, the user will be able to control the car through processing using the arrows/buttons.
Components:
I am using the Adafruit motor shield to control the wheels and add movement to the car. To use it correctly, I had to download a new library called “Adafruit Motor Shield V2 Library”, and a couple of other sub-libraries that come with it.
#include <Adafruit_MotorShield.h> Adafruit_MotorShield AFMS(0x61); Adafruit_DCMotor *rb = AFMS.getMotor(3); Adafruit_DCMotor *lf = AFMS.getMotor(2); Adafruit_DCMotor *lb = AFMS.getMotor(1); Adafruit_DCMotor *rf = AFMS.getMotor(4);
I am also using a Servo and an Ultrasonic Sensor to detect obstacles around the car.
Setup:
To begin with, in my setup function, I am setting the pin modes and the motors’ speed, then ensuring that all motors are stopped.
void setup(){
AFMS.begin();
Serial.begin(9600);
Serial.println("0,0");
myservo.attach(3);
pinMode(4,OUTPUT);
pinMode(5,INPUT);
rb->setSpeed(car_speed);
lb->setSpeed(car_speed);
rf->setSpeed(car_speed);
lf->setSpeed(car_speed);
delay(1500);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
Autopilot mode:
Measuring the distance:
At first, I am setting my servo to look ahead, then using an ultrasonic I am measuring the distance to the next obstacle (basically to check if there are objects ahead).
myservo.write(90); // calculate the distance ahead digitalWrite(4, LOW); delayMicroseconds(2); digitalWrite(4, HIGH); delayMicroseconds(5); digitalWrite(4, LOW); duration = pulseIn(5, HIGH); dist = duration/29/2;
Then, using a while() loop, I keep measuring the distance while moving forward, until the obstacle is within the minimum stopping distance.
while (dist >= min_dist){
rb->run(FORWARD);
lb->run(FORWARD);
rf->run(FORWARD);
lf->run(FORWARD);
digitalWrite(4, LOW);
delayMicroseconds(2);
digitalWrite(4, HIGH);
delayMicroseconds(5);
digitalWrite(4, LOW);
duration = pulseIn(5, HIGH);
dist = duration/29/2;
}
If the car is close enough to the obstacle, the wheels are stopped, the servo checks both the right and left directions, and using the ultrasonic, we get both distances.
int rdist = 0; int ldist = 0; // calculate right distance myservo.write(0); delay(1500); pinMode(4, OUTPUT); digitalWrite(4, LOW); delayMicroseconds(2); digitalWrite(4, HIGH); delayMicroseconds(5); digitalWrite(4, LOW); pinMode(5, INPUT); duration = pulseIn(5, HIGH); rdist = duration/29/2; // calculate left distance myservo.write(180); delay(1500); pinMode(4, OUTPUT); digitalWrite(4, LOW); delayMicroseconds(2); digitalWrite(4, HIGH); delayMicroseconds(5); digitalWrite(4, LOW); pinMode(5, INPUT); duration = pulseIn(5, HIGH); ldist = duration/29/2;
Here, we come across 4 scenarios:
1st scenario:
If both directions are blocked (distance is below the minimum stopping distance), the car does a U-turn. To do it, the car keeps turning to the right until it reaches 180 degrees (using Delay).
// if both directions are blocked, do a u-turn
if (rdist<= min_dist && ldist<= min_dist) {
rb->setSpeed(255);
lb->setSpeed(255);
rf->setSpeed(255);
lf->setSpeed(255);
rb->run(BACKWARD);
lb->run(FORWARD);
rf->run(BACKWARD);
lf->run(FORWARD);
delay(1400);
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
2nd scenario:
If both directions are free (distance is much higher than the minimum stopping distance), the car chooses the right direction. 🙂
// if both directions are free, choose right
else if (rdist>=20 && ldist>=20){
rb->setSpeed(255);
lb->setSpeed(255);
rf->setSpeed(255);
lf->setSpeed(255);
rb->run(BACKWARD);
lb->run(FORWARD);
rf->run(BACKWARD);
lf->run(FORWARD);
delay(700);
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
3rd scenario:
If the right direction is free, but the left is blocked, choose the right direction.
// if right way is free, choose right
else if (rdist>=ldist){
rb->setSpeed(255);
lb->setSpeed(255);
rf->setSpeed(255);
lf->setSpeed(255);
rb->run(BACKWARD);
lb->run(FORWARD);
rf->run(BACKWARD);
lf->run(FORWARD);
delay(700);
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
4th scenario:
If the left direction is free, but the right is blocked, choose the left direction.
// if left way is free, choose left
else if (rdist<ldist){
rb->setSpeed(255);
lb->setSpeed(255);
rf->setSpeed(255);
lf->setSpeed(255);
rb->run(FORWARD);
lb->run(BACKWARD);
rf->run(FORWARD);
lf->run(BACKWARD);
delay(700);
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
Manual mode:
When enabled, the user can control the car through Processing using either the arrows or the screen buttons. When an action is specified, Processing sends the data to Arduino.
To receive data from Processing, I am doing the following:
while (Serial.available()) {
if (Serial.read() == '\n') {
state = Serial.parseInt();
direc = Serial.parseInt();
}
}
To control the car:
else if (autopilot==false) {
// forward
if (direc == 0) {
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(FORWARD);
lb->run(FORWARD);
rf->run(FORWARD);
lf->run(FORWARD);
delay(200);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
// right
if (direc == 1) {
rb->setSpeed(255);
lb->setSpeed(255);
rf->setSpeed(255);
lf->setSpeed(255);
rb->run(BACKWARD);
lb->run(FORWARD);
rf->run(BACKWARD);
lf->run(FORWARD);
delay(100);
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
// backward
if (direc == 2) {
rb->run(BACKWARD);
lb->run(BACKWARD);
rf->run(BACKWARD);
lf->run(BACKWARD);
delay(200);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
// left
if (direc == 3) {
rb->setSpeed(255);
lb->setSpeed(255);
rf->setSpeed(255);
lf->setSpeed(255);
rb->run(FORWARD);
lb->run(BACKWARD);
rf->run(FORWARD);
lf->run(BACKWARD);
delay(100);
rb->setSpeed(60);
lb->setSpeed(60);
rf->setSpeed(60);
lf->setSpeed(60);
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
// stop
if (direc == 4) {
rb->run(RELEASE);
lb->run(RELEASE);
rf->run(RELEASE);
lf->run(RELEASE);
}
}
Processing:
import processing.serial.*;
Serial myPort;
PImage img;
void setup() {
size(960,720);
printArray(Serial.list());
String portname=Serial.list()[1];
println(portname);
myPort = new Serial(this,portname,9600);
myPort.clear();
myPort.bufferUntil('\n');
img = loadImage("bg.png");
}
void draw() {
background(#e9eaea);
image(img, width*1/2-135, height/2-125,270,250);
fill(#555555);
textSize(30);
text("Use the arrows to control the car.", width*1/2-200, height/2+ 145);
text("Click on S to switch to stop the car.", width*1/2-210, height/2+ 195);
text("Click on A to switch to autopilot mode.", width*1/2-225, height/2+ 245);
if(keyPressed) {
//if a key is pressed, do the following
if (key == 'm'){
myPort.write(0+","+9+"\n");
}
else if (keyCode == UP){
myPort.write(0+","+0+"\n");
}
else if (keyCode == DOWN){
myPort.write(0+","+2+"\n");
}
else if (keyCode == RIGHT){
myPort.write(0+","+1+"\n");
}
else if (keyCode == LEFT){
myPort.write(0+","+3+"\n");
}
else if (key == 's'){
myPort.write(0+","+4+"\n");
}
else if (key == 'a'){
myPort.write(1+","+0+"\n");
}
else {
myPort.write(4+","+9+"\n");
}
}
else {
myPort.write(4+","+9+"\n");
}
}
IM Showcase:
Here are a couple of videos from the IM showcase: