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 |
|---|
