라인트레이서 만들어보기

굉장히 빠르고 정밀하게 제어되는 라인트레이서를 보고 저도 만들어보고 싶어 시도를 해봤습니다.

2015-05-17일 최종 튜닝했습니다.

라인트래킹제어 알고리즘을 좀 수정했고, 트랙을 좀손봤습니다(검정테이프질). 운행가능한 최대 속도로 운행될수 있게 했습니다.

센서는 newtc의 AM-IRS4D라는 모듈을 이용햇습니다. 4개의 적외선 센서를 장착하고 있습니다.

가변저항으로 감도를 조정할 수 있으며, 0또는 1로 신호를 출력하게 됩니다.

제어는 아두이노 프로미니 호환 모듈과 ET-DCM이라는 모터드라이버를 사용했습니다. 프로그램은 기존 자율주행 자동차등에서 사용했던 프로그램을 조금 수정하고

라인트래킹 제어루틴을 추가했습니다.

차체는 아카데미 과학의 라인X-II라는 것을 사용했습니다. 구입시 같이 딸려오는 기판과 각종 회로부품들은 잘 짱박아 뒀습니다.

아래는 완성 후 주행영상입니다.

원래 계획은 굉장히 빠르고 정밀하게 제어되는 라인트레이서였는데

그것은 저의 허황된 꿈일 뿐이었습니다.

정밀제어를 위해서는 좀더 많은 수의 센서로부터 입력을 받아야할듯하고, 차체도 정밀제어가 가능하지 않으면 안된다는 것을 알았습니다.

아카데미의 차체는 pwm응당특성이 별로 좋지 않습니다.

그냥 한번만들어봤다는것에 만족하기로 했습니다.

allowfullscreen>

값을 튜닝후

//소스-----------------------------------------------------------------------------------

class CMyTime

{

long m_lTime;

public:

CMyTime()

{

m_lTime = millis();

}

//현재 시간캡쳐

void Capture()

{

m_lTime = millis();

}

//이전에 capture한 시점부터 이함수를 호출하는 시점까지의 경과 시간을 초단위로 구함

float GetPassedTime()

{

long lpassedTime = millis() - m_lTime;

return (float)lpassedTime * (1.0f / 1000.0f);

}

};

#define LINE_PIN_1 2

#define LINE_PIN_2 3

#define LINE_PIN_3 4

#define LINE_PIN_4 7

#define LINE_LEFT_OUTER 0x08

#define LINE_LEFT_INNER 0x04

#define LINE_RIGHT_INNER 0x02

#define LINE_RIGHT_OUTER 0x01

#define LINE_STATUS_ALL (LINE_LEFT_OUTER | LINE_LEFT_INNER | LINE_RIGHT_INNER | LINE_RIGHT_OUTER)

class CLineCheker

{

protected:

char m_cPin1;

char m_cPin2;

char m_cPin3;

char m_cPin4;

public:

CLineCheker(char pin1, char pin2, char pin3, char pin4)

{

m_cPin1 = pin1;

m_cPin2 = pin2;

m_cPin3 = pin3;

m_cPin4 = pin4;

}

void Init()

{

pinMode(m_cPin1, INPUT);

pinMode(m_cPin2, INPUT);

pinMode(m_cPin3, INPUT);

pinMode(m_cPin4, INPUT);

}

char Check()

{

char ret = 0;

if (CheckLine1())

ret |=  LINE_LEFT_OUTER;

if (CheckLine2())

ret |=  LINE_LEFT_INNER;

if (CheckLine3())

ret |=  LINE_RIGHT_INNER;

if (CheckLine4())

ret |=  LINE_RIGHT_OUTER;

return ret;

}

bool CheckLine1()

{

return digitalRead(m_cPin1) == LOW;

}

bool CheckLine2()

{

return digitalRead(m_cPin2) == LOW;

}

bool CheckLine3()

{

return digitalRead(m_cPin3) == LOW;

}

bool CheckLine4()

{

return digitalRead(m_cPin4) == LOW;

}

};

#define LT_UPDATE_TIME (1.0f / 30.0f)

#define LT_DIR_CENTER 0

#define LT_DIR_LEFT 1

#define LT_DIR_RIGHT 2

class CLineTracker

