Skip to content

Instantly share code, notes, and snippets.

@liyonghelpme
Created November 2, 2013 00:18
Show Gist options
  • Save liyonghelpme/7273951 to your computer and use it in GitHub Desktop.
Save liyonghelpme/7273951 to your computer and use it in GitHub Desktop.
#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();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment