본문 바로가기

프로젝트/KityRobotCar Project(아두이노)

KityRobot Car 소스코드

L238D :사용 가능한 Pin 정보

L239D 모터 쉴드는  M1 M2 M3 M4 Servo1 Servo2를 제어 할수 있다.

위 핀맵은  사용하는 모터에 따른 이용 가능한 디지탈 핀을 나타내고 있다.  아날로그는 모두 사용 가능

여기서는 M3 M4 모터를 사용하니 사용 가능한 디지탈 핀은 2,3,9,10,11,13번 핀이다.

아래의 코드를 보면 센서들이 이 번호를 사용 하고 있을 것이다.

참고로 디지탈 핀이 많이 필요하면 Mega나 쉬프트 레지스터를 이용하면 된다.


 

#include <AFMotor.h>                       // L293D 모터 드라이브 라이브러리
#include <Wire.h>                             // I2C 통신을 위한 라이브러리
#include <LiquidCrystal_I2C.h>        // LCD 1602 I2C용 라이브러리
LiquidCrystal_I2C lcd(0x27,16,2);     // 접근주소: 0x3F or 0x27
// SDA  A4     I2C 통신용 고정배당핀
// SCL  A5     I2C 통신용 고정배당핀  

/// IR Sensor Pin
#define IR_LeftPin       9
#define IR_RightPin   10
#define IR_UnderPin  11
#define IR_FrontPin    A0

AF_DCMotor mtR(3);          // 오른쪽 모터 M3에 연결
AF_DCMotor mtL(4);           // 왼쪽   모터 M4에 연결

int   dt = 0;                            // 거리 Cm단위
int   BlinkEyeStep=0;            // 눈깜박용 변수
float mtCalibrating=0.9 ;       // mtR을 기준으로 mtL를 보정값 (모터 성능이 달라 같은 속도를 내기 위함)
#define TURN_TIME  500    // IR감지후 회전시간
#define BACK_TIME  200    // IR감지후 후진시간'

#define eyeMode_Normal 0
#define eyeMode_Slow     1
#define eyeMode_Stop      3
int eyeMode= eyeMode_Normal; // 눈상태

#define IrMode_Right            0b100     
#define IrMode_Down           0b010
#define IrMode_Left              0b001
#define IrMode_RightDown  0b110
#define IrMode_LeftDown     0b011
#define IrMode_RightLeft      0b101
#define IrMode_All                 0b111
int irMode;                  // IR센서 감지 상태                




void mtPower( int _power)  // 모터 출력설정

  mtR.setSpeed( _power );
  mtL.setSpeed( _power * mtCalibrating );
}

void mtStop() //정지
{
  mtR.run(RELEASE);
  mtL.run(RELEASE);  
}

void mtGo()   //전진

  mtR.run(FORWARD);
  mtL.run(FORWARD); 
}

void mtBack()  //후진
{
  mtR.run(BACKWARD);
  mtL.run(BACKWARD); 
}

void mtClkTurn() // 시계방향 회전
{
  mtR.run(BACKWARD);
  mtL.run(FORWARD);
}

void mtRvClkTurn() // 반시계방향 회전
{
  mtR.run(FORWARD);
  mtL.run(BACKWARD); 
}



void searchFront()  // 전방 IR거리센서

  int volt = map(analogRead(A0), 0, 1023, 0, 5000); // 0~1023 사이 값을 갖는 아날로그 신호값을
                                                   //  0~5000 (5V) 사이 값으로 변환     
  dt = (27.61 / (volt - 0.1696)) * 1000; // 읽어들인 Voltage 값을 거리값(단위: cm)로 변환하는 공식 
  if( dt<20 ) 
        if( dt < 15 ) eyeMode = eyeMode_Stop;         
        else            eyeMode = eyeMode_Slow;                                           
  else  eyeMode = eyeMode_Normal;      
}



void irCheck() // IR 센서 감지 체크
{
  irMode=0;
  
  if( digitalRead( IR_LeftPin )== LOW )     irMode+=IrMode_Left;         // Left IR 감지                             
  if( digitalRead( IR_RightPin)== LOW )    irMode+=IrMode_Right;      // RightIR 감지 
  if( digitalRead( IR_UnderPin )== HIGH || dt < 15) irMode+=IrMode_Down;     // Down IR 감지 

  switch( irMode )
  {     
      case IrMode_RightDown: 

      case IrMode_Right:                     mtRvClkTurn();
                                                            delay(TURN_TIME);
                                                            mtGo();
                                                            break;      
      case IrMode_LeftDown: 

       case IrMode_Left:                       mtClkTurn();
                                                            delay(TURN_TIME);
                                                            mtGo();
                                                            break;
     case IrMode_Down: 

     case IrMode_All: 

     case IrMode_RightLeft:                mtBack();
                                                            delay(BACK_TIME);

                                                            mtClkTurn();
                                                            delay(TURN_TIME);
                                                            mtGo();
                                                            break;                                                  
      } // end switch
}



void displayLCD()  //  LCD에 출력

  lcd.setCursor(0,0);    // 첫줄 
  if( irMode & IrMode_Right ) lcd.print("<<");   // Right IR이 감지 되었면
  else  lcd.print("  ");

  if( BlinkEyeStep > 50 ) lcd.print("=_   ^   _=");  // 깜박모양
  else switch( eyeMode)
       {
           case eyeMode_Normal :  lcd.print("=O   ^   O="); break;
           case eyeMode_Slow:       lcd.print("=S   ^   S="); break; 
           case eyeMode_Stop:       lcd.print("-V   ^   V-");    break;      
       } //end switch if
   
  if( irMode & IrMode_Left ) lcd.print(">>");   // Leftt IR이 감지 되었다면
  else  lcd.print("  ");

  lcd.setCursor(0,1);  // 두번째줄
  if( irMode & IrMode_Down )  lcd.print("Stop No Road !!!"); 
  else
    { 
      if( eyeMode == eyeMode_Slow )lcd.print("Slow !!! ");
            else if( eyeMode == eyeMode_Stop )lcd.print("Stop !!! "); 
                        else lcd.print("     ");     
      lcd.print(dt);
      lcd.print(" Cm      "); 
     } //end if     
  BlinkEyeStep++;
  if( BlinkEyeStep > 60) BlinkEyeStep=0;  
}                     


void startRobot()   // 초기시작화면
{
  int timer=0;
  String msg = "Close to the sensor for Start Robot    "; 
  lcd.init();    
  lcd.backlight();

  lcd.setCursor(0,0);       
  lcd.print("  Kitty Robot  ");
   
  while( digitalRead( IR_LeftPin )== HIGH && digitalRead( IR_RightPin )== HIGH )
  {    // IR 센스 감지까지
    lcd.setCursor(0,1);    
    switch( timer)
    {
      case  0 : lcd.print("    Ver 1.0    ");        break;
      case 10 : lcd.print("    Close to   ");      break;
      case 20 : lcd.print("    IR Sensor  ");    break;
      case 30 : lcd.print("   Start Robot ");    break;
      case 40 : lcd.print("<- IR Sensor   ");   break;
      case 45 : lcd.print("   IR Sensor ->");   break;
      case 50 : lcd.print("<- IR Sensor ->");  break;
    }    
    delay(100);  // 너무 빠르면 LCD화면이 잘 표시가 안됨
    timer++;
    if(timer > 55 ) timer=0; 
  }

  lcd.setCursor(0,1);   
  lcd.print("               ");
    
  for( int i=3; i> -1 ;i--)
  { 
    lcd.setCursor(0,0);       
    lcd.print("Starting Robot ");
    lcd.setCursor(0,1);
    lcd.print("     [ ");
    lcd.print(i);
    lcd.print(" ]");
         
    delay(500);     
  }  
}


void setup() 
{      
  pinMode( IR_LeftPin,  INPUT);  // AnalogRead는 pnMode설정 안함 fornt_IrSensor  A0핀
  pinMode( IR_RightPin, INPUT);   
  pinMode( IR_UnderPin, INPUT);  
  
  startRobot();
  lcd.clear();  
  mtPower(100);     // 초기 모터파워 
  mtGo();
}


void loop() 
{
  irCheck();          // IR 센서체크
  searchFront();   // 전방 IR거리측정
  displayLCD();    // 화면출력 
}




    

    

'프로젝트 > KityRobotCar Project(아두이노)' 카테고리의 다른 글

KityRobot Car 소개  (0) 2022.07.31