> SDA; blue line >> SCL PRIZM prizm; EXPANSION exc1; EXPANSION exc2; #define RED 1 #define GREEN 2 #define BLUE 3 #define YELLOW 4 #define TRUE 1 #define FALSE 0 // 0 = 첫번째줄 / 1 = 두번째줄 / 2 = 세번째줄 / 3 = 네번째줄 / 4 = 다섯번째줄 // 현재 자신이 있는 줄 int currentLine = 0; // 가운데 라인 기준 왼쪽 오른쪽 // 0 = 왼쪽 1 = 오른쪽 int currentLineFlag = 0; // 처음 기둥 위치 // 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3, 노란색 = 4 int columnBlock[5][2] = { {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}}; //한번 결정되면 그 자리 고정 // 오브젝트의 위치 // 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3 int objectBlock[5][2] = { {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}}; // 적재해야하는 오브젝트의 색 배열 // 빨간색 = 1, 초록색 = 2, 파란색 = 3 int objectColumnFlag[4] = {0, 0, 0, 0}; // 위 배열의 인덱스 int objectFlagCount = 0; // 현재 잡고 있는 오브젝트의 색깔을 저장할 변수 int objectFlag = 0; "> > SDA; blue line >> SCL PRIZM prizm; EXPANSION exc1; EXPANSION exc2; #define RED 1 #define GREEN 2 #define BLUE 3 #define YELLOW 4 #define TRUE 1 #define FALSE 0 // 0 = 첫번째줄 / 1 = 두번째줄 / 2 = 세번째줄 / 3 = 네번째줄 / 4 = 다섯번째줄 // 현재 자신이 있는 줄 int currentLine = 0; // 가운데 라인 기준 왼쪽 오른쪽 // 0 = 왼쪽 1 = 오른쪽 int currentLineFlag = 0; // 처음 기둥 위치 // 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3, 노란색 = 4 int columnBlock[5][2] = { {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}}; //한번 결정되면 그 자리 고정 // 오브젝트의 위치 // 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3 int objectBlock[5][2] = { {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}}; // 적재해야하는 오브젝트의 색 배열 // 빨간색 = 1, 초록색 = 2, 파란색 = 3 int objectColumnFlag[4] = {0, 0, 0, 0}; // 위 배열의 인덱스 int objectFlagCount = 0; // 현재 잡고 있는 오브젝트의 색깔을 저장할 변수 int objectFlag = 0; "> > SDA; blue line >> SCL PRIZM prizm; EXPANSION exc1; EXPANSION exc2; #define RED 1 #define GREEN 2 #define BLUE 3 #define YELLOW 4 #define TRUE 1 #define FALSE 0 // 0 = 첫번째줄 / 1 = 두번째줄 / 2 = 세번째줄 / 3 = 네번째줄 / 4 = 다섯번째줄 // 현재 자신이 있는 줄 int currentLine = 0; // 가운데 라인 기준 왼쪽 오른쪽 // 0 = 왼쪽 1 = 오른쪽 int currentLineFlag = 0; // 처음 기둥 위치 // 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3, 노란색 = 4 int columnBlock[5][2] = { {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}}; //한번 결정되면 그 자리 고정 // 오브젝트의 위치 // 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3 int objectBlock[5][2] = { {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}}; // 적재해야하는 오브젝트의 색 배열 // 빨간색 = 1, 초록색 = 2, 파란색 = 3 int objectColumnFlag[4] = {0, 0, 0, 0}; // 위 배열의 인덱스 int objectFlagCount = 0; // 현재 잡고 있는 오브젝트의 색깔을 저장할 변수 int objectFlag = 0; ">
/**
* 허스키렌즈 학습 순서 : 빨간색(1), 초록색(2), 파란색(3), 노란색(4)
*/
#include <PRIZM.h>
#include <stdio.h>
#include "HUSKYLENS.h"
#include "SoftwareSerial.h"
HUSKYLENS huskylens;
// HUSKYLENS green line >> SDA; blue line >> SCL
PRIZM prizm;
EXPANSION exc1;
EXPANSION exc2;
#define RED 1
#define GREEN 2
#define BLUE 3
#define YELLOW 4
#define TRUE 1
#define FALSE 0
// 0 = 첫번째줄 / 1 = 두번째줄 / 2 = 세번째줄 / 3 = 네번째줄 / 4 = 다섯번째줄
// 현재 자신이 있는 줄
int currentLine = 0;
// 가운데 라인 기준 왼쪽 오른쪽
// 0 = 왼쪽 1 = 오른쪽
int currentLineFlag = 0;
// 처음 기둥 위치
// 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3, 노란색 = 4
int columnBlock[5][2] = {
{0, 0},
{0, 0},
{0, 0},
{0, 0},
{0, 0}}; //한번 결정되면 그 자리 고정
// 오브젝트의 위치
// 없으면 = 0, 빨간색 = 1, 초록색 = 2, 파란색 = 3
int objectBlock[5][2] = {
{0, 0},
{0, 0},
{0, 0},
{0, 0},
{0, 0}};
// 적재해야하는 오브젝트의 색 배열
// 빨간색 = 1, 초록색 = 2, 파란색 = 3
int objectColumnFlag[4] = {0, 0, 0, 0};
// 위 배열의 인덱스
int objectFlagCount = 0;
// 현재 잡고 있는 오브젝트의 색깔을 저장할 변수
int objectFlag = 0;
// 배열에 저장된 기둥과 오브젝트의 합계
int columnCount = 0, objectCount = 0;
// 아날로그 센서의 차이값
int diff = 0;
void setup()
{
prizm.PrizmBegin();
prizm.resetEncoder(1);
Wire.begin();
// 허스키렌즈 연결될 때까지 로깅
while (!huskylens.begin(Wire))
{
Serial.println(F("Begin failed!"));
Serial.println(F("1.Please recheck the \\"Protocol Type\\" in HUSKYLENS (General Settings>>Protocol Type>>I2C)"));
Serial.println(F("2.Please recheck the connection."));
delay(100);
}
Serial.println(F("허스키렌즈 연결 완료"));
Serial.begin(115200);
}
void loop()
{
start();
scanAll();
findFirstColumn(); // 노란색 기둥을 제외한 가장 앞 쪽 기둥 탐색 후 이동
columnStack(); // 바로 앞에 있는 기둥의 색을 저장해놓기
objectLiftup(); // 초음파센서 이용해서 거리 조절하고 리프트업 하고 백함수
findYellowColumn(); // 노란색 기둥을 향해 이동
objectLiftdown(); // 초음파센서 이용해서 거리 조절하고 리프트다운 하고 백함수
for (int i = 0; i < 3; i++)
{
Serial.println("for문");
flagColorLine(objectColumnFlag[i]); // 처음기둥 색과 맞는 오브젝트를 찾아서 이동 //
Serial.println("1-1");
columnStack(); // 현재 서 있는 라인과 방향의 기둥색 가져오기
Serial.println("1-2");
objectLiftup(); // 초음파센서 이용해서 거리 조절하고 리프트업 하고 백함수
Serial.println("1-3");
findTargetColumn(objectFlag); // 잡고있는 오브젝트와 같은 색의 기둥을 찾아서 이동 //
Serial.println("1-4");
objectLiftdown(); // 초음파센서 이용해서 거리 조절하고 리프트다운 하고 백함수
Serial.println("1-5");
}
finish(currentLineFlag);
prizm.PrizmEnd();
}
/**
* @param x +좌 / -우
* @param y +후 / -전
* @param z +우회전 / -좌회전
*
* @brief
* 은쪽이
* 전직진 (0, -40, 0)
* 후직진 (0, 40, 0)
* 좌직진 (40, 0, -2)
* 우직진 (-40, 0, 2)
*
* 금쪽이
* 전직진 (0, -40, 2)
* 후직진 (0, 40, -1)
* 좌직진 (40, 0, -1)
* 우직진 (-40, 0, 1)
*/
void wheel(int x, int y, int z)
{
int A = 0, B = 0, C = 0, D = 0;
A = (x * 0.5) + (y * 0.5) + (z * 0.841471);
B = (x * 0.5 * -1) + (y * 0.5) + (z * 0.841471);
C = (x * 0.5 * -1) + (y * 0.5 * -1) + (z * 0.841471);
D = (x * 0.5) + (y * 0.5 * -1) + (z * 0.841471);
exc1.setMotorPowers(1, A, B);
exc2.setMotorPowers(2, C, D);
}
/**
* @brief 시작지점 -> 경기장 이동하는 코드
*/
void start()
{
setDiff();
startGrab();
wheel(-80, -80, 0);
delay(1000);
wheel(-50, 0, 0);
delay(500);
bool D2;
// 줄 만날 때까지 우직진
while (true)
{
D2 = prizm.readLineSensor(2);
if (D2)
{
delay(300);
break;
}
}
// 교차로 만날 때까지 직진
wheel(0, -40, 2);
while (true)
{
D2 = prizm.readLineSensor(2);
if (D2 == true)
{
wheel(0, 0, 0);
return;
}
}
}
/**
* @brief 경기장 -> 끝지점 이동하는 코드
*/
void finish(int currentLineFlag)
{
directionFind(currentLine, 0, currentLineFlag, currentLineFlag);
if (currentLineFlag == 0)
{
// wheel(50, 0, -1);
// delay(2400);
// wheel(0, 0, 0);
// wheel(0, -50, 2);
// delay(1600);
// wheel(0, 0, 0);
wheel(80, -80, 0);
delay(1000);
wheel(80, 0, 0);
delay(500);
wheel(0, 0, 0);
}
else
{
wheel(80, -80, 0);
delay(1200);
wheel(-80, 0, 0);
delay(500);
wheel(0, 0, 0);
// wheel(-50, 0, 1);
// delay(2400);
// wheel(0, 0, 0);
// wheel(0, 50, -2);
// delay(1600);
// wheel(0, 0, 0);
}
}
/**
* @brief 블록 가까이 다가가는 함수
* 블록 잡는 위치 : 7cm
*/
void fowardToBlock()
{
int a1, a2, a3;
while (true)
{
a1 = analogRead(A1) + diff;
a2 = analogRead(A2);
// a1 = analogRead(A1);
// a2 = analogRead(A2);
a3 = prizm.readSonicSensorCM(A3);
if (a3 <= 6)
{
wheel(0, 0, 0);
return;
}
if (a1 > a2)
{
wheel(0, -18, -7);
}
else if (a1 < a2)
{
wheel(0, -18, 7);
}
// else if (a1 >= 100 && a2 < 100)
// {
// // 좌직진
// wheel(0, -15, -7);
// }
// else if (a1 < 100 && a2 >= 100)
// {
// // 우직진
// wheel(0, -15, 7);
// }
else
{
// 전진
wheel(0, -25, 0);
}
delay(10);
}
}
/**
* @brief 블록으로부터 멀어지는 함수
* 교차로 <-> 블록 : 12cm
*/
void backwardFromBlock()
{
int a3;
bool D2;
wheel(0, 40, -1);
while (true)
{
a3 = prizm.readSonicSensorCM(A3);
if (a3 >= 11)
{
wheel(0, 0, 0);
return;
}
delay(10);
}
}
/**
* @brief 180도 회전하는 코드.
* 회전하면서 currentLineFlag도 전환
*/
void turn()
{
wheel(0, 0, 50);
delay(1000);
wheel(0, 0, 20);
while (true)
{
if (analogRead(A2) > 200)
{
delay(150);
wheel(0, 0, 0);
currentLineFlag = !currentLineFlag;
return;
}
}
}
/**
* @brief 경로를 찾아서 이동하는 함수
*
* @param currentLine 현재 서있는 라인
* @param targetLine 목적지 라인
* @param currentFace 현재 보고있는 방향
* @param targetFace 봐야하는 방향
*/
void directionFind(int currentLine, int targetLine, int currentFace, int targetFace)
{
// 0번쪽을 보고 있으면 우측이동 = 줄 증가
// 1번쪽을 보고 있으면 우측이동 = 줄 감소
// move 음수 = 오른쪽 이동
// move 양수 = 왼쪽 이동
int move = 0;
// 0번쪽을 보고 있을 때
if (currentFace == 0)
{
move = currentLine - targetLine;
}
// 1번쪽을 보고 있을 때
else
{
move = targetLine - currentLine;
}
int moveAbs = abs(move);
// 지금 보는 방향과 봐야하는 방향이 다를 경우
if (currentFace ^ targetFace)
{
// 우측 이동
if (move < 0)
{
for (int i = 0; i < moveAbs; i++)
{
rightLineTracing();
}
}
// 좌측 이동
else if (move > 0)
{
for (int i = 0; i < moveAbs; i++)
{
leftLineTracing();
}
}
turn();
return;
}
// 지금 보는 방향과 봐야하는 방향이 같은 경우
else
{
// 우측 이동
if (move < 0)
{
for (int i = 0; i < moveAbs; i++)
{
rightLineTracing();
}
}
// 좌측 이동
else if (move > 0)
{
for (int i = 0; i < moveAbs; i++)
{
leftLineTracing();
}
}
return;
}
}
/**
* @brief 줄 위에 서있는 상태에서 교차로 중간에 갈 때까지 라인트레이싱을 하면서 오른쪽 이동
* 이동하면서 currentLine 값도 수정함
*/
void rightLineTracing()
{
bool D2 = 0, D3 = 0, D4 = 0, D5 = 0;
int a2, start, now, start2, now2;
start = millis();
while (true)
{
D2 = prizm.readLineSensor(2);
D3 = prizm.readLineSensor(3);
a2 = analogRead(A2);
now = millis();
if (a2 > 100 && now - start > 800)
{
wheel(0, 0, 0);
if (currentLineFlag == 0) // 왼쪽을 보고있을 경우엔 currentLine 증가
{
currentLine++;
}
else if (currentLineFlag == 1) // 오른쪽을 보고있을 경우엔 currentLine 감소
{
currentLine--;
}
return;
}
else if (now - start < 1000)
{
//== 라인트레이싱 ==//
// D2 = true, D3 = false
if (D2 && !D3)
{
wheel(-55, -0, -8);
}
// D2 = false, D3 = true
else if (!D2 && D3)
{
wheel(-55, 0, 8);
}
// D2 = false, D3 = false
else if ((!D2 && !D3) || (D2 && D3))
{
wheel(-60, 0, 1);
}
}
else
{
//== 라인트레이싱 ==//
// D2 = true, D3 = false
if (D2 && !D3)
{
wheel(-35, -0, -4);
}
// D2 = false, D3 = true
else if (!D2 && D3)
{
wheel(-35, 0, 4);
}
// D2 = false, D3 = false
else if ((!D2 && !D3) || (D2 && D3))
{
wheel(-40, 0, 1);
}
}
}
}
/**
* @brief 줄 위에 서있는 상태에서 교차로 중간에 갈 때까지 라인트레이싱을 하면서 왼쪽 이동
*/
void leftLineTracing()
{
bool D2 = 0, D3 = 0, D4 = 0, D5 = 0;
int a1, start, now;
start = millis();
while (true)
{
D4 = prizm.readLineSensor(4);
D5 = prizm.readLineSensor(5);
a1 = analogRead(A1);
now = millis();
if (a1 > 100 && now - start > 800)
{
wheel(0, 0, 0);
if (currentLineFlag == 0) // 왼쪽을 보고있을 경우엔 currentLine 증가
{
currentLine--;
}
else if (currentLineFlag == 1) // 오른쪽을 보고있을 경우엔 currentLine 감소
{
currentLine++;
}
return;
}
else if (now - start < 1000)
{
//== 라인트레이싱 ==//
// D4 = true, D5 = false
if (D4 && !D5)
{
wheel(55, 0, -8);
}
// D4 = false, D5 = true
else if (!D4 && D5)
{
wheel(55, -0, 8);
}
// D4 = false, D5 = false
else if ((!D4 && !D5) || (D4 && D5))
{
wheel(60, 0, -1);
}
}
else
{
//== 라인트레이싱 ==//
// D4 = true, D5 = false
if (D4 && !D5)
{
wheel(35, 0, -4);
}
// D4 = false, D5 = true
else if (!D4 && D5)
{
wheel(35, -0, 4);
}
// D4 = false, D5 = false
else if ((!D4 && !D5) || (D4 && D5))
{
wheel(40, 0, -1);
}
}
}
}
void printHusky()
{
while (true)
{
if (!huskylens.request())
;
else if (!huskylens.isLearned())
;
else if (!huskylens.available())
;
else
{
while (huskylens.available())
{
HUSKYLENSResult result = huskylens.read();
if (result.command == COMMAND_RETURN_BLOCK)
{
Serial.println(String() + F("Block:xCenter=") + result.xCenter + F(",yCenter=") + result.yCenter + F(",width=") + result.width + F(",height=") + result.height + F(",ID=") + result.ID);
}
}
}
}
}
/**
* @brief 처음 배열을 채우는 함수
*/
void scanAll()
{
// 다 못채웠을 경우를 대비하여 반복문
while (true)
{
for (int i = 1; i <= 4; i++)
{
// 1번 줄에서 4번 줄까지
directionFind(currentLine, i, currentLineFlag, 0);
colorCheck();
}
// 조기종료
if (objectCount == 3 && columnCount == 4)
return;
// 4번줄에서 턴
directionFind(4, 4, 0, 1);
colorCheck();
// 조기종료
if (objectCount == 3 && columnCount == 4)
return;
for (int i = 3; i >= 1; i--)
{
// 4번 줄에서 1번 줄까지
directionFind(currentLine, i, currentLineFlag, 1);
colorCheck();
// 조기종료
if (objectCount == 3 && columnCount == 4)
return;
}
printObjectColumn();
objectCount = 0;
columnCount = 0;
columnBlock[5][2] = {
0,
};
objectBlock[5][2] = {
0,
};
}
}
/**
* @brief 허스키렌즈를 사용하여 색깔 판별 후 columnBlock과 objectBlock에 값 채워넣는 함수
*/
void colorCheck()
{
Serial.println("Color Checking...");
if (!huskylens.request())
Serial.println(F("Fail to request data from HUSKYLENS, recheck the connection!"));
else if (!huskylens.isLearned())
Serial.println(F("Nothing learned, press learn button on HUSKYLENS to learn one!"));
else if (!huskylens.available())
{
columnBlock[currentLine][currentLineFlag] = 0;
objectBlock[currentLine][currentLineFlag] = 0;
}
else
{
Serial.println(F("오브젝트 감지함"));
// 하나만 감지 = 노란색 기둥
if (huskylens.count() == 1)
{
columnBlock[currentLine][currentLineFlag] = 4;
columnCount++;
}
else
{
while (huskylens.available())
{
HUSKYLENSResult result1 = huskylens.read();
HUSKYLENSResult result2 = huskylens.read();
if (result1.yCenter >= result2.yCenter)
{
columnBlock[currentLine][currentLineFlag] = result1.ID;
objectBlock[currentLine][currentLineFlag] = result2.ID;
columnCount++;
objectCount++;
}
else
{
columnBlock[currentLine][currentLineFlag] = result2.ID;
objectBlock[currentLine][currentLineFlag] = result1.ID;
columnCount++;
objectCount++;
}
}
}
}
}
/**
* @brief 허스키 렌즈로 얻은 result를 시리얼 출력하는 함수
*/
void printResult(HUSKYLENSResult result)
{
if (result.command == COMMAND_RETURN_BLOCK)
{
Serial.println(String() + F("Block:xCenter=") + result.xCenter + F(",yCenter=") + result.yCenter + F(",width=") + result.width + F(",height=") + result.height + F(",ID=") + result.ID + F(", 오브젝트 배열상태 = ") + objectBlock[0][0] + F(", 기둥 배열상태 = ") + columnBlock[0][0]);
}
else
{
}
}
/**
* @brief objectBlock 배열에 들어있는 값 출력
*/
void printObjectColumn()
{
Serial.println("오브젝트배열 : ");
for (int j = 0; j <= 4; j++)
{
for (int k = 0; k <= 1; k++)
{
Serial.println(objectBlock[j][k]);
}
}
Serial.println("기둥배열 : ");
for (int j = 0; j <= 4; j++)
{
for (int k = 0; k <= 1; k++)
{
Serial.println(columnBlock[j][k]);
}
}
}
/**
* @brief 노란색기둥말고 가장 앞쪽에서 가까운 기둥을 찾아서 이동하는 함수
*/
void findFirstColumn()
{
/**
* 지금 서있는곳의 기둥이 노란색이면,
* 양쪽에 블록이 먼저 있는지 확인하고,
* 둘 다 없으면 뒤쪽을 탐색한다.
* 근데 탐색할 때 현재 줄이 4면 오버플로우가 발생한다.
* 이거는 예외처리를 해줘야겠지.
* 현재 줄이 4면 무조건적으로
*
* 지금 서있는곳의 기둥이 색깔 기둥이면,
* 바로 잡는다.(return)
*
*/
if (columnBlock[currentLine][currentLineFlag] == YELLOW)
{
// 4번 줄에 있을 경우 둘중에 한쪽이 없다.
if (currentLine == 4)
{
// 같은 방향 먼저
if (columnBlock[3][currentLineFlag] != 0)
{
objectFlag = objectBlock[3][currentLineFlag];
objectBlock[3][currentLineFlag] = 0;
directionFind(currentLine, 3, currentLineFlag, currentLineFlag);
return;
}
// 그 다음에 다른 방향
else if (columnBlock[3][!currentLineFlag] != 0)
{
objectFlag = objectBlock[3][!currentLineFlag];
objectBlock[3][!currentLineFlag] = 0;
directionFind(currentLine, 3, currentLineFlag, !currentLineFlag);
return;
}
}
// 4번 줄이 아닐 경우 양쪽에 모두 있을 수 있다.
else
{
if (columnBlock[currentLine + 1][currentLineFlag] != 0)
{
objectFlag = objectBlock[currentLine + 1][currentLineFlag];
objectBlock[currentLine + 1][currentLineFlag] = 0;
directionFind(currentLine, currentLine + 1, currentLineFlag, currentLineFlag);
return;
}
else if (columnBlock[currentLine - 1][currentLineFlag] != 0)
{
objectFlag = objectBlock[currentLine - 1][currentLineFlag];
objectBlock[currentLine - 1][currentLineFlag] = 0;
directionFind(currentLine, currentLine - 1, currentLineFlag, currentLineFlag);
return;
}
else if (columnBlock[currentLine + 1][!currentLineFlag] != 0)
{
objectFlag = objectBlock[currentLine + 1][!currentLineFlag];
objectBlock[currentLine + 1][!currentLineFlag] = 0;
directionFind(currentLine, currentLine + 1, currentLineFlag, !currentLineFlag);
return;
}
else if (columnBlock[currentLine - 1][!currentLineFlag] != 0)
{
objectFlag = objectBlock[currentLine - 1][!currentLineFlag];
objectBlock[currentLine - 1][!currentLineFlag] = 0;
directionFind(currentLine, currentLine - 1, currentLineFlag, !currentLineFlag);
return;
}
}
}
// 현재 보고있는 블록이 노란색이 아니면 바로 들어올리면 된다.
else
{
objectFlag = objectBlock[currentLine][currentLineFlag];
objectBlock[currentLine][currentLineFlag] = 0;
return;
}
}
/**
* @brief 노란색 기둥을 찾아서 이동하는 함수
*/
void findYellowColumn()
{
int forflag = 0, targetLine, targetLineFlag;
for (int i = 0; i <= 4; i++)
{
for (int j = 0; j <= 1; j++)
{
if (columnBlock[i][j] == YELLOW)
{
targetLine = i;
targetLineFlag = j;
objectBlock[i][j] = objectFlag;
forflag = 1;
break;
}
}
if (forflag == 1)
{
break;
}
}
directionFind(currentLine, targetLine, currentLineFlag, targetLineFlag);
}
/**
* @brief 매개변수로 주어진 기둥을 찾아서 이동하는 함수
*
* @param targetColumn 찾을 기둥 값
*/
void findTargetColumn(int targetColumn)
{
int forflag = 0, targetLine, targetLineFlag;
for (int i = 0; i <= 4; i++)
{
for (int j = 0; j <= 1; j++)
{
if (columnBlock[i][j] == targetColumn)
{
targetLine = i;
targetLineFlag = j;
objectBlock[i][j] = targetColumn;
forflag = 1;
break;
}
}
if (forflag == 1)
{
break;
}
}
directionFind(currentLine, targetLine, currentLineFlag, targetLineFlag);
}
/**
* @brief 매개변수로 주어진 오브젝트를 찾아서 이동하는 함수
*
* @param targetObject 찾을 오브젝트 값
*/
void flagColorLine(int targetObject)
{
int forflag = 0, targetLine, targetLineFlag;
for (int i = 0; i <= 4; i++)
{
for (int j = 0; j <= 1; j++)
{
if (objectBlock[i][j] == targetObject)
{
targetLine = i;
targetLineFlag = j;
objectFlag = objectBlock[i][j];
objectBlock[i][j] = 0;
forflag = 1;
break;
}
}
if (forflag == 1)
{
break;
}
}
directionFind(currentLine, targetLine, currentLineFlag, targetLineFlag);
}
/**
* @brief 현재 바라보고 있는 오브젝트의 색깔을 판별하여
* 다음으로 이동할 기둥 배열에 저장
*/
void columnStack()
{
objectColumnFlag[objectFlagCount] = columnBlock[currentLine][currentLineFlag];
objectFlagCount++;
}
/**
* @brief 전진, 잡기, 올리기, 후진
*/
void objectLiftup()
{
fowardToBlock();
objectGrab();
lift_up();
backwardFromBlock();
}
/**
* @brief 전진, 내리기, 놓기, 후진
*/
void objectLiftdown()
{
fowardToBlock();
lift_down();
objectDrop();
backwardFromBlock();
}
/**
* @brief 그랩 초기화
*/
void startGrab()
{
prizm.setMotorSpeed(2, 600);
delay(600);
prizm.setMotorSpeed(2, 0);
}
/**
* @brief 오브젝트 잡는 함수
*/
void objectGrab()
{
prizm.setMotorSpeed(2, -600);
delay(500);
prizm.setMotorSpeed(2, 0);
}
/**
* @brief 오브젝트 놓는 함수
*/
void objectDrop()
{
prizm.setMotorSpeed(2, 600);
delay(500);
prizm.setMotorSpeed(2, 0);
}
//리프트 들기 함수
void lift_up()
{
prizm.setMotorSpeed(1, -600);
delay(800);
prizm.setMotorSpeed(1, 0);
}
//리프트 내리기 함수
void lift_down()
{
prizm.setMotorSpeed(1, 600);
delay(800);
prizm.setMotorSpeed(1, 0);
}
void setDiff()
{
int a1 = analogRead(A1);
int a2 = analogRead(A2);
diff = a2 - a1;
}
이잉~ 기모링~