{

protected:

bool m_bEnabled;

CLineCheker m_Checker;

unsigned char m_cLastLeftSpeed, m_cLastRightSpeed;

char m_cLtDir;

char m_cLastStatus;

CMyTime m_Time;

bool m_bEndMark;

float m_fSpeedScale;

float m_fTurnSpeedSlow;

float m_fTurnSpeedQuick;

public:

CLineTracker() : m_Checker(LINE_PIN_1, LINE_PIN_2, LINE_PIN_3, LINE_PIN_4)

{

m_fSpeedScale = 1.0f;

Clear();

}

void Clear()

{

m_bEnabled = false;

m_cLtDir = LT_DIR_CENTER;

m_cLastStatus = 0;

m_bEndMark = false;

m_cLastLeftSpeed = (unsigned char)(255 * m_fSpeedScale);

m_cLastRightSpeed = (unsigned char)(255 * m_fSpeedScale);

m_fTurnSpeedSlow = (255.0f * (54.0f/100.0f));

m_fTurnSpeedQuick = (255.0f * (7.0f/100.0f));

}

void Enable(bool bEnable)

{

Stop();

m_bEnabled = bEnable;

}

bool IsEnabled() {return m_bEnabled;}

char GetLastStatus() {return m_cLastStatus;}

float GetSpeedScale() {return m_fSpeedScale;}

void SetSpeedScale(float f) {m_fSpeedScale = f;}

void SetTurnSpeedSlow(float fPer)

{

m_fTurnSpeedSlow = fPer * 0.01f * 255.0f;

}

void SetTurnSpeedQuick(float fPer)

{

m_fTurnSpeedQuick = fPer * 0.01f * 255.0f;

}

void Init()

{

m_Checker.Init();

m_Time.Capture();

}

void Move(int left, int right)

{

if (left > 255)

left = 255;

if (right > 255)

right = 255;

m_cLastLeftSpeed = (unsigned char)left;

m_cLastRightSpeed = (unsigned char)right;

//모터 드라이버 제어

g_Motor.SmoothTurn(left, right, true);

}

void Stop()

{

g_Motor.StopAll();

Clear();

}

void Forward()

{

m_cLtDir = LT_DIR_CENTER;

Move((int)(255 * m_fSpeedScale), (int)(255 * m_fSpeedScale));

}

void TurnLeft(int speed)

{

m_cLtDir = LT_DIR_LEFT;

//Move((int)(speed * m_fSpeedScale), int(255 * m_fSpeedScale));

Move(speed, 255);

}

void TurnRight(int speed)

{

m_cLtDir = LT_DIR_RIGHT;

//Move((int)(255 * m_fSpeedScale), (int)(speed * m_fSpeedScale));

Move(255, speed);

}

void MoveLastStatus()

{

Move(m_cLastLeftSpeed, m_cLastRightSpeed);

}

char Update()

{

if (!m_bEnabled)

return 0;

if (m_Time.GetPassedTime() < LT_UPDATE_TIME)

return m_cLastStatus;

m_Time.Capture();

m_cLastStatus = m_Checker.Check();

if (m_cLastStatus == LINE_LEFT_OUTER)

{//크게 좌회전

m_bEndMark = false;

TurnLeft(m_fTurnSpeedQuick);

}

else if (m_cLastStatus == LINE_LEFT_INNER)

{//좌회전

m_bEndMark = false;

TurnLeft(m_fTurnSpeedSlow);

}

else if (m_cLastStatus == LINE_RIGHT_INNER)

{//우회전

m_bEndMark = false;

TurnRight(m_fTurnSpeedSlow);

}

else if (m_cLastStatus == LINE_RIGHT_OUTER)

{//크게 우회전

m_bEndMark = false;

TurnRight(m_fTurnSpeedQuick);

}

else if (m_cLastStatus == 0) //아무런 라인인식안됨

{

/*if (m_bEndMark)

Stop();

else*/

Forward();

}

else if (m_cLastStatus == (LINE_LEFT_OUTER | LINE_LEFT_INNER | LINE_RIGHT_OUTER | LINE_RIGHT_INNER))

{

m_bEndMark = true;

Forward();

}

else if ((m_cLastStatus & (LINE_LEFT_OUTER | LINE_LEFT_INNER)) == (LINE_LEFT_OUTER | LINE_LEFT_INNER))

{

m_bEndMark = false;

TurnLeft(m_fTurnSpeedQuick);

}

else if ((m_cLastStatus & (LINE_RIGHT_OUTER | LINE_RIGHT_INNER)) == (LINE_RIGHT_OUTER | LINE_RIGHT_INNER))

{

m_bEndMark = false;

TurnRight(m_fTurnSpeedQuick);

}

else

{

m_bEndMark = false;

MoveLastStatus();

}

return m_cLastStatus;

}

};