int main(void)
{
unsigned char status;
short speed;
unsigned char gain;
int position, position_now;
short angle;
int channel;
int data;
char sensor;
int i, j;
int tol;
char byte = 0x80;
CarControlInit();
// 2. speed control ----------------------------------------------------------
printf("\n\n 2. speed control\n");
//jobs to be done beforehand;
PositionControlOnOff_Write(UNCONTROL); // position controller must be OFF !!!
//control on/off
status=SpeedControlOnOff_Read();
printf("SpeedControlOnOff_Read() = %d\n", status);
SpeedControlOnOff_Write(CONTROL);
//speed controller gain set
//P-gain
gain = SpeedPIDProportional_Read(); // default value = 10, range : 1~50
printf("SpeedPIDProportional_Read() = %d \n", gain);
gain = 20;
SpeedPIDProportional_Write(gain);
//I-gain
gain = SpeedPIDIntegral_Read(); // default value = 10, range : 1~50
printf("SpeedPIDIntegral_Read() = %d \n", gain);
gain = 20;
SpeedPIDIntegral_Write(gain);
//D-gain
gain = SpeedPIDDifferential_Read(); // default value = 10, range : 1~50
printf("SpeedPIDDefferential_Read() = %d \n", gain);
gain = 20;
SpeedPIDDifferential_Write(gain);
//speed set
speed = DesireSpeed_Read();
printf("DesireSpeed_Read() = %d \n", speed);
speed = -10;
DesireSpeed_Write(speed);
sleep(2); //run time
speed = DesireSpeed_Read();
printf("DesireSpeed_Read() = %d \n", speed);
speed = 0;
DesireSpeed_Write(speed);
sleep(1);
return 0;
}
void *ControlThread(void *unused)
{
int i=0;
char fileName[30];
NvMediaTime pt1 ={0}, pt2 = {0};
NvU64 ptime1, ptime2;
struct timespec;
IplImage* imgOrigin;
IplImage* imgCanny;
// cvCreateImage
imgOrigin = cvCreateImage(cvSize(RESIZE_WIDTH, RESIZE_HEIGHT), IPL_DEPTH_8U, 3);
imgCanny = cvCreateImage(cvGetSize(imgOrigin), IPL_DEPTH_8U, 1);
int angle, speed;
IplImage* imgOrigin;
IplImage* imgResult;
unsigned char status;
unsigned int gain;
CarControlInit();
PositionControlOnOff_Write(UNCONTROL);
SpeedControlOnOff_Write(1);
//speed controller gain set
//P-gain
gain = SpeedPIDProportional_Read(); // default value = 10, range : 1~50
printf("SpeedPIDProportional_Read() = %d \n", gain);
gain = 20;
SpeedPIDProportional_Write(gain);
//I-gain
gain = SpeedPIDIntegral_Read(); // default value = 10, range : 1~50
printf("SpeedPIDIntegral_Read() = %d \n", gain);
gain = 20;
SpeedPIDIntegral_Write(gain);
//D-gain
gain = SpeedPIDDifferential_Read(); // default value = 10, range : 1~50
printf("SpeedPIDDefferential_Read() = %d \n", gain);
gain = 20;
SpeedPIDDifferential_Write(gain);
angle = 1460;
SteeringServoControl_Write(angle);
// cvCreateImage
imgOrigin = cvCreateImage(cvSize(RESIZE_WIDTH, RESIZE_HEIGHT), IPL_DEPTH_8U, 3);
imgResult = cvCreateImage(cvGetSize(imgOrigin), IPL_DEPTH_8U, 1);
int flag = 1;
while(1)
{
pthread_mutex_lock(&mutex);
pthread_cond_wait(&cond, &mutex);
GetTime(&pt1);
ptime1 = (NvU64)pt1.tv_sec * 1000000000LL + (NvU64)pt1.tv_nsec;
Frame2Ipl(imgOrigin); // save image to IplImage structure & resize image from 720x480 to 320x240
pthread_mutex_unlock(&mutex);
cvCanny(imgOrigin, imgCanny, 100, 100, 3);
sprintf(fileName, "captureImage/imgCanny%d.png", i);
cvSaveImage(fileName , imgCanny, 0);
Frame2Ipl(imgOrigin, imgResult); // save image to IplImage structure & resize image from 720x480 to 320x240
pthread_mutex_unlock(&mutex);
//cvCanny(imgOrigin, imgCanny, 100, 100, 3);
sprintf(fileName, "captureImage/imgyuv%d.png", i);
cvSaveImage(fileName , imgOrigin, 0);
//sprintf(fileName, "captureImage/imgOrigin%d.png", i);
//cvSaveImage(fileName, imgOrigin, 0);
// TODO : control steering angle based on captured image ---------------
//speed set
speed = DesireSpeed_Read();
printf("DesireSpeed_Read() = %d \n", speed);
//speed = -10;
//DesireSpeed_Write(speed);
if(flag == 1){
if(greenlight>1000)
{
printf("right go\n");
Winker_Write(LEFT_ON);
usleep(1000000);
//Winker_Write(ALL_OFF);
angle = 1400;
SteeringServoControl_Write(angle);
speed = 10;
DesireSpeed_Write(speed);
speed = DesireSpeed_Read();
printf("DesireSpeed_Read() = %d \n", speed);
sleep(1);
flag = 0;
}
else
{
printf("left go\n");
Winker_Write(RIGHT_ON);
usleep(10000);
Winker_Write(ALL_OFF);
speed = 20;
DesireSpeed_Write(speed);
usleep(1300000);
angle = 1950;
SteeringServoControl_Write(angle);
usleep(5000000);
angle = 1460;
SteeringServoControl_Write(angle);
usleep(1000000);
speed = 0;
DesireSpeed_Write(speed);
flag = 0;
}
}
// ---------------------------------------------------------------------
GetTime(&pt2);
ptime2 = (NvU64)pt2.tv_sec * 1000000000LL + (NvU64)pt2.tv_nsec;
printf("--------------------------------operation time=%llu.%09llu[s]\n", (ptime2-ptime1)/1000000000LL, (ptime2-ptime1)%1000000000LL);
i++;
}
}