robo.car.v01

  /***************************/
  /* simple robot car         */
  /***************************/
  
  // engine part from 
  // http://www.instructables.com/id/Control-DC-and-stepper-motors-with-L298N-Dual-Moto/?ALLSTEPS
  // sndor part from 
  // http://www.instructables.com/id/Simple-Arduino-and-HC-SR04-Example/?ALLSTEPS
  
  /*
   HC-SR04 Ping distance sensor]
   VCC to arduino 5v GND to arduino GND
   Echo to Arduino pin 13 Trig to Arduino pin 12
   */
  
  // Sensors
  #define trigPinA 13
  #define echoPinA 12
  #define trigPinB 3
  #define echoPinB 2
  
  // Engines
  // connect motor controller pins to Arduino digital pins
  // motor one
  const int enA = 10;
  const int in1 = 9;
  const int in2 = 8;
  // motor two
  const int enB = 5;
  const int in3 = 7;
  const int in4 = 6;
  
  const int min_dist = 25;
  const int slowdown_dist=40;
  const int speedA=240;
  const int speedB=240;
  
  boolean direction;
  
  long dist(short trig, short echo) {
    digitalWrite(trig, LOW);  // Added this line
    delayMicroseconds(2); // Added this line
    digitalWrite(trig, HIGH);
    delayMicroseconds(10); // Added this line
    digitalWrite(trig, LOW);
  
    const long duration = pulseIn(echo, HIGH);
    const long distance = (duration/2) / 29.1;
    return distance;
  }
  
  void set_direction(boolean set_engine_dir) {
    // turn on motor A
    if (set_engine_dir) {
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, HIGH);
      digitalWrite(in4, LOW);
    } else {
      digitalWrite(in2, HIGH);
      digitalWrite(in1, LOW);
      digitalWrite(in4, HIGH);
      digitalWrite(in3, LOW);
    }
  }
  
  
  void setup() {
    Serial.begin (9600);
    pinMode(trigPinA, OUTPUT);
    pinMode(echoPinA, INPUT);
    pinMode(trigPinB, OUTPUT);
    pinMode(echoPinB, INPUT);
  
    // set all the motor control pins to outputs
    pinMode(enA, OUTPUT);
    pinMode(enB, OUTPUT);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);
    pinMode(in3, OUTPUT);
    pinMode(in4, OUTPUT);
    
    direction=false;
    set_direction(direction);
    analogWrite(enA, speedA);
    analogWrite(enB, speedB);
  }
  
  void loop() {
    //  long duration, distance;
    const long dA = dist(trigPinA,echoPinA);
    Serial.print("Side A distance:");
    Serial.print(dA);
    Serial.println(" cm");
  
    const long dB = dist(trigPinB,echoPinB);
    Serial.print("Side B distance:");
    Serial.print(dB);
    Serial.println(" cm");
  
    // slow to halt
    analogWrite(enA, speedA * 0.2);
    analogWrite(enB, speedB * 0.2);   
    delay(200);
   
    if ((direction && (dA<slowdown_dist)) || ((!direction) && (dB<slowdown_dist))) {
        analogWrite(enA, speedA * 0.7);
        analogWrite(enB, speedB * 0.7);
        Serial.println("Slow");    
    } else {    
      analogWrite(enA, speedA);
      analogWrite(enB, speedB);    
      Serial.println("Full Speed");    
    }
   
    if ((dA<min_dist) and (dB<min_dist)) {
        analogWrite(enA, 0);
        analogWrite(enB, 0);
        Serial.println("Stop");   
        delay(2000);
    }
  
   
    if ((direction && (dA<min_dist)) || ((!direction) && (dB<min_dist))) {
      direction = (!direction);
      set_direction(direction);
      delay(250);
    }
    
    
    if (direction) {
      Serial.println("Diraction A");
    }
    else {
     Serial.println("Diraction B");
    }
    delay(50); 
  }