Micon Car Rally / Mbed 2 deprecated Operation_Test

This is operation test program.

Dependencies:   GR-PEACH_video mbed

main.cpp@0:a749dcbc0cf6, 2016-09-02 (annotated)

Committer:
TetsuyaKonno
Date:
Fri Sep 02 06:53:21 2016 +0000
Revision:
0:a749dcbc0cf6
First program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:a749dcbc0cf6 1 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:a749dcbc0cf6 3 //File Contents: Operation Test Program (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:a749dcbc0cf6 4 //Version number: Ver.1.02
TetsuyaKonno 0:a749dcbc0cf6 5 //Date: 2016.02.23
TetsuyaKonno 0:a749dcbc0cf6 6 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:a749dcbc0cf6 7 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 8
TetsuyaKonno 0:a749dcbc0cf6 9 //This program supports the following boards:
TetsuyaKonno 0:a749dcbc0cf6 10 //* GR-PEACH(E version)
TetsuyaKonno 0:a749dcbc0cf6 11 //* Motor drive board Ver.5
TetsuyaKonno 0:a749dcbc0cf6 12 //* Camera module (SC-310)
TetsuyaKonno 0:a749dcbc0cf6 13
TetsuyaKonno 0:a749dcbc0cf6 14 //Include
TetsuyaKonno 0:a749dcbc0cf6 15 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 16 #include "mbed.h"
TetsuyaKonno 0:a749dcbc0cf6 17 #include "iodefine.h"
TetsuyaKonno 0:a749dcbc0cf6 18 #include "DisplayBace.h"
TetsuyaKonno 0:a749dcbc0cf6 19
TetsuyaKonno 0:a749dcbc0cf6 20 //Define
TetsuyaKonno 0:a749dcbc0cf6 21 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 22 //Motor PWM cycle
TetsuyaKonno 0:a749dcbc0cf6 23 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */
TetsuyaKonno 0:a749dcbc0cf6 24 /* 1ms P0φ/1 = 0.03us */
TetsuyaKonno 0:a749dcbc0cf6 25 //Motor speed
TetsuyaKonno 0:a749dcbc0cf6 26 #define MAX_SPEED 50 /* motor() set: 0 to 100 */
TetsuyaKonno 0:a749dcbc0cf6 27
TetsuyaKonno 0:a749dcbc0cf6 28 //Servo PWM cycle
TetsuyaKonno 0:a749dcbc0cf6 29 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
TetsuyaKonno 0:a749dcbc0cf6 30 /* 16ms P0φ/16 = 0.48us */
TetsuyaKonno 0:a749dcbc0cf6 31 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
TetsuyaKonno 0:a749dcbc0cf6 32 #define HANDLE_STEP 18 /* 1 degree value */
TetsuyaKonno 0:a749dcbc0cf6 33
TetsuyaKonno 0:a749dcbc0cf6 34 //LED Color on GR-PEACH
TetsuyaKonno 0:a749dcbc0cf6 35 #define LED_OFF 0x00
TetsuyaKonno 0:a749dcbc0cf6 36 #define LED_RED 0x01
TetsuyaKonno 0:a749dcbc0cf6 37 #define LED_GREEN 0x02
TetsuyaKonno 0:a749dcbc0cf6 38 #define LED_YELLOW 0x03
TetsuyaKonno 0:a749dcbc0cf6 39 #define LED_BLUE 0x04
TetsuyaKonno 0:a749dcbc0cf6 40 #define LED_PURPLE 0x05
TetsuyaKonno 0:a749dcbc0cf6 41 #define LED_SKYBLUE 0x06
TetsuyaKonno 0:a749dcbc0cf6 42 #define LED_WHITE 0x07
TetsuyaKonno 0:a749dcbc0cf6 43
TetsuyaKonno 0:a749dcbc0cf6 44 //Status
TetsuyaKonno 0:a749dcbc0cf6 45 #define RUN 0x00
TetsuyaKonno 0:a749dcbc0cf6 46 #define SENSOR 0x01
TetsuyaKonno 0:a749dcbc0cf6 47 #define MARK 0x02
TetsuyaKonno 0:a749dcbc0cf6 48 #define STOP 0x03
TetsuyaKonno 0:a749dcbc0cf6 49 #define ERROR 0xff
TetsuyaKonno 0:a749dcbc0cf6 50
TetsuyaKonno 0:a749dcbc0cf6 51 //Define(NTSC-Video)
TetsuyaKonno 0:a749dcbc0cf6 52 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 53 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0)
TetsuyaKonno 0:a749dcbc0cf6 54 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD)
TetsuyaKonno 0:a749dcbc0cf6 55 #define DATA_SIZE_PER_PIC (2u)
TetsuyaKonno 0:a749dcbc0cf6 56
TetsuyaKonno 0:a749dcbc0cf6 57 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
TetsuyaKonno 0:a749dcbc0cf6 58 in accordance with the frame buffer burst transfer mode. */
TetsuyaKonno 0:a749dcbc0cf6 59 #define PIXEL_HW (320u) /* QVGA */
TetsuyaKonno 0:a749dcbc0cf6 60 #define PIXEL_VW (240u) /* QVGA */
TetsuyaKonno 0:a749dcbc0cf6 61 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
TetsuyaKonno 0:a749dcbc0cf6 62 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW)
TetsuyaKonno 0:a749dcbc0cf6 63
TetsuyaKonno 0:a749dcbc0cf6 64 //Constructor
TetsuyaKonno 0:a749dcbc0cf6 65 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 66 Ticker interrput;
TetsuyaKonno 0:a749dcbc0cf6 67 DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */
TetsuyaKonno 0:a749dcbc0cf6 68 DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */
TetsuyaKonno 0:a749dcbc0cf6 69 Serial pc(USBTX, USBRX);
TetsuyaKonno 0:a749dcbc0cf6 70 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */
TetsuyaKonno 0:a749dcbc0cf6 71 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */
TetsuyaKonno 0:a749dcbc0cf6 72 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */
TetsuyaKonno 0:a749dcbc0cf6 73 DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */
TetsuyaKonno 0:a749dcbc0cf6 74 DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */
TetsuyaKonno 0:a749dcbc0cf6 75 DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */
TetsuyaKonno 0:a749dcbc0cf6 76 DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */
TetsuyaKonno 0:a749dcbc0cf6 77
TetsuyaKonno 0:a749dcbc0cf6 78 //Prototype
TetsuyaKonno 0:a749dcbc0cf6 79 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 80 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */
TetsuyaKonno 0:a749dcbc0cf6 81 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
TetsuyaKonno 0:a749dcbc0cf6 82 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:a749dcbc0cf6 83 void led_rgb(int led);
TetsuyaKonno 0:a749dcbc0cf6 84 unsigned int user_button_get( void );
TetsuyaKonno 0:a749dcbc0cf6 85 void led_status_process( void ); /* Function for only interrupt */
TetsuyaKonno 0:a749dcbc0cf6 86 void led_status_set( int set );
TetsuyaKonno 0:a749dcbc0cf6 87 void led_out(int led);
TetsuyaKonno 0:a749dcbc0cf6 88 unsigned int pushsw_get( void );
TetsuyaKonno 0:a749dcbc0cf6 89 void motor( int accele_l, int accele_r );
TetsuyaKonno 0:a749dcbc0cf6 90 void handle( int angle );
TetsuyaKonno 0:a749dcbc0cf6 91
TetsuyaKonno 0:a749dcbc0cf6 92 //Prototype(NTSC-video)
TetsuyaKonno 0:a749dcbc0cf6 93 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 94 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:a749dcbc0cf6 95 static void WaitVfield(const int32_t wait_count);
TetsuyaKonno 0:a749dcbc0cf6 96 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:a749dcbc0cf6 97 static void WaitVsync(const int32_t wait_count);
TetsuyaKonno 0:a749dcbc0cf6 98
TetsuyaKonno 0:a749dcbc0cf6 99 //Globle
TetsuyaKonno 0:a749dcbc0cf6 100 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 101 volatile unsigned long cnt0; /* Used by timer function */
TetsuyaKonno 0:a749dcbc0cf6 102 volatile unsigned long cnt1; /* Used within main */
TetsuyaKonno 0:a749dcbc0cf6 103 volatile int pattern; /* Pattern numbers */
TetsuyaKonno 0:a749dcbc0cf6 104 volatile int status_set; /* Status */
TetsuyaKonno 0:a749dcbc0cf6 105
TetsuyaKonno 0:a749dcbc0cf6 106 //Globle(NTSC-video)
TetsuyaKonno 0:a749dcbc0cf6 107 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 108 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:a749dcbc0cf6 109 static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:a749dcbc0cf6 110 static volatile int32_t vsync_count;
TetsuyaKonno 0:a749dcbc0cf6 111 static volatile int32_t vfield_count;
TetsuyaKonno 0:a749dcbc0cf6 112
TetsuyaKonno 0:a749dcbc0cf6 113 volatile long int Xp;
TetsuyaKonno 0:a749dcbc0cf6 114 volatile long int Yp;
TetsuyaKonno 0:a749dcbc0cf6 115 volatile int x;
TetsuyaKonno 0:a749dcbc0cf6 116 volatile int y;
TetsuyaKonno 0:a749dcbc0cf6 117
TetsuyaKonno 0:a749dcbc0cf6 118 //Main
TetsuyaKonno 0:a749dcbc0cf6 119 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 120 int main( void )
TetsuyaKonno 0:a749dcbc0cf6 121 {
TetsuyaKonno 0:a749dcbc0cf6 122 /* NTSC-Video */
TetsuyaKonno 0:a749dcbc0cf6 123 DisplayBase::graphics_error_t error;
TetsuyaKonno 0:a749dcbc0cf6 124 uint8_t * write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:a749dcbc0cf6 125 uint8_t * save_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:a749dcbc0cf6 126
TetsuyaKonno 0:a749dcbc0cf6 127 /* Create DisplayBase object */
TetsuyaKonno 0:a749dcbc0cf6 128 DisplayBase Display;
TetsuyaKonno 0:a749dcbc0cf6 129
TetsuyaKonno 0:a749dcbc0cf6 130 /* Graphics initialization process */
TetsuyaKonno 0:a749dcbc0cf6 131 error = Display.Graphics_init(NULL);
TetsuyaKonno 0:a749dcbc0cf6 132 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 133 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 134 while (1);
TetsuyaKonno 0:a749dcbc0cf6 135 }
TetsuyaKonno 0:a749dcbc0cf6 136
TetsuyaKonno 0:a749dcbc0cf6 137 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
TetsuyaKonno 0:a749dcbc0cf6 138 if( error != DisplayBase::GRAPHICS_OK ) {
TetsuyaKonno 0:a749dcbc0cf6 139 while(1);
TetsuyaKonno 0:a749dcbc0cf6 140 }
TetsuyaKonno 0:a749dcbc0cf6 141
TetsuyaKonno 0:a749dcbc0cf6 142 /* Interrupt callback function setting (Vsync signal input to scaler 0) */
TetsuyaKonno 0:a749dcbc0cf6 143 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
TetsuyaKonno 0:a749dcbc0cf6 144 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 145 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 146 while (1);
TetsuyaKonno 0:a749dcbc0cf6 147 }
TetsuyaKonno 0:a749dcbc0cf6 148 /* Video capture setting (progressive form fixed) */
TetsuyaKonno 0:a749dcbc0cf6 149 error = Display.Video_Write_Setting(
TetsuyaKonno 0:a749dcbc0cf6 150 VIDEO_INPUT_CH,
TetsuyaKonno 0:a749dcbc0cf6 151 DisplayBase::COL_SYS_NTSC_358,
TetsuyaKonno 0:a749dcbc0cf6 152 write_buff_addr,
TetsuyaKonno 0:a749dcbc0cf6 153 VIDEO_BUFFER_STRIDE,
TetsuyaKonno 0:a749dcbc0cf6 154 DisplayBase::VIDEO_FORMAT_YCBCR422,
TetsuyaKonno 0:a749dcbc0cf6 155 DisplayBase::WR_RD_WRSWA_32_16BIT,
TetsuyaKonno 0:a749dcbc0cf6 156 PIXEL_VW,
TetsuyaKonno 0:a749dcbc0cf6 157 PIXEL_HW
TetsuyaKonno 0:a749dcbc0cf6 158 );
TetsuyaKonno 0:a749dcbc0cf6 159 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 160 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 161 while (1);
TetsuyaKonno 0:a749dcbc0cf6 162 }
TetsuyaKonno 0:a749dcbc0cf6 163
TetsuyaKonno 0:a749dcbc0cf6 164 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
TetsuyaKonno 0:a749dcbc0cf6 165 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
TetsuyaKonno 0:a749dcbc0cf6 166 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 167 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 168 while (1);
TetsuyaKonno 0:a749dcbc0cf6 169 }
TetsuyaKonno 0:a749dcbc0cf6 170
TetsuyaKonno 0:a749dcbc0cf6 171 /* Video write process start */
TetsuyaKonno 0:a749dcbc0cf6 172 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:a749dcbc0cf6 173 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 174 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 175 while (1);
TetsuyaKonno 0:a749dcbc0cf6 176 }
TetsuyaKonno 0:a749dcbc0cf6 177
TetsuyaKonno 0:a749dcbc0cf6 178 /* Video write process stop */
TetsuyaKonno 0:a749dcbc0cf6 179 error = Display.Video_Stop (VIDEO_INPUT_CH);
TetsuyaKonno 0:a749dcbc0cf6 180 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 181 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 182 while (1);
TetsuyaKonno 0:a749dcbc0cf6 183 }
TetsuyaKonno 0:a749dcbc0cf6 184
TetsuyaKonno 0:a749dcbc0cf6 185 /* Video write process start */
TetsuyaKonno 0:a749dcbc0cf6 186 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:a749dcbc0cf6 187 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 188 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 189 while (1);
TetsuyaKonno 0:a749dcbc0cf6 190 }
TetsuyaKonno 0:a749dcbc0cf6 191
TetsuyaKonno 0:a749dcbc0cf6 192 /* Wait vsync to update resister */
TetsuyaKonno 0:a749dcbc0cf6 193 WaitVsync(1);
TetsuyaKonno 0:a749dcbc0cf6 194
TetsuyaKonno 0:a749dcbc0cf6 195 /* Wait 2 Vfield(Top or bottom field) */
TetsuyaKonno 0:a749dcbc0cf6 196 WaitVfield(2);
TetsuyaKonno 0:a749dcbc0cf6 197
TetsuyaKonno 0:a749dcbc0cf6 198 /* Initialize MCU functions */
TetsuyaKonno 0:a749dcbc0cf6 199 init_MTU2_PWM_Motor();
TetsuyaKonno 0:a749dcbc0cf6 200 init_MTU2_PWM_Servo();
TetsuyaKonno 0:a749dcbc0cf6 201 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:a749dcbc0cf6 202 pc.baud(230400);
TetsuyaKonno 0:a749dcbc0cf6 203
TetsuyaKonno 0:a749dcbc0cf6 204 /* Initialize Micon Car state */
TetsuyaKonno 0:a749dcbc0cf6 205 led_out( 0x0 );
TetsuyaKonno 0:a749dcbc0cf6 206 handle( 0 );
TetsuyaKonno 0:a749dcbc0cf6 207 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 208
TetsuyaKonno 0:a749dcbc0cf6 209 while(1) {
TetsuyaKonno 0:a749dcbc0cf6 210
TetsuyaKonno 0:a749dcbc0cf6 211 if( user_button_get() ){
TetsuyaKonno 0:a749dcbc0cf6 212 led_out( 0x0 );
TetsuyaKonno 0:a749dcbc0cf6 213 handle( 0 );
TetsuyaKonno 0:a749dcbc0cf6 214 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 215 pattern = 0;
TetsuyaKonno 0:a749dcbc0cf6 216 }
TetsuyaKonno 0:a749dcbc0cf6 217
TetsuyaKonno 0:a749dcbc0cf6 218 switch( pattern ) {
TetsuyaKonno 0:a749dcbc0cf6 219 case 0:
TetsuyaKonno 0:a749dcbc0cf6 220 led_status_set( STOP );
TetsuyaKonno 0:a749dcbc0cf6 221 pc.printf( "GR-PEACH MCR Operation Test Program V1.00\n\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 222 pc.printf( " 1.LEDs light alternately at 0.5-second intervals\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 223 pc.printf( " 2.Pushbutton test: OFF: LED0 lit, ON: LED1 lit\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 224 pc.printf( " 3.Servo test: Repeated switching from 0 to right 30 to left 30 \n\r" );
TetsuyaKonno 0:a749dcbc0cf6 225 pc.printf( " 4.Right motor test: Repeated switching from forward to brake\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 226 pc.printf( " 5.Right motor test: Repeated switching from reverse to brake\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 227 pc.printf( " 6.Left motor test: Repeated switching from forward to brake\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 228 pc.printf( " 7.Left motor test: Repeated switching from reverse to brake\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 229 pc.printf( " 8.Straight running check: Forward at 50 PWM, stop after 2 seconds.\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 230 pc.printf( " 9.Straight running check: Forward at 50 PWM, stop after 5 seconds.\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 231 pc.printf( "10.Straight running check: Forward at 100 PWM, stop after 2 seconds.\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 232 pc.printf( "11.Straight running check: Forward at 100 PWM, stop after 5 seconds.\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 233 pc.printf( "12.Camera module test\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 234
TetsuyaKonno 0:a749dcbc0cf6 235 pc.printf( "\n\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 236
TetsuyaKonno 0:a749dcbc0cf6 237 pc.printf( "No = " );
TetsuyaKonno 0:a749dcbc0cf6 238 pc.scanf( "%d", &pattern );
TetsuyaKonno 0:a749dcbc0cf6 239 pc.printf( "\n\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 240
TetsuyaKonno 0:a749dcbc0cf6 241 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 242 led_status_set( RUN );
TetsuyaKonno 0:a749dcbc0cf6 243
TetsuyaKonno 0:a749dcbc0cf6 244 break;
TetsuyaKonno 0:a749dcbc0cf6 245
TetsuyaKonno 0:a749dcbc0cf6 246 case 1:
TetsuyaKonno 0:a749dcbc0cf6 247 /* LEDs light alternately at 0.5-second intervals */
TetsuyaKonno 0:a749dcbc0cf6 248 if( cnt1 < 500 ) {
TetsuyaKonno 0:a749dcbc0cf6 249 led_out( 0x1 );
TetsuyaKonno 0:a749dcbc0cf6 250 } else if( cnt1 < 1000 ) {
TetsuyaKonno 0:a749dcbc0cf6 251 led_out( 0x2 );
TetsuyaKonno 0:a749dcbc0cf6 252 } else {
TetsuyaKonno 0:a749dcbc0cf6 253 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 254 }
TetsuyaKonno 0:a749dcbc0cf6 255 break;
TetsuyaKonno 0:a749dcbc0cf6 256
TetsuyaKonno 0:a749dcbc0cf6 257 case 2:
TetsuyaKonno 0:a749dcbc0cf6 258 /* Pushbutton test: OFF: LED0 lit, ON: LED1 lit */
TetsuyaKonno 0:a749dcbc0cf6 259 led_out( pushsw_get() + 1 );
TetsuyaKonno 0:a749dcbc0cf6 260 break;
TetsuyaKonno 0:a749dcbc0cf6 261
TetsuyaKonno 0:a749dcbc0cf6 262 case 3:
TetsuyaKonno 0:a749dcbc0cf6 263 /* Servo test: Repeated switching from 0° to right 30°to left 30°*/
TetsuyaKonno 0:a749dcbc0cf6 264 if( cnt1 < 1000 ) {
TetsuyaKonno 0:a749dcbc0cf6 265 handle( 0 );
TetsuyaKonno 0:a749dcbc0cf6 266 } else if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 267 handle( 30 );
TetsuyaKonno 0:a749dcbc0cf6 268 } else if( cnt1 < 3000 ) {
TetsuyaKonno 0:a749dcbc0cf6 269 handle( -30 );
TetsuyaKonno 0:a749dcbc0cf6 270 } else {
TetsuyaKonno 0:a749dcbc0cf6 271 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 272 }
TetsuyaKonno 0:a749dcbc0cf6 273 break;
TetsuyaKonno 0:a749dcbc0cf6 274
TetsuyaKonno 0:a749dcbc0cf6 275 case 4:
TetsuyaKonno 0:a749dcbc0cf6 276 /* Right motor test: Repeated switching from forward to brake */
TetsuyaKonno 0:a749dcbc0cf6 277 if( cnt1 < 1000 ) {
TetsuyaKonno 0:a749dcbc0cf6 278 motor( 0, 100 );
TetsuyaKonno 0:a749dcbc0cf6 279 } else if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 280 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 281 } else {
TetsuyaKonno 0:a749dcbc0cf6 282 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 283 }
TetsuyaKonno 0:a749dcbc0cf6 284 break;
TetsuyaKonno 0:a749dcbc0cf6 285
TetsuyaKonno 0:a749dcbc0cf6 286 case 5:
TetsuyaKonno 0:a749dcbc0cf6 287 /* Right motor test: Repeated switching from reverse to brake */
TetsuyaKonno 0:a749dcbc0cf6 288 if( cnt1 < 1000 ) {
TetsuyaKonno 0:a749dcbc0cf6 289 motor( 0, -100 );
TetsuyaKonno 0:a749dcbc0cf6 290 } else if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 291 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 292 } else {
TetsuyaKonno 0:a749dcbc0cf6 293 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 294 }
TetsuyaKonno 0:a749dcbc0cf6 295 break;
TetsuyaKonno 0:a749dcbc0cf6 296
TetsuyaKonno 0:a749dcbc0cf6 297 case 6:
TetsuyaKonno 0:a749dcbc0cf6 298 /* Left motor test: Repeated switching from forward to brake */
TetsuyaKonno 0:a749dcbc0cf6 299 if( cnt1 < 1000 ) {
TetsuyaKonno 0:a749dcbc0cf6 300 motor( 100, 0 );
TetsuyaKonno 0:a749dcbc0cf6 301 } else if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 302 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 303 } else {
TetsuyaKonno 0:a749dcbc0cf6 304 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 305 }
TetsuyaKonno 0:a749dcbc0cf6 306 break;
TetsuyaKonno 0:a749dcbc0cf6 307
TetsuyaKonno 0:a749dcbc0cf6 308 case 7:
TetsuyaKonno 0:a749dcbc0cf6 309 /* Left motor test: Repeated switching from reverse to brake */
TetsuyaKonno 0:a749dcbc0cf6 310 if( cnt1 < 1000 ) {
TetsuyaKonno 0:a749dcbc0cf6 311 motor( -100, 0 );
TetsuyaKonno 0:a749dcbc0cf6 312 } else if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 313 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 314 } else {
TetsuyaKonno 0:a749dcbc0cf6 315 cnt1 = 0;
TetsuyaKonno 0:a749dcbc0cf6 316 }
TetsuyaKonno 0:a749dcbc0cf6 317 break;
TetsuyaKonno 0:a749dcbc0cf6 318
TetsuyaKonno 0:a749dcbc0cf6 319 case 8:
TetsuyaKonno 0:a749dcbc0cf6 320 /* Straight running check: Forward at 50% PWM, stop after 2 seconds */
TetsuyaKonno 0:a749dcbc0cf6 321 if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 322 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 323 } else if( cnt1 < 4000 ) {
TetsuyaKonno 0:a749dcbc0cf6 324 motor( 50, 50 );
TetsuyaKonno 0:a749dcbc0cf6 325 } else {
TetsuyaKonno 0:a749dcbc0cf6 326 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 327 }
TetsuyaKonno 0:a749dcbc0cf6 328 break;
TetsuyaKonno 0:a749dcbc0cf6 329
TetsuyaKonno 0:a749dcbc0cf6 330 case 9:
TetsuyaKonno 0:a749dcbc0cf6 331 /* Straight running check: Forward at 50% PWM, stop after 5 seconds. */
TetsuyaKonno 0:a749dcbc0cf6 332 if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 333 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 334 } else if( cnt1 < 7000 ) {
TetsuyaKonno 0:a749dcbc0cf6 335 motor( 50, 50 );
TetsuyaKonno 0:a749dcbc0cf6 336 } else {
TetsuyaKonno 0:a749dcbc0cf6 337 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 338 }
TetsuyaKonno 0:a749dcbc0cf6 339 break;
TetsuyaKonno 0:a749dcbc0cf6 340
TetsuyaKonno 0:a749dcbc0cf6 341 case 10:
TetsuyaKonno 0:a749dcbc0cf6 342 /* Straight running check: Forward at 100% PWM, stop after 2 seconds. */
TetsuyaKonno 0:a749dcbc0cf6 343 if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 344 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 345 } else if( cnt1 < 4000 ) {
TetsuyaKonno 0:a749dcbc0cf6 346 motor( 100, 100 );
TetsuyaKonno 0:a749dcbc0cf6 347 } else {
TetsuyaKonno 0:a749dcbc0cf6 348 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 349 }
TetsuyaKonno 0:a749dcbc0cf6 350 break;
TetsuyaKonno 0:a749dcbc0cf6 351
TetsuyaKonno 0:a749dcbc0cf6 352 case 11:
TetsuyaKonno 0:a749dcbc0cf6 353 /* Straight running check: Forward at 100% PWM, stop after 5 seconds. */
TetsuyaKonno 0:a749dcbc0cf6 354 if( cnt1 < 2000 ) {
TetsuyaKonno 0:a749dcbc0cf6 355 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 356 } else if( cnt1 < 7000 ) {
TetsuyaKonno 0:a749dcbc0cf6 357 motor( 100, 100 );
TetsuyaKonno 0:a749dcbc0cf6 358 } else {
TetsuyaKonno 0:a749dcbc0cf6 359 motor( 0, 0 );
TetsuyaKonno 0:a749dcbc0cf6 360 }
TetsuyaKonno 0:a749dcbc0cf6 361 break;
TetsuyaKonno 0:a749dcbc0cf6 362
TetsuyaKonno 0:a749dcbc0cf6 363 case 12:
TetsuyaKonno 0:a749dcbc0cf6 364 /* Camera module test process 1 */
TetsuyaKonno 0:a749dcbc0cf6 365 pc.printf( "Please push the SW ( on the Motor drive board )" );
TetsuyaKonno 0:a749dcbc0cf6 366 pc.printf( "\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 367 pattern = 13;
TetsuyaKonno 0:a749dcbc0cf6 368 break;
TetsuyaKonno 0:a749dcbc0cf6 369
TetsuyaKonno 0:a749dcbc0cf6 370 case 13:
TetsuyaKonno 0:a749dcbc0cf6 371 /* SW check */
TetsuyaKonno 0:a749dcbc0cf6 372 if ( pushsw_get() ) {
TetsuyaKonno 0:a749dcbc0cf6 373 WaitVfield( 2 );
TetsuyaKonno 0:a749dcbc0cf6 374 if (write_buff_addr == FrameBuffer_Video_A) {
TetsuyaKonno 0:a749dcbc0cf6 375 write_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:a749dcbc0cf6 376 save_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:a749dcbc0cf6 377 } else {
TetsuyaKonno 0:a749dcbc0cf6 378 write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:a749dcbc0cf6 379 save_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:a749dcbc0cf6 380 }
TetsuyaKonno 0:a749dcbc0cf6 381 /* Change write buffer */
TetsuyaKonno 0:a749dcbc0cf6 382 error = Display.Video_Write_Change(
TetsuyaKonno 0:a749dcbc0cf6 383 VIDEO_INPUT_CH,
TetsuyaKonno 0:a749dcbc0cf6 384 write_buff_addr,
TetsuyaKonno 0:a749dcbc0cf6 385 VIDEO_BUFFER_STRIDE);
TetsuyaKonno 0:a749dcbc0cf6 386 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:a749dcbc0cf6 387 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:a749dcbc0cf6 388 while (1);
TetsuyaKonno 0:a749dcbc0cf6 389 }
TetsuyaKonno 0:a749dcbc0cf6 390 pattern = 14;
TetsuyaKonno 0:a749dcbc0cf6 391 }
TetsuyaKonno 0:a749dcbc0cf6 392 break;
TetsuyaKonno 0:a749dcbc0cf6 393
TetsuyaKonno 0:a749dcbc0cf6 394 case 14:
TetsuyaKonno 0:a749dcbc0cf6 395 /* Camera module test process 2 */
TetsuyaKonno 0:a749dcbc0cf6 396 pc.printf( "//,X-Size,Y-Size" );
TetsuyaKonno 0:a749dcbc0cf6 397 pc.printf( "\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 398 pc.printf( "#SIZE,320,240" );
TetsuyaKonno 0:a749dcbc0cf6 399 pc.printf( "\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 400 pc.printf( "//,X-Point,Y-Point" );
TetsuyaKonno 0:a749dcbc0cf6 401 pc.printf( "\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 402
TetsuyaKonno 0:a749dcbc0cf6 403 for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){
TetsuyaKonno 0:a749dcbc0cf6 404 for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){
TetsuyaKonno 0:a749dcbc0cf6 405 pc.printf( "#YCbCr," );
TetsuyaKonno 0:a749dcbc0cf6 406 /*Xp*/pc.printf( "%d,", x);
TetsuyaKonno 0:a749dcbc0cf6 407 /*Yp*/pc.printf( "%d,", y);
TetsuyaKonno 0:a749dcbc0cf6 408 /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6
TetsuyaKonno 0:a749dcbc0cf6 409 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
TetsuyaKonno 0:a749dcbc0cf6 410 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
TetsuyaKonno 0:a749dcbc0cf6 411 pc.printf( "\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 412
TetsuyaKonno 0:a749dcbc0cf6 413 pc.printf( "#YCbCr," );
TetsuyaKonno 0:a749dcbc0cf6 414 /*Xp*/pc.printf( "%d,", x+1);
TetsuyaKonno 0:a749dcbc0cf6 415 /*Yp*/pc.printf( "%d,", y);
TetsuyaKonno 0:a749dcbc0cf6 416 /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4
TetsuyaKonno 0:a749dcbc0cf6 417 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
TetsuyaKonno 0:a749dcbc0cf6 418 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
TetsuyaKonno 0:a749dcbc0cf6 419 pc.printf( "\n\r" );
TetsuyaKonno 0:a749dcbc0cf6 420 }
TetsuyaKonno 0:a749dcbc0cf6 421 }
TetsuyaKonno 0:a749dcbc0cf6 422
TetsuyaKonno 0:a749dcbc0cf6 423 /* Break Point */
TetsuyaKonno 0:a749dcbc0cf6 424 while( !user_button_get() ) led_status_set( STOP );
TetsuyaKonno 0:a749dcbc0cf6 425
TetsuyaKonno 0:a749dcbc0cf6 426 break;
TetsuyaKonno 0:a749dcbc0cf6 427
TetsuyaKonno 0:a749dcbc0cf6 428 default:
TetsuyaKonno 0:a749dcbc0cf6 429 /* If neither, return to standby state */
TetsuyaKonno 0:a749dcbc0cf6 430 pattern = 0;
TetsuyaKonno 0:a749dcbc0cf6 431 break;
TetsuyaKonno 0:a749dcbc0cf6 432 }
TetsuyaKonno 0:a749dcbc0cf6 433 }
TetsuyaKonno 0:a749dcbc0cf6 434 }
TetsuyaKonno 0:a749dcbc0cf6 435
TetsuyaKonno 0:a749dcbc0cf6 436 //Initialize MTU2 PWM functions
TetsuyaKonno 0:a749dcbc0cf6 437 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 438 //MTU2_3, MTU2_4
TetsuyaKonno 0:a749dcbc0cf6 439 //Reset-Synchronized PWM mode
TetsuyaKonno 0:a749dcbc0cf6 440 //TIOC4A(P4_4) :Left-motor
TetsuyaKonno 0:a749dcbc0cf6 441 //TIOC4B(P4_5) :Right-motor
TetsuyaKonno 0:a749dcbc0cf6 442 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 443 void init_MTU2_PWM_Motor( void )
TetsuyaKonno 0:a749dcbc0cf6 444 {
TetsuyaKonno 0:a749dcbc0cf6 445 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:a749dcbc0cf6 446 /* alternative mode */
TetsuyaKonno 0:a749dcbc0cf6 447
TetsuyaKonno 0:a749dcbc0cf6 448 /* MTU2_4 (P4_4)(P4_5) */
TetsuyaKonno 0:a749dcbc0cf6 449 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:a749dcbc0cf6 450 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:a749dcbc0cf6 451 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */
TetsuyaKonno 0:a749dcbc0cf6 452 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:a749dcbc0cf6 453 /* 2nd altemative function/output */
TetsuyaKonno 0:a749dcbc0cf6 454 GPIOP4 &= 0xffcf; /* */
TetsuyaKonno 0:a749dcbc0cf6 455 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */
TetsuyaKonno 0:a749dcbc0cf6 456 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */
TetsuyaKonno 0:a749dcbc0cf6 457
TetsuyaKonno 0:a749dcbc0cf6 458 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:a749dcbc0cf6 459 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:a749dcbc0cf6 460
TetsuyaKonno 0:a749dcbc0cf6 461 /* MTU2_3 and MTU2_4 (Motor PWM) */
TetsuyaKonno 0:a749dcbc0cf6 462 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */
TetsuyaKonno 0:a749dcbc0cf6 463 MTU2TOCR1 = 0x04; /* */
TetsuyaKonno 0:a749dcbc0cf6 464 MTU2TOCR2 = 0x40; /* N L>H P H>L */
TetsuyaKonno 0:a749dcbc0cf6 465 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */
TetsuyaKonno 0:a749dcbc0cf6 466 MTU2TMDR_4 = 0x30; /* Buff:ON */
TetsuyaKonno 0:a749dcbc0cf6 467 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */
TetsuyaKonno 0:a749dcbc0cf6 468 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
TetsuyaKonno 0:a749dcbc0cf6 469 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
TetsuyaKonno 0:a749dcbc0cf6 470 /* PWM-Cycle(1ms) */
TetsuyaKonno 0:a749dcbc0cf6 471 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */
TetsuyaKonno 0:a749dcbc0cf6 472 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */
TetsuyaKonno 0:a749dcbc0cf6 473 MTU2TSTR |= 0x40; /* TCNT_4 Start */
TetsuyaKonno 0:a749dcbc0cf6 474 }
TetsuyaKonno 0:a749dcbc0cf6 475
TetsuyaKonno 0:a749dcbc0cf6 476 //Initialize MTU2 PWM functions
TetsuyaKonno 0:a749dcbc0cf6 477 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 478 //MTU2_0
TetsuyaKonno 0:a749dcbc0cf6 479 //PWM mode 1
TetsuyaKonno 0:a749dcbc0cf6 480 //TIOC0A(P4_0) :Servo-motor
TetsuyaKonno 0:a749dcbc0cf6 481 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 482 void init_MTU2_PWM_Servo( void )
TetsuyaKonno 0:a749dcbc0cf6 483 {
TetsuyaKonno 0:a749dcbc0cf6 484 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:a749dcbc0cf6 485 /* alternative mode */
TetsuyaKonno 0:a749dcbc0cf6 486
TetsuyaKonno 0:a749dcbc0cf6 487 /* MTU2_0 (P4_0) */
TetsuyaKonno 0:a749dcbc0cf6 488 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:a749dcbc0cf6 489 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:a749dcbc0cf6 490 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:a749dcbc0cf6 491 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
TetsuyaKonno 0:a749dcbc0cf6 492 /* 2nd alternative function/output */
TetsuyaKonno 0:a749dcbc0cf6 493 GPIOP4 &= 0xfffe; /* */
TetsuyaKonno 0:a749dcbc0cf6 494 GPIOPM4 &= 0xfffe; /* p4_0:output */
TetsuyaKonno 0:a749dcbc0cf6 495 GPIOPMC4 |= 0x0001; /* P4_0:double */
TetsuyaKonno 0:a749dcbc0cf6 496
TetsuyaKonno 0:a749dcbc0cf6 497 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:a749dcbc0cf6 498 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:a749dcbc0cf6 499
TetsuyaKonno 0:a749dcbc0cf6 500 /* MTU2_0 (Motor PWM) */
TetsuyaKonno 0:a749dcbc0cf6 501 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
TetsuyaKonno 0:a749dcbc0cf6 502 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
TetsuyaKonno 0:a749dcbc0cf6 503 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
TetsuyaKonno 0:a749dcbc0cf6 504 /* PWM-mode1 */
TetsuyaKonno 0:a749dcbc0cf6 505 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
TetsuyaKonno 0:a749dcbc0cf6 506 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
TetsuyaKonno 0:a749dcbc0cf6 507 /* PWM-Cycle(16ms) */
TetsuyaKonno 0:a749dcbc0cf6 508 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
TetsuyaKonno 0:a749dcbc0cf6 509 MTU2TSTR |= 0x01; /* TCNT_0 Start */
TetsuyaKonno 0:a749dcbc0cf6 510 }
TetsuyaKonno 0:a749dcbc0cf6 511
TetsuyaKonno 0:a749dcbc0cf6 512 //Interrupt Timer
TetsuyaKonno 0:a749dcbc0cf6 513 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 514 void intTimer( void )
TetsuyaKonno 0:a749dcbc0cf6 515 {
TetsuyaKonno 0:a749dcbc0cf6 516 cnt0++;
TetsuyaKonno 0:a749dcbc0cf6 517 cnt1++;
TetsuyaKonno 0:a749dcbc0cf6 518
TetsuyaKonno 0:a749dcbc0cf6 519 led_status_process();
TetsuyaKonno 0:a749dcbc0cf6 520 }
TetsuyaKonno 0:a749dcbc0cf6 521
TetsuyaKonno 0:a749dcbc0cf6 522 //LED_RGB(on GR-PEACH board)
TetsuyaKonno 0:a749dcbc0cf6 523 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 524 void led_rgb(int led)
TetsuyaKonno 0:a749dcbc0cf6 525 {
TetsuyaKonno 0:a749dcbc0cf6 526 LED_R = led & 0x1;
TetsuyaKonno 0:a749dcbc0cf6 527 LED_G = (led >> 1 ) & 0x1;
TetsuyaKonno 0:a749dcbc0cf6 528 LED_B = (led >> 2 ) & 0x1;
TetsuyaKonno 0:a749dcbc0cf6 529 }
TetsuyaKonno 0:a749dcbc0cf6 530
TetsuyaKonno 0:a749dcbc0cf6 531 //user_button_get(on GR-PEACH board)
TetsuyaKonno 0:a749dcbc0cf6 532 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 533 unsigned int user_button_get( void )
TetsuyaKonno 0:a749dcbc0cf6 534 {
TetsuyaKonno 0:a749dcbc0cf6 535 return (~user_botton) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:a749dcbc0cf6 536 }
TetsuyaKonno 0:a749dcbc0cf6 537
TetsuyaKonno 0:a749dcbc0cf6 538 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:a749dcbc0cf6 539 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 540 void led_status_process( void )
TetsuyaKonno 0:a749dcbc0cf6 541 {
TetsuyaKonno 0:a749dcbc0cf6 542 static unsigned long led_timer;
TetsuyaKonno 0:a749dcbc0cf6 543 int led_set;
TetsuyaKonno 0:a749dcbc0cf6 544 int on_time;
TetsuyaKonno 0:a749dcbc0cf6 545 int off_time;
TetsuyaKonno 0:a749dcbc0cf6 546
TetsuyaKonno 0:a749dcbc0cf6 547 /* setting */
TetsuyaKonno 0:a749dcbc0cf6 548 switch( status_set ){
TetsuyaKonno 0:a749dcbc0cf6 549 case RUN:
TetsuyaKonno 0:a749dcbc0cf6 550 led_set = LED_GREEN;
TetsuyaKonno 0:a749dcbc0cf6 551 on_time = 500;
TetsuyaKonno 0:a749dcbc0cf6 552 off_time = 500;
TetsuyaKonno 0:a749dcbc0cf6 553 break;
TetsuyaKonno 0:a749dcbc0cf6 554
TetsuyaKonno 0:a749dcbc0cf6 555 case SENSOR:
TetsuyaKonno 0:a749dcbc0cf6 556 led_set = LED_BLUE;
TetsuyaKonno 0:a749dcbc0cf6 557 on_time = 50;
TetsuyaKonno 0:a749dcbc0cf6 558 off_time = 50;
TetsuyaKonno 0:a749dcbc0cf6 559 break;
TetsuyaKonno 0:a749dcbc0cf6 560
TetsuyaKonno 0:a749dcbc0cf6 561 case MARK:
TetsuyaKonno 0:a749dcbc0cf6 562 led_set = LED_RED;
TetsuyaKonno 0:a749dcbc0cf6 563 on_time = 250;
TetsuyaKonno 0:a749dcbc0cf6 564 off_time = 250;
TetsuyaKonno 0:a749dcbc0cf6 565 break;
TetsuyaKonno 0:a749dcbc0cf6 566
TetsuyaKonno 0:a749dcbc0cf6 567 case STOP:
TetsuyaKonno 0:a749dcbc0cf6 568 led_set = LED_RED;
TetsuyaKonno 0:a749dcbc0cf6 569 on_time = 1;
TetsuyaKonno 0:a749dcbc0cf6 570 off_time = 0;
TetsuyaKonno 0:a749dcbc0cf6 571 break;
TetsuyaKonno 0:a749dcbc0cf6 572
TetsuyaKonno 0:a749dcbc0cf6 573 case ERROR:
TetsuyaKonno 0:a749dcbc0cf6 574 led_set = LED_RED;
TetsuyaKonno 0:a749dcbc0cf6 575 on_time = 50;
TetsuyaKonno 0:a749dcbc0cf6 576 off_time = 50;
TetsuyaKonno 0:a749dcbc0cf6 577 break;
TetsuyaKonno 0:a749dcbc0cf6 578
TetsuyaKonno 0:a749dcbc0cf6 579 default:
TetsuyaKonno 0:a749dcbc0cf6 580 led_set = LED_OFF;
TetsuyaKonno 0:a749dcbc0cf6 581 on_time = 0;
TetsuyaKonno 0:a749dcbc0cf6 582 off_time = 1;
TetsuyaKonno 0:a749dcbc0cf6 583 break;
TetsuyaKonno 0:a749dcbc0cf6 584 }
TetsuyaKonno 0:a749dcbc0cf6 585
TetsuyaKonno 0:a749dcbc0cf6 586 /* Display */
TetsuyaKonno 0:a749dcbc0cf6 587 led_timer++;
TetsuyaKonno 0:a749dcbc0cf6 588 if( led_timer < on_time ) led_rgb( led_set );
TetsuyaKonno 0:a749dcbc0cf6 589 else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF );
TetsuyaKonno 0:a749dcbc0cf6 590 else led_timer = 0;
TetsuyaKonno 0:a749dcbc0cf6 591 }
TetsuyaKonno 0:a749dcbc0cf6 592
TetsuyaKonno 0:a749dcbc0cf6 593 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:a749dcbc0cf6 594 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 595 void led_status_set( int set )
TetsuyaKonno 0:a749dcbc0cf6 596 {
TetsuyaKonno 0:a749dcbc0cf6 597 status_set = set;
TetsuyaKonno 0:a749dcbc0cf6 598 }
TetsuyaKonno 0:a749dcbc0cf6 599
TetsuyaKonno 0:a749dcbc0cf6 600 //led_out(on Motor drive board)
TetsuyaKonno 0:a749dcbc0cf6 601 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 602 void led_out(int led)
TetsuyaKonno 0:a749dcbc0cf6 603 {
TetsuyaKonno 0:a749dcbc0cf6 604 led = ~led;
TetsuyaKonno 0:a749dcbc0cf6 605 LED_3 = led & 0x1;
TetsuyaKonno 0:a749dcbc0cf6 606 LED_2 = ( led >> 1 ) & 0x1;
TetsuyaKonno 0:a749dcbc0cf6 607 }
TetsuyaKonno 0:a749dcbc0cf6 608
TetsuyaKonno 0:a749dcbc0cf6 609 //pushsw_get(on Motor drive board)
TetsuyaKonno 0:a749dcbc0cf6 610 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 611 unsigned int pushsw_get( void )
TetsuyaKonno 0:a749dcbc0cf6 612 {
TetsuyaKonno 0:a749dcbc0cf6 613 return (~push_sw) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:a749dcbc0cf6 614 }
TetsuyaKonno 0:a749dcbc0cf6 615
TetsuyaKonno 0:a749dcbc0cf6 616 //motor speed control(PWM)
TetsuyaKonno 0:a749dcbc0cf6 617 //Arguments: motor:-100 to 100
TetsuyaKonno 0:a749dcbc0cf6 618 //Here, 0 is stop, 100 is forward, -100 is reverse
TetsuyaKonno 0:a749dcbc0cf6 619 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 620 void motor( int accele_l, int accele_r )
TetsuyaKonno 0:a749dcbc0cf6 621 {
TetsuyaKonno 0:a749dcbc0cf6 622 accele_l = ( accele_l * MAX_SPEED ) / 100;
TetsuyaKonno 0:a749dcbc0cf6 623 accele_r = ( accele_r * MAX_SPEED ) / 100;
TetsuyaKonno 0:a749dcbc0cf6 624
TetsuyaKonno 0:a749dcbc0cf6 625 /* Left Motor Control */
TetsuyaKonno 0:a749dcbc0cf6 626 if( accele_l >= 0 ) {
TetsuyaKonno 0:a749dcbc0cf6 627 /* forward */
TetsuyaKonno 0:a749dcbc0cf6 628 Left_motor_signal = 0;
TetsuyaKonno 0:a749dcbc0cf6 629 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
TetsuyaKonno 0:a749dcbc0cf6 630 } else {
TetsuyaKonno 0:a749dcbc0cf6 631 /* reverse */
TetsuyaKonno 0:a749dcbc0cf6 632 Left_motor_signal = 1;
TetsuyaKonno 0:a749dcbc0cf6 633 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
TetsuyaKonno 0:a749dcbc0cf6 634 }
TetsuyaKonno 0:a749dcbc0cf6 635
TetsuyaKonno 0:a749dcbc0cf6 636 /* Right Motor Control */
TetsuyaKonno 0:a749dcbc0cf6 637 if( accele_r >= 0 ) {
TetsuyaKonno 0:a749dcbc0cf6 638 /* forward */
TetsuyaKonno 0:a749dcbc0cf6 639 Right_motor_signal = 0;
TetsuyaKonno 0:a749dcbc0cf6 640 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
TetsuyaKonno 0:a749dcbc0cf6 641 } else {
TetsuyaKonno 0:a749dcbc0cf6 642 /* reverse */
TetsuyaKonno 0:a749dcbc0cf6 643 Right_motor_signal = 1;
TetsuyaKonno 0:a749dcbc0cf6 644 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
TetsuyaKonno 0:a749dcbc0cf6 645 }
TetsuyaKonno 0:a749dcbc0cf6 646 }
TetsuyaKonno 0:a749dcbc0cf6 647
TetsuyaKonno 0:a749dcbc0cf6 648 //Handle fanction
TetsuyaKonno 0:a749dcbc0cf6 649 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 650 void handle( int angle )
TetsuyaKonno 0:a749dcbc0cf6 651 {
TetsuyaKonno 0:a749dcbc0cf6 652 /* When the servo move from left to right in reverse, replace "-" with "+" */
TetsuyaKonno 0:a749dcbc0cf6 653 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
TetsuyaKonno 0:a749dcbc0cf6 654 }
TetsuyaKonno 0:a749dcbc0cf6 655
TetsuyaKonno 0:a749dcbc0cf6 656 //******************************************************************//
TetsuyaKonno 0:a749dcbc0cf6 657 // @brief Interrupt callback function
TetsuyaKonno 0:a749dcbc0cf6 658 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:a749dcbc0cf6 659 // @retval None
TetsuyaKonno 0:a749dcbc0cf6 660 //*******************************************************************/
TetsuyaKonno 0:a749dcbc0cf6 661 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:a749dcbc0cf6 662 {
TetsuyaKonno 0:a749dcbc0cf6 663 if (vfield_count > 0) {
TetsuyaKonno 0:a749dcbc0cf6 664 vfield_count--;
TetsuyaKonno 0:a749dcbc0cf6 665 }
TetsuyaKonno 0:a749dcbc0cf6 666 }
TetsuyaKonno 0:a749dcbc0cf6 667
TetsuyaKonno 0:a749dcbc0cf6 668 //******************************************************************//
TetsuyaKonno 0:a749dcbc0cf6 669 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:a749dcbc0cf6 670 // @param[in] wait_count : Wait count
TetsuyaKonno 0:a749dcbc0cf6 671 // @retval None
TetsuyaKonno 0:a749dcbc0cf6 672 //*******************************************************************/
TetsuyaKonno 0:a749dcbc0cf6 673 static void WaitVfield(const int32_t wait_count)
TetsuyaKonno 0:a749dcbc0cf6 674 {
TetsuyaKonno 0:a749dcbc0cf6 675 vfield_count = wait_count;
TetsuyaKonno 0:a749dcbc0cf6 676 while (vfield_count > 0) {
TetsuyaKonno 0:a749dcbc0cf6 677 /* Do nothing */
TetsuyaKonno 0:a749dcbc0cf6 678 }
TetsuyaKonno 0:a749dcbc0cf6 679 }
TetsuyaKonno 0:a749dcbc0cf6 680
TetsuyaKonno 0:a749dcbc0cf6 681 //******************************************************************//
TetsuyaKonno 0:a749dcbc0cf6 682 // @brief Interrupt callback function for Vsync interruption
TetsuyaKonno 0:a749dcbc0cf6 683 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:a749dcbc0cf6 684 // @retval None
TetsuyaKonno 0:a749dcbc0cf6 685 //*******************************************************************/
TetsuyaKonno 0:a749dcbc0cf6 686 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:a749dcbc0cf6 687 {
TetsuyaKonno 0:a749dcbc0cf6 688 if (vsync_count > 0) {
TetsuyaKonno 0:a749dcbc0cf6 689 vsync_count--;
TetsuyaKonno 0:a749dcbc0cf6 690 }
TetsuyaKonno 0:a749dcbc0cf6 691 }
TetsuyaKonno 0:a749dcbc0cf6 692
TetsuyaKonno 0:a749dcbc0cf6 693 //******************************************************************//
TetsuyaKonno 0:a749dcbc0cf6 694 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:a749dcbc0cf6 695 // @param[in] wait_count : Wait count
TetsuyaKonno 0:a749dcbc0cf6 696 // @retval None
TetsuyaKonno 0:a749dcbc0cf6 697 //*******************************************************************/
TetsuyaKonno 0:a749dcbc0cf6 698 static void WaitVsync(const int32_t wait_count)
TetsuyaKonno 0:a749dcbc0cf6 699 {
TetsuyaKonno 0:a749dcbc0cf6 700 vsync_count = wait_count;
TetsuyaKonno 0:a749dcbc0cf6 701 while (vsync_count > 0) {
TetsuyaKonno 0:a749dcbc0cf6 702 /* Do nothing */
TetsuyaKonno 0:a749dcbc0cf6 703 }
TetsuyaKonno 0:a749dcbc0cf6 704 }
TetsuyaKonno 0:a749dcbc0cf6 705
TetsuyaKonno 0:a749dcbc0cf6 706 //------------------------------------------------------------------//
TetsuyaKonno 0:a749dcbc0cf6 707 // End of file
TetsuyaKonno 0:a749dcbc0cf6 708 //------------------------------------------------------------------//