Arduino Car code for friend homework :P

auto_car.ino

#include <SoftwareSerial.h>
#include <AFMotor.h>
AF_DCMotor motor_L(4);              // 모터드라이버 L293D  3: M3에 연결,  4: M4에 연결
AF_DCMotor motor_R(3);

// ????
int i;
int j;

//초음파센서 출력핀(trig)과 입력핀(echo), 변수, 함수 선언//

enum Direction {
  left, center, right
};

// Ultra Sensor0 pinID (Left)
int ultraLeftOut = A10;
int ultraLeftIn = A11;

// Ultra Sensor1 pinID (Middle)
int ultraCenterOut = A0;
int ultraCenterIn = A1;

// Ultra Sensor2 pinID (Right)
int ultraRightOut = A8;
int ultraRightIn = A9;


bool autoPilotEnabled = false;

// Get Distance between car and obstacle
long GetDistance(Direction dir);

// Actual Movement Wrapping
void Move(char dir);
// Auto Pilot Move
void AutoMove();

// Actual 'Move' Action
void Forward();
void Backward();
void Right();
void Left();

// STOP
void Stop();

void setup() {

  // Conneciton for bluetooth
  // Init Serial Connection with speed of...
  Serial1.begin(9600);

  // Set Pin In-Out Mode for Sonic Sensor
  pinMode(ultraLeftIn, INPUT);
  pinMode(ultraLeftOut, OUTPUT);

  pinMode(ultraCenterIn, INPUT);
  pinMode(ultraCenterOut, OUTPUT);

  pinMode(ultraRightIn, INPUT);  // EchoPin 입력
  pinMode(ultraRightOut, OUTPUT); // TrigPin 출력

  // Setup Motor
  motor_L.setSpeed(250);
  motor_L.run(RELEASE);

  motor_R.setSpeed(250);
  motor_R.run(RELEASE);
}

// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
unsigned long previousMillis = 0;        // will store last time LED was updated

// constants won't change:
const long interval = 200;           // interval at which to blink (milliseconds)

// Update Function
void loop() {

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis < interval) {
    return;
  }

  previousMillis = currentMillis;


  char carInput = 's';

  // GET INPUT
  // if serial(connection byte data port) is ready to do something
  if (Serial1.available())
  {
    // read first data from remote device
    carInput = Serial1.read();
  }


  // Toogle Auto Pilot Mode
  if (carInput == 'p')
  {
    autoPilotEnabled = !autoPilotEnabled;
  }

  if (autoPilotEnabled)
  {
    AutoMove();
  }
  else
  {
    // then do some auto things...
    Move(carInput);
  }

}

// Actual Move
void Move(char dir) {

  if (dir == 'g') { // 전진, go
    Forward();
  } else if (dir == 'b') { // 후진, back
    Backward();
  } else if (dir == 'l') { // 좌회전, Go Left
    Left();
  } else if (dir == 'r') { // 우회전, Go Right
    Right();
  } else if (dir == 'w') { // 제자리 회전, Right Rotation
    Right();
  } else if (dir == 'q') { // 제자리 회전, Left Rotation
    Left();
  } else if (dir == 's') { // Stop
    Stop();
  }
}

///////////장애물 확인 및 회피 방향 결정///////////
void AutoMove() {

  delay(10);
  long leftDistance = GetDistance(left);
  delay(10);
  long centerDistance = GetDistance(center);
  delay(10);
  long rightDistance = GetDistance(right);
  delay(10);

  if (centerDistance <= 50 || leftDistance <= 50 || rightDistance <= 50)
  {
    Backward();
    delay(100);

    if (leftDistance > rightDistance)
    {
      for (int i = 0 ; i < 8; i++)
      {
        Left();
        delay(100);
      }

    }
    else
    {
      for (int i = 0 ; i < 8; i++)
      {
        Right();
        delay(100);
      }

    }
  }
  else if (centerDistance >= 300 || (centerDistance > rightDistance && centerDistance > leftDistance))
  {
    Forward();
    delay(100);
    Left();
    delay(50);

    Forward();
    delay(100);
    Right();
    delay(50);
  }
  else if (leftDistance >= rightDistance)
  {
    Left();
    delay(200);
    Left();
    delay(100);
  }
  else
  {
    Right();
    delay(200);
    Right();
    delay(100);
  }

  Stop();
}



////////거리감지///////////
long GetDistance(Direction dir) {

  long duration = 0;
  long distance = 0;

  int trigPin = A0;
  int echoPin = A1;

  if (dir == left)
  {
    trigPin = A10;
    echoPin = A11;
  }
  else if (dir == center)
  {
    trigPin = A0;
    echoPin = A1;
  }
  else if (dir == right)
  {
    trigPin = A8;
    echoPin = A9;
  }


  digitalWrite(trigPin, LOW);
  delay(2);
  digitalWrite(trigPin, HIGH);  // trigPin에서 초음파 발생(echoPin도 HIGH)
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);    // echoPin 이 HIGH를 유지한 시간을 저장 한다.
  distance = ((float)(340 * duration) / 1000) / 2;
  return distance;
}


///////////방향 제어 함수////////////
void Forward() {
  motor_L.run(FORWARD);  motor_R.run(FORWARD);
  for (i = 0; i < 200; i = i + 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
  for (i = 200; i < 0; i = i - 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
}

void Backward() {
  motor_L.run(BACKWARD);  motor_R.run(BACKWARD);
  for (i = 0; i < 200; i = i + 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
  for (i = 200; i < 0; i = i - 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
}

void Right() {
  motor_L.run(FORWARD);  motor_R.run(BACKWARD);
  for (i = 0; i < 180; i = i + 20) {
    //j = i*1.3;     if(j >= 200) j = 200;
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
  for (i = 180; i < 0; i = i - 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
}

void Left() {
  motor_L.run(BACKWARD);  motor_R.run(FORWARD);
  for (i = 0; i < 180; i = i + 20) {
    //j = i*1.3;     if(j >= 200) j = 200;
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
  for (i = 180; i < 0; i = i - 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
}

void Stop() {
  motor_L.run(RELEASE);       motor_R.run(RELEASE);
  for (i = 200; i >= 0; i = i - 20) {
    motor_L.setSpeed(i);  motor_R.setSpeed(i);
    delay(2);
  }
}