July 16, 2024

디바이스마트 미디어:

[66호] 원하는 색상으로 제어가 가능한 아두이노 IoT 스마트 무드등 키트 -

2021-06-25

★2021 ICT 융합 프로젝트 공모전 결과 발표! -

2021-05-12

디바이스마트 국내 온라인 유통사 유일 벨로다인 라이다 공급! -

2021-02-16

★총 상금 500만원 /2021 ICT 융합 프로젝트 공모전★ -

2021-01-18

디바이스마트 온라인 매거진 전자책(PDF)이 무료! -

2020-09-29

[61호]음성으로 제어하는 간접등 만들기 -

2020-08-26

디바이스마트 자체제작 코딩키트 ‘코딩 도담도담’ 출시 -

2020-08-10

GGM AC모터 대량등록! -

2020-07-10

[60호]초소형 레이더 MDR, 어떻게 제어하고 활용하나 -

2020-06-30

[60호]NANO 33 IoT보드를 활용한 블루투스 수평계 만들기 -

2020-06-30

라즈베리파이3가 드디어 출시!!! (Now Raspberry Pi 3 is Coming!!) -

2016-02-29

MoonWalker Actuator 판매개시!! -

2015-08-27

디바이스마트 레이저가공, 밀링, 선반, 라우터 등 커스텀서비스 견적요청 방법 설명동영상 입니다. -

2015-06-09

디바이스마트와 인텔®이 함께하는 IoT 경진대회! -

2015-05-19

드디어 adafruit도 디바이스마트에서 쉽고 저렴하게 !! -

2015-03-25

[29호] Intel Edison Review -

2015-03-10

Pololu 공식 Distributor 디바이스마트, Pololu 상품 판매 개시!! -

2015-03-09

[칩센]블루투스 전 제품 10%가격할인!! -

2015-02-02

[Arduino]Uno(R3) 구입시 37종 센서키트 할인이벤트!! -

2015-02-02

[M.A.I]Ahram_ISP_V1.5 60개 한정수량 할인이벤트!! -

2015-02-02

[66호] Automatic Serving & Order System

66_ict_환기제어 (1)

2020 ICT 융합 프로젝트 공모전 장려상

Automatic Serving & Order System

 

글 | 서울시립대학교 박진석, 서재원, 송태헌

1. 심사평
칩센 라인트레이서의 기본 기능에 서버 시스템과의 연계를 통한 특정 위치를 찾아가게 하는 동작으로 무인 서버 로봇을 구성한 것으로, 사실 특이할 만한 점은 보이지 않아 보입니다. 모터구동과 관련하여 부품을 선정하고, 적용하기 위하여 사전에 촘촘한 선행 검토와 모델링을 진행한 것은 매우 잘 진행 된 듯 합니다. 최근에 Indoor-position 관련한 솔루션이 매우 활발하게 검토되고 개발이 되고 있는데, 이를 이용하여 라인트레이싱이 아닌 일반적인 환경에서의 서버 로봇을 만들 수 있다면 최근 트렌드와 맞는 더 진보한 작품이 되지 않을까 하는 생각이 듭니다.

펌테크 아이디어와 실용성, 상업성을 두루 갖춘 작품이라고 생각합니다. 전체적으로 세심하고 짜임새 있게 잘 구성이 되었다고 생각되며 기술적 구현도, 작품 완성도 등이 높은 작품이라고 생각합니다.

위드로봇 이동로봇의 주행 중에 발생하는 문제는 어떻게 해결하였는지 고민이 더 추가되면 좋은 작품이 될 것 같습니다.

2. 개요
본 프로젝트는 식당에서 사용할 수 있는 자동으로 서빙을 해주는 무인 서빙 로봇, 주문을 해주는 어플리케이션과 주문을 확인하고 서빙 명령이 가능한 웹페이지 제작을 한다. 서빙 로봇은 Wi-Fi와 연결되었기 때문에 공유기가 설치된 환경이면 어디서든 환경이 가능하고, 주문한 음식은 서버에 저장되기 때문에 완벽한 IoT를 구현하였다.

2.1. 개발 배경
현대사회에서 생산성과 효율을 위해서 많은 분야에서 무인/자동화가 이루어지고 있다. 이러한 추세에 따라 매장에 무인 주문 결제기인 ‘키오스크’가 설치되고 있고, 일부 매장은 어플로 주문을 하는 시스템을 도입했다. 따라서 위의 주문 시스템에 자동으로 서빙을 로봇을 융합함으로 IoT를 이용한 무인 서비스 제공을 목표로 한다.

