극한의 아두이노 DIY생활 - 곡률측정기13
안녕하세요 Jimae입니다.
오늘 레이저부분 프로그램을 올려보겠습니다.
#include <QueueArray.h>
#define DISTANCE 'D'
#define TEMPVOLT 'S'
#define LASERON 'O'
#define LASEROFF 'C'
#define START 1
#define STOP 0
#define LEFTLASER Serial1
#define CENTERLASER Serial2
#define RIGHTLASER Serial3
#define LEFTANGLE 105
#define CENTERANGLE 90
#define RIGHTANGLE 75
#define LEFTDISTANCECORRECTION 0.7 // 단위 cm
#define CENTERDISTANCECORRECTION 0.3
#define RIGHTDISTANCECORRECTION 0
//#define CALCULATIONCORRENTION 9.739 // 임시
#define CALCULATIONCORRENTION 10.539 // 임시
#define LASERFILITER 6
enum{LASERSHOOT = 0, LASERRREAD, DATAPROCESS, DATACALCULATION};
QueueArray <unsigned char> LeftLaserqueue;
QueueArray <unsigned char> CenterLaserqueue;
QueueArray <unsigned char> RightLaserqueue;
QueueArray <unsigned char> Distancequeue;
unsigned char leftLaserData[6] = {0.};
unsigned char centerLaserData[6] = {0.};
unsigned char rightLaserData[6] = {0.};
unsigned char DistanceData[6] = {0.};
unsigned char detectionOrderDistance = LASERSHOOT;
unsigned char detectionOrderCurvature = LASERSHOOT;
unsigned long laserAnswerTimeD = 0;
unsigned long waitTimeD = 0;
unsigned long laserAnswerTimeC = 0;
unsigned long waitTimeC = 0;
unsigned char distanceFlag = STOP;
unsigned char CurvatureFlag = STOP;
unsigned char CurvatureSynchronization = 0;
double laserCenterDistance = 0;
double leftData = 0;
double centerData = 0;
double rightData = 0;
double sumData = 0;
double previousLeftData = 0;
double previousCenterData = 0;
double previousRightData = 0;
double previousSumData = 0;
double leftX = 0;
double leftY = 0;
double centerX = 0;
double centerY = 0;
double rightX = 0;
double rightY = 0;
double d1 = 0;
double d2 = 0;
double cX = 0;
double cY = 0;
double curvature = 0;
void LaserInit(void)
{
LEFTLASER.begin(19200);
CENTERLASER.begin(19200);
RIGHTLASER.begin(19200);
}
void LeftLaserShoot(void)
{
LEFTLASER.write(DISTANCE);
}
void CenterLaserShoot(void) // 거리측정 전용 레이저 즉 가운데
{
CENTERLASER.write(DISTANCE);
}
void RightLaserShoot(void)
{
RIGHTLASER.write(DISTANCE);
}
double DistanceRead(void)
{
return laserCenterDistance;
}
void DistanceCalculationStart(void)
{
distanceFlag = START;
}
void DistanceCalculationStop(void)
{
distanceFlag = STOP;
}
double CurvatureRead(void)
{
return curvature;
}
void CurvatureCalculationStart(void)
{
CurvatureFlag = START;
}
void CurvatureCalculationStop(void)
{
CurvatureFlag = STOP;
}
void LaserDistanceCalculation(void)
{
unsigned char c;
unsigned char num = 0;
if(distanceFlag != START) return;
switch(detectionOrderDistance)
{
case LASERSHOOT:
CenterLaserShoot();
laserAnswerTimeD = millis(); // 레이저 오류일때 다시보내기 위한 시간
detectionOrderDistance = LASERRREAD;
break;
case LASERRREAD:
waitTimeD = millis() - laserAnswerTimeD; // 레이저 오류일때 다시보내기 비교 시간
if(waitTimeD >= 1000) // 1초 이상
{
detectionOrderDistance = LASERSHOOT; // error 만약 3개의 레이저를 쓸때 한개라도 에러나면 전부다 다시 해줘야 한다.
break;
}
if (CENTERLASER.available())
{
c = CENTERLASER.read();
Distancequeue.enqueue(c);
if(c == ',') detectionOrderDistance = DATAPROCESS;
c = 0;
}
break;
case DATAPROCESS:
if(!Distancequeue.isEmpty())
{
c = Distancequeue.dequeue();
if(c == ':') // 길이 데이터만 건짐
{
for(num = 0; num < 6; num++) // 데이터 길이 6개 단위 M
{
DistanceData[num] = (Distancequeue.dequeue() - 48); // 아스키 코드를 숫자값으로 변환 - 48
}
}
else if(c == 'm') //큐 비우기
{
while(!Distancequeue.isEmpty()) c = Distancequeue.dequeue();
}
c = 0;
}
else detectionOrderDistance = DATACALCULATION;
break;
case DATACALCULATION:
laserCenterDistance = (double) (DistanceData[1] + (DistanceData[3] * 0.1) + (DistanceData[4] * 0.01) + (DistanceData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'
//현재 cm단위
laserCenterDistance *= 100;
laserCenterDistance -= CENTERDISTANCECORRECTION;
if(laserCenterDistance <= 0) laserCenterDistance = 0;
detectionOrderDistance = LASERSHOOT;
break;
default:
detectionOrderDistance = LASERSHOOT;
break;
}
}
void LaserCurvatureCalculation(void)
{
unsigned char c1, c2, c3;
unsigned char num = 0;
double plusdata = 0;
if(CurvatureFlag != START) return;
switch(detectionOrderCurvature)
{
case LASERSHOOT:
LeftLaserShoot();
delay(10);
CenterLaserShoot();
delay(10);
RightLaserShoot();
delay(10);
laserAnswerTimeC = millis(); // 레이저 오류일때 다시보내기 위한 시간
detectionOrderCurvature = LASERRREAD;
break;
case LASERRREAD:
waitTimeC = millis() - laserAnswerTimeC; // 레이저 오류일때 다시보내기 비교 시간
if(waitTimeC >= 3000) // 2초 이상
{
CurvatureSynchronization = 0;
detectionOrderCurvature = LASERSHOOT; // error 만약 3개의 레이저를 쓸때 한개라도 에러나면 전부다 다시 해줘야 한다.
break;
}
if (LEFTLASER.available() > 0)
{
c1 = LEFTLASER.read();
LeftLaserqueue.enqueue(c1);
if(c1 == ',') CurvatureSynchronization++;
c1 = 0;
}
if (CENTERLASER.available() > 0)
{
c2 = CENTERLASER.read();
CenterLaserqueue.enqueue(c2);
if(c2 == ',') CurvatureSynchronization++;
c2 = 0;
}
if (RIGHTLASER.available() > 0)
{
c3 = RIGHTLASER.read();
RightLaserqueue.enqueue(c3);
if(c3 == ',') CurvatureSynchronization++;
c3 = 0;
}
//Serial.println(CurvatureSynchronization);
if(CurvatureSynchronization >= 3)
{
CurvatureSynchronization = 0;
detectionOrderCurvature = DATAPROCESS;
}
break;
case DATAPROCESS:
if(!LeftLaserqueue.isEmpty())
{
c1 = LeftLaserqueue.dequeue();
if(c1 == ':') // 길이 데이터만 건짐
{
for(num = 0; num < 6; num++) // 데이터 길이 6개 단위 M
{
leftLaserData[num] = (LeftLaserqueue.dequeue() - 48); // 아스키 코드를 숫자값으로 변환 - 48
}
}
else if(c1 == 'm') //큐 비우기
{
CurvatureSynchronization++;
while(!LeftLaserqueue.isEmpty()) c1 = LeftLaserqueue.dequeue();
}
c1 = 0;
}
if(!CenterLaserqueue.isEmpty())
{
c2 = CenterLaserqueue.dequeue();
if(c2 == ':') // 길이 데이터만 건짐
{
for(num = 0; num < 6; num++) // 데이터 길이 6개 단위 M
{
centerLaserData[num] = (CenterLaserqueue.dequeue() - 48); // 아스키 코드를 숫자값으로 변환 - 48
}
}
else if(c2 == 'm') //큐 비우기
{
CurvatureSynchronization++;
while(!CenterLaserqueue.isEmpty()) c2 = CenterLaserqueue.dequeue();
}
c2 = 0;
}
if(!RightLaserqueue.isEmpty())
{
c3 = RightLaserqueue.dequeue();
if(c3 == ':') // 길이 데이터만 건짐
{
for(num = 0; num < 6; num++) // 데이터 길이 6개 단위 M
{
rightLaserData[num] = (RightLaserqueue.dequeue() - 48); // 아스키 코드를 숫자값으로 변환 - 48
}
}
else if(c3 == 'm') //큐 비우기
{
CurvatureSynchronization++;
while(!RightLaserqueue.isEmpty()) c3 = RightLaserqueue.dequeue();
}
c3 = 0;
}
if(CurvatureSynchronization >= 3)
{
CurvatureSynchronization = 0;
detectionOrderCurvature = DATACALCULATION;
}
break;
case DATACALCULATION:
leftData = (double) (leftLaserData[1] + (leftLaserData[3] * 0.1) + (leftLaserData[4] * 0.01) + (leftLaserData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'
centerData = (double) (centerLaserData[1] + (centerLaserData[3] * 0.1) + (centerLaserData[4] * 0.01) + (centerLaserData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'
rightData = (double) (rightLaserData[1] + (rightLaserData[3] * 0.1) + (rightLaserData[4] * 0.01) + (rightLaserData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'
//현재 cm단위
leftData = ((leftData * 100) - LEFTDISTANCECORRECTION) + CALCULATIONCORRENTION;
centerData = ((centerData * 100) - CENTERDISTANCECORRECTION) + CALCULATIONCORRENTION;
rightData = ((rightData * 100) - RIGHTDISTANCECORRECTION) + CALCULATIONCORRENTION;
if(leftData <= 0) leftData = 0;
else if(centerData <= 0) centerData = 0;
else if(rightData <= 0) rightData = 0;
/*
sumData = leftData + centerData + rightData;
if(sumData - previousSumData < LASERFILITER && previousSumData - sumData < LASERFILITER)
{
leftData = previousLeftData;
centerData = previousCenterData;
rightData = previousRightData;
sumData = previousSumData;
}
previousLeftData = leftData;
previousCenterData = centerData;
previousRightData = rightData;
previousSumData = sumData;
*/
leftX = leftData * cos(radians(LEFTANGLE));
leftY = leftData * sin(radians(LEFTANGLE));
centerX = 0;
centerY = centerData * sin(radians(CENTERANGLE));
rightX = rightData * cos(radians(RIGHTANGLE));
rightY = rightData * sin(radians(RIGHTANGLE));
d1 = (centerX - leftX)/(centerY - leftY);
d2 = (rightX - centerX)/(rightY - centerY);
cX = ((rightY - leftY) + (centerX + rightX) * d2 - (leftX + centerX) * d1) / (2 * (d2 - d1));
cY = -d1 * (cX - (leftX + centerX) / 2) + (leftY + centerY) / 2;
curvature = sqrt(pow((leftX - cX),2) + pow((leftY - cY),2));
plusdata = curvature / 100;
plusdata = plusdata * 1.5; // 1.5 % 데이터 계산
curvature = curvature + plusdata;
/*
Serial.print("left : ");
Serial.println(leftData);
Serial.print("center : ");
Serial.println(centerData);
Serial.print("right : ");
Serial.println(rightData);
Serial.print("curva : ");
Serial.println(curvature);
Serial.println("");
*/
detectionOrderCurvature = LASERSHOOT;
break;
default:
detectionOrderCurvature = LASERSHOOT;
break;
}
}
이 프로그램이 긴이유는 레이저에 관한 프로그램만 있는것이 아닌 곡률 구하는 알고리즘 까지 포함되어 있어서 이렇게 긴것입니다.
void LaserInit(void)
{
LEFTLASER.begin(19200);
CENTERLASER.begin(19200);
RIGHTLASER.begin(19200);
}
초기화 부분은 이정도 인대 시리얼 통신 3개를 이렇게 초기화 하는것이 다입니다.
여기에 추가로 큐를 사용을 했내요.
아무래도 데이터 누락을 피하기 위해 그렇게 구현을 한것같습니다.
오늘은 프로그램 자체가 길어져서 여기까지 다음에는 loop 함수부터 시작하겠습니다.
다들 좋은하루 되세요.
극한의 아두이노 DIY생활 - 곡률측정기1
극한의 아두이노 DIY생활 - 곡률측정기2
극한의 아두이노 DIY생활 - 곡률측정기3
극한의 아두이노 DIY생활 - 곡률측정기4
극한의 아두이노 DIY생활 - 곡률측정기5
극한의 아두이노 DIY생활 - 곡률측정기6
극한의 아두이노 DIY생활 - 곡률측정기7
극한의 아두이노 DIY생활 - 곡률측정기8
극한의 아두이노 DIY생활 - 곡률측정기9
극한의 아두이노 DIY생활 - 곡률측정기10
극한의 아두이노 DIY생활 - 곡률측정기11
극한의 아두이노 DIY생활 - 곡률측정기12
[광고] STEEM 개발자 커뮤니티에 참여 하시면, 다양한 혜택을 받을 수 있습니다.