?>

Hard Copy World

HCW

Q&A

Home > Forum >

Q&A

질문 | 자율주행 프로젝트를 진행하고 있습니다.

페이지 정보

작성자 oopon 쪽지보내기 메일보내기 자기소개 아이디로 검색 전체게시물 작성일18-11-25 23:09 조회108회 댓글1건

본문

안녕하세요? 다름이 아니라 여쭤볼게 있어서 질문드립니다.
현재 자율주행 스마트카 프로젝트를 진행중입니다. 부품으로는 초음파센서, 블루투스 모듈, 전자나침반, OLED 등 입니다.  앱 인벤터로  앱을 만들어 블루투스와 연동하고 조종하는것은 성공하였습니다. 또 자율주행도 앱은 만들지 않았지만 물론 오픈소스를 참고 하였지만 단일로는 성공을 하였습니다. 한발 더 나아가서 해보고싶은것이 블루투스 무선 조종하는 앱에 자율주행모드 버튼을 추가하여 버튼을 누르게 되면 자율주행으로 동작하고 방향키를 누르게 되면  무선조종으로 동작을 하게 하고 싶은데요. 이것이 구현이 가능할까요? 아두이노가 처음이라서 잘 모르겠네요.. 혹시 구현이 가능한지 부터 여쭙고 싶습니다. 만약 구현이 가능하다면 방식이 어떻게 되는지 간략하게라도 설명해주시면 감사하겠습니다!!
아래는 만든 블루투스 무선 조종 코드입니다.
#include <SoftwareSerial.h>

SoftwareSerial btSerial(10, 11);  // RX, TX(UNO)

/*
* 1: 정방향
* 2: 좌회전
* 3: 우회전
* 4: 후진
* 0: 정지
*/
int direction = 0; //
int speed = 200; // 최대 속도의 78 % for testing.
//
// 주의: ENA, ENB 는 PWM 지원 포트에 연결한다.
//
#define ENA 6
#define EN1 7
#define EN2 3
#define EN3 4
#define EN4 2
#define ENB 5

// 부팅 후 1회 실행되는 함수. 초기화 함수. Setup()
void setup()
{
  pinMode(ENA, OUTPUT); // ENA
  pinMode(EN1, OUTPUT); // EN1
  pinMode(EN2, OUTPUT); // EN2
 
  pinMode(ENB, OUTPUT); // ENB
  pinMode(EN3, OUTPUT); // EN3
  pinMode(EN4, OUTPUT); // EN4
  Serial.begin(9600);
  btSerial.begin(9600);
}

// 계속 실행되는 함수. loop()
void loop()
{
  if(btSerial.available()) {
    char c = btSerial.read();
    Serial.write(c);
    switch (c) {
      case 'l': // 좌회전
        digitalWrite(EN1, LOW);
        digitalWrite(EN2, HIGH);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, HIGH);
        digitalWrite(EN4, LOW);
        analogWrite(ENB, speed);
        break;
       
      case 'r': // 우회전
        digitalWrite(EN1, HIGH);
        digitalWrite(EN2, LOW);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, LOW);
        digitalWrite(EN4, HIGH);
        analogWrite(ENB, speed);
        break;
       
      case 'g': //전진
        digitalWrite(EN1, HIGH);
        digitalWrite(EN2, LOW);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, HIGH);
        digitalWrite(EN4, LOW);
        analogWrite(ENB, speed);
        break;
       
      case 'b': //후진
        digitalWrite(EN1, LOW);
        digitalWrite(EN2, HIGH);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, LOW);
        digitalWrite(EN4, HIGH);
        analogWrite(ENB, speed);
        break;   

      case 's': //정지
        analogWrite(ENA, 0);
        analogWrite(ENB, 0);
        break;
//    if(Serial.available()){ // 시리얼 모니터 확인
//    char transmitchar = Serial.read(); // 시리얼 모니터 데이터 읽기
//    btSerial.write(transmitchar); // 블루투스로 데이터 전송
      }
    }
  } 

