This is operation test program.
Dependencies: GR-PEACH_video mbed
main.cpp
- Committer:
- TetsuyaKonno
- Date:
- 2016-09-02
- Revision:
- 0:a749dcbc0cf6
File content as of revision 0:a749dcbc0cf6:
//------------------------------------------------------------------// //Supported MCU: RZ/A1H //File Contents: Operation Test Program (GR-PEACH version on the Micon Car) //Version number: Ver.1.02 //Date: 2016.02.23 //Copyright: Renesas Electronics Corporation //------------------------------------------------------------------// //This program supports the following boards: //* GR-PEACH(E version) //* Motor drive board Ver.5 //* Camera module (SC-310) //Include //------------------------------------------------------------------// #include "mbed.h" #include "iodefine.h" #include "DisplayBace.h" //Define //------------------------------------------------------------------// //Motor PWM cycle #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */ /* 1ms P0φ/1 = 0.03us */ //Motor speed #define MAX_SPEED 50 /* motor() set: 0 to 100 */ //Servo PWM cycle #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */ /* 16ms P0φ/16 = 0.48us */ #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/ #define HANDLE_STEP 18 /* 1 degree value */ //LED Color on GR-PEACH #define LED_OFF 0x00 #define LED_RED 0x01 #define LED_GREEN 0x02 #define LED_YELLOW 0x03 #define LED_BLUE 0x04 #define LED_PURPLE 0x05 #define LED_SKYBLUE 0x06 #define LED_WHITE 0x07 //Status #define RUN 0x00 #define SENSOR 0x01 #define MARK 0x02 #define STOP 0x03 #define ERROR 0xff //Define(NTSC-Video) //------------------------------------------------------------------// #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0) #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD) #define DATA_SIZE_PER_PIC (2u) /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128 in accordance with the frame buffer burst transfer mode. */ #define PIXEL_HW (320u) /* QVGA */ #define PIXEL_VW (240u) /* QVGA */ #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u) #define VIDEO_BUFFER_HEIGHT (PIXEL_VW) //Constructor //------------------------------------------------------------------// Ticker interrput; DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */ DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */ Serial pc(USBTX, USBRX); DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */ DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */ DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */ DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */ DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */ DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */ DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */ //Prototype //------------------------------------------------------------------// void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */ void intTimer( void ); /* Interrupt fanction */ void led_rgb(int led); unsigned int user_button_get( void ); void led_status_process( void ); /* Function for only interrupt */ void led_status_set( int set ); void led_out(int led); unsigned int pushsw_get( void ); void motor( int accele_l, int accele_r ); void handle( int angle ); //Prototype(NTSC-video) //------------------------------------------------------------------// static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type); static void WaitVfield(const int32_t wait_count); static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type); static void WaitVsync(const int32_t wait_count); //Globle //------------------------------------------------------------------// volatile unsigned long cnt0; /* Used by timer function */ volatile unsigned long cnt1; /* Used within main */ volatile int pattern; /* Pattern numbers */ volatile int status_set; /* Status */ //Globle(NTSC-video) //------------------------------------------------------------------// static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; static volatile int32_t vsync_count; static volatile int32_t vfield_count; volatile long int Xp; volatile long int Yp; volatile int x; volatile int y; //Main //------------------------------------------------------------------// int main( void ) { /* NTSC-Video */ DisplayBase::graphics_error_t error; uint8_t * write_buff_addr = FrameBuffer_Video_A; uint8_t * save_buff_addr = FrameBuffer_Video_B; /* Create DisplayBase object */ DisplayBase Display; /* Graphics initialization process */ error = Display.Graphics_init(NULL); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL); if( error != DisplayBase::GRAPHICS_OK ) { while(1); } /* Interrupt callback function setting (Vsync signal input to scaler 0) */ error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video capture setting (progressive form fixed) */ error = Display.Video_Write_Setting( VIDEO_INPUT_CH, DisplayBase::COL_SYS_NTSC_358, write_buff_addr, VIDEO_BUFFER_STRIDE, DisplayBase::VIDEO_FORMAT_YCBCR422, DisplayBase::WR_RD_WRSWA_32_16BIT, PIXEL_VW, PIXEL_HW ); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */ error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video write process start */ error = Display.Video_Start (VIDEO_INPUT_CH); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video write process stop */ error = Display.Video_Stop (VIDEO_INPUT_CH); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video write process start */ error = Display.Video_Start (VIDEO_INPUT_CH); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Wait vsync to update resister */ WaitVsync(1); /* Wait 2 Vfield(Top or bottom field) */ WaitVfield(2); /* Initialize MCU functions */ init_MTU2_PWM_Motor(); init_MTU2_PWM_Servo(); interrput.attach(&intTimer, 0.001); pc.baud(230400); /* Initialize Micon Car state */ led_out( 0x0 ); handle( 0 ); motor( 0, 0 ); while(1) { if( user_button_get() ){ led_out( 0x0 ); handle( 0 ); motor( 0, 0 ); pattern = 0; } switch( pattern ) { case 0: led_status_set( STOP ); pc.printf( "GR-PEACH MCR Operation Test Program V1.00\n\n\r" ); pc.printf( " 1.LEDs light alternately at 0.5-second intervals\n\r" ); pc.printf( " 2.Pushbutton test: OFF: LED0 lit, ON: LED1 lit\n\r" ); pc.printf( " 3.Servo test: Repeated switching from 0 to right 30 to left 30 \n\r" ); pc.printf( " 4.Right motor test: Repeated switching from forward to brake\n\r" ); pc.printf( " 5.Right motor test: Repeated switching from reverse to brake\n\r" ); pc.printf( " 6.Left motor test: Repeated switching from forward to brake\n\r" ); pc.printf( " 7.Left motor test: Repeated switching from reverse to brake\n\r" ); pc.printf( " 8.Straight running check: Forward at 50 PWM, stop after 2 seconds.\n\r" ); pc.printf( " 9.Straight running check: Forward at 50 PWM, stop after 5 seconds.\n\r" ); pc.printf( "10.Straight running check: Forward at 100 PWM, stop after 2 seconds.\n\r" ); pc.printf( "11.Straight running check: Forward at 100 PWM, stop after 5 seconds.\n\r" ); pc.printf( "12.Camera module test\n\r" ); pc.printf( "\n\n\r" ); pc.printf( "No = " ); pc.scanf( "%d", &pattern ); pc.printf( "\n\n\r" ); cnt1 = 0; led_status_set( RUN ); break; case 1: /* LEDs light alternately at 0.5-second intervals */ if( cnt1 < 500 ) { led_out( 0x1 ); } else if( cnt1 < 1000 ) { led_out( 0x2 ); } else { cnt1 = 0; } break; case 2: /* Pushbutton test: OFF: LED0 lit, ON: LED1 lit */ led_out( pushsw_get() + 1 ); break; case 3: /* Servo test: Repeated switching from 0° to right 30°to left 30°*/ if( cnt1 < 1000 ) { handle( 0 ); } else if( cnt1 < 2000 ) { handle( 30 ); } else if( cnt1 < 3000 ) { handle( -30 ); } else { cnt1 = 0; } break; case 4: /* Right motor test: Repeated switching from forward to brake */ if( cnt1 < 1000 ) { motor( 0, 100 ); } else if( cnt1 < 2000 ) { motor( 0, 0 ); } else { cnt1 = 0; } break; case 5: /* Right motor test: Repeated switching from reverse to brake */ if( cnt1 < 1000 ) { motor( 0, -100 ); } else if( cnt1 < 2000 ) { motor( 0, 0 ); } else { cnt1 = 0; } break; case 6: /* Left motor test: Repeated switching from forward to brake */ if( cnt1 < 1000 ) { motor( 100, 0 ); } else if( cnt1 < 2000 ) { motor( 0, 0 ); } else { cnt1 = 0; } break; case 7: /* Left motor test: Repeated switching from reverse to brake */ if( cnt1 < 1000 ) { motor( -100, 0 ); } else if( cnt1 < 2000 ) { motor( 0, 0 ); } else { cnt1 = 0; } break; case 8: /* Straight running check: Forward at 50% PWM, stop after 2 seconds */ if( cnt1 < 2000 ) { motor( 0, 0 ); } else if( cnt1 < 4000 ) { motor( 50, 50 ); } else { motor( 0, 0 ); } break; case 9: /* Straight running check: Forward at 50% PWM, stop after 5 seconds. */ if( cnt1 < 2000 ) { motor( 0, 0 ); } else if( cnt1 < 7000 ) { motor( 50, 50 ); } else { motor( 0, 0 ); } break; case 10: /* Straight running check: Forward at 100% PWM, stop after 2 seconds. */ if( cnt1 < 2000 ) { motor( 0, 0 ); } else if( cnt1 < 4000 ) { motor( 100, 100 ); } else { motor( 0, 0 ); } break; case 11: /* Straight running check: Forward at 100% PWM, stop after 5 seconds. */ if( cnt1 < 2000 ) { motor( 0, 0 ); } else if( cnt1 < 7000 ) { motor( 100, 100 ); } else { motor( 0, 0 ); } break; case 12: /* Camera module test process 1 */ pc.printf( "Please push the SW ( on the Motor drive board )" ); pc.printf( "\n\r" ); pattern = 13; break; case 13: /* SW check */ if ( pushsw_get() ) { WaitVfield( 2 ); if (write_buff_addr == FrameBuffer_Video_A) { write_buff_addr = FrameBuffer_Video_B; save_buff_addr = FrameBuffer_Video_A; } else { write_buff_addr = FrameBuffer_Video_A; save_buff_addr = FrameBuffer_Video_B; } /* Change write buffer */ error = Display.Video_Write_Change( VIDEO_INPUT_CH, write_buff_addr, VIDEO_BUFFER_STRIDE); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } pattern = 14; } break; case 14: /* Camera module test process 2 */ pc.printf( "//,X-Size,Y-Size" ); pc.printf( "\n\r" ); pc.printf( "#SIZE,320,240" ); pc.printf( "\n\r" ); pc.printf( "//,X-Point,Y-Point" ); pc.printf( "\n\r" ); for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){ for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){ pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", x); /*Yp*/pc.printf( "%d,", y); /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7 pc.printf( "\n\r" ); pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", x+1); /*Yp*/pc.printf( "%d,", y); /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7 pc.printf( "\n\r" ); } } /* Break Point */ while( !user_button_get() ) led_status_set( STOP ); break; default: /* If neither, return to standby state */ pattern = 0; break; } } } //Initialize MTU2 PWM functions //------------------------------------------------------------------// //MTU2_3, MTU2_4 //Reset-Synchronized PWM mode //TIOC4A(P4_4) :Left-motor //TIOC4B(P4_5) :Right-motor //------------------------------------------------------------------// void init_MTU2_PWM_Motor( void ) { /* Port setting for S/W I/O Contorol */ /* alternative mode */ /* MTU2_4 (P4_4)(P4_5) */ GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */ GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */ GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */ /* 2nd altemative function/output */ GPIOP4 &= 0xffcf; /* */ GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */ GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */ /* Mosule stop 33(MTU2) canceling */ CPGSTBCR3 &= 0xf7; /* MTU2_3 and MTU2_4 (Motor PWM) */ MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */ MTU2TOCR1 = 0x04; /* */ MTU2TOCR2 = 0x40; /* N L>H P H>L */ MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */ MTU2TMDR_4 = 0x30; /* Buff:ON */ MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */ MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE; /* PWM-Cycle(1ms) */ MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */ MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */ MTU2TSTR |= 0x40; /* TCNT_4 Start */ } //Initialize MTU2 PWM functions //------------------------------------------------------------------// //MTU2_0 //PWM mode 1 //TIOC0A(P4_0) :Servo-motor //------------------------------------------------------------------// void init_MTU2_PWM_Servo( void ) { /* Port setting for S/W I/O Contorol */ /* alternative mode */ /* MTU2_0 (P4_0) */ GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */ GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */ GPIOPFC4 |= 0x0001; /* The alternative function of a pin */ /* 2nd alternative function/output */ GPIOP4 &= 0xfffe; /* */ GPIOPM4 &= 0xfffe; /* p4_0:output */ GPIOPMC4 |= 0x0001; /* P4_0:double */ /* Mosule stop 33(MTU2) canceling */ CPGSTBCR3 &= 0xf7; /* MTU2_0 (Motor PWM) */ MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */ MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */ MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/ /* PWM-mode1 */ MTU2TCNT_0 = 0; /* TCNT0 Set 0 */ MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE; /* PWM-Cycle(16ms) */ MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */ MTU2TSTR |= 0x01; /* TCNT_0 Start */ } //Interrupt Timer //------------------------------------------------------------------// void intTimer( void ) { cnt0++; cnt1++; led_status_process(); } //LED_RGB(on GR-PEACH board) //------------------------------------------------------------------// void led_rgb(int led) { LED_R = led & 0x1; LED_G = (led >> 1 ) & 0x1; LED_B = (led >> 2 ) & 0x1; } //user_button_get(on GR-PEACH board) //------------------------------------------------------------------// unsigned int user_button_get( void ) { return (~user_botton) & 0x1; /* Read ports with switches */ } //LED_Status(on GR-PEACH board) Function for only interrupt //------------------------------------------------------------------// void led_status_process( void ) { static unsigned long led_timer; int led_set; int on_time; int off_time; /* setting */ switch( status_set ){ case RUN: led_set = LED_GREEN; on_time = 500; off_time = 500; break; case SENSOR: led_set = LED_BLUE; on_time = 50; off_time = 50; break; case MARK: led_set = LED_RED; on_time = 250; off_time = 250; break; case STOP: led_set = LED_RED; on_time = 1; off_time = 0; break; case ERROR: led_set = LED_RED; on_time = 50; off_time = 50; break; default: led_set = LED_OFF; on_time = 0; off_time = 1; break; } /* Display */ led_timer++; if( led_timer < on_time ) led_rgb( led_set ); else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF ); else led_timer = 0; } //LED_Status(on GR-PEACH board) Function for only interrupt //------------------------------------------------------------------// void led_status_set( int set ) { status_set = set; } //led_out(on Motor drive board) //------------------------------------------------------------------// void led_out(int led) { led = ~led; LED_3 = led & 0x1; LED_2 = ( led >> 1 ) & 0x1; } //pushsw_get(on Motor drive board) //------------------------------------------------------------------// unsigned int pushsw_get( void ) { return (~push_sw) & 0x1; /* Read ports with switches */ } //motor speed control(PWM) //Arguments: motor:-100 to 100 //Here, 0 is stop, 100 is forward, -100 is reverse //------------------------------------------------------------------// void motor( int accele_l, int accele_r ) { accele_l = ( accele_l * MAX_SPEED ) / 100; accele_r = ( accele_r * MAX_SPEED ) / 100; /* Left Motor Control */ if( accele_l >= 0 ) { /* forward */ Left_motor_signal = 0; MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; } else { /* reverse */ Left_motor_signal = 1; MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; } /* Right Motor Control */ if( accele_r >= 0 ) { /* forward */ Right_motor_signal = 0; MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; } else { /* reverse */ Right_motor_signal = 1; MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; } } //Handle fanction //------------------------------------------------------------------// void handle( int angle ) { /* When the servo move from left to right in reverse, replace "-" with "+" */ MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP; } //******************************************************************// // @brief Interrupt callback function // @param[in] int_type : VDC5 interrupt type // @retval None //*******************************************************************/ static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) { if (vfield_count > 0) { vfield_count--; } } //******************************************************************// // @brief Wait for the specified number of times Vsync occurs // @param[in] wait_count : Wait count // @retval None //*******************************************************************/ static void WaitVfield(const int32_t wait_count) { vfield_count = wait_count; while (vfield_count > 0) { /* Do nothing */ } } //******************************************************************// // @brief Interrupt callback function for Vsync interruption // @param[in] int_type : VDC5 interrupt type // @retval None //*******************************************************************/ static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type) { if (vsync_count > 0) { vsync_count--; } } //******************************************************************// // @brief Wait for the specified number of times Vsync occurs // @param[in] wait_count : Wait count // @retval None //*******************************************************************/ static void WaitVsync(const int32_t wait_count) { vsync_count = wait_count; while (vsync_count > 0) { /* Do nothing */ } } //------------------------------------------------------------------// // End of file //------------------------------------------------------------------//