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++; } }