아래는 한번 자율주행코드와 블루투스 무선조종코드를 이렇게 하면 될 거 같아서 합친코드입니다. 컴파일은 되는데 업로드후에 자동차가 동작을 하지 않네요... 전자나침반(hmc5883l) 헤더파일과 cpp는 같이 한 폴더에 넣고 동작하였습니다!




/*******************************************************************
 * - Auto driving
 * - Obstacle avoidance(장애물 회피)
 * - HC-SR04 : 초음파센서 3개
 * - Note : 속도에 따라 장애물 탐지거리/회전각도를 설정해야 함
 *******************************************************************/
#include <SoftwareSerial.h> // SoftwareSerial 라이브러리 불러오기
#include <U8glib.h>
SoftwareSerial BlueToothSerial(10, 11);
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE|U8G_I2C_OPT_DEV_0);

//////////////////////////////////////////////////
// Ultrasonic sensor (front/left/right)
//////////////////////////////////////////////////
#define Sonic_Front_TRIG    12      // front Trigger
#define Sonic_Front_ECHO    13      // front Echo
#define Sonic_Right_TRIG    8      // right Trigger
#define Sonic_Right_ECHO    9      // right Echo
#define Sonic_Left_TRIG    14      // left Trigger
#define Sonic_Left_ECHO    15      // left Echo

#define MAX_DISTANCE_SONIC  150

static int prev_centimeter = 0;

// HC-SR04 Ultrasonic sensor(0~150cm)
int getDistance_UltraSonic(int pin_Trig, int pin_Echo)
{
  int centimeter;
  long duration;

  digitalWrite(pin_Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pin_Trig, LOW);
  delayMicroseconds(10);
  duration = pulseIn(pin_Echo, HIGH); 
  centimeter = (int)(duration/29/2);    // microsecondsToCentimeters
 
  if (centimeter > MAX_DISTANCE_SONIC)
    centimeter = MAX_DISTANCE_SONIC;
 
  if (centimeter == 150 && prev_centimeter < 8)
    centimeter = 0;

  return centimeter;   
}

///////////////////////////////////////////////////////////////
// Note:  ENA and ENB must be connected to PWD supported pins
//
#define ENA  6    // PWD
#define IN1  7
#define IN2  3

#define IN3  4
#define IN4  2
#define ENB  5    // PWD

////////////////////////////////////
// Car direction
//
#define CAR_DIR_FORWARD  0  // forward
#define CAR_DIR_BACKWARD  1  // backward
#define CAR_DIR_LEFT      2  // left turn
#define CAR_DIR_RIGHT    3  // right turn
#define CAR_DIR_STOP      4  // stop

////////////////////////////////////////////////////
// Car Speed : 0 ~ 255
// it depends on the moter and battery power
// Obstacle distance may be linked to car speed
////////////////////////////////////////////////////
//#define CAR_SPEED_DEFAULT 130
//#define CAR_SPEED_FAST  150
//#define CAR_SPEED_SLOW  50
#define CAR_SPEED_DEFAULT 100
#define CAR_SPEED_FAST  120
#define CAR_SPEED_SLOW  20
///////////////////////////////////
// Default direction and speed
//
int g_carDirection = CAR_DIR_STOP;
int g_carSpeed_L;
int g_carSpeed_R;

////////////////////////////////////////////////
// Note : confirm HIGH/LOW for correct movement
////////////////////////////////////////////////
void car_forward()
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, g_carSpeed_R);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENB, g_carSpeed_L);
  Serial.println("*******************************car_forward**");
}

void car_backward()
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, g_carSpeed_R);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENB, g_carSpeed_L);
  Serial.println("*******************************car_back**");
}

void car_left()
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, g_carSpeed_R);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENB, g_carSpeed_L);
  Serial.println("*******************************car_left**");
}

void car_right()
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, g_carSpeed_R);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENB, g_carSpeed_L);
  Serial.println("*******************************car_right**");
}

