Skip to content

Instantly share code, notes, and snippets.

@liyonghelpme
Created January 2, 2014 05:05
Show Gist options
  • Save liyonghelpme/8215292 to your computer and use it in GitHub Desktop.
Save liyonghelpme/8215292 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);
HMC5883L compass;
unsigned int averageDist = MAX_DISTANCE;
float averageDir = 0;
unsigned long commandTime;
unsigned long waveTime;
unsigned long dirTime;
unsigned long adjustTime;
enum lowState {STOP, FOR, BACK, LEFT, RIGHT};
lowState state = STOP;
bool col = false;
bool ingo = false;
bool inChangeDir = false;
bool inCheckDir = false;
int error;
int cmdDir = 0;
bool readCmd = false;
void setup() {
Serial.begin(9600);
commandTime = millis();
waveTime = millis();
dirTime = millis();
compass = HMC5883L();
error = compass.SetScale(1.3);
error = compass.SetMeasurementMode(Measurement_Continuous);
}
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 turnLeftSlowly(int s) {
state = LEFT;
Motor1.setSpeed(s);
Motor2.setSpeed(s);
Motor1.run(BACKWARD);
Motor2.run(FORWARD);
}
void turnRightSlowly(int s) {
state = RIGHT;
Motor1.setSpeed(s);
Motor2.setSpeed(s);
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();
}
}
}
}
int curDir = 0;
bool setDir = false;
void adjustDirection() {
}
//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();
//调整角度中 不接受外部命令
//但是如果调整时间过长则 不再调整
Serial.print("Receive ");
Serial.println(ch);
if(!setDir) {
if(ch == 'f') {
forwardCar();
}else if(ch == 'b') {
backCar();
}else if(ch == 'l') {
turnLeft();
}else if(ch == 'r') {
turnRight();
} else if(ch == 's') {
stopCar();
}
}
{
if(readCmd) {
if(ch == '\n') {
curDir = -cmdDir;
setDir = true;
readCmd = fal
} else {
cmdDir = cmdDir*10+(ch-'0');
}
Serial.print("cmdDir:");
Serial.println(cmdDir);
}if(ch == 's') {
setDir = false;
stopCar();
}else if(ch == 'N') {
readCmd = true;
cmdDir = 0;
Serial.println("Set Direction");
//curDir = 0;
//setDir = true;
} else if(ch == 'S') {
curDir = 180;
setDir = true;
} else if(ch == 'W') {
curDir = 90;
setDir = true;
} else if(ch == 'E') {
curDir = 270;
setDir = true;
}
//进入方向调整状态
if(setDir) {
//start to adjust Direction
adjustTime = millis();
unsigned int r = random(2);
inChangeDir = true;
inCheckDir = false;
turnLeftSlowly(255);
}
}
}
}
}
float clamp(float d){
if(d > 180)
d -= 360;
if (d < -180)
d += 360;
return d;
}
float getDirection() {
MagnetometerScaled scaled = compass.ReadScaledAxis();
float heading = atan2(scaled.XAxis, scaled.ZAxis);
if(heading < 0) {
heading += 2*PI;
}
if(heading > 2*PI) {
heading -= 2*PI;
}
//
heading = heading*180/PI;
Serial.print("Direction: ");
Serial.println(heading);
return heading;
}
void checkDirection() {
if(setDir) {
unsigned long now = millis();
if(inChangeDir) {
if(now - adjustTime > 100) {
stopCar();
inChangeDir = false;
inCheckDir = true;
adjustTime = now;
}
}else if(inCheckDir) {
//
if(now - adjustTime > 1000) {
MagnetometerScaled scaled = compass.ReadScaledAxis();
float heading = atan2(scaled.XAxis, scaled.ZAxis);
if(heading < 0) {
heading += 2*PI;
}
if(heading > 2*PI) {
heading -= 2*PI;
}
//
heading = heading*180/PI;
Serial.print("Direction: ");
Serial.println(heading);
float dd = clamp(heading-curDir);
Serial.print("diff");
Serial.println(dd);
//
if(dd >= -10 && dd <= 10) {
setDir = false;
stopCar();
Serial.println("\n");
Serial.println("DIROK\n");
//
} else {
//< abs(90 difference)
if(dd < 0 && dd > -90) {
turnRight();
} else if(dd > 0 && dd < 90) {
turnLeft();
} else {
turnLeft();
}
adjustTime = now;
inCheckDir = false;
inChangeDir = true;
}
}
}
} else {
unsigned long now = millis();
if(now - dirTime > 5000) {
dirTime = now;
getDirection();
}
}
}
void loop() {
checkCollision();
checkDirection();
handleCommand();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment