Arduino Project: Brum – Part 4

07/10/2016.Henrik.0 Likes.0 Comments

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:

  1. No obstacle in front of car - drive forward
  2. 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.

/**
 * 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.

Downloads

BrumRunning_rev1.zip
Categories: Project Brum

All rights reserved © Design by HN