극한의 아두이노 DIY생활 - 곡률측정기19
안녕하세요 Jimae입니다.
저번에 마저 이어서 시작하도록 하겠습니다.
SwitchDetect();
LaserDistanceCalculation();
LaserCurvatureCalculation();
오랜만에 에이블로 글을쓰려다 보니 익숙해지지 않는군요.
void SwitchDetect(void)
{
if(switchInputCount > 3 && (millis() - switchTime) > SWITCHDELAY) // 스위치 인식
{
if(switchState == BACK) switchState = ENTER;
else switchState = BACK;
switchInputCount = 0;
}
}
이것은 위에 보셨듯이 스위치의 상태를 지속적으로 확인해서 업데이트를 하는 함수 입니다.
그래야 눌렸는지 안눌렸는지 판단이 되서요.
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;
}
}
이것은 단순히 거리를 측정하려고 사용하는 알고리즘 입니다.
이부분은 거리 측정하는 기능이 선택되어질때 실행되게끔
if(distanceFlag != START) return;
조건문으로 감시 하고있어요.
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;
}
}
이부분이 곡률을 계산하는 알고리즘입니다.
이부분도 마찬가지로 거리 측정하는 알고리즘과 똑같이
if(CurvatureFlag != START) return;
조건문으로 기능이 선택될때 실행되게끔 되어있죠.
이렇게 남은 3가지 함수들은 스위치의 상태 및 레이저 거리, 곡률의 계산을 담당하는 알고리즘들 입니다.
오늘은 여기까지 다음부터 거리, 곡률 알고리즘에 대해서 글을 써보겠습니다.
다들 좋은하루 되세요.
극한의 아두이노 DIY생활 - 곡률측정기1
극한의 아두이노 DIY생활 - 곡률측정기2
극한의 아두이노 DIY생활 - 곡률측정기3
극한의 아두이노 DIY생활 - 곡률측정기4
극한의 아두이노 DIY생활 - 곡률측정기5
극한의 아두이노 DIY생활 - 곡률측정기6
극한의 아두이노 DIY생활 - 곡률측정기7
극한의 아두이노 DIY생활 - 곡률측정기8
극한의 아두이노 DIY생활 - 곡률측정기9
극한의 아두이노 DIY생활 - 곡률측정기10
극한의 아두이노 DIY생활 - 곡률측정기11
극한의 아두이노 DIY생활 - 곡률측정기12
극한의 아두이노 DIY생활 - 곡률측정기13
극한의 아두이노 DIY생활 - 곡률측정기14
극한의 아두이노 DIY생활 - 곡률측정기15
극한의 아두이노 DIY생활 - 곡률측정기16
극한의 아두이노 DIY생활 - 곡률측정기17
극한의 아두이노 DIY생활 - 곡률측정기18
[광고] STEEM 개발자 커뮤니티에 참여 하시면, 다양한 혜택을 받을 수 있습니다.
대단하십니다
감사합니다.!!! ㅎㅎ