void car_stop()
{
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

////////////////////////////
// Execute car moving
////////////////////////////
void update_Car()
{
  switch ( g_carDirection ) {
    case CAR_DIR_FORWARD:
        car_forward();
        break;
    case CAR_DIR_BACKWARD:
        car_backward();
        break;
    case CAR_DIR_LEFT:
        car_left();
        break;
    case CAR_DIR_RIGHT:
        car_right();
        break;
    case CAR_DIR_STOP:
        car_stop();
        break;
    default :
        ;
  }
  return;
}

// option for obstacles
bool isLeftObstacle = false;
bool isRightObstacle = false;
bool isFrontObstacle = false;
bool isAlmostBump = false;

// option for the previous actions
bool isBack = false;    // check the prev BACK operation

// distance of obstacles
int cmLeftObstacle = 0;
int cmRightObstacle = 0;
int cmFrontObstacle = 0;


// Obtacle distance 장애물거리
//#define DISTANCE_OBSTACLE_FRONT    10  //(15+CAR_SPEED_DEFAULT/10)
//#define DISTANCE_OBSTACLE_LEFT    5  //(CAR_SPEED_DEFAULT/10)
//#define DISTANCE_OBSTACLE_RIGHT    5  //(CAR_SPEED_DEFAULT/10)
//#define DISTANCE_ALMOST_BUMP_FRONT 5  //(CAR_SPEED_DEFAULT/10)

#define DISTANCE_OBSTACLE_FRONT    40  //(15+CAR_SPEED_DEFAULT/10)
#define DISTANCE_OBSTACLE_LEFT    20  //(CAR_SPEED_DEFAULT/10)
#define DISTANCE_OBSTACLE_RIGHT    20  //(CAR_SPEED_DEFAULT/10)
#define DISTANCE_ALMOST_BUMP_FRONT 20  //(CAR_SPEED_DEFAULT/10)


void searchObstacles()
{
  // reset obstacle options
  isFrontObstacle = isLeftObstacle = isRightObstacle = isAlmostBump = false;
   
  // HC-SR04 : front
  cmFrontObstacle = getDistance_UltraSonic(Sonic_Front_TRIG, Sonic_Front_ECHO);
  if ((cmFrontObstacle != 0) && (cmFrontObstacle < DISTANCE_OBSTACLE_FRONT)) {
    car_stop();
    isFrontObstacle = true;
    if (cmFrontObstacle < DISTANCE_ALMOST_BUMP_FRONT)
      isAlmostBump = true;  // must go back at once
  }

  if (isBack== true) {  // check backward moving
    isFrontObstacle = true;
  }

  // HC-SR04 : right/left
  cmRightObstacle = getDistance_UltraSonic(Sonic_Right_TRIG, Sonic_Right_ECHO);  // right sensor
  if ((cmRightObstacle != 0) && (cmRightObstacle < DISTANCE_OBSTACLE_RIGHT)) {
    car_stop();
    isRightObstacle = true;
  }
 
  cmLeftObstacle = getDistance_UltraSonic(Sonic_Left_TRIG, Sonic_Left_ECHO);  // left sensor
  if ((cmLeftObstacle != 0) && (cmLeftObstacle < DISTANCE_OBSTACLE_LEFT)) {
    car_stop();
    isLeftObstacle = true;
  }

  Serial.println("** SearchObstacles() **");
  Serial.println("Left-----Front-----Right");
  Serial.print(cmLeftObstacle);
  Serial.print(" ------ ");
  Serial.print(cmFrontObstacle);
  Serial.print(" ------ ");
  Serial.println(cmRightObstacle);

  return;
}

////////////////////////////////////////////////////////
// compass setting
// targetDegree - target directioin to move
// Note : setup() 에서 현재 방향을 목표방향으로 설정함
////////////////////////////////////////////////////////
int targetDegree = 0;    // 0 - north (0 ~ 360)

void setup() {
 
  // for debug
  Serial.begin(9600);
  BlueToothSerial.begin(9600); // 블루투스 통신 초기화   
  //delay(100);
  Serial.println("Auto Driving(Obstacle avoidance) >> Start");
 
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
 
  pinMode(Sonic_Front_TRIG,OUTPUT); 
  pinMode(Sonic_Front_ECHO,INPUT);   
  pinMode(Sonic_Right_TRIG,OUTPUT); 
  pinMode(Sonic_Right_ECHO,INPUT);   
  pinMode(Sonic_Left_TRIG,OUTPUT); 
  pinMode(Sonic_Left_ECHO,INPUT);   

  // boot notification on OLED
  drawMessage("Setup !!!!");
  delay(2000);
 
  // initialize compass sensor(HMC5883L)
  compass_setup();
  delay(2000);

  // set target direction at first
  targetDegree = compass_heading();
  drawLED("Target :  ", targetDegree);
  delay(3000);
}

///////////////////////////////////////////////////////
// 장애물과 모터 성능에따라 delay time 으로 속도 조절
///////////////////////////////////////////////////////
#define DELAY_SHORT    100
#define DELAY_NORMAL  150
#define DELAY_LONG    200
//
//#define DELAY_SHORT    50
//#define DELAY_NORMAL  75
//#define DELAY_LONG    100

//#define DELAY_TURN_LR  200
//#define DELAY_GOFORWARD 100
//#define DELAY_GOBACK    100

#define DELAY_TURN_LR  400
#define DELAY_GOFORWARD 200
#define DELAY_GOBACK    200

#define FRONT      1
#define RIGHT_TURN  2
#define LEFT_TURN  3

#define FRONT_RIGHT 4
#define FRONT_LEFT  5

int countForward = 0;    // counting continuous goForward() for direction checking
int prevObstacle = 0;

//-- main loop --
void loop()
{
  int currentDegree = 0;
  int turnDirection = 0;
  int angleBetween, angleBetween2;
  int CurrentPlusAngle;
  if(BlueToothSerial.available()){ // 데이터 수신 대기
    char receivechar = BlueToothSerial.read(); // 수신 데이터 읽기
    Serial.write(receivechar); // 수신 데이터 시리얼모니터로 출력
    switch (receivechar) {
      case 'a': // 자율주행
        searchObstacles();
     
        currentDegree = compass_heading();
        drawLED("Current :  ", currentDegree);
     
        angleBetween = 180 - abs(abs(currentDegree - targetDegree) - 180);
        CurrentPlusAngle = (currentDegree + angleBetween) % 360;
        angleBetween2 = 180 - abs(abs(CurrentPlusAngle - targetDegree) - 180);
       
        if (angleBetween > 10) {  // check the allowed gap
          if (angleBetween2 >= 0 && angleBetween2 < 4)  // consider the small error of angle gaps
            turnDirection = RIGHT_TURN;
          else
            turnDirection = LEFT_TURN;
        }
        else {
          turnDirection = FRONT;
        }
     
        if (isFrontObstacle == false) {  // no front obstacle
          if (isLeftObstacle == true && isRightObstacle == true) {
            Serial.println("Left-----<.....>-----Right -> Forward");
            goForward(DELAY_GOFORWARD);
          }
          else if (isLeftObstacle == true) {  // left obstacle (no front)
            Serial.println("Left-----<.....>-----<.....> -> turnRight");
            turnRight(DELAY_TURN_LR*2, angleBetween);
            prevObstacle = FRONT_RIGHT;
          }
          else if (isRightObstacle == true) {  // right obstacle (no front)
            Serial.println("<.....>-----<.....>-----Right -> turnLeft");
            turnLeft(DELAY_TURN_LR*2, angleBetween);
            prevObstacle = FRONT_LEFT;
          }
          else {                              // no front/left/right obstacles
            ///////////////////////////
            // check target direction
            ///////////////////////////
            Serial.println("<.....>-----<.....>-----<.....> -> checkTarget");
            Serial.print("countForward : ");
            Serial.println(countForward);
            Serial.print("currentDegree : ");
            Serial.println(currentDegree);
            Serial.print("angleBetween : ");
            Serial.println(angleBetween);
            Serial.print("CurrentPlusAngle : ");
            Serial.println(CurrentPlusAngle);
            if (countForward > 2) {          // count limit based on obstacle situation
              // rotate for target direction
              if (turnDirection == LEFT_TURN) {
                turnLeft(DELAY_TURN_LR, angleBetween);
              } else if (turnDirection == RIGHT_TURN) {
                turnRight(DELAY_TURN_LR, angleBetween);
              }
              else {;}     
              countForward = 0;  // initialize     
            } else {
              countForward++;
            }
           
          }
          //goForward((isAlmostBump==false)?DELAY_GOFORWARD : DELAY_GOFORWARD/2);
          goForward(DELAY_GOFORWARD);
        }
        else {  // front obstacle : +-----+ +------+ +-----+  +------+
                // LR obstacle :    |<left  |<both>| right>|  nothing 
          countForward = 0;
         
          if (isBack == true) {
            if (turnDirection == LEFT_TURN /*&& prevObstacle != FRONT_RIGHT*/)
              turnLeft(DELAY_TURN_LR*5, angleBetween);
            else //if (prevObstacle == FRONT_LEFT)
              turnRight(DELAY_TURN_LR*5, angleBetween);
     
            isBack = false;
            goForward(100);
            return;
          }   
         
          if (isAlmostBump==true || (isLeftObstacle==true && isRightObstacle==true)) {
            Serial.println("[Left]-----[Front]-----[Right] -> Backward<Front>");
            goBackward(DELAY_NORMAL*2);
            isBack = true;
          }
          else if (isLeftObstacle == true) {  // front/left obstacle
            Serial.println("[Left]-----[Front]-----[.....] -> turnRight<Front>");
            turnRight(DELAY_TURN_LR, angleBetween);
            goForward(DELAY_NORMAL);
            prevObstacle = FRONT_LEFT;
          }
          else if (isRightObstacle == true) {  // front/right obstacle
            Serial.println("[.....]-----[Front]-----[Right] -> turnLeft<Front>");
            turnLeft(DELAY_TURN_LR, angleBetween);
            goForward(DELAY_NORMAL);
            prevObstacle = FRONT_RIGHT;
          }
          else {                              // front obstacle (no left/right)
            ///////////////////////////
            // check target direction
            ///////////////////////////
            goBackward(DELAY_SHORT);
     
            if (turnDirection == LEFT_TURN) {
                turnLeft(DELAY_TURN_LR*2, angleBetween);
            } else if (turnDirection == RIGHT_TURN) {
                turnRight(DELAY_TURN_LR*2, angleBetween);
            } else {  // possible to rotate at any direction to avoid front obstacle
                turnLeft(DELAY_TURN_LR*2, angleBetween);
            }
          } // no left/right obstacle
        } // front obstacle
     
        ////////////////////////////////////////////////////////
        // debug : possible to check out direction with delay()
        ////////////////////////////////////////////////////////
        //delay(500);
       
       
      case 'l': // 좌회전
        digitalWrite(EN1, LOW);
        digitalWrite(EN2, HIGH);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, HIGH);
        digitalWrite(EN4, LOW);
        analogWrite(ENB, speed);
        break;
       
      case 'r': // 우회전
        digitalWrite(EN1, HIGH);
        digitalWrite(EN2, LOW);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, LOW);
        digitalWrite(EN4, HIGH);
        analogWrite(ENB, speed);
        break;
       
      case 'g': //전진
        digitalWrite(EN1, HIGH);
        digitalWrite(EN2, LOW);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, HIGH);
        digitalWrite(EN4, LOW);
        analogWrite(ENB, speed);
        break;
       
      case 'b': //후진
        digitalWrite(EN1, LOW);
        digitalWrite(EN2, HIGH);
        analogWrite(ENA, speed);
       
        digitalWrite(EN3, LOW);
        digitalWrite(EN4, HIGH);
        analogWrite(ENB, speed);
        break;   

      case 's': //정지
        analogWrite(ENA, 0);
        analogWrite(ENB, 0);
        break;
     
    }
  }

