Exemple #1
0
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;
}
Exemple #2
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++;
    }

}