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);
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();
}