|
ret = read(fd_sensor, &sensorData, sizeof(unsigned char));
if(ret > 0)
{
#ifdef DEBUG_CARCONTROL
printf("sensorData is 0x%x \n", sensorData);
#endif
} else
{
perror("error to read fd_Sensor\n");
exit(1);
}
// 根据传感器的原始值计算出小车偏移黑线值和在黑线上的传感器数目
ret = calc_sensor(sensorData, &biasValue, &blackSum);
count += ret;
}
#ifdef DEBUG_CARCONTROL
printf("biasValue = %f, blackSum = %d \n", biasValue, blackSum);
#endif
// 检测是否到达探测点
// 若已到达则采集数据并上传,注意上传过程都分别用独立的进程来完成
if(blackSum == 6 && count > 6 && countS > 5)
{
countS = 0;
flagID ++;
printf("flagID = %d\n", flagID);
////////////////////////////////////////////////////////
wr_buf[2] = 0;
wr_buf[3] = 0;
sleep(1);
/////////////////////
fetch_picture(pCamFb);
image_recognize(pCamFb, flagID, pObj);
printf("ID: %d, x=%d, scale: %f\n", flagID, pObj->X, pObj->scale);
//if()
///////////////////
if(flagID == 1)
{
wr_buf[2] = 90;
wr_buf[3] = 90;
wr_buf[4] = 1;
wr_buf[5] = 0;
//usleep(500000);
init_imageHandler(pObj);
do
{
usleep(20000);
fetch_picture(pCamFb);
image_recognize(pCamFb, flagID, pObj);
ret = pObj->X-80;
}
while(ret < -10 || ret > 10);
wr_buf[2] = 0;
wr_buf[3] = 0;
sleep(1);
}
wr_buf[2] = 60;
wr_buf[3] = 60;
wr_buf[4] = 1;
wr_buf[5] = 1;
sleep(2);
countMs = 0;
init_imageHandler(pObj);
while(1)
{
usleep(20000);
fetch_picture(pCamFb);
image_recognize(pCamFb, flagID, pObj);
countMs += 20;
if(countMs > 600 || pObj->scale > scaleGrp[flagID-1]) break; // 注意这两个条件
}
printf("OBJ: x=%d, y=%d, scale=%f\n", pObj->X, pObj->Y, pObj->scale); |
|