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