2.2. 프로젝트 목적 및 기대효과
이 프로젝트의 목적은 주문/서빙 시스템의 자동화이다. 병렬적으로 주문을 할 수 있는 무인 주문 서비스와 무인 서빙 서비스를 만드는 것이 목표이다. 무인 서빙 서비스가 있기 때문에 서빙에 필요한 인건비가 줄어들어 경제적이고 효율적이다. 무인 주문 서비스를 통해서 순간적으로 사람이 집중이 되더라도 주문하는데 기다릴 필요가 없게 된다. 또한, 주방에서 출력된 종이 영수증을 보통 주문의 순서를 정하는데, 이 프로젝트를 통해서 종이 영수증대신 태블릿 PC와 같은 전자기기로 대체할 수 있을 것이다. 이에 따라 주방에서 낭비되는 종이의 양을 줄여서 환경보호면에서도 효과가 있다. 또한, 보드는 Arduino UNO만을 사용하고 성능을 대부분 사용하여 개발을 했기 때문에 실제 제품으로 만들 시 제품 비용 절감이 매우 절감될 것이다.

2.3. 개발 목표
서빙 할 테이블의 번호를 지정하면 자동으로 서빙을 해주는 서비스 구현과 많은 사람들이 집중되더라도 한꺼번에 처리가 가능한 무인 병렬 주문 서비스 제공이 목표이다.

2.4. 세부 개발 내용
네이버 클라우드에 비용을 지불해서 구축한 서버에 APM(Apache2, PHP, MySQL)을 설치하고, 웹 페이지와 안드로이드 어플을 제작하여 주문한 데이터가 DB인 MySQL에 저장되도록 한다. 관리자는 관리자용 웹페이지를 통해서 MySQL에 저장된 데이터를 확인해서 들어온 주문을 확인할 수 있다. 서빙을 원하는 테이블을 웹 페이지에 입력하면 마찬가지로 MySQL에 저장이 된다. 서빙 로봇은 매장에 설치된 Wi-Fi와 연결된 상태로 구동되고, 일정 간격으로 MySQL을 확인해서 서빙 요청이 왔는지 확인한다. 서빙 명령을 받게 되면 목표 테이블까지 라인트레이싱을 하며 도달하게 된다. 이 때 서빙 하는 음식의 무게에 상관없고, 가속도에 의한 음식의 미끄러짐을 최소화하기 위해서 모터 PID속도 제어를 통해서 운동한다.

3. 프로젝트 설명
3.1. 주요 동작 및 특징

66 ict_오더시스템 (1)

서빙로봇이 Wi-Fi에 연결된 상태로 1초간격으로 MySQL 데이터를 읽어들이며 주문을 기다리고 있다.

66 ict_오더시스템 (2)

원하는 음식과 테이블을 선택해서 MySQL로 데이터를 전달해주는 어플리케이션이다.

66 ict_오더시스템 (3)