//  searchObstacles();
//
//  currentDegree = compass_heading();
//  drawLED("Current :  ", currentDegree);
//
//  angleBetween = 180 - abs(abs(currentDegree - targetDegree) - 180);
//  CurrentPlusAngle = (currentDegree + angleBetween) % 360;
//  angleBetween2 = 180 - abs(abs(CurrentPlusAngle - targetDegree) - 180);
// 
//  if (angleBetween > 10) {  // check the allowed gap
//    if (angleBetween2 >= 0 && angleBetween2 < 4)  // consider the small error of angle gaps
//      turnDirection = RIGHT_TURN;
//    else
//      turnDirection = LEFT_TURN;
//  }
//  else {
//    turnDirection = FRONT;
//  }
//
//  if (isFrontObstacle == false) {  // no front obstacle
//    if (isLeftObstacle == true && isRightObstacle == true) {
//      Serial.println("Left-----<.....>-----Right -> Forward");
//      goForward(DELAY_GOFORWARD);
//    }
//    else if (isLeftObstacle == true) {  // left obstacle (no front)
//      Serial.println("Left-----<.....>-----<.....> -> turnRight");
//      turnRight(DELAY_TURN_LR*2, angleBetween);
//      prevObstacle = FRONT_RIGHT;
//    }
//    else if (isRightObstacle == true) {  // right obstacle (no front)
//      Serial.println("<.....>-----<.....>-----Right -> turnLeft");
//      turnLeft(DELAY_TURN_LR*2, angleBetween);
//      prevObstacle = FRONT_LEFT;
//    }
//    else {                              // no front/left/right obstacles
//      ///////////////////////////
//      // check target direction
//      ///////////////////////////
//      Serial.println("<.....>-----<.....>-----<.....> -> checkTarget");
//      Serial.print("countForward : ");
//      Serial.println(countForward);
//      Serial.print("currentDegree : ");
//      Serial.println(currentDegree);
//      Serial.print("angleBetween : ");
//      Serial.println(angleBetween);
//      Serial.print("CurrentPlusAngle : ");
//      Serial.println(CurrentPlusAngle);
//      if (countForward > 2) {          // count limit based on obstacle situation
//        // rotate for target direction
//        if (turnDirection == LEFT_TURN) {
//          turnLeft(DELAY_TURN_LR, angleBetween);
//        } else if (turnDirection == RIGHT_TURN) {
//          turnRight(DELAY_TURN_LR, angleBetween);
//        }
//        else {;}     
//        countForward = 0;  // initialize     
//      } else {
//        countForward++;
//      }
//     
//    }
//    //goForward((isAlmostBump==false)?DELAY_GOFORWARD : DELAY_GOFORWARD/2);
//    goForward(DELAY_GOFORWARD);
//  }
//  else {  // front obstacle : +-----+ +------+ +-----+  +------+
//          // LR obstacle :    |<left  |<both>| right>|  nothing 
//    countForward = 0;
//   
//    if (isBack == true) {
//      if (turnDirection == LEFT_TURN /*&& prevObstacle != FRONT_RIGHT*/)
//        turnLeft(DELAY_TURN_LR*5, angleBetween);
//      else //if (prevObstacle == FRONT_LEFT)
//        turnRight(DELAY_TURN_LR*5, angleBetween);
//
//      isBack = false;
//      goForward(100);
//      return;
//    }   
//   
//    if (isAlmostBump==true || (isLeftObstacle==true && isRightObstacle==true)) {
//      Serial.println("[Left]-----[Front]-----[Right] -> Backward<Front>");
//      goBackward(DELAY_NORMAL*2);
//      isBack = true;
//    }
//    else if (isLeftObstacle == true) {  // front/left obstacle
//      Serial.println("[Left]-----[Front]-----[.....] -> turnRight<Front>");
//      turnRight(DELAY_TURN_LR, angleBetween);
//      goForward(DELAY_NORMAL);
//      prevObstacle = FRONT_LEFT;
//    }
//    else if (isRightObstacle == true) {  // front/right obstacle
//      Serial.println("[.....]-----[Front]-----[Right] -> turnLeft<Front>");
//      turnLeft(DELAY_TURN_LR, angleBetween);
//      goForward(DELAY_NORMAL);
//      prevObstacle = FRONT_RIGHT;
//    }
//    else {                              // front obstacle (no left/right)
//      ///////////////////////////
//      // check target direction
//      ///////////////////////////
//      goBackward(DELAY_SHORT);
//
//      if (turnDirection == LEFT_TURN) {
//          turnLeft(DELAY_TURN_LR*2, angleBetween);
//      } else if (turnDirection == RIGHT_TURN) {
//          turnRight(DELAY_TURN_LR*2, angleBetween);
//      } else {  // possible to rotate at any direction to avoid front obstacle
//          turnLeft(DELAY_TURN_LR*2, angleBetween);
//      }
//    } // no left/right obstacle
//  } // front obstacle
//
//  ////////////////////////////////////////////////////////
//  // debug : possible to check out direction with delay()
//  ////////////////////////////////////////////////////////
//  //delay(500);
// 
}

