Here it is, a video packed post about the latest tests of Brum Rev2 with the new sensor config and code. It worked better than the last but have it's problems still.
The idea
To remedy the problem with the narrow detection field I now use two sensors mounted at each corner in the front for front detection. For the side detection there is one sensor mounted on a servo turning it to check the sides, it also checks the sides at two angles to get the best coverage.
The behaviour is a bit complicated to explain in detail so check the code if your curious, but here is the simple version:
The front sensors checks back and forth, if one is obstructed the opposite side is checked for freedom of movement. The sensor checks at two positions and if both are free Brum drives in that direction. If any of the positions is obstructed Brum reverses till any change is detected(this can be the side or front gets free) and a new full sensor check is done. If both front sensors are obstructed both sides are checked and if both are free Brum drives to the left(this will be randomised in the future). If none is free Brum reverses.
This is the idea and works when doing dry runs, but as last time the real world testing is different. I performed the run in the garage but it’s around 6°C outside(and in side the garage) and the sensor problem was back.. Below is videos showing first the test run(worked kind of), a demo of the sensor mayhem and last a demo of how it should behave.
The strange noises is the motors struggling to run, it’s hard to get the speed right so trial and error on this one.
There is no obstacles in front of the car but as you can see it behaves like a loon! And yes I forgot to focus the camera..
The Code
/**
* Self drivning RC-car code
* Sensors: front left, front right, front sides
* Servos: front side checking
* 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);
/** Servo Setup **/
#include <Servo.h>
Servo frontServo; // create servo object to control a servo
int pos = 90; // Turn servo to face forward
/** Engines **/
//Drive Engine
int motorAPin_A = 10; //Arduino digital 10 is connected to HG7881's A-1A terminal
int motorAPin_B = 11; //Arduino digital 11 is connected to HG7881's A-1B terminal
//Steering Engine
int motorBPin_A = 12; //Arduino digital 12 is connected to HG7881's B-1A terminal
int motorBPin_B = 13; //Arduino digital 13 is connected to HG7881's B-1B terminal
/** Ultrasonic sensors **/
// Ultrasonic Front left
const int Trig0_pin = 1;
const int Echo0_pin = 2;
long duration0;
// Ultrasonic Front right
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 Side Checking
const int Trig2_pin = 5;
const int Echo2_pin = 6;
long duration2;
/** Misc settings **/
// Wall sensing or not (true or false for now)
boolean isFrontLeftWall = false; //Left front
boolean isFrontRightWall = false; //Right front
boolean isLeftWall = false; //Left wall
boolean isRightWall = false; //Right wall
//Starting values
int i = 120; // Initial engine speed
int iMax = 130; // Max engine speed forward/reverse
int iMaxSteer = 130; // Max engine speed steering
int frontDistance = 50; // Detection distance, front (cm)
int sideDistance = 40; // Detection distance, sides (cm)
int motorState = 255; // Engine direction
int randNumber; // Set random direction
//Temperature sensing for distance compensation
int temperaturePin = A5;
float temperature = 30;
long duration, distance;
long cm;
float getVoltage( int pin );
long microsecondsToCentimeters(long microseconds, long temp);
long distanceCheck (int duration);
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("Run Test - Rev2");
frontServo.attach(7); // attaches the servo on pin 7 to the servo object
/**
* 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 Left
pinMode(Trig0_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo0_pin, INPUT); // initialize the echo_pin pin as an input:
// ultrasonic Front Right
pinMode(Trig1_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo1_pin, INPUT); // initialize the echo_pin pin as an input:
// ultrasonic Front Sides
pinMode(Trig2_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo2_pin, INPUT); // initialize the echo_pin pin as an input:
}
void loop() {
checkFrontLeft();
checkFrontRight();
delay(500);
if (isFrontLeftWall == false && isFrontRightWall == false) {
turnReset();
analogWrite(motorAPin_A, 255); //Drive forward
motorState = 255;
lcd.setCursor (0,1);
lcd.print("Drive - Forward ");
while (isFrontLeftWall == false && isFrontRightWall == false) {
checkFrontLeft();
checkFrontRight();
i = iMax;
analogWrite(motorAPin_B, invertOurValue( i ));
delay(100);
}
}
analogWrite(motorAPin_B, invertOurValue( i ));
}
else if (isFrontLeftWall == true && isFrontRightWall == false) { //If left front sensor i obstructed, only check right for excape path
checkRight();
if (isRightWall == true){
//Right is obstructed, reverse
turnReset();
analogWrite(motorAPin_A, 0); //Drive backward
motorState = 0;
lcd.setCursor (0,1);
lcd.print("Drive - Reverse ");
while (isFrontLeftWall == true && isFrontRightWall == false && isRightWall == true) {
checkFrontLeft();
checkFrontRight();
checkRight();
i = iMax;
analogWrite(motorAPin_B, i);
delay(100);
}
}
else {
//Right is free. Turn Right and go forward
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 255); //Turn Right
analogWrite(motorBPin_B, 0 );
motorState = 2;
lcd.setCursor (0,1);
lcd.print("Drive - Right ");
while (isFrontLeftWall == true && isFrontRightWall == false && isRightWall == false) {
checkFrontLeft();
checkFrontRight();
checkRight();
i = iMaxSteer;
analogWrite(motorAPin_B, invertOurValue( i ));
delay(100);
}
}
}
else if (isFrontLeftWall == false && isFrontRightWall == true) { //If right front sensor i obstructed, only check left for excape path
checkLeft();
if (isLeftWall == true){
//Left is obstructed, reverse
turnReset();
analogWrite(motorAPin_A, 0); //Drive backward
motorState = 0;
lcd.setCursor (0,1);
lcd.print("Drive - Reverse ");
while (isFrontLeftWall == false && isFrontRightWall == true && isLeftWall == true) {
checkFrontLeft();
checkFrontRight();
checkLeft();
i = iMax;
analogWrite(motorAPin_B, i);
delay(100);
}
}
else {
//Left is free, Right is blocked. Turn Left and go forward
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 0); //Turn left
analogWrite(motorBPin_B, 255 );
motorState = 1;
lcd.setCursor (0,1);
lcd.print("Drive - Left ");
while (isFrontLeftWall == false && isFrontRightWall == true && isLeftWall == false) {
checkFrontLeft();
checkFrontRight();
checkLeft();
i = iMaxSteer;
analogWrite(motorAPin_B, invertOurValue( i ));
delay(100);
}
}
}
else if (isFrontLeftWall == true && isFrontRightWall == true) {
checkLeft();
checkRight();
if (isLeftWall == false && isRightWall == false) {
// randNumber = random(1, 2); //Random direction, 1 = Left and 2 = Right (Temp disabled)
randNumber = 1;
//Sides are free, turn left
analogWrite(motorAPin_A, 255); //Drive forward
if (randNumber == 1) {
analogWrite(motorBPin_A, 0); //Turn left
lcd.setCursor (0,1);
lcd.print("Drive - Left ");
}
else {
analogWrite(motorBPin_A, 255); //Turn Right
lcd.setCursor (0,1);
lcd.print("Drive - Right ");
}
analogWrite(motorBPin_B, 255 );
motorState = 1;
while (isFrontLeftWall == true && isFrontRightWall == true && isLeftWall == false) {
checkFrontLeft();
checkFrontRight();
checkLeft();
i = iMaxSteer;
analogWrite(motorAPin_B, invertOurValue( i ));
delay(100);
}
}
else if (isRightWall == true && isLeftWall == true){
turnReset();
analogWrite(motorAPin_A, 0); //Drive backward
motorState = 0;
lcd.setCursor (0,1);
lcd.print("Drive - Reverse ");
while (isFrontLeftWall == true && isFrontRightWall == true) {
checkFrontLeft();
checkFrontRight();
i = iMax;
analogWrite(motorAPin_B, i);
delay(100);
}
}
else if (isRightWall == true && isLeftWall == false) {
//Left is free, Right is blocked. Turn Left and go forward
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 0); //Turn left
analogWrite(motorBPin_B, 255 );
motorState = 1;
lcd.setCursor (0,1);
lcd.print("Drive - Left ");
while (isFrontLeftWall == false && isFrontRightWall == true && isLeftWall == false) {
checkFrontLeft();
checkFrontRight();
checkLeft();
i = iMaxSteer;
analogWrite(motorAPin_B, invertOurValue( i ));
delay(100);
}
}
else if (isRightWall == false && isLeftWall == true) {
//Right is free, Left is blocked. Turn Right and go forward
analogWrite(motorAPin_A, 255); //Drive forward
analogWrite(motorBPin_A, 255); //Turn Right
analogWrite(motorBPin_B, 0 );
motorState = 2;
lcd.setCursor (0,1);
lcd.print("Drive - Right ");
while (isFrontLeftWall == true && isFrontRightWall == false && isRightWall == false) {
checkFrontLeft();
checkFrontRight();
checkRight();
i = iMaxSteer;
analogWrite(motorAPin_B, invertOurValue( i ));
delay(100);
}
}
}
}
//Invert speed value
int invertOurValue(int input) {
return 255 - input;
}
//Checking for obstacle in front of car - Left
void checkFrontLeft() {
digitalWrite(Trig1_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig1_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig1_pin, LOW);
duration1 = pulseIn(Echo1_pin,HIGH);
distanceCheck(duration1); //Magic happens and returns a distance in cm
if ((distance > frontDistance || distance == 0)) {
isFrontLeftWall = false;
}
else {
isFrontLeftWall = true;
}
}
//Checking for obstacle in front of car - Right
void checkFrontRight() {
digitalWrite(Trig2_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig2_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig2_pin, LOW);
duration2 = pulseIn(Echo2_pin,HIGH);
distanceCheck(duration2); //Magic happens and returns a distance in cm
if ((distance > frontDistance || distance == 0)) {
isFrontRightWall = false;
}
else {
isFrontRightWall = true;
}
}
void checkLeft() {
frontServo.write(135); // Turn servo to first check position
delay(400); //Wait for servo to turn
digitalWrite(Trig0_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig0_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig0_pin, LOW);
duration0 = pulseIn(Echo0_pin,HIGH);
distanceCheck(duration0); //Magic happens and returns a distance in cm
if ((distance > sideDistance || distance == 0)) { //If no obstacle at first pos, turn and check next
frontServo.write(175); // Turn servo to second check position
delay(400); //Wait for servo to turn
digitalWrite(Trig0_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig0_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig0_pin, LOW);
duration0 = pulseIn(Echo0_pin,HIGH);
distanceCheck(duration0); //Magic happens and returns a distance in cm
if ((distance > sideDistance || distance == 0)) {
isLeftWall = false;
}
else {
isLeftWall = true;
}
}
else {
isLeftWall = true;
}
}
void checkRight () {
frontServo.write(45); // Turn servo to first check position
delay(400); //Wait for servo to turn
digitalWrite(Trig0_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig0_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig0_pin, LOW);
duration0 = pulseIn(Echo0_pin,HIGH);
distanceCheck(duration0); //Magic happens and returns a distance in cm
if ((distance > sideDistance || distance == 0)) {
frontServo.write(5); // Turn servo to second check position
delay(400); //Wait for servo to turn
digitalWrite(Trig0_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig0_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig0_pin, LOW);
duration0 = pulseIn(Echo0_pin,HIGH);
distanceCheck(duration0); //Magic happens and returns a distance in cm
if ((distance > sideDistance || distance == 0)) {
isRightWall = false;
}
else {
isRightWall = true;
}
}
else {
isRightWall = true;
}
}
void turnReset() {
analogWrite(motorBPin_A, LOW );
analogWrite(motorBPin_B, LOW );
frontServo.write(90); // Reset servo
}
long distanceCheck (long duration) {
temperature = (getVoltage(temperaturePin) - 0.5) * 100;
float spdSnd;
spdSnd = 331.4 + (0.606 * temperature) + 0.62;
distance = (duration / 2) * (spdSnd / 10000) + 2;
cm = microsecondsToCentimeters(duration, temperature);
return distance;
}
float getVoltage(int pin) {
return (analogRead(pin) * .004882814);
}
long microsecondsToCentimeters(long microseconds, long temp) {
return (microseconds * (331.3 + 0.606 * temp)) / 2;
}
The Future
I have tested it indoors also but was having problems then also. I think it gets stuck in loops or slow sensor detection(when the obstacle is closer than 2cm the sensors wont detect it and think nothing is in front..), witchcraft might also be in the works hehe.. I think the set up works as it is but the biggest problem is in the code so I have started work on Rev 3. For this I will be using a better library for pinging the sensors that says are much better than the default. Will have to re-wright a lot of code for this so might take some time before the next update.
