Serving Robot
식당들의 테이블은 고정되어 있으니, 고정된 테이블까지 음식을 배달 후 출발지점까지 복귀하는 것을 목표로 했다.
사용된 센서 및 용도는 다음과 같다.
적외선 센서 5개 (라인 트레이싱용 전면 2개, 후면 2개, 컵 감지용 1개)
초음파 센서 2개 ( 장애물 감지용 전면, 후면 각 1개)
블루투스 센서 1개 ( 목적지 설정용 )
아래 동영상은 구현 완료된 동영상이다.
설계 단계
임베디드 제품들은 State Machine에 따라 움직인다. 그러므로 Serving Robot의 State를 구상하고
각 State에서 무엇을 할지 정해 둔 뒤에 기능들을 하나하나 붙여나갔다.
그래서 RobotState는 6가지 state를 가지고있다.
1. Stop : 목적지를 입력받고, 배달할 물품이 센싱될때까지 기다린다.
조건이 충족되면 Run State로 이동한다.
2. Run : 목적지까지 라인트레이싱을 하면서 진행하되, 물품이 사라지거나 장애물을 만나면 비상정지한다.
목적지에 도착하면 Arrive State로 비상 상황 발생시 Emergency State로 이동하고 이동에 문제가 없는 조건시 다시 Run State로 복귀한다.
3. Arrive : 목적지에 도달 하면 정지하고, 물품이 없어질 때까지(고객이 가져갈 때 까지) 기다렸다가 Return State로 이동한다.
4. Return : Run State와 비슷하나 물품감지는 따로 하지 않으며 장애물만 인지하여 Emergency state로 이동한다.
5. Emergency : 물품이 사라지거나 장애물이 감지되면 해당 State가 실행된다. 차체 구동을 멈추며 운행조건에 부합할 때 Run / Return State로 복귀한다. 복귀 State를 구현하기 위해 Run/Return State에서 Emergency로 넘어갈 때 EventFlag에 어디로 복귀할지 값을 저장하는 방식을 사용하였다.
6. Done : 시작 지점으로 복귀 완료한 상태이며 셋팅값을 초기화하고 , Stop State로 이동한다.
#include "linetracer.h"
int main(void)
{
RCC_Configure();
GPIO_Configure();
USART12_Init();
NVIC_Configure();
Time_Configure();
RobotState = Stop;
while (1) {
switch(RobotState){
case Stop:
IR_Array[STUFF] = GPIO_ReadInputDataBit(GPIOE, IR_STUFF); // 배달물품 감지
//목적지가 설정 되어 있고, 물체가 감지 된 상태라면 Run State로 이동.
if(EventFlag[Stop_Target] ==1 && IR_Array[STUFF] ==1){
RobotState = Run;
SendData('R');
}
break;
case Run:
//적외선 센싱과 모터 동작
IR_Sensing_Motor();
//충돌 방지를 위한 초음파센서 거리계산
distance = Read_Distance_Go();
IR_Array[STUFF] = GPIO_ReadInputDataBit(GPIOE, IR_STUFF);
//RUN하던 도중에 초음파 거리 SAFETY이하가 되거나 물체가 사라지면 Emergency State로 이동.
if(distance <= SAFETY || IR_Array[STUFF] == 0){
EventFlag[Emergency_From] = Run;
RobotState = Emergency;
}
break;
case Arrive:
//물체 감지해보고
IR_Array[STUFF] = GPIO_ReadInputDataBit(GPIOE, IR_STUFF);
while(IR_Array[STUFF] !=0) // 물건이 사라질때까지 기다림.
{
IR_Array[STUFF] = GPIO_ReadInputDataBit(GPIOE, IR_STUFF); // 지속적으로 감지.
}
//while loop를 빠져나오면 Return State로 감.
RobotState = Return;
break;
case Return:
IR_Sensing_Motor();
distance = Read_Distance_Back();
//Run State랑 로직 동일. 다만 물체 감지안해도 됨.
if(distance <= SAFETY) {
EventFlag[Emergency_From] = Return;
RobotState = Emergency;
}
break;
case Emergency:
M_StateChange(Stop);
while(EventFlag[Emergency_From] == Run)
{
distance=Read_Distance_Go();
IR_Array[STUFF] = GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_15);
if(distance >= SAFETY && IR_Array[STUFF] ==1)
{
RobotState = Run;
break;
}
}
while(EventFlag[Emergency_From] == Return)
{
distance = Read_Distance_Back();
if(distance >= SAFETY)
{
RobotState = Return;
break;
}
}
break;
case Done:
// 다시 시작지점으로 온 상태.
Var_Reset_All(); // 모든값을 초기화 해주고
RobotState = Stop; // 정지상태로 loop back
break;
}
}
return 0;
}
IR센싱 및 Motor 작동
모든 센서/ 모터 드라이브 사용법 등에 관한 라이브러리가 일체 존재하지 않아
아두이노 코드들을 ARM32에 맞는 코드로 재구현하여 사용하였다. (특히 초음파센서..)
센서들을 사용하고, 좌회전 우회전 등의 코드구현은 어려움이 없었지만 한정된 센서 내에서 갈림길과 목적지 도착신호를 어떻게 인식할 것인가에 대한 문제가 있었다.
그래서 양쪽이 감지되었을 때 어떻게 움직일 것인가가 핵심으로 두고 구현하였다.
특별한 방법은 아니고 식당 내에 테이블(목적지)는 정해진 위치이며, 그 길로 가는데 양쪽 모두 감지 되는 경우를 하드코딩해서 만들었다. (특별한 방법이 생각나지 않더라..)
코드에 적혀있지만 2차원 GoArray,BackArray를 각각 만들고
row는 목적지 Index에 따라 선택된다. (블루투스 신호에 의해 MoveIdx 선택됨)
각 row에는 양쪽이 모두 센싱 되었을 때 어디로 가야하는지에 대한 정보가 담겨있다.
그것을 dir로 받아 온 후 방향에 맞게 움직인다.
void IR_Sensing_Motor(void)
{
IR_Array[FL] = GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_11);
IR_Array[FR] = GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_12);
IR_Array[BL] = GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_14);
IR_Array[BR] = GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_13);
if(RobotState == Run){
if(IR_Array[FL] == 1 && IR_Array[FR] == 1) //앞으로 가는데, 아무것도 센싱안됐음.
{
M_StateChange(Go_Forward);
}
else if(IR_Array[FL] == 0 && IR_Array[FR] ==1) // 왼쪽 센싱됐으면 좌회전.
{
M_StateChange(Go_Tank_Left);
Stay(TURNING_FIXTIME);
M_StateChange(Go_Forward);
Stay(TURNING_GO);
}
else if(IR_Array[FL] == 1 && IR_Array[FR] == 0)
{
M_StateChange(Go_Tank_Right);
Stay(TURNING_FIXTIME);
M_StateChange(Go_Forward);
Stay(TURNING_GO);
}
else{ //양쪽 다 감지되는 상황.
int dir = GoArray[MoveIdx][Go_Idx];
if(dir == D_ARRIVE ){
//도착이면.
RobotState = Arrive;
M_StateChange(M_Stop);
}
else if(dir == LEFT){
//좌회전이면
M_StateChange(Go_Turn_Left);
Stay(TURNING_TIME);
}
else if(dir ==RIGHT){
//우회전이면
M_StateChange(Go_Turn_Right);
Stay(TURNING_TIME);
}
//시작지점 도착했으면
else if(dir ==S_ARRIVE){
RobotState = Done;
M_StateChange(Stop);
}
else if(dir == IGNORE)
{
M_StateChange(Go_Forward);
Stay(IGNORE_TIME);
}
Go_Idx++;
}
}
else if(RobotState == Return){
if(IR_Array[BL] == 1 && IR_Array[BR] ==1)
{
M_StateChange(Back_Forward);
}
else if(IR_Array[BL] == 0 && IR_Array[BR] ==1)
{
M_StateChange(Back_Tank_Left);
Stay(TURNING_FIXTIME);
M_StateChange(Back_Forward);
Stay(TURNING_GO);
}
else if(IR_Array[BL] == 1 && IR_Array[BR] ==0)
{
M_StateChange(Back_Tank_Right);
Stay(TURNING_FIXTIME);
M_StateChange(Back_Forward);
Stay(TURNING_GO);
}
else{
int dir = BackArray[MoveIdx][Back_Idx];
if(dir == D_ARRIVE ){
//도착이면.
RobotState = Arrive;
M_StateChange(Stop);
}
else if(dir == LEFT){
//좌회전이면
M_StateChange(Back_Turn_Left);
Stay(TURNING_TIME);
}
else if(dir ==RIGHT){
//우회전이면
M_StateChange(Back_Turn_Right);
Stay(TURNING_TIME);
}
else if(dir ==S_ARRIVE){
RobotState = Done;
M_StateChange(Stop);
}
else if(dir == IGNORE)
{
M_StateChange(Back_Forward);
Stay(IGNORE_TIME);
}
Back_Idx++;
}
}
}
Lee-SiHyeon/Embadded_ServingRobot
Contribute to Lee-SiHyeon/Embadded_ServingRobot development by creating an account on GitHub.
github.com
세부 코드들은 GitHub에 업로드 되어있다.
내년 해당 실험 조교하는데,,, 제 코드로 텀프 제출시 당연히 감점입니다.