void goForward(int term)
{
  g_carDirection = CAR_DIR_FORWARD;
 
  g_carSpeed_L = CAR_SPEED_DEFAULT;
  g_carSpeed_R = CAR_SPEED_DEFAULT;

  update_Car();
  delay(term);
}

void goBackward(int term)
{
  g_carDirection = CAR_DIR_BACKWARD;
 
  g_carSpeed_L = CAR_SPEED_DEFAULT;
  g_carSpeed_R = CAR_SPEED_DEFAULT;

  update_Car();
  delay(term);
}

void turnLeft(int term, int angle)
{
  //car_stop();

  g_carDirection = CAR_DIR_LEFT;
 
  g_carSpeed_L = CAR_SPEED_DEFAULT;
  g_carSpeed_R = CAR_SPEED_DEFAULT;

  update_Car();

  if (angle > 100)
    term = term*2;

  delay(term);
}

void turnRight(int term, int angle)
{
  //car_stop();
 
  g_carDirection = CAR_DIR_RIGHT;
 
  g_carSpeed_L = CAR_SPEED_DEFAULT;
  g_carSpeed_R = CAR_SPEED_DEFAULT;

  update_Car();

  if (angle > 100)
    term = term*2;

  delay(term);
}

/*************************************************
 * U8g lib functions
 ************************************************/

void drawMessage(char *message)
{
  u8g.firstPage();
  do {
  u8g.setFont(u8g_font_6x10);
  u8g.setPrintPos(20,12);
  u8g.print(message);
  } while( u8g.nextPage() );
   
}

void drawLED(char *title, int smoothDegree)
{
  u8g.firstPage();
  do {
  u8g.setFont(u8g_font_6x10);
  u8g.setPrintPos(20,52);
  u8g.print(title);
  u8g.setPrintPos(70,52);
  u8g.print(smoothDegree);
  } while( u8g.nextPage() ); 
}

댓글목록

최고관리자님의 댓글

최고관리자 쪽지보내기 메일보내기 홈페이지 자기소개 아이디로 검색 전체게시물 작성일

자율주행 버튼에 따라 mode 값이 변경되도록 만들고, mode 값에 따라 실행되는 코드의 블럭이 달라지게 코딩하세요. 그러면 수동/자동 주행이 가능해질듯 합니다.