sketch_nov01a.ino

#include <NewPing.h>
#include <AFMotor.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <HMC5883L.h>


#define TRIGGER_PIN 14
#define ECHO_PIN 15
#define MAX_DISTANCE 200

AF_DCMotor Motor1(1);
AF_DCMotor Motor2(2);

NewPing DistanceSensor(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
unsigned int averageDist = MAX_DISTANCE;

unsigned long commandTime;
unsigned long waveTime;

enum lowState {STOP, FOR, BACK, LEFT, RIGHT};
lowState state = STOP;
bool col = false;
bool ingo = false;

void setup() {
  Serial.begin(9600);
  commandTime = millis();  
  waveTime = millis();
}
void stopCar(){
  state = STOP;
  Motor1.setSpeed(0);
  Motor2.setSpeed(0);
  Motor1.run(BRAKE);
  Motor2.run(BRAKE);
  
}
void forwardCar() {
  state = FOR;
  Motor1.setSpeed(255);
  Motor2.setSpeed(255);
  Motor1.run(FORWARD);
  Motor2.run(FORWARD);
  ingo = true;
  
}
void backCar() {
  state = BACK;
  Motor1.setSpeed(255);
  Motor2.setSpeed(255);
  Motor1.run(BACKWARD);
  Motor2.run(BACKWARD);
  
}
void turnLeft() {
  state = LEFT;
  Motor1.setSpeed(255);
  Motor2.setSpeed(255);
  Motor1.run(BACKWARD);
  Motor2.run(FORWARD);
}
void turnRight() {
  state = RIGHT;
  Motor1.setSpeed(255);
  Motor2.setSpeed(255);
  Motor1.run(FORWARD);
  Motor2.run(BACKWARD);
}
void checkCollision() {
    unsigned long now = millis();
    if(now - waveTime > 1000) {
        waveTime = now;
        unsigned int cm = DistanceSensor.ping_cm();
        averageDist = averageDist*0.5+cm*0.5;
        Serial.print("Distance: ");
        Serial.println(averageDist);
        Serial.println(cm);
        if(averageDist < 40) {
           //in moving then try to avoid collision
            if(!col && ingo) {
                //stopCar();
                col = true;
                unsigned int r = random(2);
                if(r == 0) {
                  turnLeft();
                } else {
                  turnRight();
                }
                ingo = false;
            }
        } else {
            if(col) {
              col = false;
              stopCar();
            }
        }
    }
}
//app button transfer char data
void handleCommand() {
  unsigned long now = millis();
  if(now-commandTime > 50) {
    commandTime = now;
    if(Serial.available() > 0) {
      char ch = Serial.read();
      if(ch == 'f') {
        forwardCar();
      }else if(ch == 'b') {
        backCar();
      }else if(ch == 'l') {
        turnLeft();
      }else if(ch == 'r') {
        turnRight();
      } else if(ch == 's') {
        stopCar();
      }
    } 
  }
}
void avoidCollision() {
  if(col) {
    
  }
}
void loop() {
    checkCollision();
    handleCommand();
    avoidCollision();
}