main.cpp

00001 //------------------------------------------------------------------//
00002 //Supported MCU:   RZ/A1H
00003 //File Contents:   Operation Test Program (GR-PEACH version on the Micon Car)
00004 //Version number:  Ver.1.02
00005 //Date:            2016.02.23
00006 //Copyright:       Renesas Electronics Corporation
00007 //------------------------------------------------------------------//
00008  
00009 //This program supports the following boards:
00010 //* GR-PEACH(E version)
00011 //* Motor drive board Ver.5
00012 //* Camera module (SC-310)
00013  
00014 //Include
00015 //------------------------------------------------------------------//
00016 #include "mbed.h"
00017 #include "iodefine.h"
00018 #include "DisplayBace.h"
00019  
00020 //Define
00021 //------------------------------------------------------------------//
00022 //Motor PWM cycle
00023 #define     MOTOR_PWM_CYCLE     33332   /* Motor PWM period         */
00024                                         /* 1ms    P0φ/1  = 0.03us   */
00025 //Motor speed
00026 #define     MAX_SPEED           50      /* motor()  set: 0 to 100   */
00027 
00028 //Servo PWM cycle
00029 #define     SERVO_PWM_CYCLE     33332   /* SERVO PWM period         */
00030                                         /* 16ms   P0φ/16 = 0.48us   */
00031 #define     SERVO_CENTER        3124    /* 1.5ms / 0.48us - 1 = 3124*/
00032 #define     HANDLE_STEP         18      /* 1 degree value           */
00033 
00034 //LED Color on GR-PEACH
00035 #define     LED_OFF             0x00
00036 #define     LED_RED             0x01
00037 #define     LED_GREEN           0x02
00038 #define     LED_YELLOW          0x03
00039 #define     LED_BLUE            0x04
00040 #define     LED_PURPLE          0x05
00041 #define     LED_SKYBLUE         0x06
00042 #define     LED_WHITE           0x07
00043 
00044 //Status
00045 #define     RUN                 0x00
00046 #define     SENSOR              0x01
00047 #define     MARK                0x02
00048 #define     STOP                0x03
00049 #define     ERROR               0xff
00050 
00051 //Define(NTSC-Video)
00052 //------------------------------------------------------------------//
00053 #define VIDEO_INPUT_CH          (DisplayBase::VIDEO_INPUT_CHANNEL_0)
00054 #define VIDEO_INT_TYPE          (DisplayBase::INT_TYPE_S0_VFIELD)
00055 #define DATA_SIZE_PER_PIC       (2u)
00056 
00057 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
00058     in accordance with the frame buffer burst transfer mode. */
00059 #define PIXEL_HW                (320u)  /* QVGA */
00060 #define PIXEL_VW                (240u)  /* QVGA */
00061 #define VIDEO_BUFFER_STRIDE     (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
00062 #define VIDEO_BUFFER_HEIGHT     (PIXEL_VW)
00063 
00064 //Constructor
00065 //------------------------------------------------------------------//
00066 Ticker      interrput;
00067 DigitalOut  Left_motor_signal(P4_6);    /* Used by motor fanction   */
00068 DigitalOut  Right_motor_signal(P4_7);   /* Used by motor fanction   */
00069 Serial      pc(USBTX, USBRX);
00070 DigitalOut  LED_R(P6_13);               /* LED1 on the GR-PEACH board */
00071 DigitalOut  LED_G(P6_14);               /* LED2 on the GR-PEACH board */
00072 DigitalOut  LED_B(P6_15);               /* LED3 on the GR-PEACH board */
00073 DigitalIn   user_botton(P6_0);          /* SW1 on the GR-PEACH board */
00074 DigitalIn   push_sw(P2_13);             /* SW1 on the Motor Drive board */
00075 DigitalOut  LED_3(P2_14);               /* LED3 on the Motor Drive board */
00076 DigitalOut  LED_2(P2_15);               /* LED2 on the Motor Drive board */
00077 
00078 //Prototype
00079 //------------------------------------------------------------------//
00080 void init_MTU2_PWM_Motor( void );       /* Initialize PWM functions */
00081 void init_MTU2_PWM_Servo( void );       /* Initialize PWM functions */
00082 void intTimer( void );                  /* Interrupt fanction       */
00083 void led_rgb(int led);
00084 unsigned int user_button_get( void );
00085 void led_status_process( void );        /* Function for only interrupt */
00086 void led_status_set( int set );
00087 void led_out(int led);
00088 unsigned int pushsw_get( void );
00089 void motor( int accele_l, int accele_r );
00090 void handle( int angle );
00091 
00092 //Prototype(NTSC-video)
00093 //------------------------------------------------------------------//
00094 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
00095 static void WaitVfield(const int32_t wait_count);
00096 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
00097 static void WaitVsync(const int32_t wait_count);
00098 
00099 //Globle
00100 //------------------------------------------------------------------//
00101 volatile unsigned long  cnt0;           /* Used by timer function   */
00102 volatile unsigned long  cnt1;           /* Used within main         */
00103 volatile int            pattern;        /* Pattern numbers          */
00104 volatile int            status_set;     /* Status                   */
00105 
00106 //Globle(NTSC-video)
00107 //------------------------------------------------------------------//
00108 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16)));  //16 bytes aligned!;
00109 static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16)));  //16 bytes aligned!;
00110 static volatile int32_t vsync_count;
00111 static volatile int32_t vfield_count;
00112 
00113 volatile long int       Xp;
00114 volatile long int       Yp;
00115 volatile int            x;
00116 volatile int            y;
00117 
00118 //Main
00119 //------------------------------------------------------------------//
00120 int main( void )
00121 {
00122     /* NTSC-Video */
00123     DisplayBase::graphics_error_t error;
00124     uint8_t * write_buff_addr = FrameBuffer_Video_A;
00125     uint8_t * save_buff_addr  = FrameBuffer_Video_B;
00126 
00127     /* Create DisplayBase object */
00128     DisplayBase Display;
00129 
00130     /* Graphics initialization process */
00131     error = Display.Graphics_init(NULL);
00132     if (error != DisplayBase::GRAPHICS_OK) {
00133         printf("Line %d, error %d\n", __LINE__, error);
00134         while (1);
00135     }
00136 
00137     error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
00138     if( error != DisplayBase::GRAPHICS_OK ) {
00139         while(1);
00140     }
00141 
00142     /* Interrupt callback function setting (Vsync signal input to scaler 0) */
00143     error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
00144     if (error != DisplayBase::GRAPHICS_OK) {
00145         printf("Line %d, error %d\n", __LINE__, error);
00146         while (1);
00147     }
00148     /* Video capture setting (progressive form fixed) */
00149     error = Display.Video_Write_Setting(
00150                 VIDEO_INPUT_CH,
00151                 DisplayBase::COL_SYS_NTSC_358,
00152                 write_buff_addr,
00153                 VIDEO_BUFFER_STRIDE,
00154                 DisplayBase::VIDEO_FORMAT_YCBCR422,
00155                 DisplayBase::WR_RD_WRSWA_32_16BIT,
00156                 PIXEL_VW,
00157                 PIXEL_HW
00158             );
00159     if (error != DisplayBase::GRAPHICS_OK) {
00160         printf("Line %d, error %d\n", __LINE__, error);
00161         while (1);
00162     }
00163 
00164     /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
00165     error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
00166     if (error != DisplayBase::GRAPHICS_OK) {
00167         printf("Line %d, error %d\n", __LINE__, error);
00168         while (1);
00169     }
00170 
00171     /* Video write process start */
00172     error = Display.Video_Start (VIDEO_INPUT_CH);
00173     if (error != DisplayBase::GRAPHICS_OK) {
00174         printf("Line %d, error %d\n", __LINE__, error);
00175         while (1);
00176     }
00177 
00178     /* Video write process stop */
00179     error = Display.Video_Stop (VIDEO_INPUT_CH);
00180     if (error != DisplayBase::GRAPHICS_OK) {
00181         printf("Line %d, error %d\n", __LINE__, error);
00182         while (1);
00183     }
00184 
00185     /* Video write process start */
00186     error = Display.Video_Start (VIDEO_INPUT_CH);
00187     if (error != DisplayBase::GRAPHICS_OK) {
00188         printf("Line %d, error %d\n", __LINE__, error);
00189         while (1);
00190     }
00191 
00192     /* Wait vsync to update resister */
00193     WaitVsync(1);
00194 
00195     /* Wait 2 Vfield(Top or bottom field) */
00196     WaitVfield(2);
00197 
00198     /* Initialize MCU functions */
00199     init_MTU2_PWM_Motor();
00200     init_MTU2_PWM_Servo();
00201     interrput.attach(&intTimer, 0.001);
00202     pc.baud(230400);
00203 
00204     /* Initialize Micon Car state */
00205     led_out( 0x0 );
00206     handle( 0 );
00207     motor( 0, 0 );
00208 
00209     while(1) {
00210 
00211     if( user_button_get() ){
00212         led_out( 0x0 );
00213         handle( 0 );
00214         motor( 0, 0 );
00215         pattern = 0;
00216     }
00217 
00218     switch( pattern ) {
00219     case 0:
00220         led_status_set( STOP );
00221         pc.printf( "GR-PEACH MCR Operation Test Program V1.00\n\n\r" );
00222         pc.printf( " 1.LEDs light alternately at 0.5-second intervals\n\r" );
00223         pc.printf( " 2.Pushbutton test: OFF: LED0 lit, ON: LED1 lit\n\r" );
00224         pc.printf( " 3.Servo test: Repeated switching from 0 to right 30 to left 30 \n\r" );
00225         pc.printf( " 4.Right motor test: Repeated switching from forward to brake\n\r" );
00226         pc.printf( " 5.Right motor test: Repeated switching from reverse to brake\n\r" );
00227         pc.printf( " 6.Left motor test: Repeated switching from forward to brake\n\r" );
00228         pc.printf( " 7.Left motor test: Repeated switching from reverse to brake\n\r" );
00229         pc.printf( " 8.Straight running check: Forward at 50 PWM, stop after 2 seconds.\n\r" );
00230         pc.printf( " 9.Straight running check: Forward at 50 PWM, stop after 5 seconds.\n\r" );
00231         pc.printf( "10.Straight running check: Forward at 100 PWM, stop after 2 seconds.\n\r" );
00232         pc.printf( "11.Straight running check: Forward at 100 PWM, stop after 5 seconds.\n\r" );
00233         pc.printf( "12.Camera module test\n\r" );
00234 
00235         pc.printf( "\n\n\r" );
00236 
00237         pc.printf( "No = " );
00238         pc.scanf( "%d", &pattern );
00239         pc.printf( "\n\n\r" );
00240  
00241         cnt1 = 0;
00242         led_status_set( RUN );
00243  
00244         break;
00245 
00246     case 1:
00247         /* LEDs light alternately at 0.5-second intervals */
00248         if( cnt1 < 500 ) {
00249             led_out( 0x1 );
00250         } else if( cnt1 < 1000 ) {
00251             led_out( 0x2 );
00252         } else {
00253             cnt1 = 0;
00254         }
00255         break;
00256 
00257     case 2:
00258         /* Pushbutton test: OFF: LED0 lit, ON: LED1 lit */
00259         led_out( pushsw_get() + 1 );
00260         break;
00261 
00262     case 3:
00263         /* Servo test: Repeated switching from 0° to right 30°to left 30°*/
00264         if( cnt1 < 1000 ) {
00265             handle( 0 );
00266         } else if( cnt1 < 2000 ) {
00267             handle( 30 );
00268         } else if( cnt1 < 3000 ) {
00269             handle( -30 );
00270         } else {
00271             cnt1 = 0;
00272         }
00273         break;
00274 
00275     case 4:
00276         /* Right motor test: Repeated switching from forward to brake */
00277         if( cnt1 < 1000 ) {
00278             motor( 0, 100 );
00279         } else if( cnt1 < 2000 ) {
00280             motor( 0, 0 );
00281         } else {
00282             cnt1 = 0;
00283         }
00284         break;
00285 
00286     case 5:
00287         /* Right motor test: Repeated switching from reverse to brake */
00288         if( cnt1 < 1000 ) {
00289             motor( 0, -100 );
00290         } else if( cnt1 < 2000 ) {
00291             motor( 0, 0 );
00292         } else {
00293             cnt1 = 0;
00294         }
00295         break;
00296 
00297     case 6:
00298         /* Left motor test: Repeated switching from forward to brake */
00299         if( cnt1 < 1000 ) {
00300             motor( 100, 0 );
00301         } else if( cnt1 < 2000 ) {
00302             motor( 0, 0 );
00303         } else {
00304             cnt1 = 0;
00305         }
00306         break;
00307 
00308     case 7:
00309         /*  Left motor test: Repeated switching from reverse to brake */
00310         if( cnt1 < 1000 ) {
00311             motor( -100, 0 );
00312         } else if( cnt1 < 2000 ) {
00313             motor( 0, 0 );
00314         } else {
00315             cnt1 = 0;
00316         }
00317         break;
00318 
00319     case 8:
00320         /* Straight running check: Forward at 50% PWM, stop after 2 seconds */
00321         if( cnt1 < 2000 ) {
00322             motor( 0, 0 );
00323         } else if( cnt1 < 4000 ) {
00324             motor( 50, 50 );
00325         } else {
00326             motor( 0, 0 );
00327         }
00328         break;
00329 
00330     case 9:
00331         /* Straight running check: Forward at 50% PWM, stop after 5 seconds. */
00332         if( cnt1 < 2000 ) {
00333             motor( 0, 0 );
00334         } else if( cnt1 < 7000 ) {
00335             motor( 50, 50 );
00336         } else {
00337             motor( 0, 0 );
00338         }
00339         break;
00340 
00341     case 10:
00342         /* Straight running check: Forward at 100% PWM, stop after 2 seconds. */
00343         if( cnt1 < 2000 ) {
00344             motor( 0, 0 );
00345         } else if( cnt1 < 4000 ) {
00346             motor( 100, 100 );
00347         } else {
00348             motor( 0, 0 );
00349         }
00350         break;
00351 
00352     case 11:
00353         /* Straight running check: Forward at 100% PWM, stop after 5 seconds. */
00354         if( cnt1 < 2000 ) {
00355             motor( 0, 0 );
00356         } else if( cnt1 < 7000 ) {
00357             motor( 100, 100 );
00358         } else {
00359             motor( 0, 0 );
00360         }
00361         break;
00362 
00363     case 12:
00364         /* Camera module test process 1 */
00365         pc.printf( "Please push the SW ( on the Motor drive board )" );
00366         pc.printf( "\n\r" );
00367         pattern = 13;
00368         break;
00369 
00370     case 13:
00371         /* SW check */
00372         if ( pushsw_get() ) {
00373             WaitVfield( 2 );
00374             if (write_buff_addr == FrameBuffer_Video_A) {
00375                 write_buff_addr = FrameBuffer_Video_B;
00376                 save_buff_addr  = FrameBuffer_Video_A;
00377             } else {
00378                 write_buff_addr = FrameBuffer_Video_A;
00379                 save_buff_addr  = FrameBuffer_Video_B;
00380             }
00381             /* Change write buffer */
00382             error = Display.Video_Write_Change(
00383                         VIDEO_INPUT_CH,
00384                         write_buff_addr,
00385                         VIDEO_BUFFER_STRIDE);
00386             if (error != DisplayBase::GRAPHICS_OK) {
00387                 printf("Line %d, error %d\n", __LINE__, error);
00388                 while (1);
00389             }
00390             pattern = 14;
00391         }
00392         break;
00393 
00394     case 14:
00395         /* Camera module test process 2 */
00396         pc.printf( "//,X-Size,Y-Size" );
00397         pc.printf( "\n\r" );
00398         pc.printf( "#SIZE,320,240" );
00399         pc.printf( "\n\r" );
00400         pc.printf( "//,X-Point,Y-Point" );
00401         pc.printf( "\n\r" );
00402 
00403         for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){
00404             for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){
00405                 pc.printf( "#YCbCr," );
00406           /*Xp*/pc.printf( "%d,", x);
00407           /*Yp*/pc.printf( "%d,", y);
00408           /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6
00409           /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
00410           /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
00411                 pc.printf( "\n\r" );
00412 
00413                 pc.printf( "#YCbCr," );
00414           /*Xp*/pc.printf( "%d,", x+1);
00415           /*Yp*/pc.printf( "%d,", y);
00416           /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4
00417           /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
00418           /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
00419                 pc.printf( "\n\r" );
00420             }
00421         }
00422 
00423         /* Break Point */
00424         while( !user_button_get() ) led_status_set( STOP );
00425 
00426         break;
00427 
00428     default:
00429         /* If neither, return to standby state */
00430         pattern = 0;
00431         break;
00432     }
00433     }
00434 }
00435 
00436 //Initialize MTU2 PWM functions
00437 //------------------------------------------------------------------//
00438 //MTU2_3, MTU2_4
00439 //Reset-Synchronized PWM mode
00440 //TIOC4A(P4_4) :Left-motor
00441 //TIOC4B(P4_5) :Right-motor
00442 //------------------------------------------------------------------//
00443 void init_MTU2_PWM_Motor( void )
00444 {
00445     /* Port setting for S/W I/O Contorol */
00446     /* alternative mode     */
00447 
00448     /* MTU2_4 (P4_4)(P4_5)  */
00449     GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
00450     GPIOPFCAE4 &= 0xffcf;               /* The alternative function of a pin */
00451     GPIOPFCE4  |= 0x0030;               /* The alternative function of a pin */
00452     GPIOPFC4   &= 0xffcf;               /* The alternative function of a pin */
00453                                         /* 2nd altemative function/output   */
00454     GPIOP4     &= 0xffcf;               /*                          */
00455     GPIOPM4    &= 0xffcf;               /* p4_4,P4_5:output         */
00456     GPIOPMC4   |= 0x0030;               /* P4_4,P4_5:double         */
00457 
00458     /* Mosule stop 33(MTU2) canceling */
00459     CPGSTBCR3  &= 0xf7;
00460 
00461     /* MTU2_3 and MTU2_4 (Motor PWM) */
00462     MTU2TCR_3   = 0x20;                 /* TCNT Clear(TGRA), P0φ/1  */
00463     MTU2TOCR1   = 0x04;                 /*                          */
00464     MTU2TOCR2   = 0x40;                 /* N L>H  P H>L             */
00465     MTU2TMDR_3  = 0x38;                 /* Buff:ON Reset-Synchronized PWM mode */
00466     MTU2TMDR_4  = 0x30;                 /* Buff:ON                  */
00467     MTU2TOER    = 0xc6;                 /* TIOC3B,4A,4B enabled output */
00468     MTU2TCNT_3  = MTU2TCNT_4 = 0;       /* TCNT3,TCNT4 Set 0        */
00469     MTU2TGRA_3  = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
00470                                         /* PWM-Cycle(1ms)           */
00471     MTU2TGRA_4  = MTU2TGRC_4 = 0;       /* Left-motor(P4_4)         */
00472     MTU2TGRB_4  = MTU2TGRD_4 = 0;       /* Right-motor(P4_5)        */
00473     MTU2TSTR   |= 0x40;                 /* TCNT_4 Start             */
00474 }
00475 
00476 //Initialize MTU2 PWM functions
00477 //------------------------------------------------------------------//
00478 //MTU2_0
00479 //PWM mode 1
00480 //TIOC0A(P4_0) :Servo-motor
00481 //------------------------------------------------------------------//
00482 void init_MTU2_PWM_Servo( void )
00483 {
00484     /* Port setting for S/W I/O Contorol */
00485     /* alternative mode     */
00486 
00487     /* MTU2_0 (P4_0)        */
00488     GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
00489     GPIOPFCAE4 &= 0xfffe;               /* The alternative function of a pin */
00490     GPIOPFCE4  &= 0xfffe;               /* The alternative function of a pin */
00491     GPIOPFC4   |= 0x0001;               /* The alternative function of a pin */
00492                                         /* 2nd alternative function/output   */
00493     GPIOP4     &= 0xfffe;               /*                          */
00494     GPIOPM4    &= 0xfffe;               /* p4_0:output              */
00495     GPIOPMC4   |= 0x0001;               /* P4_0:double              */
00496 
00497     /* Mosule stop 33(MTU2) canceling */
00498     CPGSTBCR3  &= 0xf7;
00499 
00500     /* MTU2_0 (Motor PWM) */
00501     MTU2TCR_0   = 0x22;                 /* TCNT Clear(TGRA), P0φ/16 */
00502     MTU2TIORH_0 = 0x52;                 /* TGRA L>H, TGRB H>L       */
00503     MTU2TMDR_0  = 0x32;                 /* TGRC and TGRD = Buff-mode*/
00504                                         /* PWM-mode1                */
00505     MTU2TCNT_0  = 0;                    /* TCNT0 Set 0              */
00506     MTU2TGRA_0  = MTU2TGRC_0 = SERVO_PWM_CYCLE;
00507                                         /* PWM-Cycle(16ms)          */
00508     MTU2TGRB_0  = MTU2TGRD_0 = 0;       /* Servo-motor(P4_0)        */
00509     MTU2TSTR   |= 0x01;                 /* TCNT_0 Start             */
00510 }
00511 
00512 //Interrupt Timer
00513 //------------------------------------------------------------------//
00514 void intTimer( void )
00515 {
00516     cnt0++;
00517     cnt1++;
00518  
00519     led_status_process();
00520 }
00521 
00522 //LED_RGB(on GR-PEACH board)
00523 //------------------------------------------------------------------//
00524 void led_rgb(int led)
00525 {
00526     LED_R = led & 0x1;
00527     LED_G = (led >> 1 ) & 0x1;
00528     LED_B = (led >> 2 ) & 0x1;
00529 }
00530 
00531 //user_button_get(on GR-PEACH board)
00532 //------------------------------------------------------------------//
00533 unsigned int user_button_get( void )
00534 {
00535     return (~user_botton) & 0x1;        /* Read ports with switches */
00536 }
00537 
00538 //LED_Status(on GR-PEACH board) Function for only interrupt
00539 //------------------------------------------------------------------//
00540 void led_status_process( void )
00541 {
00542     static unsigned long    led_timer;
00543     int                     led_set;
00544     int                     on_time;
00545     int                     off_time;
00546 
00547     /* setting */
00548     switch( status_set ){
00549     case RUN:
00550         led_set  = LED_GREEN;
00551         on_time  = 500;
00552         off_time = 500;
00553         break;
00554 
00555     case SENSOR:
00556         led_set  = LED_BLUE;
00557         on_time  = 50;
00558         off_time = 50;
00559         break;
00560 
00561     case MARK:
00562         led_set  = LED_RED;
00563         on_time  = 250;
00564         off_time = 250;
00565         break;
00566 
00567     case STOP:
00568         led_set  = LED_RED;
00569         on_time  = 1;
00570         off_time = 0;
00571         break;
00572 
00573     case ERROR:
00574         led_set  = LED_RED;
00575         on_time  = 50;
00576         off_time = 50;
00577         break;
00578 
00579     default:
00580         led_set  = LED_OFF;
00581         on_time  = 0;
00582         off_time = 1;
00583         break;
00584     }
00585 
00586     /* Display */
00587     led_timer++;
00588     if( led_timer < on_time ) led_rgb( led_set );
00589     else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF );
00590     else led_timer = 0;
00591 }
00592 
00593 //LED_Status(on GR-PEACH board) Function for only interrupt
00594 //------------------------------------------------------------------//
00595 void led_status_set( int set )
00596 {
00597     status_set = set;
00598 }
00599 
00600 //led_out(on Motor drive board)
00601 //------------------------------------------------------------------//
00602 void led_out(int led)
00603 {
00604     led = ~led;
00605     LED_3 = led & 0x1;
00606     LED_2 = ( led >> 1 ) & 0x1;
00607 }
00608 
00609 //pushsw_get(on Motor drive board)
00610 //------------------------------------------------------------------//
00611 unsigned int pushsw_get( void )
00612 {
00613     return (~push_sw) & 0x1;            /* Read ports with switches */
00614 }
00615 
00616 //motor speed control(PWM)
00617 //Arguments: motor:-100 to 100
00618 //Here, 0 is stop, 100 is forward, -100 is reverse
00619 //------------------------------------------------------------------//
00620 void motor( int accele_l, int accele_r )
00621 {
00622     accele_l = ( accele_l * MAX_SPEED ) / 100;
00623     accele_r = ( accele_r * MAX_SPEED ) / 100;
00624  
00625     /* Left Motor Control */
00626     if( accele_l >= 0 ) {
00627         /* forward */
00628         Left_motor_signal = 0;
00629         MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
00630     } else {
00631         /* reverse */
00632         Left_motor_signal = 1;
00633         MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
00634     }
00635  
00636     /* Right Motor Control */
00637     if( accele_r >= 0 ) {
00638         /* forward */
00639         Right_motor_signal = 0;
00640         MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
00641     } else {
00642         /* reverse */
00643         Right_motor_signal = 1;
00644         MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
00645     }
00646 }
00647 
00648 //Handle fanction
00649 //------------------------------------------------------------------//
00650 void handle( int angle )
00651 {
00652     /* When the servo move from left to right in reverse, replace "-" with "+" */
00653     MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
00654 }
00655 
00656 //******************************************************************//
00657 // @brief       Interrupt callback function
00658 // @param[in]   int_type    : VDC5 interrupt type
00659 // @retval      None
00660 //*******************************************************************/
00661 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
00662 {
00663     if (vfield_count > 0) {
00664         vfield_count--;
00665     }
00666 }
00667 
00668 //******************************************************************//
00669 // @brief       Wait for the specified number of times Vsync occurs
00670 // @param[in]   wait_count          : Wait count
00671 // @retval      None
00672 //*******************************************************************/
00673 static void WaitVfield(const int32_t wait_count)
00674 {
00675     vfield_count = wait_count;
00676     while (vfield_count > 0) {
00677         /* Do nothing */
00678     }
00679 }
00680 
00681 //******************************************************************//
00682 // @brief       Interrupt callback function for Vsync interruption
00683 // @param[in]   int_type    : VDC5 interrupt type
00684 // @retval      None
00685 //*******************************************************************/
00686 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
00687 {
00688     if (vsync_count > 0) {
00689         vsync_count--;
00690     }
00691 }
00692 
00693 //******************************************************************//
00694 // @brief       Wait for the specified number of times Vsync occurs
00695 // @param[in]   wait_count          : Wait count
00696 // @retval      None
00697 //*******************************************************************/
00698 static void WaitVsync(const int32_t wait_count)
00699 {
00700     vsync_count = wait_count;
00701     while (vsync_count > 0) {
00702         /* Do nothing */
00703     }
00704 }
00705 
00706 //------------------------------------------------------------------//
00707 // End of file
00708 //------------------------------------------------------------------//