MySQL에 저장된 데이터를 읽을 수 있고, 테이블번호를 선택하면 Arduino Called열 값을 1로 바꿔서 서빙 로봇에게 서빙 명령을 하는 관리자용 웹 페이지이다. (http://106.10.49.227/order_list.php)

66 ict_오더시스템 (4)

서빙 로봇이 Wi-Fi를 통해서 MySQL에서 Serving Completed열이 0이고 Arduino Called가 1인 테이블 번호를 읽어 들어서 해당 테이블로 음식을 서빙하고 있다.

66 ict_오더시스템 (2)

서빙이 완료된 후 MySQL의 Serving Completed열 값을 1로 변경해서 서빙이 완료됨을 알리는 서빙 로봇 창이다.

3.2. 전체 시스템 구성
3.2.1. Software Architecture_System Flowchart

66 ict_오더시스템 (3)

66 ict_오더시스템 (4)

손님이 주문을 하고 서빙 로봇(아두이노)이 서빙을 하기 위한 방법으로 서버를 사용하였다. 손님이 주문을 하면 주문내역이 서버의 데이터베이스에 저장이 되고 주방에서는 서버의 데이터베이스를 확인하여 서빙 로봇을 호출한다. 서빙 로봇에서는 와이파이에 연결되어 있으며 일정한 간격으로 서버의 데이터베이스에 접근하여 호출 신호가 왔을 때, 지정된 테이블로 음식을 서빙 한다.

3.2.2. Hardware Architecture

66 ict_오더시스템 (5)

3.3. 개발 환경(개발 언어, Tool, 사용 시스템 등)

66 ict_오더시스템 (6)

4. 단계별 제작 과정
4.1. 서버구축

66 ict_오더시스템 (7)

서버는 네이버 클라우스 서비스를 사용하여 구축하였다. 기본적으로 유료서비스이지만 작은 규모의 서버를 1년동안 무료로 사용할 수 있는 이벤트가 있어서 네이버 서버를 선택하였다. 서버의 사양은 [Micro] 1vCPU, 1GB Mem, 50GB Disk이라 성능은 좋지 않지만 이번 프로젝트에서는 문제가 없다고 판단하였다. 서버의 OS는 Ubuntu 16.04 LTS Server를 설치하였다.

4.2. APM(Apache2/PHP/MySQL) 설치 및 ACG 설정

66 ict_오더시스템 (8)

66 ict_오더시스템 (9)

네이버클라우드에서 할당받은 서버에 APM을 설치한 뒤 공인 IP인 http://106.10.49.227/에 접근할 수 있도록 80번 포트를 ACG를 통해서 열어주었다. 인터넷이 연결된 어떤 환경에서도 해당 주소로 이동하면 Apache2디폴트 페이지를 확인할 수 있다.

4.3. MySQL 테이블 구축

66 ict_오더시스템 (10)

Windows에서 MySQL Workbench를 설치하여 좀 더 쉽게 MySQL를 다룰 수 있도록 하였다. orderDB의 테이블 속, 각 데이터들은 다음을 의미한다. _order는 주문순서, time는 주문시간, _table는 주문 테이블 번호, food는 주문한 음식과 수량, serving는 서빙이 완료되었는지 안되었는지, call_arduino는 서빙 로봇이 확인하는 데이터로써 값이 1일 경우, 서빙 로봇이 음식을 서빙한다.
_order열에는 값을 입력하지 않아도 자동으로 값이 1씩 증가하도록 설정하였고, time은 ubuntu서버에 있는 시간을 자동으로 입력받도록 설정하였다. 아래에 기술할 웹페이지에서 _table, food, call_arduino열을 설정할 수 있도록 연동하였다.

4.4. 웹 페이지 및 애플리케이션 제작
4.4.1. 주문 애플리케이션 제작
AndroidStudio를 이용하여 주문 애플리케이션을 개발하였다. 메뉴 리스트 화면, 메뉴 상세 화면, 장바구니 화면으로 구성되었다.

66 ict_오더시스템 (11)

메뉴 리스트 화면: 처음 앱을 켰을 때, 나타나는 화면이다. 내부 데이터베이스에서 저장된 모든 메뉴의 사진 썸네일과 이름, 가격을 표시하며, 각 메뉴를 선택하면 메뉴 상세화면으로 넘어간다.
메뉴 상세화면: 메뉴 상세화면은 메뉴 리스트 화면에서 선택된 메뉴의 사진과 이름, 가격을 확인할 수 있으며, 수량을 체크하고 ‘장바구니 담기’버튼을 선택하여, 내부 데이터베이스에 저장한다. 만약 해당 메뉴가 이미 데이터베이스에 저장되어 있으면 해당 메뉴의 수량만 변경한다.
장바구니 화면: 장바구니 화면은 모든 화면에서 액션바에 있는 버튼을 통해 접근할 수 있다. 데이터베이스에 저장된 장바구니 리스트를 가져와서 보여준다. x버튼을 누르면, 해당 메뉴를 데이터베이스에서 지움으로 장바구니 리스트에서 지운다. 결제하기 버튼을 통해 서버와 Http통신을 하며, 테이블 번호와 함께 장바구니 리스트를 전달한다. 서버로부터 RESULT_OK 신호가 오면, “결제가 완료되었습니다.”라는 안내와 함께 장바구니 리스트를 모두 지운다.

4.4.2. 관리자용 웹 페이지 제작

66 ict_오더시스템 (12)

66 ict_오더시스템 (13)

이미 구축한 리눅스 서버에 Putty를 사용해서 터미널 접속을 한 후 /var/www/html/ 경로에 php파일을 생성하면 위와 같이 공인아이피 http://106.10.49.227/파일이름.php 로 접속할 수 있다. 본 프로젝트에서 /var/www/html /order_list.php에 php와 html을 사용해서 코딩했다.
손님용 웹페이지에서 입력한 MySQL 정보를 읽어와서 표로 표현해주는 기능을 한다. 요리가 완료되면 아래에 있는 테이블 번호를 입력하고 ‘제출’버튼을 누르면 Arduino Called열이 1로 바뀌어 서빙 로봇에게 서빙 명령을 내릴 수 있다.

4.5. Arduino에서 Wi-Fi통신을 통한 MySQL 데이터 수신
Arduino에서 Wi-Fi통신을 한 후에 MySQL로 바로 접근해서 query를 입력해주면 MySQL에서 해당하는 기능을 실행하는 동작을 구현하였다. 소스코드에서 char query[] = “SELECT _table FROM project.orderDB WHERE call_arduino = ’1′ AND serving = ’0′ ;”; char query2[] = “UPDATE project.orderDB SET serving = ’1′ WHERE call_arduino = ’1′ AND serving = ’0′ ;”; 에 명령을 저장하고, cur_mem->execute(query); 명령어로 query배열에 담긴 내용을 MySQL에 입력할 수 있다. 여기에서 query는 MySQL에서 call_arduino 열 값이 1이고 serving 열 값이 0인 _table열의 값을 return하는 기능을 하고, query2는 call_arduino열 값이 1이고 serving 열 값이 0인, 즉 query에서 return한 _table열의 같은 값의 serving열을 1로 바꿔주는 기능을 담당한다.

66 ict_오더시스템 (14)

66 ict_오더시스템 (15)

 

위의 그림은 serving열의 값이 0이고 call_arduino열의 값이 1인 (서빙 명령을 받은 행) 테이블 번호의 MySQL값을 1초마다 Arduino에서 읽어 들이는 모습을 나타낸다.

4.6. DC모터의 PID Feedback Control

66 ict_오더시스템 (16)

위의 모터의 Specification을 참고해서 [T:토크(kgf-cm) V:전압(V), I:전류(A), w:각속도(rad/s), R:저항(Ω)] 아래의 관계식을 세울 수 있다.

T∝
V=IR

두 개의 식을 연립하고 비례상수 k를 도입하면 아래의 식으로 변환이 가능하다.

Tw=k

Stall Torque는 최대 토크의 값을 의미하고, 이는 최대 전압인 6V와 Stall Current인 3.2A일 때 성립한다. 따라서 옴의 법칙에 의해서

6=3.2R
R=1.875Ω

모터의 저항은 1.875Ω이다.
또한, DC모터의 스펙으로부터 전류에 다른 토크의 값을 아래와 같이 기재할 수 있다.
DC모터, 아두이노 차체 시스템에서 차체를 모델링하여 전달함수를 구하기 위해 Free Body Diagram을 그려서 계산할 필요가 있다.

66 ict_오더시스템 (17)

66 ict_오더시스템 (18)

위의 그림에서 오른쪽에 있는 두 개의 원이 모터가 달린 바퀴이고, 왼쪽에 있는 원은 캐스터이다. 바닥과의 마찰이 충분하여 바퀴가 헛도는 상황이 발생하지 않는다고 가정하면, 모터에 의한 토크는 바퀴 두 개에 가해지는 힘으로 생각할 수 있다. 위의 그림을 다시 차체에 대해서만 Free Body Diagram을 그리면 아래와 같이 간단한 형태로 나타낼 수 있다.

66 ict_오더시스템 (19)

모터에 의한 추진력 2F와 공기저항과 같은 저항에 의한 반력이 생기기 때문에 cv를 도입하였으나, 영향은 작을 것으로 예측되어 c=0.1인 낮은 값으로 가정하였다. 바퀴의 반지름이 3.25cm인 것을 고려하여 운동방정식을 세우면 아래와 같다.

66 ict_오더시스템 (20)

여기에서 은 비선형이므로, 라플라스 변환을 하기 위하여 테일러 급수를 이용해 정상상태에서의 선형화한 값을 도출해낸다.

66 ict_오더시스템 (21)

차체의 무게는 약 400g이고, 정상상태에서 가속도는 작은 것을 목표로 =0.01cm/s2을 대입하여 이 식을 정리한다.

66 ict_오더시스템 (22)

Fitting Line에서 구한 T=2.844·I+1.088을 대입하여 위의 식을 정리한다.

66 ict_오더시스템 (23)

상수를 모두 없애기 위해서, 이 식을 편차변수로 나타낼 수 있다.

66 ict_오더시스템 (24)

옴의 법칙을 이용해서 전류를 전압에 대한 식으로 바꾼다.

66 ict_오더시스템 (25)
위 식을 라플라스 변환하면 다음과 같이 표현할 수 있다.

66 ict_오더시스템 (26)

이 전달함수를 구할 수 있다.

66 ict_오더시스템 (27)
이 식을 바탕으로 MATLAB을 활용하여 Simulink를 구성하면 다음과 같다.

66 ict_오더시스템 (28)

 

66 ict_오더시스템 (29)

 

MATLAB에 내장된 PID Tuner를 통해서 PID 계수를 각각 4.227, 2.297, -0.03976으로 결정했다.

 

66 ict_오더시스템 (30) 66 ict_오더시스템 (31)

예상된 시뮬레이션으로, 외란(서빙 로봇의 무게 변화)이 존재하는 경우와 존재하지 않는 경우에도 모두 목표 값을 잘 찾아가는 것을 확인할 수 있다.

66 ict_오더시스템 (32)

위의 그림은 아두이노에서 DC모터를 실제로 구동하고 시리얼 모니터에서 받아온 Encoder값을 Simulink에서 예측한 그래프에 도사한 것이다. 필터를 사용하지 않았기 때문에 Encoder 자체에서 발생한 노이즈에 의해서 벗어나는 데이터가 보이지만 평균적으로 목표 속도인 30cm/s에 수렴하기 때문에 안정적으로 제어에 성공하였다.

4.7. 아두이노 모터와 센서 핀 구성

66 ict_오더시스템 (33)

IR센서는 아날로그 신호가 필요하기 때문에 아날로그 핀인 A1, A2, A3, A4를 사용하였다. 모터의 경우는 모터의 속도를 조절하기 위해 PWM 신호를 줄 수 있는 5,6 10,11번 핀을 사용하였다.
모터의 엔코더의 경우는 인터럽트 신호를 사용한다. 따라서 아두이노 우노에서 인터럽트 신호를 사용할 수 있는 유일한 핀인 2번 3번 핀을 사용하였다. 모터의 2개 모두 엔코더를 사용하고 싶었으나 아두이노 우노에는 핀이 부족하여 모터 하나의 엔코더만 사용하기로 하였다.
Wi-Fi를 연결을 위해 ESP-01 어뎁터는 8, 9번 핀을 사용하였다.

4.8. 원하는 테이블로 찾아가는 알고리즘 제작
기본적으로 길찾기와 라인트레이서를 분리를 하여 제작을 하였다. 전체적으로 원하는 테이블로 가기 위해서 길을 찾아가는 알고리즘이 돌아가고 길 찾기 알고리즘에서는 모터에게 세부적인 명령을 내리지 않는다. 앞으로 갈지, 좌회전이나 우회전을 할 지 서빙 로봇에게 명령만 내리도록 하였다. 그리고 서빙 로봇은 명령을 받는 대로 라인트레이서 모듈을 활용하여 모터를 가동하도록 하였다.
원하는 테이블로 가기 위해서 교차로를 탐지하였다. 각 테이블마다의 고유의 코스별로 명령들을 저장을 하였다. 예를들어 1번 테이블의 경우는 [직진, 좌회전(첫번째 교차로), 직진]이고 4번 테이블의 경우는 [직진, 우회전(두번째 교차로, 직진)]이다. 교차로를 탐지하는 방법으로는 IR sensor의 양끝부분에서 모두 검정색이 감지가 되면 교차로라고 판단을 하였다. 양끝부분 사이의 거리가 도로의 폭보다 넓기 때문에 가능하다.

4.9. 상황 별 IR sensor 값에 따른 라인트레이서

66 ict_오더시스템 (35) 66 ict_오더시스템 (34)

직진을 하라는 명령이 들어왔을 경우, IR sensor 값 중에서 가운데 두개가 모두 검정색일 때에는 도로 위에 잘 있는 경우이므로 양 모터의 속도를 일정하게 주었다. 하지만 가운데 두개중에서 하나라도 검정색이 아닌 경우에는 도로를 이탈하려는 경우이므로 양 모터의 속도를 다르게 주어서 원래 도로로 복귀할 수 있도록 하였다.
좌회전, 우회전 하라는 명령이 들어왔을 경우에는 양 끝 센서의 값을 확인하면서 회전을 하도록 하였다. 예를들어 좌회전을 하는 경우 가장자리 우측 센서의 값이 검, 흰, 검, 흰 순서대로 바뀌게 된다. 따라서 우측 센서의 값이 바뀔 때마다 카운트를 하도록 하였다. 카운트가 3번되는 순간 회전이 끝났다고 판단을 하였다.

4.10. 서빙 로봇 프레임 및 모형 식당 제작

66 ict_오더시스템 (5) 66 ict_오더시스템 (6)

아두이노 RC카용 프레임에 IR센서, Arduino UNO, Wi-Fi 모듈, 바퀴 등을 조립했다. 배터리와 DC 모터는 프레임의 하단부에 조립했고, 음식을 싣기 위해서 프레임과 조립할 수 있는 받침대를 CATIA로 모델링해서 3D 프린터로 출력하였다.

5. 소스코드
프로젝트 깃허브 주소 : https://github.com/ParkJinSuk/ASOS-AutomaticServingOrderSystem

FINDTABLE

void lineTracing()
{
/* 0.5초마다 라인트레이서 모듈 센서 측정 */
if((millis() – time) >= SENSING_PERIOD)
{
time = millis();
getIRSensor();
// showIRSensor();
if(isCrosswalk()){
Serial.println(F(“갈림길 감지”));
crosswalkCnt += 1;
if(isTurningTime(crosswalkCnt)){//회전할 갈림길
if(turn[target_table] == LEFT)
Serial.println(F(“왼쪽”));
else if(turn[target_table] == RIGHT){
Serial.println(F(“오른쪽”));
}
moveTracer(turn[target_table]);//회전한다.
}else{
Serial.println(F(“갈림길 통과”));
moveTracer(FORWARD_THROUGH_CROSSWALK);//갈림길 통과
}

}else if(isRoad()){
Serial.println(F(“straight”));
moveTracer(STRAIGHT);//앞으로 이동.
}else{
Serial.println(F(“stop”));
moveTracer(STOP);//멈춘다.
isDone = true;
}
}

}

bool isCrosswalk(){
return !RightOut && !LeftOut;//검검이면 갈림길!!
}

bool isTurningTime(int cnt){
Serial.println(cnt);
return cnt == crosswalk[target_table];
}

bool isRoad(){
if (!LeftOut || !LeftIn || !RightIn || !RightOut)
{
cnt_isRoad = 0;
return true;
}
else
{
if (cnt_isRoad != 5){cnt_isRoad += 1; return true;}
if (cnt_isRoad == 5){cnt_isRoad = 0; return false;}
}
}

void moveTracer(int dir){
// Serial.print(“moveTracer”);
// Serial.println(dir);

switch(dir){
case STRAIGHT : moveForward();break;
case LEFT : turnLeft();break;
case RIGHT : turnRight();break;
case STOP : stopMoving();break;
case FORWARD_THROUGH_CROSSWALK : moveThroughCrosswalk();break;
}
}
/* 라인트레이서 모듈을 사용하여 검은색 선을 따라가는 함수 */
void moveForward()
{
if (!LeftIn && !RightIn)
{
// Serial.println(“forward 11″);
pidControl_Hz(Hz, 20);
}
else if (LeftIn && !RightIn)
{
// Serial.println(“forward 01″);
MotorA(STRAIGHT, STRAIGHT_SPEED_WEEK);
MotorB(STRAIGHT, STRAIGHT_SPEED_STRONG);
}
else if (!LeftIn && RightIn)
{
// Serial.println(“forward 10″);
MotorA(STRAIGHT, STRAIGHT_SPEED_STRONG);
MotorB(STRAIGHT, STRAIGHT_SPEED_WEEK);
}
else
{
// Serial.println(“forward xx”);
pidControl_Hz(Hz, 20);
}

}

void turnLeft()
{
// Serial.println(“turn left”);

while(!RightOut || !LeftOut){//하양이면 끝. 도는 동안은 계속 검정.
if((millis() – time) >= SENSING_PERIOD){
time = millis();
getIRSensor();
}
Serial.println(F(“왼쪽~~~~~~~~~~~~~~~~”));
MotorA(STRAIGHT, TURN_SPEED);
MotorB(STRAIGHT, 0);
}
Serial.println(F(“왼쪽~~~~~~~~~~~~끝~~~~”));

}
void turnRight()
{
// Serial.println(“turn right”);

while(!RightOut || !LeftOut){//하양이면 끝. 도는 동안은 계속 검정.
if((millis() – time) >= SENSING_PERIOD){
time = millis();
getIRSensor();
}
// if(LeftOut != pre){
// chgCnt += 1;
// pre = LeftOut;
// }
Serial.println(F(“오른쪽~”));

MotorA(STRAIGHT, 0);
MotorB(STRAIGHT, TURN_SPEED);
}
Serial.println(F(“오른끝쪽”));
}
void stopMoving()
{
// Serial.println(“stop”);
MotorA(STOP, 0);
MotorB(STOP, 0);
}

void moveThroughCrosswalk(){
// Serial.println(“move Through crosswalk”);
while(isCrosswalk()){
if((millis() – time) >= SENSING_PERIOD){
time = millis();
getIRSensor();
}

MotorA(STRAIGHT, STRAIGHT_SPEED);
MotorB(STRAIGHT, STRAIGHT_SPEED);
delay(100);
}
}
/****************************/

 

IRSensor

/* 프로그램 내용 : bfd-1000 IR센서 관련 함수
*
* getIRSensor IR센서 값 읽어오는 함수
* getIRSensor_Hz 주어진 Hz 속도로 IR센서 값 읽어오는 함수
* showIRSensorChar IR센서 값을 문자로 시리얼모니터로 출력해주는 함수
* showIRSensorValue IR센서 값을 시리얼모니터로 출력해주는 함수
*
*/

void getIRSensor()
{
LeftOut = digitalRead(SS1_LEFT_OUT);
LeftIn = digitalRead(SS2_LEFT_IN);
//Center = digitalRead(SS3_CENTER);
RightIn = digitalRead(SS4_RIGHT_IN);
RightOut = digitalRead(SS5_RIGHT_OUT);
//Bump = digitalRead(CLP_BUMP);
//Near = digitalRead(NEAR);
}

void getIRSensor_Hz(int Hz)
{
if( (millis()-time) % (1000 / Hz) == 0 )
{
getIRSensor();
}
}

void showIRSensorChar()
{
if (LeftOut == 1) {Serial.print(“L “);} else {Serial.print(“- “);}
if (LeftIn == 1) {Serial.print(“l “);} else {Serial.print(“- “);}
//if (Center == 1) {Serial.print(“C “);} else {Serial.print(“- “);}
if (RightIn == 1) {Serial.print(“r “);} else {Serial.print(“- “);}
if (RightOut == 1) {Serial.print(“R “);} else {Serial.print(“- “);}
//if (Bump == 1) {Serial.print(” BUMP!”);} else {Serial.print(” “);}
//if (Near == 1) {Serial.print(” NEAR”);} else {Serial.print(” “);}
Serial.println();
}

void showIRSensorValue()
{
Serial.print(“IRSensor : “);
Serial.print(LeftOut);Serial.print(LeftIn);
Serial.print(RightIn);Serial.print(RightOut);
Serial.println();
}

main_ver3

#include “WiFiEsp.h”
#include <MySQL_Connection.h>
#include <MySQL_Cursor.h>
#include “SoftwareSerial.h”

/* Arduino Pin */
#define SS1_LEFT_OUT A1
#define SS2_LEFT_IN A2
#define SS4_RIGHT_IN A3
#define SS5_RIGHT_OUT A4

#define encoderPinA 2
#define encoderPinB 3

#define MotorA1 5
#define MotorA2 6
#define MotorB1 10
#define MotorB2 11
#define TURN_SPEED 200
#define STRAIGHT_SPEED 110

#define STRAIGHT_SPEED_STRONG 200
#define STRAIGHT_SPEED_WEEK 110

#define SENSING_PERIOD 100

/* global variable – IR sensor */
byte LeftOut;
byte LeftIn;
byte RightIn;
byte RightOut;

/* global variable – ServingRobot Direction */
int cmd_direction = 0;
int cnt_isRoad = 0;

/* 서빙로봇 명령 */
const int STOP = 0;
const int LEFT = 1;
const int STRAIGHT = 2;
const int RIGHT = 3;
const int BACK = 4;
const int FORWARD_THROUGH_CROSSWALK = 5;
/* global variable – PID Control */
long encoderPos = 0;
double angle = 0, angle_pre=0;
int time1 = 0;
int value = 0;

double v;
double v_pre = 0;
float Kp = 4.2227;
float Ki = 2.29743;
float Kd = -0.03976;
float Ke = 0.084;

/*
float Kp = 1.674;
float Ki = 0.8239;
float Kd = -0.3584;
float Ke = 0.084;
*/
int k = 0;

double PControl, IControl=0, DControl, PIDControl;
double error, error_pre;
int pwm_in;

/* global variable – WiFi */
long target_table = 0;

/* global variable – Find Table */
int crosswalk[9] = {0, 1, 1, 2, 2, 3, 3, 4, 4};//갈림길 지나야하는 횟수. index 0은 사용하지 않는다.
int turn[9] = {0, LEFT, RIGHT, LEFT, RIGHT, LEFT, RIGHT, LEFT, RIGHT};//회전해야할 때, left로 가야하는지 right로 가야하는지
int crosswalkCnt = 0;//몇 번째 갈림길을 만났는지 체크!
bool isDone = true;

WiFiEspClient client;
MySQL_Connection conn((Client *)&client);
// Create an instance of the cursor passing in the connection
MySQL_Cursor cur = MySQL_Cursor(&conn);
MySQL_Cursor *cur_mem = new MySQL_Cursor(&conn);

SoftwareSerial esp(8, 9); // RX, TX
byte mac_addr[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };

IPAddress server_addr(106,10,49,227); // IP of the MySQL *server* here
char user[] = “sexymandoo”; // MySQL user login username
char password[] = “sexymandoo”; // MySQL user login password

char ssid[] = “Sexy_Jaewon”; // 공유기 이름 SSID
char pass[] = “jaewonsexy”; // 공유기 암호 Password
// Sample query
char query[] = “SELECT _table FROM project.orderDB WHERE call_arduino = ’1′ AND serving = ’0′ ;”;
char query2[] = “UPDATE project.orderDB SET serving = ’1′ WHERE call_arduino = ’1′ AND serving = ’0′ ;”;
int status = WL_IDLE_STATUS; // Status
int Hz = 20;
unsigned long time; // 시간 측정을 위한 변수
void setup() {
pinMode(encoderPinA, INPUT_PULLUP);
attachInterrupt(0, doEncoderA, CHANGE);
pinMode(encoderPinB, INPUT_PULLUP);
attachInterrupt(1, doEncoderB, CHANGE);

pinMode(MotorA1, OUTPUT);
pinMode(MotorA2, OUTPUT);
pinMode(MotorB1, OUTPUT);
pinMode(MotorB2, OUTPUT);

Serial.begin(9600);
esp.begin(9600);
time = millis();

WiFi.init(&esp);

if (WiFi.status() == WL_NO_SHIELD)
{
Serial.println(F(“WiFi shield not present”));
while (true);
}

// 와이파이 접속여부 확인
while (WiFi.status() != WL_CONNECTED)
{
Serial.print(F(“Attempting to connect to WPA SSID: “));
Serial.println(ssid);

status = WiFi.begin(ssid, pass);
}

// 와이파이 접속정보
Serial.println(F(“You’re connected to the network”));
Serial.print(F(“SSID: “));
Serial.println(WiFi.SSID());

IPAddress ip = WiFi.localIP();
Serial.print(F(“IP Address: “));
Serial.println(ip);

long rssi = WiFi.RSSI();

Serial.println(F(“Connecting to the server”));
if (conn.connect(server_addr, 3306, user, password)) {
delay(1000);
Serial.println(F(“Connected to the server”));
}
else
Serial.println(F(“Server connection failed.”));
}

void loop() {

if(target_table == 0){
get_table_number();
isDone = false;
crosswalkCnt = 0;
}
if(target_table != 0)
{
lineTracing();
}
if(isDone && (target_table != 0))
{
cur_mem->execute(query2); //MySQL에 서빙 완료를 알림
cur.close();
target_table = 0;
}

//pidControl_Hz(Hz, 30); // 30cm/s 속도로 주행
}

void get_table_number()
{
row_values *row = NULL;
target_table = 0;

delay(1000);

cur_mem = new MySQL_Cursor(&conn);
// query를 실행시켜서 MySQL에서 데이터를 읽어옴
cur_mem->execute(query);
column_names *columns = cur_mem->get_columns();

do {
row = cur_mem->get_next_row();
if (row != NULL) {

target_table = atol(row->values[0]);
}
} while (row != NULL);
// 동적 메모리 할당 지우기
delete cur_mem;

// 결과 출력
Serial.print(F(” 서빙테이블 = “));
Serial.println(target_table);

delay(500);
}

 

 motor

/* 프로그램 내용 : DC Motor 컨트롤하는 함수들
*
* MotorA MotorA를 PWM 전압으로 조절하는 함수
* MotorB MotorB를 PWM 전압으로 조절하는 함수
*
* doEncoderA DC모터 엔코더 카운트 하기 위한 함수
* doEncoderB DC모터 엔코더 카운트 하기 위한 함수
* pos2ang 엔코더로부터 회전 각도 구하는 함수
* pos2ang_Hz 주어진 Hz속도로 회전 각도 구하는 함수
*
*/

void MotorA(int dir, int _speed)
{
if (dir == STRAIGHT)
{
analogWrite(MotorA1, 0);
analogWrite(MotorA2, _speed);
}
else
{
analogWrite(MotorA1, 0);
analogWrite(MotorA2, 0);
}
}

void MotorB(int dir, int _speed)
{
if (dir == STRAIGHT)
{
analogWrite(MotorB1, _speed);
analogWrite(MotorB2, 0);
}
else
{
analogWrite(MotorB1, 0);
analogWrite(MotorB2, 0);
}
}

void doEncoderA()
{
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?1:-1;
}

void doEncoderB()
{
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?-1:1;
}

void pos2ang()
{
angle = -encoderPos/(341.2*4) * 360 /180*3.141592 ;
}

 

 PIDControl

void pidControl_Hz(int Hz, double input_v)
{
double t = 0.1;
pos2ang(); // 회전 각도 측정(라디안)
v = 3.15*(angle-angle_pre)/t;
angle_pre = angle;
error=(input_v-v)*Ke;
PControl=Kp*error;
if(IControl <= 4){IControl+=Ki*error*t;} //Windup 방지
//IControl+=Ki*error*t;
DControl=Kd*(error-error_pre)/t;

PIDControl=PControl+IControl+DControl;

pwm_in=255/6*PIDControl;
if(pwm_in>=255){pwm_in=255;}
else if(pwm_in<=60){pwm_in=60;}

Serial.print(“pwm_in: “); Serial.print(pwm_in);
Serial.print(” v:”); Serial.print(v);
Serial.print(” error:”); Serial.print(error/Ke);
Serial.print(” PIDControl: “); Serial.print(PIDControl);
Serial.print(” P:”); Serial.print(PControl);
Serial.print(” I:”); Serial.print(IControl);
Serial.print(” D:”); Serial.println(DControl);

//Serial.println(v);

MotorA(STRAIGHT, pwm_in);
MotorB(STRAIGHT, pwm_in);
//MotorA(STRAIGHT, -50);
//MotorB(STRAIGHT, -50);

//angle_pre = angle;
v_pre = v;
error_pre = error;
}

 

 

 

Leave A Comment

*