All the front sensors are hooked up and the code is uploaded to the Arduino, will it work or will it catch fire? Only one way to find out!
No fire, but I have to admit that in early testing of the motor board there was some smoke from bad wiring.., but found some bugs in the code like the steering not turning left. Also when testing this code it’s very good to have the motor board shut off in the beginning so you can focus on the most important parts, the obstacle avoidance.
Also I have rearranged the wiring on the Arduino board to make it nicer and easier to see what IO-ports that are free.. Not many is the answer if you was wondering.
The idea
The behaviour in this code is the following:
- No obstacle in front of car – drive forward
- Obstacle in front of car:
- Both left and right is free – drive left until front obstacle is passed, go back to 2
- Left is free but right isn’t – drive left until front obstacle is passed or obstacle to left is found, then go back to 2
- Left isn’t free but right is free – drive right until front obstacle is passed or obstacle to right is found, then go back to 2
- Left and right have obstacle – reverse until front obstacle is lost
The turn left if both sides are free is just set for now, will randomise this in the future.
After you have checked so the right behaviour is displayed on the LCD I suggest switching on the engines to see if they behave correctly. You will probably have issues with them running in the wrong direction but en easy fix. I also had some trouble with getting the breaking to kick in but after some changes to the logic part of the code this was fixed.
I have noticed that the sensors have a much smaller detection angle than I initially thought and if you get to close to the sensors they wont detect anything. This might create some problems in the future..
Below is the code, a demo video and a download link for the example code.
The Code
/**
* Self drivning RC-car code, Sensors: front, left front, right front
* Author: Henrik Norberg
* E-mail: henrik@dbhn.se
*/
/** LCD Setup **/
#include <Wire.h>
#include <LCD.h>
#include <LiquidCrystal_I2C.h>
#define I2C_ADDR 0x38 // <<----- Add your address here. Find it from I2C Scanner
#define BACKLIGHT_PIN 3
#define En_pin 2
#define Rw_pin 1
#define Rs_pin 0
#define D4_pin 4
#define D5_pin 5
#define D6_pin 6
#define D7_pin 7
LiquidCrystal_I2C lcd(I2C_ADDR,En_pin,Rw_pin,Rs_pin,D4_pin,D5_pin,D6_pin,D7_pin);
/** Engines **/
//Drive Engine
int motorAPin_A = 10; //Arduino digital 8 is connected to HG7881's A-1A terminal
int motorAPin_B = 11; //Arduino digital 9 is connected to HG7881's A-1B terminal
//Steering Engine
int motorBPin_A = 12; //Arduino digital 8 is connected to HG7881's B-1A terminal
int motorBPin_B = 13; //Arduino digital 9 is connected to HG7881's B-1B terminal
/** Ultrasonic sensors **/
// Ultrasonic Front
const int Trig0_pin = 1;
const int Echo0_pin = 2;
long duration0;
// Ultrasonic Front Left
const int Trig1_pin = 3; // pin for triggering pulse INPUT
const int Echo1_pin = 4; // pin for recieving echo OUPUT
long duration1; // how long it takes for the sound to rebound
// Ultrasonic Front Right
const int Trig2_pin = 5;
const int Echo2_pin = 6;
long duration2;
/** Misc settings **/
// Wall sensing or not (true or false for now)
boolean isFrontWall = false;
boolean isFrontLeftWall = false;
boolean isFrontRightWall = false;
//Starting values
int i = 150; // Initial engine speed
int motorState = 255; // Drive motor direction
void setup(){
lcd.begin (16,2); // <<----- My LCD was 16x2
// Switch on the backlight and print headline
lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
lcd.setBacklight(HIGH);
lcd.home (); // go home
lcd.print("Engine Test");
/**
* When program starts set Arduino pinmode for 10 to 13 digital to be OUTPUT
* so we can use analogWrite to output values from 0 to 255 (0-5V) (PWM)
*/
pinMode(motorAPin_A, OUTPUT); //direction
pinMode(motorAPin_B, OUTPUT); //speed
pinMode(motorBPin_A, OUTPUT); //direction
pinMode(motorBPin_B, OUTPUT); //speed
// ultrasonic Front
pinMode(Trig0_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo0_pin, INPUT); // initialize the echo_pin pin as an input:
// ultrasonic Front Left
pinMode(Trig1_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo1_pin, INPUT); // initialize the echo_pin pin as an input:
// ultrasonic Front Right
pinMode(Trig2_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo2_pin, INPUT); // initialize the echo_pin pin as an input:
}
void loop() {
checkFront();
if (isFrontWall == false) {
turnReset();
// Break if the car wasn't driving forward, saves on the engine and gears
if (motorState != 255) {
lcd.setCursor (0,1);
lcd.print("Drive - Stop ");
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, LOW);
delay(1000);
analogWrite(motorAPin_A, 255); //Drive forward
motorState = 255;
i = 150;
}
else {
analogWrite(motorAPin_A, 255); //Drive forward
motorState = 255;
lcd.setCursor (0,1);
lcd.print("Drive - Forward");
if (i << 180){ // Speed up if not at max speed
for(i;i<=180; i++){
analogWrite(motorAPin_B, invertOurValue( i ));
delay(40);
}
}
analogWrite(motorAPin_B, invertOurValue( i ));
}
}
else {
checkFrontLeft();
checkFrontRight();
if (isFrontRightWall == false && isFrontLeftWall == false) {
//Sides are free, turn left
analogWrite(motorBPin_B, 255 );
// Break if the car was driving forward or reversing
if (motorState != 1) {
lcd.setCursor (0,1);
lcd.print("Drive - Stop ");
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, LOW);
delay(1000);
i = 150;
analogWrite(motorAPin_A, 255); //Drive forward
motorState = 1;
}
else {
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 0); //Turn left
motorState = 1;
lcd.setCursor (0,1);
lcd.print("Drive - Left ");
if (i << 180){ // Speed up if not at max speed
for(i; i<=180; i++){
analogWrite(motorAPin_B, i);
delay(40);
}
}
analogWrite(motorAPin_B, i);
delay(500);
}
}
else {
if (isFrontRightWall == true && isFrontLeftWall == true){
// Break if the car wasn't reversing, saves on the engine and gears
turnReset();
if (motorState != 0) {
lcd.setCursor (0,1);
lcd.print("Drive - Stop ");
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, LOW);
delay(1000);
i = 150;
analogWrite(motorAPin_A, 0); //Drive backward
motorState = 0;
}
else {
analogWrite(motorAPin_A, 0); //Drive backward
motorState = 0;
lcd.setCursor (0,1);
lcd.print("Drive - Reverse");
if (i << 180){ // Speed up if not at max speed
for(i; i<=180; i++){
analogWrite(motorAPin_B, i);
delay(40);
}
}
analogWrite(motorAPin_B, i);
delay(500);
}
}
else {
if (isFrontRightWall == true && isFrontLeftWall == false) {
//Left is free, Right is blocked. Turn Left and go forward
analogWrite(motorBPin_B, 255 );
// Break if the car was driving forward or reversing
if (motorState != 1) {
lcd.setCursor (0,1);
lcd.print("Drive - Stop ");
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, LOW);
delay(1000);
i = 150;
analogWrite(motorAPin_A, 255); //Drive forward
motorState = 1;
}
else {
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 0); //Turn left
motorState = 1;
lcd.setCursor (0,1);
lcd.print("Drive - Left ");
if (i << 180){ // Speed up if not at max speed
for(i; i<=180; i++){
analogWrite(motorAPin_B, i);
delay(40);
}
}
analogWrite(motorAPin_B, i);
delay(500);
}
}
else
if (isFrontRightWall == false && isFrontLeftWall == true) {
//Right is free, Left is blocked. Turn Right and go forward
analogWrite(motorBPin_B, 0 );
// Break if the car was driving forward or reversing
if (motorState != 2) {
lcd.setCursor (0,1);
lcd.print("Drive - Stop ");
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, LOW);
delay(1000);
i = 150;
analogWrite(motorAPin_A, 255); //Drive forward
motorState = 2;
}
else {
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 255); //Turn Right
motorState = 2;
lcd.setCursor (0,1);
lcd.print("Drive - Right ");
if (i << 180){ // Speed up if not at max speed
for(i; i<=180; i++){
analogWrite(motorAPin_B, i);
delay(40);
}
}
analogWrite(motorAPin_B, i);
delay(500);
}
}
}
}
}
}
//Invert speed value
int invertOurValue(int input) {
return 255 - input;
}
//Checking for obstacle in front of car
void checkFront() {
digitalWrite(Trig0_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig0_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig0_pin, LOW);
duration0 = pulseIn(Echo0_pin,10);
if ((duration0 > 4000 || duration0 == 0)) {
isFrontWall = false;
}
else {
isFrontWall = true;
}
}
void checkFrontLeft() {
digitalWrite(Trig1_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig1_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig1_pin, LOW);
duration1 = pulseIn(Echo1_pin,10);
if ((duration1 > 4000 || duration1 == 0)) {
isFrontLeftWall = false;
}
else {
isFrontLeftWall = true;
}
}
void checkFrontRight () {
digitalWrite(Trig2_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig2_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig2_pin, LOW);
duration2 = pulseIn(Echo2_pin,10);
if ((duration2 > 4000 || duration2 == 0)) {
isFrontRightWall = false;
}
else {
isFrontRightWall = true;
}
}
void turnReset() {
analogWrite(motorBPin_A, LOW );
analogWrite(motorBPin_B, LOW );
}
The Future
Next step is to set it free and see what happens.
