Single Sensor Test Run! Say that one fast 10 times hehe..

The test was successful to my surprise, I was actually expecting at least one crash but nope! It behaved as I wanted, drive forward, break when detecting an obstacle and reverse until obstacle is gone, then break and drive forward again. Like the auto-breaking system on modern cars(except for the reversing part, that would be bad).

Sensor test

Not much else to say, was far less eventful than expected. So the code, demo video and example file download is below.

The Code

/**
 * Self drivning RC-car code, Sensors: front
 * Author: Henrik Norberg
 * E-mail: henrik@dbhn.se
 */

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

/**
 * Create variables to be used to run motor A
 */
int motorAPin_A = 8; //Arduino digital 8 is connected to HG7881's A-1A terminal
int motorAPin_B = 9; //Arduino digital 9 is connected to HG7881's A-1B terminal

int motorBPin_A = 10; //Arduino digital 8 is connected to HG7881's B-1A terminal
int motorBPin_B = 11; //Arduino digital 9 is connected to HG7881's B-1B terminal

// Ultrasonic Front
const int Trig0_pin = 12;
const int Echo0_pin = 13;
long duration0;

boolean isAFrontWallThere = false;
int i = 150; 
int motorState = 255;         // current state of the button

void setup(){
   lcd.begin (16,2); //  <<----- My LCD was 16x2

  // Switch on the backlight
  lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
  lcd.setBacklight(HIGH);
  lcd.home (); // go home

  lcd.print("Test Run"); 
  /**
   * When program starts set Arduino pinmode for 8 and 9 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:
}

void loop() {
  checkFront();
    if (isAFrontWallThere == false) {

      //now we switch direction to "Y" by setting 5V to 
      //direction pin A-1A
      if (motorState == 0) {
        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){
          for(i;i<=180; i++){
            analogWrite(motorAPin_B, invertOurValue( i ));
            delay(40);
          }
        }
        analogWrite(motorAPin_B, invertOurValue( i ));
      }
    }
    else {
      if (motorState == 255) {
        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){
          for(i; i<=180; i++){        
            analogWrite(motorAPin_B, i);
            delay(40);
          }
        }
         analogWrite(motorAPin_B, i);
      }
    }
}

int invertOurValue(int input) {
  return 255 - input;
}

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)) {
    isAFrontWallThere = false;
  }
  else {
    isAFrontWallThere = true;
  }
}

The Future

Next step is to hook up the side sensors and make it avoid and drive past obstacles! Let the crashing begin!