[44호]실내주행 로봇 플랫폼과 부착식 위치인식용 데이터 수집 시스템
2017 ICT 융합 프로젝트 공모전 입선작
실내주행 로봇 플랫폼과 부착식 위치인식용
데이터 수집 시스템
글 | 한국과학기술원 김규광, 김재우, 김휘민
심사평
칩센 실내 위치 인식은 로봇 분야의 관심을 넘어 최근 들어 산업 분야 전반에서 관심을 가지고 있는 기술 분야입니다. 작품 개발자들의 의도는 주행 로봇을 이용하여 위치 인식의 기반이 될 데이터 수집을 수행하는 것으로 보이나, 위치 인식은 작품 개발자들도 인지하고 있듯이, 단순한 몇가지 방식의 측량으로 원하는 수준을 이끌어내기 쉽지 않은 것이 사실입니다. 그럼에도 불구하고, 자기장 factor 로그를 이용하여 위치에 따른 차이점이 명확하게 구분되는 것을 결과로 보여준 점은 위치 측량을 위한 하나의 데이터로 사용이 충분히 가능할 것으로 보입니다. 이 데이터를 기반으로 사물 또는 사람의 위치를 실제로 측정할 수 있는지를 확인해 볼수 있는 방법에 대해 조금 고민을 하면 좋을듯 합니다.
뉴티씨 케이스 등 디자인만 수정하고 조금만 다듬으면 바로 제품에 적용해도 될 것 같습니다. 그리고 ‘사용 예’ 같은 것을 제시하면 좀 더 좋은 제품이 될 것 같습니다. 예를 들어, 실내 상호간 전투사격 시뮬레이터, 웨어러블 장비를 통한 HID 인터페이스 등에 추가하는 등의 어플리케이션을 구축할 수 있겠습니다.
위드로봇 localization을 수행하는 것이 아닌, 이에 필요한 데이터를 수집하는 모바일 로봇을 만든 점이 재미있습니다. 단, 이 로봇 또한 맵과 자율 주행 기능이 필요한데, 이 부분에 대한 언급이 있으면 더 좋았을 것 같습니다.
작품 개요
실내 위치 인식은 로봇 분야에서 지속적으로 관심을 받고 연구가 진행되어온 분야이며 로봇의 위치인식 뿐만 아니라 스마트폰 등에 부착된 센서를 이용한 사람의 위치인식 또한 활발히 연구되고 있다. IPIN 등의 실내위치인식을 테마로 한 top tier 국제학회들도 개최되고 있으며 MobiSys 등의 모바일 시스템 컨퍼런스에서도 여러 위치인식 논문들이 발표되고 있다.
실내 위치 인식에는 엔코더를 이용한 간단한 데드레코닝 방법부터 LIDAR 스캐너, Kinect 등을 이용한 pointcloud, 이미지 마커나 NFC/RFID 태그까지 다양한 방법이 사용될 수 있고 이들을 이용한 particle filter 및 SLAM 등의 여러 알고리즘 또한 개발되어 왔다. 이러한 위치인식 방법들 중 Wi-Fi의 신호 세기를 이용한 위치인식 방법이 스마트폰과 Wi-Fi 존의 보급으로 추가적인 인프라를 설치하지 않고도 이를 이용해 위치인식이 가능하다는 점, 특별한 외부 센서 없이 스마트폰에 내장된 시스템만 가지고도 구현이 가능하다는 이점에 힘입어 코엑스 위치인식 앱 등에 적용되기도 하였다.
Wi-Fi를 위시한 전파 비콘의 경우 위치를 아는 전파 발생원으로부터 전파의 세기를 측정하고 이를 이용해 삼각 측량으로 수신자의 위치를 계산하는 방법이 일반적이나 실내 구조물을 통과하면서 발생하는 신호의 감쇄, 복도 및 벽 등에 반사되어서 들어오는 전파, 안테나의 타입과 측정 중 방향 등 여러가지 요인으로 인해 단순 삼각측량으로는 정확한 위치측정이 매우 어렵다. 이러한 문제를 해결하기 위해 RANSAC 알고리즘을 이용하여 전파 신호를 균일화하고 여러 지점에서 다수의 AP의 전파 신호를 측정해 fingerprinting과 기계학습을 하는 방식이 개발되었다.
최근 또 다른 실내위치인식 방법으로 각광받는 것은 자기장 왜곡을 이용한 위치인식이다. 건물은 기본적으로 철골 구조로 만들어지며 전류가 흐르는 전선 등이 복도나 벽에 설치되는 등 자연적으로 측정되는 지구 자기장을 제외하고도 자기장을 발생시키는 물체들이 존재한다. 지자기센서를 이용하여 이러한 자기장 anomaly를 구역별로 수집해 이를 사용해 SLAM을 하는 방식이 제안되었으며 GPS나 Wi-Fi와는 다르게 전파 방해 요인들에 영향 받지 않으며 스마트폰에 내장된 센서로도 측정 가능하다는 장점이 있어 각광받고 있다.
이러한 요소들로 실내 위치 인식을 위해서는 충분히 많은 지점에서 위치인식에 사용할 데이터를 수집하여야 하고 특히 기계학습을 위해서는 지점마다 충분한 데이터를 확보해 주어야 할 필요가 있다. 연구 목적 및 알고리즘 실험용으로는 사람이 로봇 혹은 센서를 들거나 몰고 돌아다니면서 ground truth에 대한 위치 신호를 수집할 수 있으나 실제로 이러한 실내 위치인식을 적용하려 하는 대형 빌딩, 박물관, 전시장 같은 곳은 그 크기로 인해 이러한 수동 데이터 수집이 매우 힘들다.
본 프로젝트에서는 이러한 문제를 해결하기 위해 Wi-Fi 신호와 3축 지자기 센서의 값을 자동 수집하는 센서 logger를 제작했으며 이러한 센서 태그를 사람이 직접 들고 다니는 것이 아닌 넓은 실내 시설의 내부 청소에 사용되는 로봇 청소기 및 청소 카트 등의 시스템에 부착한다. 또한 이러한 로봇/이동형 플랫폼들이 청소 작업을 위해 시설 내부를 돌아다니면서 위치인식에 필요한 데이터를 자연스럽게 수집할 수 있는 방식을 제안하고자 한다. 로봇청소기에 부착 가능하며 해당 로봇의 엔코더에 연결되는 개조 키트를 개발하였다. 또한 굳이 청소로봇에 부착하지 않더라도 센서와 함께 자율주행이 가능한 로봇의 경우 이러한 데이터 수집에 사용될 수 있어 호환 가능한 이동형 플랫폼 또한 제작하였다.
작품 설명
주요 동작 및 특징
본 프로젝트에서 제출하는 작품은 크게 세 부분으로 구성되어 있다.
A. 로봇청소기용 모터를 갖추고 비슷한 형체를 가진 실제 로봇청소기를 모사한 형태의 이동형 플랫폼
B. 지자기센서, Wi-Fi 모듈과 SD 카드 리더기가 부착되어 위치인식용 센서로 데이터를 logging하는 센서 태그
C. 이동형 플랫폼과 지그비 통신을 통해 이를 무선 제어하는 아두이노 esplora 기반 조종기
이동식 로봇청소기 플랫폼
로봇청소기 플랫폼은 탐색하려는 공간을 원활히 돌아다니며, 원하는 데이터를 최대한 수집하는 것을 목표로 한다. 이를 위해서는 무선 통신으로 조종이 가능하며 안정적인 주행 능력이 요구된다.
로봇의 주 제어장치는 ARM Cortex-M3 기반의 Arduino Due를 사용하였으며 센서와 하드웨어 인터페이스를 위해 Arduino Uno를 부착하였다. 복원력을 가지는 바퀴를 이용해 문턱 등의 장애물을 손쉽게 넘어갈 수 있으며 모터에 지자기 센서와 모터에 부착된 엔코더를 사용해 현재 위치를 계산한다. 엔코더를 읽기 위한 카운터는 LSI/CSI 사의 LS7366을 사용하였다. 로봇의 모터는 아두이노 부트로더가 내장된 Atmega328p 칩과 Toshiba 사의 TA7291P 모터 드라이버를 사용하여 제어된다.
무선 통신을 위해 XBEE radio module로 통신하는 방법을 택하였다. 통신 알고리즘을 간단히 소개하자면 다음과 같다. 송신기 역할을 하는 Arduino Esplora에서는 100ms 간격으로 눌린 버튼의 종류를 검사하여 신호를 보낸다. 전진, 좌회전, 우회전, 또는 아무 것도 눌리지 않은 경우에는 정지 신호를 보낸다. 로봇청소기 플랫폼에서 신호를 수신하면, Arduino Due 는 UART 버퍼에 들어 있는 마지막으로 수신된 데이터를 읽고 버퍼를 모두 비운다. (이는 다른 명령을 처리하던 중 들어온 데이터 중 가장 최근의 명령을 사용하기 위함이다.) 그리고, 100ms 간격으로 엔코더의 상태를 비교하여 바퀴의 속도를 측정한다. 이렇게 측정된 각 바퀴의 실제 구동 속도, 원하는 바퀴의 구동 속도, 동작의 종류를 I2C 인터페이스를 통해 모터를 제어하는 Atmega328 칩으로 전송한다.
구동 명령과 바퀴의 실제 구동 속도를 받은 Atmega328은 PID 제어를 통해 바퀴의 실제 구동 속도가 이상적인 구동 속도에 맞도록 제어한다. 여기서 PID 제어를 사용하는 이유는 양쪽 바퀴의 약간의 출력 차이나 무게 배분의 차이로 인해 로봇이 한 쪽으로 불균형적으로 움직이는 현상을 최소화하기 위함이다.
전원은 12000mAh의 Li-ion 배터리를 사용하였으며, LM7805와 DC-DC 모듈로 전자 회로를 제어한다. 모터는 배터리 전원을 그대로 사용해 구동하였다.
데이터 logging 모듈
데이터 logging 모듈은 아두이노를 기반으로 지자기센서, Wi-Fi 쉴드를 사용하여 제작되었다. 지자기센서는 I2C 통신으로 3축 지자기 값을 받아오며 단위는 Gauss이다. Wi-Fi 쉴드는 Wi-Fi AP 스캔과 SD카드 데이터 저장에 사용된다. Wi-Fi 모듈과 SD카드는 SPI 통신을 이용한다. Wi-Fi AP 스캔 프로그램은 아두이노 예제 중에 ScanNetworks를 변형하여 작성하였고, SD카드 라이브러리를 이용하여 데이터를 저장하였다.
아두이노는 지자기센서와 Wi-Fi모듈의 데이터를 받아 아래 그림과 같은 Log_data 형태로 시간과 함께 데이터를 저장한다. 추가적으로 PCInt(Pin Change Interrupt)처리로 A, B상 Pulse Encoder 값을 측정하여 누적값을 기록한다. 바퀴를 PID 제어하는 데에 사용했던 엔코더 카운터와는 별도로 엔코더 신호를 측정한다. 사용한 지자기센서 모듈인 GY-86의 경우 MPU6050의 자체 I2C 포트에 지자기센서가 연결되어 있고 MPU6050의 I2C가 pinout으로 연결되어서 mpu6050의 bypass 모드를 활성화시켜준 후 지자기센서 HMC5883L의 데이터를 요청하는 방법으로 구현하였다.
해당 모듈을 제작된 로봇청소기에 장착했으며 로봇청소기의 엔코더 모듈에서 신호를 bypass시켜 로봇과 부착된 수집 모듈 둘다 독립적으로 구동이 가능하게끔 제작하였다.
엔코더를 이용한 간단한 주행 실험과 이동 궤적 획득, 그리고 수집된 자기장과 전파 신호를 이용한 fingerprinting 기반 위치인식 실험 또한 진행하였다. 실험의 경우 건물 복도에서 일정 간격의 바닥 타일을 기준점 삼아 L자 형태의 궤적을 잡고 이를 따라 주행하며 엔코더 값이 정상적으로 획득되는지를 확인하였다.
자기장/전파 기반 위치인식 실험의 경우 로봇을 같은 방향을 바라보게 한 다음 L자 궤적 내 5개 지점을 선별하여 각각의 지점으로 이동시켜 데이터를 수집한 후 신경망(Multilayer Perceptron)을 사용하여 데이터를 학습시켜 위치인식을 하는 방식으로 진행하였다. 신경망의 경우 최근 다층 hidden layer와 입력 부분에 복층의 convolution/pooling 구조를 가지는 이미지 분류용 합성곱신경망(CNN), 혹은 음성 데이터 등의 연속 데이터 등을 처리하기 위한 RNN으로 대표되는 딥 러닝이 주로 사용되고 있으나 샘플 수가 적고 입력 데이터가 이미지나 연속 신호가 아닌 독립 항목인 점, 그리고 학습에 필요한 연산 속도 등의 여러 편의성으로 인해 기존의 1개 hidden layer를 가지는 구식 신경망(MLP)를 사용하였다.
딥 러닝 이전 기계학습과 분류에는 주로 Support Vector Machine이 사용되었으나 기본적으로 이진 분류기라는 특성이 있어, 비록 여러 성능상의 단점이 제기되어 왔으나 가장 간편한 multiclass 분류기라는 장점이 있는 신경망을 사용하였다. Python으로 tanh 함수를 activation function으로 사용하는 신경망을 제작했으며 입력단의 경우 최소 3개 (3축 지자기)에 wifi 신호당 하나의 node를 더 추가했고 hidden layer의 경우 30개, output의 경우 5개 위치를 구분하기를 원하므로 5개의 hidden layer를 달아 주었다.
Learning rate의 경우 0.001로 설정하였으며 약 1000회의 iteration을 거쳐 forward propagation, softmax를 이용한 확률 계산, 학습 데이터셋의 답안과의 오차 계산과 해당 오차의 back propagation을 진행하며 신경망 내 행렬들의 가중치와 bias를 갱신하였다. 지자기 데이터가 각 구역별로 특징이 워낙 뚜렷해 지속적인 갱신 시 Wi-Fi 신호의 가중치가 사실상 0으로 수렴할 것이 예상되어 기본적인 성능 테스트는 지자기센서의 값으로만 진행하였다. Training iteration을 반복함으로써 loss function의 출력이 점점 감소하는 것을 확인하였고 overfitting 여부를 확인하기 위해 training set이 아닌 각 구역과 비슷한 값의 지자기 값을 생성하여 test set으로 사용했으며 100%의 정답률로 구분해 내는 것을 확인하였다.
비록 안테나 방향과 XYZ 축 문제로 방향을 고정시킨 상태에서 수집한 데이터지만 SLAM 알고리즘 등을 사용하여 자기장 map을 만들고 3축 벡터의 norm으로 측정하는 등의 sensor fusion 알고리즘을 더한다면 더 좋은 위치인식이 가능할 것으로 기대된다. 위치인식 데이터를 자동 수집해주는 모듈이 본 프로젝트의 중점이므로 이를 위한 간단한 구별 데모를 보였다.
개발 환경
아두이노와 아두이노 IDE를 사용하였으며 신경망 코드는 x86 컴퓨터에서 Python과 numpy Python 라이브러리를 사용하여 제작하였다.
소스코드
로봇 주행 코드
//#include <PID_AutoTune_v0.h>
#include <PID_v1.h>
#include<Wire.h>
#include<EEPROM.h>
boolean tuningL=true,tuningR=true;
byte valL,valR;
double Setpoint, InputL, OutputL,InputR,OutputR;
PID myPID(&InputL, &OutputL, &Setpoint,0.8,1.0,0, DIRECT);
PID mePID(&InputR,&OutputR,&Setpoint,0.8,1.0,0,DIRECT);
/*PID_ATune AmyPID(&InputL, &OutputL);
PID_ATune AmePID(&InputR, &OutputR);*/
const float robotradius=1;
const float transfactor=0.5;
float rightspeed,leftspeed;
byte Mode;
byte Speed,angleSpeed;
boolean FB;
long timeoutbefore,timeoutafter;
byte Hradius,Lradius; uint16_t radius;
float rpm, leftrpm, rightrpm; float t;
float leftvalue, rightvalue;
float value; byte acceleration;
void setup() {
Serial.begin(9600);
Serial.print(“Serial begin\n”);
pinMode(13,OUTPUT);//busy flag LED
pinMode(12,OUTPUT);//busy flag signal
pinMode(8,OUTPUT);//timeout flag detection
digitalWrite(12,HIGH);// busy flag enabled
digitalWrite(13,HIGH);//busy led on
digitalWrite(8,HIGH);//error led off
Wire.begin(0x0A);//Adress of motor controller MCU
Wire.onReceive(receiveEvent);
myPID.SetMode(AUTOMATIC);
mePID.SetMode(AUTOMATIC);
myPID.SetControllerDirection(DIRECT);
mePID.SetControllerDirection(DIRECT);
/*AmyPID.SetOutputStep(20);
AmePID.SetOutputStep(20);
AmyPID.SetControlType(1);
AmePID.SetControlType(1);
AmyPID.SetNoiseBand(2);
AmePID.SetNoiseBand(2);
AmyPID.SetLookbackSec(2);
AmePID.SetLookbackSec(2);
*/
myPID.SetOutputLimits(50,95);
mePID.SetOutputLimits(30,85);
leftspeed=0; rightspeed=0; Speed=0; angleSpeed=0; FB=true;
timeoutbefore=millis();//previous value of millis for timeout detection
Mode=0×00;
digitalWrite(13,LOW); digitalWrite(12,LOW); digitalWrite(8,HIGH);
}
void loop() {
if(Wire.available()<=0){// if there’s no signal from i2c bus
//Serial.print(“ready\n”);
//delay(10);
}
timeoutafter=millis();
/*
Motor stops automatically when there is no signal certain period
*/
if(timeoutafter-timeoutbefore>300){//0.3seconds before stop
analogWrite(11,0); analogWrite(5,0);
analogWrite(10,0); analogWrite(6,0);
digitalWrite(8,LOW);//for timeout error indication //Serial.print(“Timeout\n”);
leftspeed=0; rightspeed=0; Speed=0; angleSpeed=0;
}
if(Mode!=0×00){// receiveEvent function changes Mode
digitalWrite(13,HIGH);//busy flag indication by LED
digitalWrite(12,HIGH);//busy flag
// Serial.print(“busy flag set\n”);
// Serial.print(“Mode:”); Serial.println(Mode,HEX); Serial.print(“\n”);
digitalWrite(8,HIGH);//activated LOW
if(Wire.available()>0){ // Mode=Wire.read();
///
if(Mode==0×01){//forward mode with accleration
Serial.print(“forward Mode\n”);
Speed=Wire.read();
acceleration=Wire.read();
Serial.print(Speed);
Serial.print(“\n”);
Serial.print(acceleration);
Serial.print(“\n”);
leftspeed=Speed; rightspeed=Speed; FB=true;
rpm=transfactor*Speed;//speed to rpm
t=Speed/acceleration; t=t*1000;//seconds to milliseconds
analogWrite(10,0); analogWrite(5,0); //drives motor forward
value=0;
Serial.print(“rpm:”);
Serial.print(rpm/t);
Serial.print(“\n”);
while(rpm>=value){//accleration code
Serial.print(value);
Serial.print(“\n”);
analogWrite(11,value);//attatched to motor drivers
analogWrite(6,value);
value+=(rpm/t);
delay(1);
}
}
///
if(Mode==0×02){//backward mode with accleration
Speed=Wire.read();
acceleration=Wire.read();
leftspeed=Speed; rightspeed=Speed; FB=false;
rpm=transfactor*Speed;//speed to rpm
t=Speed/acceleration; t=t*1000;//seconds to milliseconds
analogWrite(11,0); analogWrite(6,0); //drives motor backward
value=0;
while(rpm>=value){//accleration code
analogWrite(10,value);//attatched to motor drivers
analogWrite(5,value);
value+=(rpm/t);
delay(1);
}
}
///
if(Mode==0×03){//robot moves on a circle anticlockwise with accleration
Hradius=Wire.read();
Lradius=Wire.read();
radius=(Hradius<<8)|Lradius;
Speed=Wire.read();
acceleration=Wire.read(); FB=true;
t=Speed/acceleration; t=t*1000;//get time value required to acclerate to appropriate speed
rightspeed=(1+robotradius/radius)*Speed; rightrpm=transfactor*rightspeed;
leftspeed=(1-robotradius/radius)*Speed; leftrpm=transfactor*leftspeed;
analogWrite(10,0); analogWrite(5,0);
leftvalue=0; rightvalue=0;
while((leftrpm>=leftvalue)&&(rightrpm>=rightvalue)){
analogWrite(11,leftvalue);
analogWrite(6,rightvalue);
leftvalue+=(leftrpm/t);
rightvalue+=(rightrpm/t);
delay(1);
}
}
///
if(Mode==0×04){//robot moves on a circle clockwise with accleration
Hradius=Wire.read();
Lradius=Wire.read();
radius=(Hradius<<8)|Lradius;
Serial.print(radius);
Speed=Wire.read();
acceleration=Wire.read(); FB=true;
t=Speed/acceleration; t=t*1000;//get time value required to acclerateto appropriate speed
leftspeed=(1+robotradius/radius)*Speed; leftrpm=transfactor*leftspeed;
rightspeed=(1-robotradius/radius)*Speed; rightrpm=transfactor*rightspeed;
analogWrite(10,0); analogWrite(5,0);
leftvalue=0;rightvalue=0;
while((leftrpm>=leftvalue)&&(rightrpm>=rightvalue)){
analogWrite(11,leftvalue);
analogWrite(6,rightvalue);
leftvalue+=(leftrpm/t);
rightvalue+=(rightrpm/t);
delay(1);
}
}
///
if(Mode==0×05){//motor stops slowly with acceleration value
acceleration=Wire.read();
t=Speed/acceleration; t=t*1000;
leftrpm=leftspeed*transfactor; rightrpm=rightspeed*transfactor;
Serial.print(“leftrpm: “); Serial.print(leftrpm);
Serial.print(“rightrpm: “); Serial.print(rightrpm);
if(FB==true){analogWrite(10,0); analogWrite(5,0);}else{
analogWrite(11,0); analogWrite(6,0);
}
float r=rightrpm; float l=leftrpm;
while(leftrpm!=0||rightrpm!=0){
leftrpm=leftrpm-(l/t);
rightrpm=rightrpm-(r/t);
if(leftrpm<=0)leftrpm=0;
if(rightrpm<=0)rightrpm=0;
/* Serial.print(“leftrpm: “); Serial.print(leftrpm);
Serial.print(“rightrpm: “); Serial.print(rightrpm); */
if(FB==true){analogWrite(11,leftrpm); analogWrite(6,rightrpm);}else{
analogWrite(10,leftrpm); analogWrite(5,rightrpm);}
delay(1);
}
}
///
if(Mode==0×06){//go forward
Speed=Wire.read();
Serial.print(“\nSpeed:”);
Serial.print(Speed);
byte Currentleftspeed=Wire.read();
byte Currentrightspeed=Wire.read();
Serial.print(“\nCurrentleftspeed:”);
Serial.print(Currentleftspeed);
Serial.print(“\nCurrentrightspeed:”);
Serial.print(Currentrightspeed);
//if(abs(Setpoint-Currentleftspeed)>=70){
// myPID.SetTunings(0.8,1.5,0); mePID.SetTunings(0.8,1.5,0);
//}else{
myPID.SetTunings(0.45,0.6,0); mePID.SetTunings(0.23,0.6,0);
//}
Setpoint=Speed;
InputL=Currentleftspeed;
/* if(tuningL){ valL=(AmyPID.Runtime());}
if(valL!=0){
tuningL=false;
}*/
myPID.Compute();
leftrpm=OutputL;
Serial.print(“leftrpm:”);
Serial.print(leftrpm);
InputR=Currentrightspeed;
/*if(tuningL){ valR=(AmyPID.Runtime());}
if(valR!=0){
tuningL=false;
}*/
mePID.Compute();
/* if(tuningL==false&&tuningR==false){
double pR=AmyPID.GetKp(); double iR=AmyPID.GetKi(); double dR=AmyPID.GetKd();
double pL=AmePID.GetKp(); double iL=AmePID.GetKi(); double dL=AmePID.GetKd();
Serial.print(“\npR:”);Serial.print(pR); Serial.print(” iR:”); Serial.print(iR); Serial.print(” dR:”);Serial.print(dR);
Serial.print(“\npL:”);Serial.print(pL); Serial.print(” iL:”);Serial.print(iL); Serial.print(” dL:”);Serial.print(dL);
while(true);
}*/
rightrpm=OutputR+5;
Serial.print(” rightrpm:”);
Serial.print(rightrpm);
leftspeed=Speed; rightspeed=Speed;
analogWrite(10,0); analogWrite(5,0);
analogWrite(11,rightrpm); analogWrite(6,leftrpm);
}
///
if(Mode==0×07){//go backward
Speed=Wire.read();
rpm=transfactor*Speed;
leftspeed=Speed; rightspeed=Speed;
analogWrite(11,0); analogWrite(6,0);
analogWrite(10,rpm); analogWrite(5,rpm);
}
///
if(Mode==0×08){//turn on the circle anticlockwise
Hradius=Wire.read();
Lradius=Wire.read();
radius=(Hradius<<8)|Lradius;
Speed=Wire.read();
rightspeed=(1+robotradius/radius)*Speed; rightrpm=transfactor*rightspeed;
leftspeed=(1-robotradius/radius)*Speed; leftrpm=transfactor*leftspeed;
// Serial.print(rightspeed);
//Serial.print(leftspeed);
analogWrite(10,0); analogWrite(5,0);
analogWrite(11,leftrpm); analogWrite(6,rightrpm);
}
///
if(Mode==0×09){//turn on the circle clockwise
Hradius=Wire.read();
Lradius=Wire.read();
radius=(Hradius<<8)|Lradius;
Speed=Wire.read();
leftspeed=(1+robotradius/radius)*Speed; leftrpm=transfactor*leftspeed;
rightspeed=(1-robotradius/radius)*Speed; rightrpm=transfactor*rightspeed;
analogWrite(10,0); analogWrite(5,0);
analogWrite(11,leftrpm); analogWrite(6,rightrpm);
}
///
if(Mode==0x0B){//robot turns right
angleSpeed=Wire.read();
rightspeed=angleSpeed*robotradius; rightrpm=transfactor*rightspeed;
leftspeed=angleSpeed*robotradius; leftrpm=transfactor*leftspeed;
Serial.print(“rightspeed: “);
Serial.print(rightspeed);
Serial.print(“leftspeed: “);
Serial.print(leftspeed);
analogWrite(10,0); analogWrite(6,0);
analogWrite(11,leftrpm); analogWrite(5,rightrpm);
}
///
if(Mode==0x0C){//robot turns left
angleSpeed=Wire.read();
rightspeed=angleSpeed*robotradius; rightrpm=transfactor*rightspeed;
leftspeed=angleSpeed*robotradius; leftrpm=transfactor*leftspeed;
analogWrite(11,0); analogWrite(5,0);
analogWrite(10,leftrpm); analogWrite(6,rightrpm);
}
///
if(Mode==0x0D){//motor stops immediately
Speed=Wire.read();
leftspeed=Speed; rightspeed=Speed;
analogWrite(11,255); analogWrite(5,255);
analogWrite(10,255); analogWrite(6,255);
}
///
if(Mode==0x0E){//motor stops normally
Speed=Wire.read();
leftspeed=Speed; rightspeed=Speed;
analogWrite(11,0); analogWrite(5,0);
analogWrite(10,0); analogWrite(6,0);
}
///
if(Mode==0x0F){
byte afterSpeed=Wire.read();
acceleration=Wire.read();
byte afterleftspeed=afterSpeed; byte afterrightspeed=afterSpeed;
byte afterleftrpm=afterleftspeed*transfactor; byte afterrightrpm=afterrightspeed*transfactor;
leftrpm=leftspeed*transfactor; rightrpm=rightspeed*transfactor;
int L=abs(leftrpm-afterleftrpm); int R=abs(rightrpm-afterrightrpm);
t=abs(Speed-afterSpeed)/acceleration; t=t*1000;
if(FB==true){analogWrite(10,0); analogWrite(5,0);}else{
analogWrite(11,0); analogWrite(6,0);}
while(leftrpm>afterleftrpm || rightrpm>afterrightrpm){
if(FB==true){analogWrite(11,leftrpm); analogWrite(6,rightrpm);}else{
analogWrite(10,leftrpm); analogWrite(5,rightrpm);}
leftrpm=leftrpm-(L/t); rightrpm=rightrpm-(R/t);
delay(1);
}
while(leftrpm<afterleftrpm || rightrpm<afterrightrpm){
if(FB==true){analogWrite(11,leftrpm); analogWrite(6,rightrpm);}else{
analogWrite(10,leftrpm); analogWrite(5,rightrpm);}
leftrpm=leftrpm+(L/t); rightrpm=rightrpm+(R/t);
delay(1);
}
if(FB==true){analogWrite(11,leftrpm); analogWrite(6,rightrpm);}else{
analogWrite(10,leftrpm); analogWrite(5,rightrpm);}
}
///
if(Mode==0×10){
byte rpm=Wire.read();
analogWrite(9,rpm);
}
///
if(Mode==0×11){
byte brush=Wire.read();
analogWrite(3,brush);
}
}
timeoutbefore=millis();
Mode=0×00;
//Serial.print(“busy flag cleared\n”);
delay(40);
digitalWrite(13,LOW);digitalWrite(12,LOW);
}
}
void receiveEvent(int howmany){
digitalWrite(12,HIGH);
Mode=Wire.read();
}
Arduino esplora 기반 조종기 코드
#include<Esplora.h>
const byte wait=150;
void setup() { // put your setup code here, to run once:
Serial1.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
//Serial1.write(104);
if(Esplora.readButton(SWITCH_UP)==LOW)
{
Serial1.write(0xA0);delay(wait);//forward
}else if(Esplora.readButton(SWITCH_RIGHT)==LOW)
{
Serial1.write(0xB0);delay(wait);//turn right while staying still
}else if(Esplora.readButton(SWITCH_LEFT)==LOW)
{
Serial1.write(0xC0);delay(wait);//turn left while staying still
}else if(Esplora.readButton(SWITCH_DOWN)==LOW)
{
Serial1.write(0xD0);delay(wait);// go forward for 30cm
}else
{
Serial1.write(0xE0);delay(wait);Serial.print(“E0\n”);//stops if none of the buttons are pressed
}
}
Arduino esplora 기반 조종기 코드
#include <SPI.h>
#include <WiFi.h>
#include <SD.h>
#include <Wire.h>
#include “MPU6050.h”
#include “HMC5883L.h”
MPU6050 mpu;
HMC5883L compass;
const int chipSelect = 4;
long encoder_tick;
long encoder_tick2;
File dataFile;
void log_data(String str);
//———————————————-//
void setup() { //Initialize serial and wait for port to open:
Serial.begin(9600);
Wire.begin();
while (!Serial) {
; // wait for serial port to connect. Needed for native USB port only
} // check for the presence of the shield:
if (WiFi.status() == WL_NO_SHIELD) {
Serial.println(“WiFi shield not present”); // don’t continue:
while (true);
}
String fv = WiFi.firmwareVersion();
if (fv != “1.1.0″) {
Serial.println(“Please upgrade the firmware”);
}
Serial.print(“Initializing SD card…”);
// see if the card is present and can be initialized:
if (!SD.begin(chipSelect)) {
Serial.println(“Card failed, or not present”); // don’t do anything more:
return;
}
Serial.println(“card initialized.”);
File dataFile = SD.open(“datalog.txt”, FILE_WRITE);
if (dataFile) {
dataFile.println(“”);
dataFile.close();
}
else {
Serial.println(“error opening datalog.txt”);
return;
}
// Sensor init //
mpu.init();
mpu.setMasterEnabled(0);
mpu.setBypassEnabled(1);
compass.init();
pinMode(A0, INPUT_PULLUP);
pinMode(A1, INPUT_PULLUP);
pinMode(A2, INPUT_PULLUP);
pinMode(A3, INPUT_PULLUP); //Encoder PinChangeInterrupt Setting//
PCMSK1 |= 0b00001111; // pin A0, A1 set interrupt
PCICR |= 0b00000010; // PortC set interrupt
sei();
}
void loop() {
long timer;
compass.update();
int numSsid = WiFi.scanNetworks();
if (numSsid == -1) {
Serial.println(“Couldn’t get a wifi connection”);
while (true);
}
dataFile = SD.open(“datalog.txt”, FILE_WRITE);
if (!dataFile) Serial.println(“error opening datalog.txt”);
timer = micros();
log_data(String(timer));
log_data(String(‘,’));
log_data(String(numSsid));
for (int thisNet = 0; thisNet < numSsid; thisNet++) {
log_data(String(‘,’));
log_data(String(WiFi.SSID(thisNet)));
log_data(String(‘,’));
log_data(String(WiFi.RSSI(thisNet)));
}
log_data(String(‘,’));
log_data(String(compass.getCompassX(), 2));
log_data(String(‘,’));
log_data(String(compass.getCompassY(), 2));
log_data(String(‘,’));
log_data(String(compass.getCompassZ(), 2));
log_data(String(‘,’));
log_data(String(encoder_tick));
log_data(String(‘,’));
log_data(String(encoder_tick2));
log_data(String(“\r\n”));
dataFile.close();
}
//———————————————–/
void log_data(String str) {
dataFile.print(str);
Serial.print(str);
}
//———————————————-//
ISR(PCINT1_vect) {
static uint8_t preMask = 0×00000011;
static uint8_t preMask2 = 0×00000011;
uint8_t mask;
uint8_t mask2;
//Encoder//
mask = (PINC >> 0) & 0b00000011;
if (mask != preMask) {
switch (preMask) {
case 0:
if (mask == 0×01) encoder_tick–;
else if (mask == 0×02) encoder_tick++;
break;
case 1:
if (mask == 0×03) encoder_tick–;
else if (mask == 0×00) encoder_tick++;
break;
case 2:
if (mask == 0×00) encoder_tick–;
else if (mask == 0×03) encoder_tick++;
break;
case 3:
if (mask == 0×02) encoder_tick–;
else if (mask == 0×01) encoder_tick++;
break;
}
preMask = mask;
}
mask2 = (PINC >> 2) & 0b00000011;
if (mask2 != preMask2) {
switch (preMask2) {
case 0:
if (mask2 == 0×01) encoder_tick2–;
else if (mask2 == 0×02) encoder_tick2++;
break;
case 1:
if (mask2 == 0×03) encoder_tick2–;
else if (mask2 == 0×00) encoder_tick2++;
break;
case 2:
if (mask2 == 0×00) encoder_tick2–;
else if (mask2 == 0×03) encoder_tick2++;
break;
case 3:
if (mask2 == 0×02) encoder_tick2–;
else if (mask2 == 0×01) encoder_tick2++;
break;
}
preMask2 = mask2;
}
}
