Micon Car Rally / Mbed 2 deprecated Trace_Program

This program is line trace program by image processing.

Dependencies:   GR-PEACH_video mbed

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

Committer:
TetsuyaKonno
Date:
Fri Sep 02 07:26:12 2016 +0000
Revision:
0:98993ca640e2
First program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:98993ca640e2 1 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:98993ca640e2 3 //File Contents: Trace Program by Image Processing
TetsuyaKonno 0:98993ca640e2 4 // (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:98993ca640e2 5 //Version number: Ver.1.01
TetsuyaKonno 0:98993ca640e2 6 //Date: 2016.07.20
TetsuyaKonno 0:98993ca640e2 7 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:98993ca640e2 8 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 9
TetsuyaKonno 0:98993ca640e2 10 //This program supports the following boards:
TetsuyaKonno 0:98993ca640e2 11 //* GR-PEACH(E version)
TetsuyaKonno 0:98993ca640e2 12 //* Motor drive board Ver.5
TetsuyaKonno 0:98993ca640e2 13 //* Camera module (SC-310)
TetsuyaKonno 0:98993ca640e2 14
TetsuyaKonno 0:98993ca640e2 15 //Include
TetsuyaKonno 0:98993ca640e2 16 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 17 #include "mbed.h"
TetsuyaKonno 0:98993ca640e2 18 #include "math.h"
TetsuyaKonno 0:98993ca640e2 19 #include "iodefine.h"
TetsuyaKonno 0:98993ca640e2 20 #include "DisplayBace.h"
TetsuyaKonno 0:98993ca640e2 21
TetsuyaKonno 0:98993ca640e2 22 //Define
TetsuyaKonno 0:98993ca640e2 23 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 24 //Motor PWM cycle
TetsuyaKonno 0:98993ca640e2 25 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */
TetsuyaKonno 0:98993ca640e2 26 /* 1ms P0φ/1 = 0.03us */
TetsuyaKonno 0:98993ca640e2 27 //Motor speed
TetsuyaKonno 0:98993ca640e2 28 #define MAX_SPEED 40 /* motor() set: 0 to 100 */
TetsuyaKonno 0:98993ca640e2 29
TetsuyaKonno 0:98993ca640e2 30 //Servo PWM cycle
TetsuyaKonno 0:98993ca640e2 31 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
TetsuyaKonno 0:98993ca640e2 32 /* 16ms P0φ/16 = 0.48us */
TetsuyaKonno 0:98993ca640e2 33 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
TetsuyaKonno 0:98993ca640e2 34 #define HANDLE_STEP 18 /* 1 degree value */
TetsuyaKonno 0:98993ca640e2 35
TetsuyaKonno 0:98993ca640e2 36 //LED Color on GR-PEACH
TetsuyaKonno 0:98993ca640e2 37 #define LED_OFF 0x00
TetsuyaKonno 0:98993ca640e2 38 #define LED_RED 0x01
TetsuyaKonno 0:98993ca640e2 39 #define LED_GREEN 0x02
TetsuyaKonno 0:98993ca640e2 40 #define LED_YELLOW 0x03
TetsuyaKonno 0:98993ca640e2 41 #define LED_BLUE 0x04
TetsuyaKonno 0:98993ca640e2 42 #define LED_PURPLE 0x05
TetsuyaKonno 0:98993ca640e2 43 #define LED_SKYBLUE 0x06
TetsuyaKonno 0:98993ca640e2 44 #define LED_WHITE 0x07
TetsuyaKonno 0:98993ca640e2 45
TetsuyaKonno 0:98993ca640e2 46 //Status
TetsuyaKonno 0:98993ca640e2 47 #define RUN 0x00
TetsuyaKonno 0:98993ca640e2 48 #define SENSOR 0x01
TetsuyaKonno 0:98993ca640e2 49 #define MARK_T 0x02
TetsuyaKonno 0:98993ca640e2 50 #define MARK_C 0x03
TetsuyaKonno 0:98993ca640e2 51 #define MARK_R 0x04
TetsuyaKonno 0:98993ca640e2 52 #define MARK_L 0x05
TetsuyaKonno 0:98993ca640e2 53 #define STOP 0x06
TetsuyaKonno 0:98993ca640e2 54 #define ERROR 0xff
TetsuyaKonno 0:98993ca640e2 55
TetsuyaKonno 0:98993ca640e2 56 //Define(NTSC-Video)
TetsuyaKonno 0:98993ca640e2 57 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 58 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0)
TetsuyaKonno 0:98993ca640e2 59 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD)
TetsuyaKonno 0:98993ca640e2 60 #define DATA_SIZE_PER_PIC (2u)
TetsuyaKonno 0:98993ca640e2 61
TetsuyaKonno 0:98993ca640e2 62 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
TetsuyaKonno 0:98993ca640e2 63 in accordance with the frame buffer burst transfer mode. */
TetsuyaKonno 0:98993ca640e2 64 #define PIXEL_HW (320u) /* QVGA */
TetsuyaKonno 0:98993ca640e2 65 #define PIXEL_VW (240u) /* QVGA */
TetsuyaKonno 0:98993ca640e2 66 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
TetsuyaKonno 0:98993ca640e2 67 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW)
TetsuyaKonno 0:98993ca640e2 68
TetsuyaKonno 0:98993ca640e2 69 //Constructor
TetsuyaKonno 0:98993ca640e2 70 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 71 Ticker interrput;
TetsuyaKonno 0:98993ca640e2 72 Serial pc(USBTX, USBRX);
TetsuyaKonno 0:98993ca640e2 73 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */
TetsuyaKonno 0:98993ca640e2 74 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */
TetsuyaKonno 0:98993ca640e2 75 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */
TetsuyaKonno 0:98993ca640e2 76 DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */
TetsuyaKonno 0:98993ca640e2 77
TetsuyaKonno 0:98993ca640e2 78 DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */
TetsuyaKonno 0:98993ca640e2 79 DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */
TetsuyaKonno 0:98993ca640e2 80 DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */
TetsuyaKonno 0:98993ca640e2 81 DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */
TetsuyaKonno 0:98993ca640e2 82 DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */
TetsuyaKonno 0:98993ca640e2 83
TetsuyaKonno 0:98993ca640e2 84 //Prototype
TetsuyaKonno 0:98993ca640e2 85 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 86 //Peripheral functions
TetsuyaKonno 0:98993ca640e2 87 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */
TetsuyaKonno 0:98993ca640e2 88 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
TetsuyaKonno 0:98993ca640e2 89 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:98993ca640e2 90
TetsuyaKonno 0:98993ca640e2 91 //GR-peach board
TetsuyaKonno 0:98993ca640e2 92 void led_rgb(int led);
TetsuyaKonno 0:98993ca640e2 93 unsigned int user_button_get( void );
TetsuyaKonno 0:98993ca640e2 94
TetsuyaKonno 0:98993ca640e2 95 //Motor drive board
TetsuyaKonno 0:98993ca640e2 96 void led_out(int led);
TetsuyaKonno 0:98993ca640e2 97 unsigned int pushsw_get( void );
TetsuyaKonno 0:98993ca640e2 98 void motor( int accele_l, int accele_r );
TetsuyaKonno 0:98993ca640e2 99 void handle( int angle );
TetsuyaKonno 0:98993ca640e2 100 int diff( int pwm );
TetsuyaKonno 0:98993ca640e2 101
TetsuyaKonno 0:98993ca640e2 102 //Interrupt function
TetsuyaKonno 0:98993ca640e2 103 void led_status_process( void ); /* Function for only interrupt */
TetsuyaKonno 0:98993ca640e2 104 void led_status_set( int set );
TetsuyaKonno 0:98993ca640e2 105
TetsuyaKonno 0:98993ca640e2 106 //Prototype(NTSC-video)
TetsuyaKonno 0:98993ca640e2 107 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 108 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:98993ca640e2 109 static void WaitVfield(const int32_t wait_count);
TetsuyaKonno 0:98993ca640e2 110 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:98993ca640e2 111 static void WaitVsync(const int32_t wait_count);
TetsuyaKonno 0:98993ca640e2 112
TetsuyaKonno 0:98993ca640e2 113 //Prototype(Display Debug)
TetsuyaKonno 0:98993ca640e2 114 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 115 void ImageData_Serial_Out( unsigned char *Data_Y, int Width );
TetsuyaKonno 0:98993ca640e2 116 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width );
TetsuyaKonno 0:98993ca640e2 117 void ImageData_Serial_Out3( void );
TetsuyaKonno 0:98993ca640e2 118
TetsuyaKonno 0:98993ca640e2 119 //Prototype(Trace by Image Processing)
TetsuyaKonno 0:98993ca640e2 120 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 121 int CenterLine_Corrective( unsigned char *Binary );
TetsuyaKonno 0:98993ca640e2 122 void change_framebuffer_process( void ); /* Function for only interrupt */
TetsuyaKonno 0:98993ca640e2 123 void digital_sensor_process( unsigned char *Binary ); /* Function for only interrupt */
TetsuyaKonno 0:98993ca640e2 124 unsigned char digital_sensor( void );
TetsuyaKonno 0:98993ca640e2 125
TetsuyaKonno 0:98993ca640e2 126 //Prototype(Mark detection)
TetsuyaKonno 0:98993ca640e2 127 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 128 void Image_Extraction( unsigned char *Data_Y );
TetsuyaKonno 0:98993ca640e2 129 void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M );
TetsuyaKonno 0:98993ca640e2 130 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items );
TetsuyaKonno 0:98993ca640e2 131
TetsuyaKonno 0:98993ca640e2 132 //Globle
TetsuyaKonno 0:98993ca640e2 133 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 134 volatile unsigned long cnt0; /* Used by timer function */
TetsuyaKonno 0:98993ca640e2 135 volatile unsigned long cnt1; /* Used within main */
TetsuyaKonno 0:98993ca640e2 136 volatile int pattern; /* Pattern numbers */
TetsuyaKonno 0:98993ca640e2 137 volatile int status_set; /* Status */
TetsuyaKonno 0:98993ca640e2 138 volatile int handle_buff;
TetsuyaKonno 0:98993ca640e2 139
TetsuyaKonno 0:98993ca640e2 140 const int revolution_difference[] = {
TetsuyaKonno 0:98993ca640e2 141 100, 98, 97, 95, 93,
TetsuyaKonno 0:98993ca640e2 142 92, 90, 88, 87, 85,
TetsuyaKonno 0:98993ca640e2 143 84, 82, 81, 79, 78,
TetsuyaKonno 0:98993ca640e2 144 76, 75, 73, 72, 71,
TetsuyaKonno 0:98993ca640e2 145 69, 68, 66, 65, 64,
TetsuyaKonno 0:98993ca640e2 146 62, 61, 59, 58, 57,
TetsuyaKonno 0:98993ca640e2 147 55, 54, 52, 51, 50,
TetsuyaKonno 0:98993ca640e2 148 48, 47, 45, 44, 42,
TetsuyaKonno 0:98993ca640e2 149 41, 39, 38, 36, 35,
TetsuyaKonno 0:98993ca640e2 150 33 };
TetsuyaKonno 0:98993ca640e2 151
TetsuyaKonno 0:98993ca640e2 152 /* Trace by image processing */
TetsuyaKonno 0:98993ca640e2 153 volatile int Sensor_X[8][6];
TetsuyaKonno 0:98993ca640e2 154 volatile unsigned char sensor_value;
TetsuyaKonno 0:98993ca640e2 155
TetsuyaKonno 0:98993ca640e2 156 /* Mark detection */
TetsuyaKonno 0:98993ca640e2 157 unsigned char ImageData[320*240];
TetsuyaKonno 0:98993ca640e2 158 unsigned char ImageComp[160*120];
TetsuyaKonno 0:98993ca640e2 159 unsigned char ImageBinary[160*120];
TetsuyaKonno 0:98993ca640e2 160
TetsuyaKonno 0:98993ca640e2 161 int Xt, Yt;
TetsuyaKonno 0:98993ca640e2 162 int Xc, Yc;
TetsuyaKonno 0:98993ca640e2 163 int Xr, Yr;
TetsuyaKonno 0:98993ca640e2 164 int Xl, Yl;
TetsuyaKonno 0:98993ca640e2 165
TetsuyaKonno 0:98993ca640e2 166 //Globle(NTSC-video)
TetsuyaKonno 0:98993ca640e2 167 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 168 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:98993ca640e2 169 static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:98993ca640e2 170 static volatile int32_t vsync_count;
TetsuyaKonno 0:98993ca640e2 171 static volatile int32_t vfield_count;
TetsuyaKonno 0:98993ca640e2 172 uint8_t * write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:98993ca640e2 173 uint8_t * save_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:98993ca640e2 174
TetsuyaKonno 0:98993ca640e2 175 //Main
TetsuyaKonno 0:98993ca640e2 176 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 177 int main( void )
TetsuyaKonno 0:98993ca640e2 178 {
TetsuyaKonno 0:98993ca640e2 179 /* NTSC-Video */
TetsuyaKonno 0:98993ca640e2 180 DisplayBase::graphics_error_t error;
TetsuyaKonno 0:98993ca640e2 181
TetsuyaKonno 0:98993ca640e2 182 /* Create DisplayBase object */
TetsuyaKonno 0:98993ca640e2 183 DisplayBase Display;
TetsuyaKonno 0:98993ca640e2 184
TetsuyaKonno 0:98993ca640e2 185 /* Graphics initialization process */
TetsuyaKonno 0:98993ca640e2 186 error = Display.Graphics_init(NULL);
TetsuyaKonno 0:98993ca640e2 187 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 188 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 189 while (1);
TetsuyaKonno 0:98993ca640e2 190 }
TetsuyaKonno 0:98993ca640e2 191
TetsuyaKonno 0:98993ca640e2 192 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
TetsuyaKonno 0:98993ca640e2 193 if( error != DisplayBase::GRAPHICS_OK ) {
TetsuyaKonno 0:98993ca640e2 194 while(1);
TetsuyaKonno 0:98993ca640e2 195 }
TetsuyaKonno 0:98993ca640e2 196
TetsuyaKonno 0:98993ca640e2 197 /* Interrupt callback function setting (Vsync signal input to scaler 0) */
TetsuyaKonno 0:98993ca640e2 198 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
TetsuyaKonno 0:98993ca640e2 199 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 200 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 201 while (1);
TetsuyaKonno 0:98993ca640e2 202 }
TetsuyaKonno 0:98993ca640e2 203
TetsuyaKonno 0:98993ca640e2 204 /* Video capture setting (progressive form fixed) */
TetsuyaKonno 0:98993ca640e2 205 error = Display.Video_Write_Setting(
TetsuyaKonno 0:98993ca640e2 206 VIDEO_INPUT_CH,
TetsuyaKonno 0:98993ca640e2 207 DisplayBase::COL_SYS_NTSC_358,
TetsuyaKonno 0:98993ca640e2 208 write_buff_addr,
TetsuyaKonno 0:98993ca640e2 209 VIDEO_BUFFER_STRIDE,
TetsuyaKonno 0:98993ca640e2 210 DisplayBase::VIDEO_FORMAT_YCBCR422,
TetsuyaKonno 0:98993ca640e2 211 DisplayBase::WR_RD_WRSWA_32_16BIT,
TetsuyaKonno 0:98993ca640e2 212 PIXEL_VW,
TetsuyaKonno 0:98993ca640e2 213 PIXEL_HW
TetsuyaKonno 0:98993ca640e2 214 );
TetsuyaKonno 0:98993ca640e2 215 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 216 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 217 while (1);
TetsuyaKonno 0:98993ca640e2 218 }
TetsuyaKonno 0:98993ca640e2 219
TetsuyaKonno 0:98993ca640e2 220 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
TetsuyaKonno 0:98993ca640e2 221 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
TetsuyaKonno 0:98993ca640e2 222 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 223 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 224 while (1);
TetsuyaKonno 0:98993ca640e2 225 }
TetsuyaKonno 0:98993ca640e2 226
TetsuyaKonno 0:98993ca640e2 227 /* Video write process start */
TetsuyaKonno 0:98993ca640e2 228 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:98993ca640e2 229 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 230 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 231 while (1);
TetsuyaKonno 0:98993ca640e2 232 }
TetsuyaKonno 0:98993ca640e2 233
TetsuyaKonno 0:98993ca640e2 234 /* Video write process stop */
TetsuyaKonno 0:98993ca640e2 235 error = Display.Video_Stop (VIDEO_INPUT_CH);
TetsuyaKonno 0:98993ca640e2 236 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 237 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 238 while (1);
TetsuyaKonno 0:98993ca640e2 239 }
TetsuyaKonno 0:98993ca640e2 240
TetsuyaKonno 0:98993ca640e2 241 /* Video write process start */
TetsuyaKonno 0:98993ca640e2 242 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:98993ca640e2 243 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 244 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 245 while (1);
TetsuyaKonno 0:98993ca640e2 246 }
TetsuyaKonno 0:98993ca640e2 247
TetsuyaKonno 0:98993ca640e2 248 /* Wait vsync to update resister */
TetsuyaKonno 0:98993ca640e2 249 WaitVsync(1);
TetsuyaKonno 0:98993ca640e2 250
TetsuyaKonno 0:98993ca640e2 251 /* Wait 2 Vfield(Top or bottom field) */
TetsuyaKonno 0:98993ca640e2 252 WaitVfield(2);
TetsuyaKonno 0:98993ca640e2 253
TetsuyaKonno 0:98993ca640e2 254 /* Initialize MCU functions */
TetsuyaKonno 0:98993ca640e2 255 init_MTU2_PWM_Motor();
TetsuyaKonno 0:98993ca640e2 256 init_MTU2_PWM_Servo();
TetsuyaKonno 0:98993ca640e2 257 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:98993ca640e2 258 pc.baud(230400);
TetsuyaKonno 0:98993ca640e2 259
TetsuyaKonno 0:98993ca640e2 260 /* Initialize Micon Car state */
TetsuyaKonno 0:98993ca640e2 261 led_out( 0x0 );
TetsuyaKonno 0:98993ca640e2 262 handle( 0 );
TetsuyaKonno 0:98993ca640e2 263 motor( 0, 0 );
TetsuyaKonno 0:98993ca640e2 264
TetsuyaKonno 0:98993ca640e2 265 /* wait to stabilize NTSC signal (about 170ms) */
TetsuyaKonno 0:98993ca640e2 266 wait(0.2);
TetsuyaKonno 0:98993ca640e2 267
TetsuyaKonno 0:98993ca640e2 268 led_status_set( SENSOR );
TetsuyaKonno 0:98993ca640e2 269 CenterLine_Corrective( ImageBinary );
TetsuyaKonno 0:98993ca640e2 270 if( pushsw_get() ) {
TetsuyaKonno 0:98993ca640e2 271 while( 1 ){
TetsuyaKonno 0:98993ca640e2 272 ImageData_Serial_Out2( ImageBinary, 20 );
TetsuyaKonno 0:98993ca640e2 273 }
TetsuyaKonno 0:98993ca640e2 274 }
TetsuyaKonno 0:98993ca640e2 275 led_status_set( RUN );
TetsuyaKonno 0:98993ca640e2 276
TetsuyaKonno 0:98993ca640e2 277 while(1) {
TetsuyaKonno 0:98993ca640e2 278
TetsuyaKonno 0:98993ca640e2 279 switch( pattern ) {
TetsuyaKonno 0:98993ca640e2 280 /*****************************************************************
TetsuyaKonno 0:98993ca640e2 281 About patern
TetsuyaKonno 0:98993ca640e2 282 0:wait for switch input
TetsuyaKonno 0:98993ca640e2 283 1:check if start bar is open
TetsuyaKonno 0:98993ca640e2 284 11:normal trace
TetsuyaKonno 0:98993ca640e2 285 *****************************************************************/
TetsuyaKonno 0:98993ca640e2 286 case 0:
TetsuyaKonno 0:98993ca640e2 287 /* wait for switch input */
TetsuyaKonno 0:98993ca640e2 288 if( pushsw_get() ) {
TetsuyaKonno 0:98993ca640e2 289 led_out( 0x0 );
TetsuyaKonno 0:98993ca640e2 290 led_status_set( RUN );
TetsuyaKonno 0:98993ca640e2 291 pattern = 11;
TetsuyaKonno 0:98993ca640e2 292 cnt1 = 0;
TetsuyaKonno 0:98993ca640e2 293 break;
TetsuyaKonno 0:98993ca640e2 294 }
TetsuyaKonno 0:98993ca640e2 295 if( cnt1 < 100 ) {
TetsuyaKonno 0:98993ca640e2 296 led_out( 0x1 );
TetsuyaKonno 0:98993ca640e2 297 } else if( cnt1 < 200 ) {
TetsuyaKonno 0:98993ca640e2 298 led_out( 0x2 );
TetsuyaKonno 0:98993ca640e2 299 } else {
TetsuyaKonno 0:98993ca640e2 300 cnt1 = 0;
TetsuyaKonno 0:98993ca640e2 301 }
TetsuyaKonno 0:98993ca640e2 302 break;
TetsuyaKonno 0:98993ca640e2 303
TetsuyaKonno 0:98993ca640e2 304 case 11:
TetsuyaKonno 0:98993ca640e2 305 /* normal trace */
TetsuyaKonno 0:98993ca640e2 306 switch( (digital_sensor()&0x0f) ) {
TetsuyaKonno 0:98993ca640e2 307 case 0x00:
TetsuyaKonno 0:98993ca640e2 308 handle( 0 );
TetsuyaKonno 0:98993ca640e2 309 motor( 100, 100 );
TetsuyaKonno 0:98993ca640e2 310 break;
TetsuyaKonno 0:98993ca640e2 311 case 0x02:
TetsuyaKonno 0:98993ca640e2 312 handle( 3 );
TetsuyaKonno 0:98993ca640e2 313 motor( 100, diff(100) );
TetsuyaKonno 0:98993ca640e2 314 break;
TetsuyaKonno 0:98993ca640e2 315 case 0x03:
TetsuyaKonno 0:98993ca640e2 316 handle( 12 );
TetsuyaKonno 0:98993ca640e2 317 motor( 100, diff(100) );
TetsuyaKonno 0:98993ca640e2 318 break;
TetsuyaKonno 0:98993ca640e2 319 case 0x01:
TetsuyaKonno 0:98993ca640e2 320 handle( 20 );
TetsuyaKonno 0:98993ca640e2 321 motor( 100, diff(100) );
TetsuyaKonno 0:98993ca640e2 322 pattern = 12;
TetsuyaKonno 0:98993ca640e2 323 break;
TetsuyaKonno 0:98993ca640e2 324 case 0x04:
TetsuyaKonno 0:98993ca640e2 325 handle( -3 );
TetsuyaKonno 0:98993ca640e2 326 motor( diff(100), 100 );
TetsuyaKonno 0:98993ca640e2 327 break;
TetsuyaKonno 0:98993ca640e2 328 case 0x0c:
TetsuyaKonno 0:98993ca640e2 329 handle( -12 );
TetsuyaKonno 0:98993ca640e2 330 motor( diff(100), 100 );
TetsuyaKonno 0:98993ca640e2 331 break;
TetsuyaKonno 0:98993ca640e2 332 case 0x08:
TetsuyaKonno 0:98993ca640e2 333 handle( -20 );
TetsuyaKonno 0:98993ca640e2 334 motor( diff(100), 100 );
TetsuyaKonno 0:98993ca640e2 335 pattern = 13;
TetsuyaKonno 0:98993ca640e2 336 break;
TetsuyaKonno 0:98993ca640e2 337 default:
TetsuyaKonno 0:98993ca640e2 338 break;
TetsuyaKonno 0:98993ca640e2 339 }
TetsuyaKonno 0:98993ca640e2 340 break;
TetsuyaKonno 0:98993ca640e2 341
TetsuyaKonno 0:98993ca640e2 342 case 12:
TetsuyaKonno 0:98993ca640e2 343 /* Left side */
TetsuyaKonno 0:98993ca640e2 344 if( (digital_sensor()&0x02) == 0x02 ) {
TetsuyaKonno 0:98993ca640e2 345 pattern = 11;
TetsuyaKonno 0:98993ca640e2 346 break;
TetsuyaKonno 0:98993ca640e2 347 }
TetsuyaKonno 0:98993ca640e2 348 switch( (digital_sensor()&0x0f) ) {
TetsuyaKonno 0:98993ca640e2 349 case 0x01:
TetsuyaKonno 0:98993ca640e2 350 handle( 20 );
TetsuyaKonno 0:98993ca640e2 351 motor( 100, diff(100) );
TetsuyaKonno 0:98993ca640e2 352 break;
TetsuyaKonno 0:98993ca640e2 353 case 0x00:
TetsuyaKonno 0:98993ca640e2 354 case 0x08:
TetsuyaKonno 0:98993ca640e2 355 case 0x0c:
TetsuyaKonno 0:98993ca640e2 356 handle( 22 );
TetsuyaKonno 0:98993ca640e2 357 motor( 100, diff(100) );
TetsuyaKonno 0:98993ca640e2 358 break;
TetsuyaKonno 0:98993ca640e2 359 default:
TetsuyaKonno 0:98993ca640e2 360 break;
TetsuyaKonno 0:98993ca640e2 361 }
TetsuyaKonno 0:98993ca640e2 362 break;
TetsuyaKonno 0:98993ca640e2 363
TetsuyaKonno 0:98993ca640e2 364 case 13:
TetsuyaKonno 0:98993ca640e2 365 /* right side */
TetsuyaKonno 0:98993ca640e2 366 if( (digital_sensor()&0x04) == 0x04 ) {
TetsuyaKonno 0:98993ca640e2 367 pattern = 11;
TetsuyaKonno 0:98993ca640e2 368 }
TetsuyaKonno 0:98993ca640e2 369 switch( (digital_sensor()&0x0f) ) {
TetsuyaKonno 0:98993ca640e2 370 case 0x08:
TetsuyaKonno 0:98993ca640e2 371 handle( -20 );
TetsuyaKonno 0:98993ca640e2 372 motor( diff(100), 100 );
TetsuyaKonno 0:98993ca640e2 373 break;
TetsuyaKonno 0:98993ca640e2 374 case 0x00:
TetsuyaKonno 0:98993ca640e2 375 case 0x01:
TetsuyaKonno 0:98993ca640e2 376 case 0x03:
TetsuyaKonno 0:98993ca640e2 377 handle( -22 );
TetsuyaKonno 0:98993ca640e2 378 motor( diff(100), 100 );
TetsuyaKonno 0:98993ca640e2 379 break;
TetsuyaKonno 0:98993ca640e2 380 default:
TetsuyaKonno 0:98993ca640e2 381 break;
TetsuyaKonno 0:98993ca640e2 382 }
TetsuyaKonno 0:98993ca640e2 383 break;
TetsuyaKonno 0:98993ca640e2 384
TetsuyaKonno 0:98993ca640e2 385 default:
TetsuyaKonno 0:98993ca640e2 386 break;
TetsuyaKonno 0:98993ca640e2 387 }
TetsuyaKonno 0:98993ca640e2 388 }
TetsuyaKonno 0:98993ca640e2 389 }
TetsuyaKonno 0:98993ca640e2 390
TetsuyaKonno 0:98993ca640e2 391 //Initialize MTU2 PWM functions
TetsuyaKonno 0:98993ca640e2 392 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 393 //MTU2_3, MTU2_4
TetsuyaKonno 0:98993ca640e2 394 //Reset-Synchronized PWM mode
TetsuyaKonno 0:98993ca640e2 395 //TIOC4A(P4_4) :Left-motor
TetsuyaKonno 0:98993ca640e2 396 //TIOC4B(P4_5) :Right-motor
TetsuyaKonno 0:98993ca640e2 397 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 398 void init_MTU2_PWM_Motor( void )
TetsuyaKonno 0:98993ca640e2 399 {
TetsuyaKonno 0:98993ca640e2 400 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:98993ca640e2 401 /* alternative mode */
TetsuyaKonno 0:98993ca640e2 402
TetsuyaKonno 0:98993ca640e2 403 /* MTU2_4 (P4_4)(P4_5) */
TetsuyaKonno 0:98993ca640e2 404 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:98993ca640e2 405 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:98993ca640e2 406 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */
TetsuyaKonno 0:98993ca640e2 407 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:98993ca640e2 408 /* 2nd altemative function/output */
TetsuyaKonno 0:98993ca640e2 409 GPIOP4 &= 0xffcf; /* */
TetsuyaKonno 0:98993ca640e2 410 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */
TetsuyaKonno 0:98993ca640e2 411 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */
TetsuyaKonno 0:98993ca640e2 412
TetsuyaKonno 0:98993ca640e2 413 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:98993ca640e2 414 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:98993ca640e2 415
TetsuyaKonno 0:98993ca640e2 416 /* MTU2_3 and MTU2_4 (Motor PWM) */
TetsuyaKonno 0:98993ca640e2 417 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */
TetsuyaKonno 0:98993ca640e2 418 MTU2TOCR1 = 0x04; /* */
TetsuyaKonno 0:98993ca640e2 419 MTU2TOCR2 = 0x40; /* N L>H P H>L */
TetsuyaKonno 0:98993ca640e2 420 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */
TetsuyaKonno 0:98993ca640e2 421 MTU2TMDR_4 = 0x30; /* Buff:ON */
TetsuyaKonno 0:98993ca640e2 422 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */
TetsuyaKonno 0:98993ca640e2 423 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
TetsuyaKonno 0:98993ca640e2 424 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
TetsuyaKonno 0:98993ca640e2 425 /* PWM-Cycle(1ms) */
TetsuyaKonno 0:98993ca640e2 426 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */
TetsuyaKonno 0:98993ca640e2 427 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */
TetsuyaKonno 0:98993ca640e2 428 MTU2TSTR |= 0x40; /* TCNT_4 Start */
TetsuyaKonno 0:98993ca640e2 429 }
TetsuyaKonno 0:98993ca640e2 430
TetsuyaKonno 0:98993ca640e2 431 //Initialize MTU2 PWM functions
TetsuyaKonno 0:98993ca640e2 432 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 433 //MTU2_0
TetsuyaKonno 0:98993ca640e2 434 //PWM mode 1
TetsuyaKonno 0:98993ca640e2 435 //TIOC0A(P4_0) :Servo-motor
TetsuyaKonno 0:98993ca640e2 436 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 437 void init_MTU2_PWM_Servo( void )
TetsuyaKonno 0:98993ca640e2 438 {
TetsuyaKonno 0:98993ca640e2 439 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:98993ca640e2 440 /* alternative mode */
TetsuyaKonno 0:98993ca640e2 441
TetsuyaKonno 0:98993ca640e2 442 /* MTU2_0 (P4_0) */
TetsuyaKonno 0:98993ca640e2 443 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:98993ca640e2 444 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:98993ca640e2 445 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:98993ca640e2 446 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
TetsuyaKonno 0:98993ca640e2 447 /* 2nd alternative function/output */
TetsuyaKonno 0:98993ca640e2 448 GPIOP4 &= 0xfffe; /* */
TetsuyaKonno 0:98993ca640e2 449 GPIOPM4 &= 0xfffe; /* p4_0:output */
TetsuyaKonno 0:98993ca640e2 450 GPIOPMC4 |= 0x0001; /* P4_0:double */
TetsuyaKonno 0:98993ca640e2 451
TetsuyaKonno 0:98993ca640e2 452 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:98993ca640e2 453 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:98993ca640e2 454
TetsuyaKonno 0:98993ca640e2 455 /* MTU2_0 (Motor PWM) */
TetsuyaKonno 0:98993ca640e2 456 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
TetsuyaKonno 0:98993ca640e2 457 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
TetsuyaKonno 0:98993ca640e2 458 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
TetsuyaKonno 0:98993ca640e2 459 /* PWM-mode1 */
TetsuyaKonno 0:98993ca640e2 460 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
TetsuyaKonno 0:98993ca640e2 461 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
TetsuyaKonno 0:98993ca640e2 462 /* PWM-Cycle(16ms) */
TetsuyaKonno 0:98993ca640e2 463 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
TetsuyaKonno 0:98993ca640e2 464 MTU2TSTR |= 0x01; /* TCNT_0 Start */
TetsuyaKonno 0:98993ca640e2 465 }
TetsuyaKonno 0:98993ca640e2 466
TetsuyaKonno 0:98993ca640e2 467 //Interrupt Timer
TetsuyaKonno 0:98993ca640e2 468 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 469 void intTimer( void )
TetsuyaKonno 0:98993ca640e2 470 {
TetsuyaKonno 0:98993ca640e2 471 static int counter = 0;
TetsuyaKonno 0:98993ca640e2 472
TetsuyaKonno 0:98993ca640e2 473 cnt0++;
TetsuyaKonno 0:98993ca640e2 474 cnt1++;
TetsuyaKonno 0:98993ca640e2 475
TetsuyaKonno 0:98993ca640e2 476 /* Mark Check Process */
TetsuyaKonno 0:98993ca640e2 477 switch( counter++ ) {
TetsuyaKonno 0:98993ca640e2 478 case 0:
TetsuyaKonno 0:98993ca640e2 479 change_framebuffer_process();
TetsuyaKonno 0:98993ca640e2 480 break;
TetsuyaKonno 0:98993ca640e2 481 case 1:
TetsuyaKonno 0:98993ca640e2 482 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 483 break;
TetsuyaKonno 0:98993ca640e2 484 case 2:
TetsuyaKonno 0:98993ca640e2 485 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 486 break;
TetsuyaKonno 0:98993ca640e2 487 case 3:
TetsuyaKonno 0:98993ca640e2 488 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 489 break;
TetsuyaKonno 0:98993ca640e2 490 case 4:
TetsuyaKonno 0:98993ca640e2 491 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 492 break;
TetsuyaKonno 0:98993ca640e2 493 case 5:
TetsuyaKonno 0:98993ca640e2 494 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 495 break;
TetsuyaKonno 0:98993ca640e2 496 case 6:
TetsuyaKonno 0:98993ca640e2 497 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 498 break;
TetsuyaKonno 0:98993ca640e2 499 case 7:
TetsuyaKonno 0:98993ca640e2 500 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 501 break;
TetsuyaKonno 0:98993ca640e2 502 case 8:
TetsuyaKonno 0:98993ca640e2 503 Image_Extraction( ImageData );
TetsuyaKonno 0:98993ca640e2 504 break;
TetsuyaKonno 0:98993ca640e2 505 case 9:
TetsuyaKonno 0:98993ca640e2 506 Image_Compression2( ImageData, 320, ImageComp, 16 );
TetsuyaKonno 0:98993ca640e2 507 break;
TetsuyaKonno 0:98993ca640e2 508 case 10:
TetsuyaKonno 0:98993ca640e2 509 Image_Compression2( ImageData, 320, ImageComp, 16 );
TetsuyaKonno 0:98993ca640e2 510 break;
TetsuyaKonno 0:98993ca640e2 511 case 11:
TetsuyaKonno 0:98993ca640e2 512 Binarization_process( ImageComp, ImageBinary, 20*15 );
TetsuyaKonno 0:98993ca640e2 513 break;
TetsuyaKonno 0:98993ca640e2 514 case 33:
TetsuyaKonno 0:98993ca640e2 515 counter = 0;
TetsuyaKonno 0:98993ca640e2 516 break;
TetsuyaKonno 0:98993ca640e2 517 default:
TetsuyaKonno 0:98993ca640e2 518 break;
TetsuyaKonno 0:98993ca640e2 519 }
TetsuyaKonno 0:98993ca640e2 520
TetsuyaKonno 0:98993ca640e2 521 /* Trace by image processing */
TetsuyaKonno 0:98993ca640e2 522 digital_sensor_process( ImageBinary );
TetsuyaKonno 0:98993ca640e2 523
TetsuyaKonno 0:98993ca640e2 524 /* LED processing */
TetsuyaKonno 0:98993ca640e2 525 led_status_process();
TetsuyaKonno 0:98993ca640e2 526 }
TetsuyaKonno 0:98993ca640e2 527
TetsuyaKonno 0:98993ca640e2 528 //LED_RGB(on GR-PEACH board)
TetsuyaKonno 0:98993ca640e2 529 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 530 void led_rgb(int led)
TetsuyaKonno 0:98993ca640e2 531 {
TetsuyaKonno 0:98993ca640e2 532 LED_R = led & 0x1;
TetsuyaKonno 0:98993ca640e2 533 LED_G = (led >> 1 ) & 0x1;
TetsuyaKonno 0:98993ca640e2 534 LED_B = (led >> 2 ) & 0x1;
TetsuyaKonno 0:98993ca640e2 535 }
TetsuyaKonno 0:98993ca640e2 536
TetsuyaKonno 0:98993ca640e2 537 //user_button_get(on GR-PEACH board)
TetsuyaKonno 0:98993ca640e2 538 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 539 unsigned int user_button_get( void )
TetsuyaKonno 0:98993ca640e2 540 {
TetsuyaKonno 0:98993ca640e2 541 return (~user_botton) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:98993ca640e2 542 }
TetsuyaKonno 0:98993ca640e2 543
TetsuyaKonno 0:98993ca640e2 544 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:98993ca640e2 545 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 546 void led_status_process( void )
TetsuyaKonno 0:98993ca640e2 547 {
TetsuyaKonno 0:98993ca640e2 548 static unsigned long led_timer;
TetsuyaKonno 0:98993ca640e2 549 int led_set;
TetsuyaKonno 0:98993ca640e2 550 int on_time;
TetsuyaKonno 0:98993ca640e2 551 int off_time;
TetsuyaKonno 0:98993ca640e2 552
TetsuyaKonno 0:98993ca640e2 553 /* setting */
TetsuyaKonno 0:98993ca640e2 554 switch( status_set ){
TetsuyaKonno 0:98993ca640e2 555 case RUN:
TetsuyaKonno 0:98993ca640e2 556 led_set = LED_GREEN;
TetsuyaKonno 0:98993ca640e2 557 on_time = 500;
TetsuyaKonno 0:98993ca640e2 558 off_time = 500;
TetsuyaKonno 0:98993ca640e2 559 break;
TetsuyaKonno 0:98993ca640e2 560
TetsuyaKonno 0:98993ca640e2 561 case SENSOR:
TetsuyaKonno 0:98993ca640e2 562 led_set = LED_BLUE;
TetsuyaKonno 0:98993ca640e2 563 on_time = 50;
TetsuyaKonno 0:98993ca640e2 564 off_time = 50;
TetsuyaKonno 0:98993ca640e2 565 break;
TetsuyaKonno 0:98993ca640e2 566
TetsuyaKonno 0:98993ca640e2 567 case MARK_T:
TetsuyaKonno 0:98993ca640e2 568 led_set = LED_WHITE;
TetsuyaKonno 0:98993ca640e2 569 on_time = 250;
TetsuyaKonno 0:98993ca640e2 570 off_time = 250;
TetsuyaKonno 0:98993ca640e2 571 break;
TetsuyaKonno 0:98993ca640e2 572
TetsuyaKonno 0:98993ca640e2 573 case MARK_C:
TetsuyaKonno 0:98993ca640e2 574 led_set = LED_YELLOW;
TetsuyaKonno 0:98993ca640e2 575 on_time = 250;
TetsuyaKonno 0:98993ca640e2 576 off_time = 250;
TetsuyaKonno 0:98993ca640e2 577 break;
TetsuyaKonno 0:98993ca640e2 578
TetsuyaKonno 0:98993ca640e2 579 case MARK_R:
TetsuyaKonno 0:98993ca640e2 580 led_set = LED_PURPLE;
TetsuyaKonno 0:98993ca640e2 581 on_time = 250;
TetsuyaKonno 0:98993ca640e2 582 off_time = 250;
TetsuyaKonno 0:98993ca640e2 583 break;
TetsuyaKonno 0:98993ca640e2 584
TetsuyaKonno 0:98993ca640e2 585 case MARK_L:
TetsuyaKonno 0:98993ca640e2 586 led_set = LED_SKYBLUE;
TetsuyaKonno 0:98993ca640e2 587 on_time = 250;
TetsuyaKonno 0:98993ca640e2 588 off_time = 250;
TetsuyaKonno 0:98993ca640e2 589 break;
TetsuyaKonno 0:98993ca640e2 590
TetsuyaKonno 0:98993ca640e2 591 case STOP:
TetsuyaKonno 0:98993ca640e2 592 led_set = LED_RED;
TetsuyaKonno 0:98993ca640e2 593 on_time = 1;
TetsuyaKonno 0:98993ca640e2 594 off_time = 0;
TetsuyaKonno 0:98993ca640e2 595 break;
TetsuyaKonno 0:98993ca640e2 596
TetsuyaKonno 0:98993ca640e2 597 case ERROR:
TetsuyaKonno 0:98993ca640e2 598 led_set = LED_RED;
TetsuyaKonno 0:98993ca640e2 599 on_time = 50;
TetsuyaKonno 0:98993ca640e2 600 off_time = 50;
TetsuyaKonno 0:98993ca640e2 601 break;
TetsuyaKonno 0:98993ca640e2 602
TetsuyaKonno 0:98993ca640e2 603 default:
TetsuyaKonno 0:98993ca640e2 604 led_set = LED_OFF;
TetsuyaKonno 0:98993ca640e2 605 on_time = 0;
TetsuyaKonno 0:98993ca640e2 606 off_time = 1;
TetsuyaKonno 0:98993ca640e2 607 break;
TetsuyaKonno 0:98993ca640e2 608 }
TetsuyaKonno 0:98993ca640e2 609
TetsuyaKonno 0:98993ca640e2 610 /* Display */
TetsuyaKonno 0:98993ca640e2 611 led_timer++;
TetsuyaKonno 0:98993ca640e2 612 if( led_timer < on_time ) led_rgb( led_set );
TetsuyaKonno 0:98993ca640e2 613 else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF );
TetsuyaKonno 0:98993ca640e2 614 else led_timer = 0;
TetsuyaKonno 0:98993ca640e2 615 }
TetsuyaKonno 0:98993ca640e2 616
TetsuyaKonno 0:98993ca640e2 617 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:98993ca640e2 618 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 619 void led_status_set( int set )
TetsuyaKonno 0:98993ca640e2 620 {
TetsuyaKonno 0:98993ca640e2 621 status_set = set;
TetsuyaKonno 0:98993ca640e2 622 }
TetsuyaKonno 0:98993ca640e2 623
TetsuyaKonno 0:98993ca640e2 624 //led_out(on Motor drive board)
TetsuyaKonno 0:98993ca640e2 625 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 626 void led_out(int led)
TetsuyaKonno 0:98993ca640e2 627 {
TetsuyaKonno 0:98993ca640e2 628 led = ~led;
TetsuyaKonno 0:98993ca640e2 629 LED_3 = led & 0x1;
TetsuyaKonno 0:98993ca640e2 630 LED_2 = ( led >> 1 ) & 0x1;
TetsuyaKonno 0:98993ca640e2 631 }
TetsuyaKonno 0:98993ca640e2 632
TetsuyaKonno 0:98993ca640e2 633 //pushsw_get(on Motor drive board)
TetsuyaKonno 0:98993ca640e2 634 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 635 unsigned int pushsw_get( void )
TetsuyaKonno 0:98993ca640e2 636 {
TetsuyaKonno 0:98993ca640e2 637 return (~push_sw) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:98993ca640e2 638 }
TetsuyaKonno 0:98993ca640e2 639
TetsuyaKonno 0:98993ca640e2 640 //motor speed control(PWM)
TetsuyaKonno 0:98993ca640e2 641 //Arguments: motor:-100 to 100
TetsuyaKonno 0:98993ca640e2 642 //Here, 0 is stop, 100 is forward, -100 is reverse
TetsuyaKonno 0:98993ca640e2 643 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 644 void motor( int accele_l, int accele_r )
TetsuyaKonno 0:98993ca640e2 645 {
TetsuyaKonno 0:98993ca640e2 646 accele_l = ( accele_l * MAX_SPEED ) / 100;
TetsuyaKonno 0:98993ca640e2 647 accele_r = ( accele_r * MAX_SPEED ) / 100;
TetsuyaKonno 0:98993ca640e2 648
TetsuyaKonno 0:98993ca640e2 649 /* Left Motor Control */
TetsuyaKonno 0:98993ca640e2 650 if( accele_l >= 0 ) {
TetsuyaKonno 0:98993ca640e2 651 /* forward */
TetsuyaKonno 0:98993ca640e2 652 Left_motor_signal = 0;
TetsuyaKonno 0:98993ca640e2 653 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
TetsuyaKonno 0:98993ca640e2 654 } else {
TetsuyaKonno 0:98993ca640e2 655 /* reverse */
TetsuyaKonno 0:98993ca640e2 656 Left_motor_signal = 1;
TetsuyaKonno 0:98993ca640e2 657 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
TetsuyaKonno 0:98993ca640e2 658 }
TetsuyaKonno 0:98993ca640e2 659
TetsuyaKonno 0:98993ca640e2 660 /* Right Motor Control */
TetsuyaKonno 0:98993ca640e2 661 if( accele_r >= 0 ) {
TetsuyaKonno 0:98993ca640e2 662 /* forward */
TetsuyaKonno 0:98993ca640e2 663 Right_motor_signal = 0;
TetsuyaKonno 0:98993ca640e2 664 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
TetsuyaKonno 0:98993ca640e2 665 } else {
TetsuyaKonno 0:98993ca640e2 666 /* reverse */
TetsuyaKonno 0:98993ca640e2 667 Right_motor_signal = 1;
TetsuyaKonno 0:98993ca640e2 668 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
TetsuyaKonno 0:98993ca640e2 669 }
TetsuyaKonno 0:98993ca640e2 670 }
TetsuyaKonno 0:98993ca640e2 671
TetsuyaKonno 0:98993ca640e2 672 //Handle fanction
TetsuyaKonno 0:98993ca640e2 673 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 674 void handle( int angle )
TetsuyaKonno 0:98993ca640e2 675 {
TetsuyaKonno 0:98993ca640e2 676 handle_buff = angle;
TetsuyaKonno 0:98993ca640e2 677 /* When the servo move from left to right in reverse, replace "-" with "+" */
TetsuyaKonno 0:98993ca640e2 678 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
TetsuyaKonno 0:98993ca640e2 679 }
TetsuyaKonno 0:98993ca640e2 680
TetsuyaKonno 0:98993ca640e2 681 //handle(on Motor drive board)
TetsuyaKonno 0:98993ca640e2 682 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 683 int diff( int pwm )
TetsuyaKonno 0:98993ca640e2 684 {
TetsuyaKonno 0:98993ca640e2 685 int i, ret;
TetsuyaKonno 0:98993ca640e2 686
TetsuyaKonno 0:98993ca640e2 687 i = handle_buff;
TetsuyaKonno 0:98993ca640e2 688 if( i < 0 ) i = -i;
TetsuyaKonno 0:98993ca640e2 689 if( i > 45 ) i = 45;
TetsuyaKonno 0:98993ca640e2 690 ret = revolution_difference[i] * pwm / 100;
TetsuyaKonno 0:98993ca640e2 691
TetsuyaKonno 0:98993ca640e2 692 return ret;
TetsuyaKonno 0:98993ca640e2 693 }
TetsuyaKonno 0:98993ca640e2 694
TetsuyaKonno 0:98993ca640e2 695 //Image Data Output( for the Excel )
TetsuyaKonno 0:98993ca640e2 696 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 697 void ImageData_Serial_Out( unsigned char *Data_Y, int Width )
TetsuyaKonno 0:98993ca640e2 698 {
TetsuyaKonno 0:98993ca640e2 699 int Xp, Yp, inc, Height;
TetsuyaKonno 0:98993ca640e2 700
TetsuyaKonno 0:98993ca640e2 701 Height = (Width / (double)4) * 3;
TetsuyaKonno 0:98993ca640e2 702 for( Yp = 0, inc = 0; Yp < Height; Yp++ ) {
TetsuyaKonno 0:98993ca640e2 703 for( Xp = 0; Xp < Width; Xp++, inc++ ) {
TetsuyaKonno 0:98993ca640e2 704 pc.printf( "%d,", Data_Y[ inc ] );
TetsuyaKonno 0:98993ca640e2 705 }
TetsuyaKonno 0:98993ca640e2 706 pc.printf("\n\r");
TetsuyaKonno 0:98993ca640e2 707 }
TetsuyaKonno 0:98993ca640e2 708 }
TetsuyaKonno 0:98993ca640e2 709
TetsuyaKonno 0:98993ca640e2 710 //Image Data Output2( for TeraTerm )
TetsuyaKonno 0:98993ca640e2 711 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 712 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width )
TetsuyaKonno 0:98993ca640e2 713 {
TetsuyaKonno 0:98993ca640e2 714 int Xp, Yp, Height;
TetsuyaKonno 0:98993ca640e2 715
TetsuyaKonno 0:98993ca640e2 716 Height = (Width / (double)4) * 3;
TetsuyaKonno 0:98993ca640e2 717 for( Yp = 0; Yp < Height; Yp++ ) {
TetsuyaKonno 0:98993ca640e2 718 for( Xp = 0; Xp < Width; Xp++ ) {
TetsuyaKonno 0:98993ca640e2 719 pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] );
TetsuyaKonno 0:98993ca640e2 720 }
TetsuyaKonno 0:98993ca640e2 721 pc.printf( "\n\r" );
TetsuyaKonno 0:98993ca640e2 722 }
TetsuyaKonno 0:98993ca640e2 723 pc.printf( "\033[%dA" , Height );
TetsuyaKonno 0:98993ca640e2 724 }
TetsuyaKonno 0:98993ca640e2 725
TetsuyaKonno 0:98993ca640e2 726 //Image Data Output3( for the converter ( csv --> jpg ) )
TetsuyaKonno 0:98993ca640e2 727 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 728 void ImageData_Serial_Out3( void )
TetsuyaKonno 0:98993ca640e2 729 {
TetsuyaKonno 0:98993ca640e2 730 int Xp, Yp, x, y;
TetsuyaKonno 0:98993ca640e2 731
TetsuyaKonno 0:98993ca640e2 732 /* Camera module test process */
TetsuyaKonno 0:98993ca640e2 733 pc.printf( "//,X-Size,Y-Size" );
TetsuyaKonno 0:98993ca640e2 734 pc.printf( "\n\r" );
TetsuyaKonno 0:98993ca640e2 735 pc.printf( "#SIZE,320,240" );
TetsuyaKonno 0:98993ca640e2 736 pc.printf( "\n\r" );
TetsuyaKonno 0:98993ca640e2 737 pc.printf( "//,X-Point,Y-Point" );
TetsuyaKonno 0:98993ca640e2 738 pc.printf( "\n\r" );
TetsuyaKonno 0:98993ca640e2 739
TetsuyaKonno 0:98993ca640e2 740 for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){
TetsuyaKonno 0:98993ca640e2 741 for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){
TetsuyaKonno 0:98993ca640e2 742 pc.printf( "#YCbCr," );
TetsuyaKonno 0:98993ca640e2 743 /*Xp*/pc.printf( "%d,", x);
TetsuyaKonno 0:98993ca640e2 744 /*Yp*/pc.printf( "%d,", y);
TetsuyaKonno 0:98993ca640e2 745 /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6
TetsuyaKonno 0:98993ca640e2 746 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
TetsuyaKonno 0:98993ca640e2 747 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
TetsuyaKonno 0:98993ca640e2 748 pc.printf( "\n\r" );
TetsuyaKonno 0:98993ca640e2 749
TetsuyaKonno 0:98993ca640e2 750 pc.printf( "#YCbCr," );
TetsuyaKonno 0:98993ca640e2 751 /*Xp*/pc.printf( "%d,", x+1);
TetsuyaKonno 0:98993ca640e2 752 /*Yp*/pc.printf( "%d,", y);
TetsuyaKonno 0:98993ca640e2 753 /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4
TetsuyaKonno 0:98993ca640e2 754 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
TetsuyaKonno 0:98993ca640e2 755 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
TetsuyaKonno 0:98993ca640e2 756 pc.printf( "\n\r" );
TetsuyaKonno 0:98993ca640e2 757 }
TetsuyaKonno 0:98993ca640e2 758 }
TetsuyaKonno 0:98993ca640e2 759 }
TetsuyaKonno 0:98993ca640e2 760
TetsuyaKonno 0:98993ca640e2 761 //Change FrameBuffer Process
TetsuyaKonno 0:98993ca640e2 762 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 763 void change_framebuffer_process( void )
TetsuyaKonno 0:98993ca640e2 764 {
TetsuyaKonno 0:98993ca640e2 765 DisplayBase::graphics_error_t error;
TetsuyaKonno 0:98993ca640e2 766 DisplayBase Display;
TetsuyaKonno 0:98993ca640e2 767
TetsuyaKonno 0:98993ca640e2 768 /* Change address buffer */
TetsuyaKonno 0:98993ca640e2 769 if (write_buff_addr == FrameBuffer_Video_A) {
TetsuyaKonno 0:98993ca640e2 770 write_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:98993ca640e2 771 save_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:98993ca640e2 772 } else {
TetsuyaKonno 0:98993ca640e2 773 write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:98993ca640e2 774 save_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:98993ca640e2 775 }
TetsuyaKonno 0:98993ca640e2 776
TetsuyaKonno 0:98993ca640e2 777 /* Change write buffer */
TetsuyaKonno 0:98993ca640e2 778 error = Display.Video_Write_Change(
TetsuyaKonno 0:98993ca640e2 779 VIDEO_INPUT_CH,
TetsuyaKonno 0:98993ca640e2 780 write_buff_addr,
TetsuyaKonno 0:98993ca640e2 781 VIDEO_BUFFER_STRIDE);
TetsuyaKonno 0:98993ca640e2 782 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:98993ca640e2 783 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:98993ca640e2 784 while (1);
TetsuyaKonno 0:98993ca640e2 785 }
TetsuyaKonno 0:98993ca640e2 786 }
TetsuyaKonno 0:98993ca640e2 787
TetsuyaKonno 0:98993ca640e2 788 //CenterLine_Corrective image size 20*15pix
TetsuyaKonno 0:98993ca640e2 789 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 790 int CenterLine_Corrective( unsigned char *Binary )
TetsuyaKonno 0:98993ca640e2 791 {
TetsuyaKonno 0:98993ca640e2 792 #define L 0
TetsuyaKonno 0:98993ca640e2 793 #define R 1
TetsuyaKonno 0:98993ca640e2 794
TetsuyaKonno 0:98993ca640e2 795 int iRet, offset_X, offset_Y;
TetsuyaKonno 0:98993ca640e2 796 int Xpix, X;
TetsuyaKonno 0:98993ca640e2 797 int Ypix;
TetsuyaKonno 0:98993ca640e2 798 int Pixel_diff[2];
TetsuyaKonno 0:98993ca640e2 799 int Error_cnt;
TetsuyaKonno 0:98993ca640e2 800 int value;
TetsuyaKonno 0:98993ca640e2 801
TetsuyaKonno 0:98993ca640e2 802 /* Center of image */
TetsuyaKonno 0:98993ca640e2 803 offset_X = 6;
TetsuyaKonno 0:98993ca640e2 804 offset_Y = 12;
TetsuyaKonno 0:98993ca640e2 805
TetsuyaKonno 0:98993ca640e2 806 /* corrective of center line */
TetsuyaKonno 0:98993ca640e2 807 for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) {
TetsuyaKonno 0:98993ca640e2 808 for( value = 0; value < 2; value++ ) {
TetsuyaKonno 0:98993ca640e2 809 for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) {
TetsuyaKonno 0:98993ca640e2 810 /* Lift side */
TetsuyaKonno 0:98993ca640e2 811 Pixel_diff[L] = 0;
TetsuyaKonno 0:98993ca640e2 812 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) {
TetsuyaKonno 0:98993ca640e2 813 for( X = Xpix; X > (Xpix - 4); X-- ) {
TetsuyaKonno 0:98993ca640e2 814 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
TetsuyaKonno 0:98993ca640e2 815 Pixel_diff[L]++;
TetsuyaKonno 0:98993ca640e2 816 } else {
TetsuyaKonno 0:98993ca640e2 817 break;
TetsuyaKonno 0:98993ca640e2 818 }
TetsuyaKonno 0:98993ca640e2 819 }
TetsuyaKonno 0:98993ca640e2 820 } else {
TetsuyaKonno 0:98993ca640e2 821 Pixel_diff[L] = -1;
TetsuyaKonno 0:98993ca640e2 822 }
TetsuyaKonno 0:98993ca640e2 823 /* Right side */
TetsuyaKonno 0:98993ca640e2 824 Pixel_diff[R] = 0;
TetsuyaKonno 0:98993ca640e2 825 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) {
TetsuyaKonno 0:98993ca640e2 826 for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) {
TetsuyaKonno 0:98993ca640e2 827 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
TetsuyaKonno 0:98993ca640e2 828 Pixel_diff[R]++;
TetsuyaKonno 0:98993ca640e2 829 } else {
TetsuyaKonno 0:98993ca640e2 830 break;
TetsuyaKonno 0:98993ca640e2 831 }
TetsuyaKonno 0:98993ca640e2 832 }
TetsuyaKonno 0:98993ca640e2 833 } else {
TetsuyaKonno 0:98993ca640e2 834 Pixel_diff[R] = 1;
TetsuyaKonno 0:98993ca640e2 835 }
TetsuyaKonno 0:98993ca640e2 836 /* check */
TetsuyaKonno 0:98993ca640e2 837 iRet = Pixel_diff[L] - Pixel_diff[R];
TetsuyaKonno 0:98993ca640e2 838 if( value >= iRet && iRet >= -value ) {
TetsuyaKonno 0:98993ca640e2 839 break;
TetsuyaKonno 0:98993ca640e2 840 }
TetsuyaKonno 0:98993ca640e2 841 }
TetsuyaKonno 0:98993ca640e2 842 if( value >= iRet && iRet >= -value ) {
TetsuyaKonno 0:98993ca640e2 843 /* X coordinate */
TetsuyaKonno 0:98993ca640e2 844 Sensor_X[Ypix][2] = Xpix;
TetsuyaKonno 0:98993ca640e2 845 Sensor_X[Ypix][3] = Xpix + 1;
TetsuyaKonno 0:98993ca640e2 846 break;
TetsuyaKonno 0:98993ca640e2 847 } else {
TetsuyaKonno 0:98993ca640e2 848 Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1;
TetsuyaKonno 0:98993ca640e2 849 Error_cnt++;
TetsuyaKonno 0:98993ca640e2 850 }
TetsuyaKonno 0:98993ca640e2 851 }
TetsuyaKonno 0:98993ca640e2 852 /* Left side sensor */
TetsuyaKonno 0:98993ca640e2 853 Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] );
TetsuyaKonno 0:98993ca640e2 854 Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] );
TetsuyaKonno 0:98993ca640e2 855 /* Right side sensor */
TetsuyaKonno 0:98993ca640e2 856 Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] );
TetsuyaKonno 0:98993ca640e2 857 Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] );
TetsuyaKonno 0:98993ca640e2 858 }
TetsuyaKonno 0:98993ca640e2 859 return Error_cnt;
TetsuyaKonno 0:98993ca640e2 860 }
TetsuyaKonno 0:98993ca640e2 861
TetsuyaKonno 0:98993ca640e2 862 //digital_sensor_process
TetsuyaKonno 0:98993ca640e2 863 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 864 void digital_sensor_process( unsigned char *Binary )
TetsuyaKonno 0:98993ca640e2 865 {
TetsuyaKonno 0:98993ca640e2 866 static int counter = 0;
TetsuyaKonno 0:98993ca640e2 867 static int Ypix = 0;
TetsuyaKonno 0:98993ca640e2 868
TetsuyaKonno 0:98993ca640e2 869 int offset_Y;
TetsuyaKonno 0:98993ca640e2 870 unsigned char sensor, data;
TetsuyaKonno 0:98993ca640e2 871
TetsuyaKonno 0:98993ca640e2 872 offset_Y = 12;
TetsuyaKonno 0:98993ca640e2 873 sensor = 0;
TetsuyaKonno 0:98993ca640e2 874
TetsuyaKonno 0:98993ca640e2 875 // Distributed processing
TetsuyaKonno 0:98993ca640e2 876 switch( counter++ ) {
TetsuyaKonno 0:98993ca640e2 877 case 15:
TetsuyaKonno 0:98993ca640e2 878 case 31:
TetsuyaKonno 0:98993ca640e2 879 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01;
TetsuyaKonno 0:98993ca640e2 880 data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01;
TetsuyaKonno 0:98993ca640e2 881 sensor |= (data << 4) & 0x10;
TetsuyaKonno 0:98993ca640e2 882 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01;
TetsuyaKonno 0:98993ca640e2 883 sensor |= (data << 3) & 0x08;
TetsuyaKonno 0:98993ca640e2 884 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01;
TetsuyaKonno 0:98993ca640e2 885 sensor |= (data << 2) & 0x04;
TetsuyaKonno 0:98993ca640e2 886 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01;
TetsuyaKonno 0:98993ca640e2 887 sensor |= (data << 1) & 0x02;
TetsuyaKonno 0:98993ca640e2 888 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01;
TetsuyaKonno 0:98993ca640e2 889 sensor |= (data << 0) & 0x01;
TetsuyaKonno 0:98993ca640e2 890 sensor &= 0x1f;
TetsuyaKonno 0:98993ca640e2 891 sensor_value = sensor;
TetsuyaKonno 0:98993ca640e2 892 Ypix += 4;
TetsuyaKonno 0:98993ca640e2 893 break;
TetsuyaKonno 0:98993ca640e2 894 case 33:
TetsuyaKonno 0:98993ca640e2 895 counter = 0;
TetsuyaKonno 0:98993ca640e2 896 Ypix = 0;
TetsuyaKonno 0:98993ca640e2 897 break;
TetsuyaKonno 0:98993ca640e2 898 default:
TetsuyaKonno 0:98993ca640e2 899 break;
TetsuyaKonno 0:98993ca640e2 900 }
TetsuyaKonno 0:98993ca640e2 901 }
TetsuyaKonno 0:98993ca640e2 902
TetsuyaKonno 0:98993ca640e2 903 //digital_sensor
TetsuyaKonno 0:98993ca640e2 904 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 905 unsigned char digital_sensor( void )
TetsuyaKonno 0:98993ca640e2 906 {
TetsuyaKonno 0:98993ca640e2 907 return sensor_value;
TetsuyaKonno 0:98993ca640e2 908 }
TetsuyaKonno 0:98993ca640e2 909
TetsuyaKonno 0:98993ca640e2 910 //Image Data YCbCr -> Y(320*240pix)
TetsuyaKonno 0:98993ca640e2 911 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 912 void Image_Extraction( unsigned char *Data_Y )
TetsuyaKonno 0:98993ca640e2 913 {
TetsuyaKonno 0:98993ca640e2 914 static int Xp, Yp, inc;
TetsuyaKonno 0:98993ca640e2 915 static int counter = 0;
TetsuyaKonno 0:98993ca640e2 916
TetsuyaKonno 0:98993ca640e2 917 // Distributed processing
TetsuyaKonno 0:98993ca640e2 918 switch( counter++ ) {
TetsuyaKonno 0:98993ca640e2 919 case 0:
TetsuyaKonno 0:98993ca640e2 920 for( Yp = 0, inc = 0; Yp < 30; Yp++ ){
TetsuyaKonno 0:98993ca640e2 921 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 922 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 923 }
TetsuyaKonno 0:98993ca640e2 924 }
TetsuyaKonno 0:98993ca640e2 925 break;
TetsuyaKonno 0:98993ca640e2 926 case 1:
TetsuyaKonno 0:98993ca640e2 927 for( ; Yp < 60; Yp++ ){
TetsuyaKonno 0:98993ca640e2 928 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 929 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 930 }
TetsuyaKonno 0:98993ca640e2 931 }
TetsuyaKonno 0:98993ca640e2 932 break;
TetsuyaKonno 0:98993ca640e2 933 case 2:
TetsuyaKonno 0:98993ca640e2 934 for( ; Yp < 90; Yp++ ){
TetsuyaKonno 0:98993ca640e2 935 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 936 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 937 }
TetsuyaKonno 0:98993ca640e2 938 }
TetsuyaKonno 0:98993ca640e2 939 break;
TetsuyaKonno 0:98993ca640e2 940 case 3:
TetsuyaKonno 0:98993ca640e2 941 for( ; Yp < 120; Yp++ ){
TetsuyaKonno 0:98993ca640e2 942 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 943 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 944 }
TetsuyaKonno 0:98993ca640e2 945 }
TetsuyaKonno 0:98993ca640e2 946 break;
TetsuyaKonno 0:98993ca640e2 947 case 4:
TetsuyaKonno 0:98993ca640e2 948 for( ; Yp < 150; Yp++ ){
TetsuyaKonno 0:98993ca640e2 949 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 950 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 951 }
TetsuyaKonno 0:98993ca640e2 952 }
TetsuyaKonno 0:98993ca640e2 953 break;
TetsuyaKonno 0:98993ca640e2 954 case 5:
TetsuyaKonno 0:98993ca640e2 955 for( ; Yp < 180; Yp++ ){
TetsuyaKonno 0:98993ca640e2 956 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 957 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 958 }
TetsuyaKonno 0:98993ca640e2 959 }
TetsuyaKonno 0:98993ca640e2 960 break;
TetsuyaKonno 0:98993ca640e2 961 case 6:
TetsuyaKonno 0:98993ca640e2 962 for( ; Yp < 210; Yp++ ){
TetsuyaKonno 0:98993ca640e2 963 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 964 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 965 }
TetsuyaKonno 0:98993ca640e2 966 }
TetsuyaKonno 0:98993ca640e2 967 break;
TetsuyaKonno 0:98993ca640e2 968 case 7:
TetsuyaKonno 0:98993ca640e2 969 for( ; Yp < 240; Yp++ ){
TetsuyaKonno 0:98993ca640e2 970 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:98993ca640e2 971 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:98993ca640e2 972 }
TetsuyaKonno 0:98993ca640e2 973 }
TetsuyaKonno 0:98993ca640e2 974 counter = 0;
TetsuyaKonno 0:98993ca640e2 975 break;
TetsuyaKonno 0:98993ca640e2 976 default:
TetsuyaKonno 0:98993ca640e2 977 break;
TetsuyaKonno 0:98993ca640e2 978 }
TetsuyaKonno 0:98993ca640e2 979
TetsuyaKonno 0:98993ca640e2 980 }
TetsuyaKonno 0:98993ca640e2 981
TetsuyaKonno 0:98993ca640e2 982 //Image_Compression2 Y ( Averaging processing )
TetsuyaKonno 0:98993ca640e2 983 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 984 void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M )
TetsuyaKonno 0:98993ca640e2 985 {
TetsuyaKonno 0:98993ca640e2 986 int Data_H, Pixel_T, Pixel_D;
TetsuyaKonno 0:98993ca640e2 987 int x, y;
TetsuyaKonno 0:98993ca640e2 988 static int Xp, Yp, inc;
TetsuyaKonno 0:98993ca640e2 989 static int counter = 0;
TetsuyaKonno 0:98993ca640e2 990
TetsuyaKonno 0:98993ca640e2 991 Data_H = (Data_W / (double)4) * 3;
TetsuyaKonno 0:98993ca640e2 992 Pixel_D = Comp_M * Comp_M;
TetsuyaKonno 0:98993ca640e2 993
TetsuyaKonno 0:98993ca640e2 994 // Distributed processing
TetsuyaKonno 0:98993ca640e2 995 switch( counter++ ) {
TetsuyaKonno 0:98993ca640e2 996 case 0:
TetsuyaKonno 0:98993ca640e2 997 for( Yp = 0, inc = 0; Yp < (Data_H / 2); Yp+=Comp_M ){
TetsuyaKonno 0:98993ca640e2 998 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:98993ca640e2 999 Pixel_T = 0;
TetsuyaKonno 0:98993ca640e2 1000 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:98993ca640e2 1001 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:98993ca640e2 1002 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:98993ca640e2 1003 }
TetsuyaKonno 0:98993ca640e2 1004 }
TetsuyaKonno 0:98993ca640e2 1005 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:98993ca640e2 1006 }
TetsuyaKonno 0:98993ca640e2 1007 }
TetsuyaKonno 0:98993ca640e2 1008 break;
TetsuyaKonno 0:98993ca640e2 1009 case 1:
TetsuyaKonno 0:98993ca640e2 1010 for( ; Yp < Data_H; Yp+=Comp_M ){
TetsuyaKonno 0:98993ca640e2 1011 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:98993ca640e2 1012 Pixel_T = 0;
TetsuyaKonno 0:98993ca640e2 1013 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:98993ca640e2 1014 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:98993ca640e2 1015 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:98993ca640e2 1016 }
TetsuyaKonno 0:98993ca640e2 1017 }
TetsuyaKonno 0:98993ca640e2 1018 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:98993ca640e2 1019 }
TetsuyaKonno 0:98993ca640e2 1020 }
TetsuyaKonno 0:98993ca640e2 1021 counter = 0;
TetsuyaKonno 0:98993ca640e2 1022 break;
TetsuyaKonno 0:98993ca640e2 1023 default:
TetsuyaKonno 0:98993ca640e2 1024 break;
TetsuyaKonno 0:98993ca640e2 1025 }
TetsuyaKonno 0:98993ca640e2 1026
TetsuyaKonno 0:98993ca640e2 1027 }
TetsuyaKonno 0:98993ca640e2 1028
TetsuyaKonno 0:98993ca640e2 1029 // Binarization_process
TetsuyaKonno 0:98993ca640e2 1030 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 1031 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items )
TetsuyaKonno 0:98993ca640e2 1032 {
TetsuyaKonno 0:98993ca640e2 1033 int i, threshold;
TetsuyaKonno 0:98993ca640e2 1034
TetsuyaKonno 0:98993ca640e2 1035 threshold = 128;
TetsuyaKonno 0:98993ca640e2 1036 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:98993ca640e2 1037 if( Comp_Y[i] >= threshold ) Binary[i] = 1;
TetsuyaKonno 0:98993ca640e2 1038 else Binary[i] = 0;
TetsuyaKonno 0:98993ca640e2 1039 }
TetsuyaKonno 0:98993ca640e2 1040 }
TetsuyaKonno 0:98993ca640e2 1041
TetsuyaKonno 0:98993ca640e2 1042 //******************************************************************//
TetsuyaKonno 0:98993ca640e2 1043 // @brief Interrupt callback function
TetsuyaKonno 0:98993ca640e2 1044 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:98993ca640e2 1045 // @retval None
TetsuyaKonno 0:98993ca640e2 1046 //*******************************************************************/
TetsuyaKonno 0:98993ca640e2 1047 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:98993ca640e2 1048 {
TetsuyaKonno 0:98993ca640e2 1049 if (vfield_count > 0) {
TetsuyaKonno 0:98993ca640e2 1050 vfield_count--;
TetsuyaKonno 0:98993ca640e2 1051 }
TetsuyaKonno 0:98993ca640e2 1052 }
TetsuyaKonno 0:98993ca640e2 1053
TetsuyaKonno 0:98993ca640e2 1054 //******************************************************************//
TetsuyaKonno 0:98993ca640e2 1055 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:98993ca640e2 1056 // @param[in] wait_count : Wait count
TetsuyaKonno 0:98993ca640e2 1057 // @retval None
TetsuyaKonno 0:98993ca640e2 1058 //*******************************************************************/
TetsuyaKonno 0:98993ca640e2 1059 static void WaitVfield(const int32_t wait_count)
TetsuyaKonno 0:98993ca640e2 1060 {
TetsuyaKonno 0:98993ca640e2 1061 vfield_count = wait_count;
TetsuyaKonno 0:98993ca640e2 1062 while (vfield_count > 0) {
TetsuyaKonno 0:98993ca640e2 1063 /* Do nothing */
TetsuyaKonno 0:98993ca640e2 1064 }
TetsuyaKonno 0:98993ca640e2 1065 }
TetsuyaKonno 0:98993ca640e2 1066
TetsuyaKonno 0:98993ca640e2 1067 //******************************************************************//
TetsuyaKonno 0:98993ca640e2 1068 // @brief Interrupt callback function for Vsync interruption
TetsuyaKonno 0:98993ca640e2 1069 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:98993ca640e2 1070 // @retval None
TetsuyaKonno 0:98993ca640e2 1071 //*******************************************************************/
TetsuyaKonno 0:98993ca640e2 1072 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:98993ca640e2 1073 {
TetsuyaKonno 0:98993ca640e2 1074 if (vsync_count > 0) {
TetsuyaKonno 0:98993ca640e2 1075 vsync_count--;
TetsuyaKonno 0:98993ca640e2 1076 }
TetsuyaKonno 0:98993ca640e2 1077 }
TetsuyaKonno 0:98993ca640e2 1078
TetsuyaKonno 0:98993ca640e2 1079 //******************************************************************//
TetsuyaKonno 0:98993ca640e2 1080 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:98993ca640e2 1081 // @param[in] wait_count : Wait count
TetsuyaKonno 0:98993ca640e2 1082 // @retval None
TetsuyaKonno 0:98993ca640e2 1083 //*******************************************************************/
TetsuyaKonno 0:98993ca640e2 1084 static void WaitVsync(const int32_t wait_count)
TetsuyaKonno 0:98993ca640e2 1085 {
TetsuyaKonno 0:98993ca640e2 1086 vsync_count = wait_count;
TetsuyaKonno 0:98993ca640e2 1087 while (vsync_count > 0) {
TetsuyaKonno 0:98993ca640e2 1088 /* Do nothing */
TetsuyaKonno 0:98993ca640e2 1089 }
TetsuyaKonno 0:98993ca640e2 1090 }
TetsuyaKonno 0:98993ca640e2 1091
TetsuyaKonno 0:98993ca640e2 1092 //------------------------------------------------------------------//
TetsuyaKonno 0:98993ca640e2 1093 // End of file
TetsuyaKonno 0:98993ca640e2 1094 //------------------------------------------------------------------//