Tomokazu Suzuki / Mbed 2 deprecated Trace_Program2

This program was added the mark detection program on the trace program.

Dependencies:   GR-PEACH_video mbed

main.cpp@1:551e0f0cd55d, 2016-03-02 (annotated)

Committer:
ssuzukito
Date:
Wed Mar 02 05:48:41 2016 +0000
Revision:
1:551e0f0cd55d
Parent:
0:5db2664ff378 
mcr_trace2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:5db2664ff378 1 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:5db2664ff378 3 //File Contents: Trace program 2 (Mark detection) by image processing
TetsuyaKonno 0:5db2664ff378 4 // (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:5db2664ff378 5 //Version number: Ver.1.01
TetsuyaKonno 0:5db2664ff378 6 //Date: 2016.02.23
TetsuyaKonno 0:5db2664ff378 7 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:5db2664ff378 8 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 9
TetsuyaKonno 0:5db2664ff378 10 //This program supports the following boards:
TetsuyaKonno 0:5db2664ff378 11 //* GR-PEACH(E version)
TetsuyaKonno 0:5db2664ff378 12 //* Motor drive board Ver.5
TetsuyaKonno 0:5db2664ff378 13 //* Camera module (SC-310)
TetsuyaKonno 0:5db2664ff378 14
TetsuyaKonno 0:5db2664ff378 15 //Include
TetsuyaKonno 0:5db2664ff378 16 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 17 #include "mbed.h"
TetsuyaKonno 0:5db2664ff378 18 #include "math.h"
TetsuyaKonno 0:5db2664ff378 19 #include "iodefine.h"
TetsuyaKonno 0:5db2664ff378 20 #include "DisplayBace.h"
TetsuyaKonno 0:5db2664ff378 21
TetsuyaKonno 0:5db2664ff378 22 //Define
TetsuyaKonno 0:5db2664ff378 23 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 24 //Motor PWM cycle
TetsuyaKonno 0:5db2664ff378 25 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */
TetsuyaKonno 0:5db2664ff378 26 /* 1ms P0φ/1 = 0.03us */
TetsuyaKonno 0:5db2664ff378 27 //Motor speed
TetsuyaKonno 0:5db2664ff378 28 #define MAX_SPEED 30 /* motor() set: 0 to 100 */
TetsuyaKonno 0:5db2664ff378 29
TetsuyaKonno 0:5db2664ff378 30 //Servo PWM cycle
TetsuyaKonno 0:5db2664ff378 31 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
TetsuyaKonno 0:5db2664ff378 32 /* 16ms P0φ/16 = 0.48us */
TetsuyaKonno 0:5db2664ff378 33 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
TetsuyaKonno 0:5db2664ff378 34 #define HANDLE_STEP 18 /* 1 degree value */
TetsuyaKonno 0:5db2664ff378 35
TetsuyaKonno 0:5db2664ff378 36 //Image sensor
TetsuyaKonno 0:5db2664ff378 37 #define IMAGE_LINE 160 /* Y Line No */
TetsuyaKonno 0:5db2664ff378 38 #define IM_GAP_SET 48 /* 160pix - 112pix = 48 */
TetsuyaKonno 0:5db2664ff378 39
TetsuyaKonno 0:5db2664ff378 40 //Handle
TetsuyaKonno 0:5db2664ff378 41 #define ANLOG_STEP 60 /* */
TetsuyaKonno 0:5db2664ff378 42
TetsuyaKonno 0:5db2664ff378 43 //LED Color on GR-PEACH
TetsuyaKonno 0:5db2664ff378 44 #define LED_OFF 0x00
TetsuyaKonno 0:5db2664ff378 45 #define LED_RED 0x01
TetsuyaKonno 0:5db2664ff378 46 #define LED_GREEN 0x02
TetsuyaKonno 0:5db2664ff378 47 #define LED_YELLOW 0x03
TetsuyaKonno 0:5db2664ff378 48 #define LED_BLUE 0x04
TetsuyaKonno 0:5db2664ff378 49 #define LED_PURPLE 0x05
TetsuyaKonno 0:5db2664ff378 50 #define LED_SKYBLUE 0x06
TetsuyaKonno 0:5db2664ff378 51 #define LED_WHITE 0x07
TetsuyaKonno 0:5db2664ff378 52
TetsuyaKonno 0:5db2664ff378 53 //Status
TetsuyaKonno 0:5db2664ff378 54 #define RUN 0x00
TetsuyaKonno 0:5db2664ff378 55 #define SENSOR 0x01
TetsuyaKonno 0:5db2664ff378 56 #define MARK 0x02
TetsuyaKonno 0:5db2664ff378 57 #define STOP 0x03
TetsuyaKonno 0:5db2664ff378 58 #define ERROR 0xff
TetsuyaKonno 0:5db2664ff378 59
TetsuyaKonno 0:5db2664ff378 60 //Define(NTSC-Video)
TetsuyaKonno 0:5db2664ff378 61 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 62 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0)
TetsuyaKonno 0:5db2664ff378 63 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD)
TetsuyaKonno 0:5db2664ff378 64 #define DATA_SIZE_PER_PIC (2u)
TetsuyaKonno 0:5db2664ff378 65
TetsuyaKonno 0:5db2664ff378 66 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
TetsuyaKonno 0:5db2664ff378 67 in accordance with the frame buffer burst transfer mode. */
TetsuyaKonno 0:5db2664ff378 68 #define PIXEL_HW (320u) /* QVGA */
TetsuyaKonno 0:5db2664ff378 69 #define PIXEL_VW (240u) /* QVGA */
TetsuyaKonno 0:5db2664ff378 70 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
TetsuyaKonno 0:5db2664ff378 71 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW)
TetsuyaKonno 0:5db2664ff378 72
TetsuyaKonno 0:5db2664ff378 73 //Constructor
TetsuyaKonno 0:5db2664ff378 74 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 75 Ticker interrput;
TetsuyaKonno 0:5db2664ff378 76 Serial pc(USBTX, USBRX);
TetsuyaKonno 0:5db2664ff378 77 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */
TetsuyaKonno 0:5db2664ff378 78 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */
TetsuyaKonno 0:5db2664ff378 79 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */
TetsuyaKonno 0:5db2664ff378 80 DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */
TetsuyaKonno 0:5db2664ff378 81
TetsuyaKonno 0:5db2664ff378 82 DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */
TetsuyaKonno 0:5db2664ff378 83 DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */
TetsuyaKonno 0:5db2664ff378 84 DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */
TetsuyaKonno 0:5db2664ff378 85 DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */
TetsuyaKonno 0:5db2664ff378 86 DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */
TetsuyaKonno 0:5db2664ff378 87
TetsuyaKonno 0:5db2664ff378 88 //Prototype
TetsuyaKonno 0:5db2664ff378 89 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 90 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */
TetsuyaKonno 0:5db2664ff378 91 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
TetsuyaKonno 0:5db2664ff378 92 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:5db2664ff378 93 void led_rgb(int led);
TetsuyaKonno 0:5db2664ff378 94 unsigned int user_button_get( void );
TetsuyaKonno 0:5db2664ff378 95 void led_status_process( void ); /* Function for only interrupt */
TetsuyaKonno 0:5db2664ff378 96 void led_status_set( int set );
TetsuyaKonno 0:5db2664ff378 97
TetsuyaKonno 0:5db2664ff378 98 void led_out(int led);
TetsuyaKonno 0:5db2664ff378 99 unsigned int pushsw_get( void );
TetsuyaKonno 0:5db2664ff378 100 void motor( int accele_l, int accele_r );
TetsuyaKonno 0:5db2664ff378 101 void handle( int angle );
TetsuyaKonno 0:5db2664ff378 102 int diff( int pwm );
TetsuyaKonno 0:5db2664ff378 103 void ServoControl_process( void );
TetsuyaKonno 0:5db2664ff378 104
TetsuyaKonno 0:5db2664ff378 105 //Prototype(NTSC-video)
TetsuyaKonno 0:5db2664ff378 106 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 107 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:5db2664ff378 108 static void WaitVfield(const int32_t wait_count);
TetsuyaKonno 0:5db2664ff378 109 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:5db2664ff378 110 static void WaitVsync(const int32_t wait_count);
TetsuyaKonno 0:5db2664ff378 111
TetsuyaKonno 0:5db2664ff378 112 //Prototype(Display Debug)
TetsuyaKonno 0:5db2664ff378 113 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 114 void ImageData_Serial_Out( unsigned char *Data_Y, int Width );
TetsuyaKonno 0:5db2664ff378 115 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width );
TetsuyaKonno 0:5db2664ff378 116 void ImageData_Serial_Out3( void );
TetsuyaKonno 0:5db2664ff378 117
TetsuyaKonno 0:5db2664ff378 118 //Prototype(Trace by Image Processing)
TetsuyaKonno 0:5db2664ff378 119 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 120 void change_framebuffer_process( void );
TetsuyaKonno 0:5db2664ff378 121 int center_line_corrective( void );
TetsuyaKonno 0:5db2664ff378 122 void digital_sensor_corrective( void );
TetsuyaKonno 0:5db2664ff378 123 void image_SensorAnalog_process( int timer33 );
TetsuyaKonno 0:5db2664ff378 124 int image_sensorAnalog_get( void );
TetsuyaKonno 0:5db2664ff378 125 unsigned char image_digital_sensor( void );
TetsuyaKonno 0:5db2664ff378 126
TetsuyaKonno 0:5db2664ff378 127 //Prototype(Mark detection)
TetsuyaKonno 0:5db2664ff378 128 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 129 void Image_Extraction( unsigned char *Data_Y );
TetsuyaKonno 0:5db2664ff378 130 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size );
TetsuyaKonno 0:5db2664ff378 131 void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W );
TetsuyaKonno 0:5db2664ff378 132 void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M );
TetsuyaKonno 0:5db2664ff378 133 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items );
TetsuyaKonno 0:5db2664ff378 134 double Standard_Deviation( unsigned char *data, double *Devi, int items );
TetsuyaKonno 0:5db2664ff378 135 double Covariance( double *Devi_A, double *Devi_B, int items );
TetsuyaKonno 0:5db2664ff378 136 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B );
TetsuyaKonno 0:5db2664ff378 137
TetsuyaKonno 0:5db2664ff378 138 //Globle
TetsuyaKonno 0:5db2664ff378 139 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 140 volatile unsigned long cnt0; /* Used by timer function */
TetsuyaKonno 0:5db2664ff378 141 volatile unsigned long cnt1; /* Used within main */
TetsuyaKonno 0:5db2664ff378 142 volatile int pattern; /* Pattern numbers */
TetsuyaKonno 0:5db2664ff378 143 volatile int status_set; /* Status */
TetsuyaKonno 0:5db2664ff378 144 volatile int handle_buff;
TetsuyaKonno 0:5db2664ff378 145 volatile int iServo;
TetsuyaKonno 0:5db2664ff378 146
TetsuyaKonno 0:5db2664ff378 147 const int revolution_difference[] = {
TetsuyaKonno 0:5db2664ff378 148 100, 98, 97, 95, 93,
TetsuyaKonno 0:5db2664ff378 149 92, 90, 88, 87, 85,
TetsuyaKonno 0:5db2664ff378 150 84, 82, 81, 79, 78,
TetsuyaKonno 0:5db2664ff378 151 76, 75, 73, 72, 71,
TetsuyaKonno 0:5db2664ff378 152 69, 68, 66, 65, 64,
TetsuyaKonno 0:5db2664ff378 153 62, 61, 59, 58, 57,
TetsuyaKonno 0:5db2664ff378 154 55, 54, 52, 51, 50,
TetsuyaKonno 0:5db2664ff378 155 48, 47, 45, 44, 42,
TetsuyaKonno 0:5db2664ff378 156 41, 39, 38, 36, 35,
TetsuyaKonno 0:5db2664ff378 157 33 };
TetsuyaKonno 0:5db2664ff378 158
TetsuyaKonno 0:5db2664ff378 159 /* Trace by image processing */
TetsuyaKonno 0:5db2664ff378 160 volatile int im_offset_x;
TetsuyaKonno 0:5db2664ff378 161 volatile int digital_sensor_threshold;
TetsuyaKonno 0:5db2664ff378 162 volatile int sensor_x[5];
TetsuyaKonno 0:5db2664ff378 163 volatile int sensor_mode;
TetsuyaKonno 0:5db2664ff378 164 volatile int Left_sensor;
TetsuyaKonno 0:5db2664ff378 165 volatile int Right_sensor;
TetsuyaKonno 0:5db2664ff378 166 volatile int white;
TetsuyaKonno 0:5db2664ff378 167 volatile int black;
TetsuyaKonno 0:5db2664ff378 168
TetsuyaKonno 0:5db2664ff378 169 /* Mark detection */
TetsuyaKonno 0:5db2664ff378 170 unsigned char ImageData[320*240];
TetsuyaKonno 0:5db2664ff378 171 unsigned char ImageComp[160*120];
TetsuyaKonno 0:5db2664ff378 172 unsigned char ImageBinary[160*120];
TetsuyaKonno 0:5db2664ff378 173
TetsuyaKonno 0:5db2664ff378 174 double TempDevi_A[15];
TetsuyaKonno 0:5db2664ff378 175 unsigned char TempBinary_A[15] = {0,1,1,1,0,
TetsuyaKonno 0:5db2664ff378 176 0,0,1,0,0,
TetsuyaKonno 0:5db2664ff378 177 0,0,0,0,0};
TetsuyaKonno 0:5db2664ff378 178 double NowDevi[15];
TetsuyaKonno 0:5db2664ff378 179 unsigned char NowImageBinary[15];
TetsuyaKonno 0:5db2664ff378 180
TetsuyaKonno 0:5db2664ff378 181 volatile double retDevi_A;
TetsuyaKonno 0:5db2664ff378 182 volatile double retDevi_B;
TetsuyaKonno 0:5db2664ff378 183 volatile double retCovari;
TetsuyaKonno 0:5db2664ff378 184 volatile int retJudgeIM;
TetsuyaKonno 0:5db2664ff378 185 volatile int retJudgeIM_Max;
TetsuyaKonno 0:5db2664ff378 186
TetsuyaKonno 0:5db2664ff378 187 int X_buff, Y_buff;
TetsuyaKonno 0:5db2664ff378 188
TetsuyaKonno 0:5db2664ff378 189 //Globle(NTSC-video)
TetsuyaKonno 0:5db2664ff378 190 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 191 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:5db2664ff378 192 static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:5db2664ff378 193 static volatile int32_t vsync_count;
TetsuyaKonno 0:5db2664ff378 194 static volatile int32_t vfield_count;
TetsuyaKonno 0:5db2664ff378 195 uint8_t * write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:5db2664ff378 196 uint8_t * save_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:5db2664ff378 197
TetsuyaKonno 0:5db2664ff378 198 //Main
TetsuyaKonno 0:5db2664ff378 199 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 200 int main( void )
TetsuyaKonno 0:5db2664ff378 201 {
TetsuyaKonno 0:5db2664ff378 202 int i;
TetsuyaKonno 0:5db2664ff378 203
TetsuyaKonno 0:5db2664ff378 204 /* NTSC-Video */
TetsuyaKonno 0:5db2664ff378 205 DisplayBase::graphics_error_t error;
TetsuyaKonno 0:5db2664ff378 206
TetsuyaKonno 0:5db2664ff378 207 /* Create DisplayBase object */
TetsuyaKonno 0:5db2664ff378 208 DisplayBase Display;
TetsuyaKonno 0:5db2664ff378 209
TetsuyaKonno 0:5db2664ff378 210 /* Graphics initialization process */
TetsuyaKonno 0:5db2664ff378 211 error = Display.Graphics_init(NULL);
TetsuyaKonno 0:5db2664ff378 212 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 213 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 214 while (1);
TetsuyaKonno 0:5db2664ff378 215 }
TetsuyaKonno 0:5db2664ff378 216
TetsuyaKonno 0:5db2664ff378 217 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
TetsuyaKonno 0:5db2664ff378 218 if( error != DisplayBase::GRAPHICS_OK ) {
TetsuyaKonno 0:5db2664ff378 219 while(1);
TetsuyaKonno 0:5db2664ff378 220 }
TetsuyaKonno 0:5db2664ff378 221
TetsuyaKonno 0:5db2664ff378 222 /* Interrupt callback function setting (Vsync signal input to scaler 0) */
TetsuyaKonno 0:5db2664ff378 223 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
TetsuyaKonno 0:5db2664ff378 224 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 225 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 226 while (1);
TetsuyaKonno 0:5db2664ff378 227 }
TetsuyaKonno 0:5db2664ff378 228
TetsuyaKonno 0:5db2664ff378 229 /* Video capture setting (progressive form fixed) */
TetsuyaKonno 0:5db2664ff378 230 error = Display.Video_Write_Setting(
TetsuyaKonno 0:5db2664ff378 231 VIDEO_INPUT_CH,
TetsuyaKonno 0:5db2664ff378 232 DisplayBase::COL_SYS_NTSC_358,
TetsuyaKonno 0:5db2664ff378 233 write_buff_addr,
TetsuyaKonno 0:5db2664ff378 234 VIDEO_BUFFER_STRIDE,
TetsuyaKonno 0:5db2664ff378 235 DisplayBase::VIDEO_FORMAT_YCBCR422,
TetsuyaKonno 0:5db2664ff378 236 DisplayBase::WR_RD_WRSWA_32_16BIT,
TetsuyaKonno 0:5db2664ff378 237 PIXEL_VW,
TetsuyaKonno 0:5db2664ff378 238 PIXEL_HW
TetsuyaKonno 0:5db2664ff378 239 );
TetsuyaKonno 0:5db2664ff378 240 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 241 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 242 while (1);
TetsuyaKonno 0:5db2664ff378 243 }
TetsuyaKonno 0:5db2664ff378 244
TetsuyaKonno 0:5db2664ff378 245 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
TetsuyaKonno 0:5db2664ff378 246 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
TetsuyaKonno 0:5db2664ff378 247 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 248 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 249 while (1);
TetsuyaKonno 0:5db2664ff378 250 }
TetsuyaKonno 0:5db2664ff378 251
TetsuyaKonno 0:5db2664ff378 252 /* Video write process start */
TetsuyaKonno 0:5db2664ff378 253 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:5db2664ff378 254 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 255 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 256 while (1);
TetsuyaKonno 0:5db2664ff378 257 }
TetsuyaKonno 0:5db2664ff378 258
TetsuyaKonno 0:5db2664ff378 259 /* Video write process stop */
TetsuyaKonno 0:5db2664ff378 260 error = Display.Video_Stop (VIDEO_INPUT_CH);
TetsuyaKonno 0:5db2664ff378 261 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 262 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 263 while (1);
TetsuyaKonno 0:5db2664ff378 264 }
TetsuyaKonno 0:5db2664ff378 265
TetsuyaKonno 0:5db2664ff378 266 /* Video write process start */
TetsuyaKonno 0:5db2664ff378 267 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:5db2664ff378 268 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 269 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 270 while (1);
TetsuyaKonno 0:5db2664ff378 271 }
TetsuyaKonno 0:5db2664ff378 272
TetsuyaKonno 0:5db2664ff378 273 /* Wait vsync to update resister */
TetsuyaKonno 0:5db2664ff378 274 WaitVsync(1);
TetsuyaKonno 0:5db2664ff378 275
TetsuyaKonno 0:5db2664ff378 276 /* Wait 2 Vfield(Top or bottom field) */
TetsuyaKonno 0:5db2664ff378 277 WaitVfield(2);
TetsuyaKonno 0:5db2664ff378 278
TetsuyaKonno 0:5db2664ff378 279 /* Initialize MCU functions */
TetsuyaKonno 0:5db2664ff378 280 init_MTU2_PWM_Motor();
TetsuyaKonno 0:5db2664ff378 281 init_MTU2_PWM_Servo();
TetsuyaKonno 0:5db2664ff378 282 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:5db2664ff378 283 pc.baud(230400);
TetsuyaKonno 0:5db2664ff378 284
TetsuyaKonno 0:5db2664ff378 285 /* Initialize Micon Car state */
TetsuyaKonno 0:5db2664ff378 286 led_out( 0x0 );
TetsuyaKonno 0:5db2664ff378 287 handle( 0 );
TetsuyaKonno 0:5db2664ff378 288 motor( 0, 0 );
TetsuyaKonno 0:5db2664ff378 289
TetsuyaKonno 0:5db2664ff378 290 /* wait to stabilize NTSC signal (about 170ms) */
TetsuyaKonno 0:5db2664ff378 291 wait(0.2);
TetsuyaKonno 0:5db2664ff378 292
TetsuyaKonno 0:5db2664ff378 293 led_status_set( SENSOR );
TetsuyaKonno 0:5db2664ff378 294 while(1) {
TetsuyaKonno 0:5db2664ff378 295 sensor_mode = 1;
TetsuyaKonno 0:5db2664ff378 296 i = center_line_corrective();
TetsuyaKonno 0:5db2664ff378 297 pc.printf( "x = %04d, cnt1 = %03d iret = %01d \r", im_offset_x, cnt1, i );
TetsuyaKonno 0:5db2664ff378 298 if( i == -1 ) {
TetsuyaKonno 0:5db2664ff378 299 /* End process */
TetsuyaKonno 0:5db2664ff378 300 pattern = 99;
TetsuyaKonno 0:5db2664ff378 301 break;
TetsuyaKonno 0:5db2664ff378 302 } else if ( i == 1 ) {
TetsuyaKonno 0:5db2664ff378 303 sensor_mode = 0;
TetsuyaKonno 0:5db2664ff378 304 pattern = 0;
TetsuyaKonno 0:5db2664ff378 305 led_status_set( RUN );
TetsuyaKonno 0:5db2664ff378 306 break;
TetsuyaKonno 0:5db2664ff378 307 }
TetsuyaKonno 0:5db2664ff378 308 }
TetsuyaKonno 0:5db2664ff378 309 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 310
TetsuyaKonno 0:5db2664ff378 311 digital_sensor_corrective();
TetsuyaKonno 0:5db2664ff378 312 pc.printf( "B = %04d, W = %04d \n\r", black, white );
TetsuyaKonno 0:5db2664ff378 313 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 314 pc.printf( "threshold = %03d \n\r", digital_sensor_threshold );
TetsuyaKonno 0:5db2664ff378 315 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 316 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 317 pc.printf( "image_sensor_value\n\r" );
TetsuyaKonno 0:5db2664ff378 318
TetsuyaKonno 0:5db2664ff378 319 while(1) {
TetsuyaKonno 0:5db2664ff378 320 i = image_digital_sensor();
ssuzukito 1:551e0f0cd55d 321 pc.printf( "= %05d, = 0x%02x, = %03d J= %3d%% X=%2d Y=%2d DIGI=%x\r",
ssuzukito 1:551e0f0cd55d 322 image_sensorAnalog_get(), i, handle_buff, retJudgeIM_Max, X_buff, Y_buff, image_digital_sensor() );
TetsuyaKonno 0:5db2664ff378 323 switch( pattern ) {
TetsuyaKonno 0:5db2664ff378 324 /*****************************************************************
TetsuyaKonno 0:5db2664ff378 325 About patern
TetsuyaKonno 0:5db2664ff378 326 0:wait for switch input
TetsuyaKonno 0:5db2664ff378 327 1:check if start bar is open
TetsuyaKonno 0:5db2664ff378 328 11:normal trace
TetsuyaKonno 0:5db2664ff378 329 *****************************************************************/
TetsuyaKonno 0:5db2664ff378 330 case 0:
TetsuyaKonno 0:5db2664ff378 331 /* wait for switch input */
TetsuyaKonno 0:5db2664ff378 332 handle( iServo );
TetsuyaKonno 0:5db2664ff378 333 if( pushsw_get() ) {
TetsuyaKonno 0:5db2664ff378 334 led_out( 0x0 );
TetsuyaKonno 0:5db2664ff378 335 led_status_set( RUN );
TetsuyaKonno 0:5db2664ff378 336 pattern = 11;
TetsuyaKonno 0:5db2664ff378 337 cnt1 = 0;
TetsuyaKonno 0:5db2664ff378 338 break;
TetsuyaKonno 0:5db2664ff378 339 }
TetsuyaKonno 0:5db2664ff378 340 if( cnt1 < 100 ) {
TetsuyaKonno 0:5db2664ff378 341 led_out( 0x1 );
TetsuyaKonno 0:5db2664ff378 342 } else if( cnt1 < 200 ) {
TetsuyaKonno 0:5db2664ff378 343 led_out( 0x2 );
TetsuyaKonno 0:5db2664ff378 344 } else {
TetsuyaKonno 0:5db2664ff378 345 cnt1 = 0;
TetsuyaKonno 0:5db2664ff378 346 }
TetsuyaKonno 0:5db2664ff378 347 break;
TetsuyaKonno 0:5db2664ff378 348
TetsuyaKonno 0:5db2664ff378 349 case 11:
TetsuyaKonno 0:5db2664ff378 350 /* normal trace */
TetsuyaKonno 0:5db2664ff378 351 handle( iServo );
TetsuyaKonno 0:5db2664ff378 352 if( retJudgeIM_Max >= 85 ) {
TetsuyaKonno 0:5db2664ff378 353 pattern = 90;
TetsuyaKonno 0:5db2664ff378 354 break;
TetsuyaKonno 0:5db2664ff378 355 }
TetsuyaKonno 0:5db2664ff378 356
TetsuyaKonno 0:5db2664ff378 357 if( handle_buff > 5 ) {
TetsuyaKonno 0:5db2664ff378 358 motor( 100, diff(100) );
TetsuyaKonno 0:5db2664ff378 359 } else if( handle_buff < -5 ) {
TetsuyaKonno 0:5db2664ff378 360 motor( diff(100), 100 );
TetsuyaKonno 0:5db2664ff378 361 } else {
TetsuyaKonno 0:5db2664ff378 362 motor( 100, 100 );
TetsuyaKonno 0:5db2664ff378 363 }
TetsuyaKonno 0:5db2664ff378 364 break;
TetsuyaKonno 0:5db2664ff378 365
TetsuyaKonno 0:5db2664ff378 366 case 90:
TetsuyaKonno 0:5db2664ff378 367 led_status_set( MARK );
TetsuyaKonno 0:5db2664ff378 368 motor( 0, 0 );
TetsuyaKonno 0:5db2664ff378 369 break;
TetsuyaKonno 0:5db2664ff378 370
TetsuyaKonno 0:5db2664ff378 371 case 99:
TetsuyaKonno 0:5db2664ff378 372 led_status_set( ERROR );
TetsuyaKonno 0:5db2664ff378 373 motor( 0, 0 );
TetsuyaKonno 0:5db2664ff378 374 break;
TetsuyaKonno 0:5db2664ff378 375 default:
TetsuyaKonno 0:5db2664ff378 376 break;
TetsuyaKonno 0:5db2664ff378 377 }
TetsuyaKonno 0:5db2664ff378 378 }
TetsuyaKonno 0:5db2664ff378 379 }
TetsuyaKonno 0:5db2664ff378 380
TetsuyaKonno 0:5db2664ff378 381 //Initialize MTU2 PWM functions
TetsuyaKonno 0:5db2664ff378 382 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 383 //MTU2_3, MTU2_4
TetsuyaKonno 0:5db2664ff378 384 //Reset-Synchronized PWM mode
TetsuyaKonno 0:5db2664ff378 385 //TIOC4A(P4_4) :Left-motor
TetsuyaKonno 0:5db2664ff378 386 //TIOC4B(P4_5) :Right-motor
TetsuyaKonno 0:5db2664ff378 387 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 388 void init_MTU2_PWM_Motor( void )
TetsuyaKonno 0:5db2664ff378 389 {
TetsuyaKonno 0:5db2664ff378 390 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:5db2664ff378 391 /* alternative mode */
TetsuyaKonno 0:5db2664ff378 392
TetsuyaKonno 0:5db2664ff378 393 /* MTU2_4 (P4_4)(P4_5) */
TetsuyaKonno 0:5db2664ff378 394 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:5db2664ff378 395 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:5db2664ff378 396 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */
TetsuyaKonno 0:5db2664ff378 397 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:5db2664ff378 398 /* 2nd altemative function/output */
TetsuyaKonno 0:5db2664ff378 399 GPIOP4 &= 0xffcf; /* */
TetsuyaKonno 0:5db2664ff378 400 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */
TetsuyaKonno 0:5db2664ff378 401 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */
TetsuyaKonno 0:5db2664ff378 402
TetsuyaKonno 0:5db2664ff378 403 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:5db2664ff378 404 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:5db2664ff378 405
TetsuyaKonno 0:5db2664ff378 406 /* MTU2_3 and MTU2_4 (Motor PWM) */
TetsuyaKonno 0:5db2664ff378 407 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */
TetsuyaKonno 0:5db2664ff378 408 MTU2TOCR1 = 0x04; /* */
TetsuyaKonno 0:5db2664ff378 409 MTU2TOCR2 = 0x40; /* N L>H P H>L */
TetsuyaKonno 0:5db2664ff378 410 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */
TetsuyaKonno 0:5db2664ff378 411 MTU2TMDR_4 = 0x30; /* Buff:ON */
TetsuyaKonno 0:5db2664ff378 412 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */
TetsuyaKonno 0:5db2664ff378 413 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
TetsuyaKonno 0:5db2664ff378 414 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
TetsuyaKonno 0:5db2664ff378 415 /* PWM-Cycle(1ms) */
TetsuyaKonno 0:5db2664ff378 416 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */
TetsuyaKonno 0:5db2664ff378 417 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */
TetsuyaKonno 0:5db2664ff378 418 MTU2TSTR |= 0x40; /* TCNT_4 Start */
TetsuyaKonno 0:5db2664ff378 419 }
TetsuyaKonno 0:5db2664ff378 420
TetsuyaKonno 0:5db2664ff378 421 //Initialize MTU2 PWM functions
TetsuyaKonno 0:5db2664ff378 422 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 423 //MTU2_0
TetsuyaKonno 0:5db2664ff378 424 //PWM mode 1
TetsuyaKonno 0:5db2664ff378 425 //TIOC0A(P4_0) :Servo-motor
TetsuyaKonno 0:5db2664ff378 426 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 427 void init_MTU2_PWM_Servo( void )
TetsuyaKonno 0:5db2664ff378 428 {
TetsuyaKonno 0:5db2664ff378 429 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:5db2664ff378 430 /* alternative mode */
TetsuyaKonno 0:5db2664ff378 431
TetsuyaKonno 0:5db2664ff378 432 /* MTU2_0 (P4_0) */
TetsuyaKonno 0:5db2664ff378 433 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:5db2664ff378 434 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:5db2664ff378 435 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:5db2664ff378 436 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
TetsuyaKonno 0:5db2664ff378 437 /* 2nd alternative function/output */
TetsuyaKonno 0:5db2664ff378 438 GPIOP4 &= 0xfffe; /* */
TetsuyaKonno 0:5db2664ff378 439 GPIOPM4 &= 0xfffe; /* p4_0:output */
TetsuyaKonno 0:5db2664ff378 440 GPIOPMC4 |= 0x0001; /* P4_0:double */
TetsuyaKonno 0:5db2664ff378 441
TetsuyaKonno 0:5db2664ff378 442 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:5db2664ff378 443 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:5db2664ff378 444
TetsuyaKonno 0:5db2664ff378 445 /* MTU2_0 (Motor PWM) */
TetsuyaKonno 0:5db2664ff378 446 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
TetsuyaKonno 0:5db2664ff378 447 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
TetsuyaKonno 0:5db2664ff378 448 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
TetsuyaKonno 0:5db2664ff378 449 /* PWM-mode1 */
TetsuyaKonno 0:5db2664ff378 450 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
TetsuyaKonno 0:5db2664ff378 451 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
TetsuyaKonno 0:5db2664ff378 452 /* PWM-Cycle(16ms) */
TetsuyaKonno 0:5db2664ff378 453 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
TetsuyaKonno 0:5db2664ff378 454 MTU2TSTR |= 0x01; /* TCNT_0 Start */
TetsuyaKonno 0:5db2664ff378 455 }
TetsuyaKonno 0:5db2664ff378 456
TetsuyaKonno 0:5db2664ff378 457 //Interrupt Timer
TetsuyaKonno 0:5db2664ff378 458 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 459 void intTimer( void )
TetsuyaKonno 0:5db2664ff378 460 {
TetsuyaKonno 0:5db2664ff378 461 int x, y;
TetsuyaKonno 0:5db2664ff378 462 static int counter = 0;
TetsuyaKonno 0:5db2664ff378 463
TetsuyaKonno 0:5db2664ff378 464 cnt0++;
TetsuyaKonno 0:5db2664ff378 465 cnt1++;
TetsuyaKonno 0:5db2664ff378 466
TetsuyaKonno 0:5db2664ff378 467 /* Trace by image processing */
TetsuyaKonno 0:5db2664ff378 468 image_SensorAnalog_process( counter );
TetsuyaKonno 0:5db2664ff378 469
TetsuyaKonno 0:5db2664ff378 470 ServoControl_process();
TetsuyaKonno 0:5db2664ff378 471
TetsuyaKonno 0:5db2664ff378 472 switch( counter++ ) {
TetsuyaKonno 0:5db2664ff378 473 case 0:
TetsuyaKonno 0:5db2664ff378 474 change_framebuffer_process();
TetsuyaKonno 0:5db2664ff378 475 break;
TetsuyaKonno 0:5db2664ff378 476 case 1:
TetsuyaKonno 0:5db2664ff378 477 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 478 break;
TetsuyaKonno 0:5db2664ff378 479 case 2:
TetsuyaKonno 0:5db2664ff378 480 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 481 break;
TetsuyaKonno 0:5db2664ff378 482 case 3:
TetsuyaKonno 0:5db2664ff378 483 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 484 break;
TetsuyaKonno 0:5db2664ff378 485 case 4:
TetsuyaKonno 0:5db2664ff378 486 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 487 break;
TetsuyaKonno 0:5db2664ff378 488 case 5:
TetsuyaKonno 0:5db2664ff378 489 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 490 break;
TetsuyaKonno 0:5db2664ff378 491 case 6:
TetsuyaKonno 0:5db2664ff378 492 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 493 break;
TetsuyaKonno 0:5db2664ff378 494 case 7:
TetsuyaKonno 0:5db2664ff378 495 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 496 break;
TetsuyaKonno 0:5db2664ff378 497 case 8:
TetsuyaKonno 0:5db2664ff378 498 Image_Extraction( ImageData );
TetsuyaKonno 0:5db2664ff378 499 break;
TetsuyaKonno 0:5db2664ff378 500 case 9:
TetsuyaKonno 0:5db2664ff378 501 Image_Compression2( ImageData, 320, ImageComp, 16 );
TetsuyaKonno 0:5db2664ff378 502 break;
TetsuyaKonno 0:5db2664ff378 503 case 10:
TetsuyaKonno 0:5db2664ff378 504 Image_Compression2( ImageData, 320, ImageComp, 16 );
TetsuyaKonno 0:5db2664ff378 505 break;
TetsuyaKonno 0:5db2664ff378 506 case 11:
TetsuyaKonno 0:5db2664ff378 507 Binarization_process( ImageComp, ImageBinary, 20*15 );
TetsuyaKonno 0:5db2664ff378 508 break;
TetsuyaKonno 0:5db2664ff378 509 case 12:
TetsuyaKonno 0:5db2664ff378 510 retDevi_A = Standard_Deviation( TempBinary_A, TempDevi_A, 15 );
TetsuyaKonno 0:5db2664ff378 511 break;
TetsuyaKonno 0:5db2664ff378 512 case 13:
TetsuyaKonno 0:5db2664ff378 513 retJudgeIM_Max = 0;
TetsuyaKonno 0:5db2664ff378 514 for( y = 0; y <= 12; y++ ) {
TetsuyaKonno 0:5db2664ff378 515 for( x = 0; x <= 15; x++ ) {
TetsuyaKonno 0:5db2664ff378 516 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 );
TetsuyaKonno 0:5db2664ff378 517 retDevi_B = Standard_Deviation( NowImageBinary, NowDevi, 15 );
TetsuyaKonno 0:5db2664ff378 518 retCovari = Covariance( TempDevi_A, NowDevi, 15 );
TetsuyaKonno 0:5db2664ff378 519 retJudgeIM = 0;
TetsuyaKonno 0:5db2664ff378 520 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_A, retDevi_B );
TetsuyaKonno 0:5db2664ff378 521 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max ) {
TetsuyaKonno 0:5db2664ff378 522 X_buff = x;
TetsuyaKonno 0:5db2664ff378 523 Y_buff = y;
TetsuyaKonno 0:5db2664ff378 524 retJudgeIM_Max = retJudgeIM;
TetsuyaKonno 0:5db2664ff378 525 }
TetsuyaKonno 0:5db2664ff378 526 }
TetsuyaKonno 0:5db2664ff378 527 }
TetsuyaKonno 0:5db2664ff378 528 break;
TetsuyaKonno 0:5db2664ff378 529 case 33:
TetsuyaKonno 0:5db2664ff378 530 counter = 0;
TetsuyaKonno 0:5db2664ff378 531 break;
TetsuyaKonno 0:5db2664ff378 532 default:
TetsuyaKonno 0:5db2664ff378 533 break;
TetsuyaKonno 0:5db2664ff378 534 }
TetsuyaKonno 0:5db2664ff378 535
TetsuyaKonno 0:5db2664ff378 536 led_status_process();
TetsuyaKonno 0:5db2664ff378 537 }
TetsuyaKonno 0:5db2664ff378 538
TetsuyaKonno 0:5db2664ff378 539 //LED_RGB(on GR-PEACH board)
TetsuyaKonno 0:5db2664ff378 540 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 541 void led_rgb(int led)
TetsuyaKonno 0:5db2664ff378 542 {
TetsuyaKonno 0:5db2664ff378 543 LED_R = led & 0x1;
TetsuyaKonno 0:5db2664ff378 544 LED_G = (led >> 1 ) & 0x1;
TetsuyaKonno 0:5db2664ff378 545 LED_B = (led >> 2 ) & 0x1;
TetsuyaKonno 0:5db2664ff378 546 }
TetsuyaKonno 0:5db2664ff378 547
TetsuyaKonno 0:5db2664ff378 548 //user_button_get(on GR-PEACH board)
TetsuyaKonno 0:5db2664ff378 549 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 550 unsigned int user_button_get( void )
TetsuyaKonno 0:5db2664ff378 551 {
TetsuyaKonno 0:5db2664ff378 552 return (~user_botton) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:5db2664ff378 553 }
TetsuyaKonno 0:5db2664ff378 554
TetsuyaKonno 0:5db2664ff378 555 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:5db2664ff378 556 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 557 void led_status_process( void )
TetsuyaKonno 0:5db2664ff378 558 {
TetsuyaKonno 0:5db2664ff378 559 static unsigned long led_timer;
TetsuyaKonno 0:5db2664ff378 560 int led_set;
TetsuyaKonno 0:5db2664ff378 561 int on_time;
TetsuyaKonno 0:5db2664ff378 562 int off_time;
TetsuyaKonno 0:5db2664ff378 563
TetsuyaKonno 0:5db2664ff378 564 /* setting */
TetsuyaKonno 0:5db2664ff378 565 switch( status_set ){
TetsuyaKonno 0:5db2664ff378 566 case RUN:
TetsuyaKonno 0:5db2664ff378 567 led_set = LED_GREEN;
TetsuyaKonno 0:5db2664ff378 568 on_time = 500;
TetsuyaKonno 0:5db2664ff378 569 off_time = 500;
TetsuyaKonno 0:5db2664ff378 570 break;
TetsuyaKonno 0:5db2664ff378 571
TetsuyaKonno 0:5db2664ff378 572 case SENSOR:
TetsuyaKonno 0:5db2664ff378 573 led_set = LED_BLUE;
TetsuyaKonno 0:5db2664ff378 574 on_time = 50;
TetsuyaKonno 0:5db2664ff378 575 off_time = 50;
TetsuyaKonno 0:5db2664ff378 576 break;
TetsuyaKonno 0:5db2664ff378 577
TetsuyaKonno 0:5db2664ff378 578 case MARK:
TetsuyaKonno 0:5db2664ff378 579 led_set = LED_RED;
TetsuyaKonno 0:5db2664ff378 580 on_time = 250;
TetsuyaKonno 0:5db2664ff378 581 off_time = 250;
TetsuyaKonno 0:5db2664ff378 582 break;
TetsuyaKonno 0:5db2664ff378 583
TetsuyaKonno 0:5db2664ff378 584 case STOP:
TetsuyaKonno 0:5db2664ff378 585 led_set = LED_RED;
TetsuyaKonno 0:5db2664ff378 586 on_time = 1;
TetsuyaKonno 0:5db2664ff378 587 off_time = 0;
TetsuyaKonno 0:5db2664ff378 588 break;
TetsuyaKonno 0:5db2664ff378 589
TetsuyaKonno 0:5db2664ff378 590 case ERROR:
TetsuyaKonno 0:5db2664ff378 591 led_set = LED_RED;
TetsuyaKonno 0:5db2664ff378 592 on_time = 50;
TetsuyaKonno 0:5db2664ff378 593 off_time = 50;
TetsuyaKonno 0:5db2664ff378 594 break;
TetsuyaKonno 0:5db2664ff378 595
TetsuyaKonno 0:5db2664ff378 596 default:
TetsuyaKonno 0:5db2664ff378 597 led_set = LED_OFF;
TetsuyaKonno 0:5db2664ff378 598 on_time = 0;
TetsuyaKonno 0:5db2664ff378 599 off_time = 1;
TetsuyaKonno 0:5db2664ff378 600 break;
TetsuyaKonno 0:5db2664ff378 601 }
TetsuyaKonno 0:5db2664ff378 602
TetsuyaKonno 0:5db2664ff378 603 /* Display */
TetsuyaKonno 0:5db2664ff378 604 led_timer++;
TetsuyaKonno 0:5db2664ff378 605 if( led_timer < on_time ) led_rgb( led_set );
TetsuyaKonno 0:5db2664ff378 606 else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF );
TetsuyaKonno 0:5db2664ff378 607 else led_timer = 0;
TetsuyaKonno 0:5db2664ff378 608 }
TetsuyaKonno 0:5db2664ff378 609
TetsuyaKonno 0:5db2664ff378 610 //LED_Status(on GR-PEACH board) Function for only interrupt
TetsuyaKonno 0:5db2664ff378 611 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 612 void led_status_set( int set )
TetsuyaKonno 0:5db2664ff378 613 {
TetsuyaKonno 0:5db2664ff378 614 status_set = set;
TetsuyaKonno 0:5db2664ff378 615 }
TetsuyaKonno 0:5db2664ff378 616
TetsuyaKonno 0:5db2664ff378 617 //led_out(on Motor drive board)
TetsuyaKonno 0:5db2664ff378 618 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 619 void led_out(int led)
TetsuyaKonno 0:5db2664ff378 620 {
TetsuyaKonno 0:5db2664ff378 621 led = ~led;
TetsuyaKonno 0:5db2664ff378 622 LED_3 = led & 0x1;
TetsuyaKonno 0:5db2664ff378 623 LED_2 = ( led >> 1 ) & 0x1;
TetsuyaKonno 0:5db2664ff378 624 }
TetsuyaKonno 0:5db2664ff378 625
TetsuyaKonno 0:5db2664ff378 626 //pushsw_get(on Motor drive board)
TetsuyaKonno 0:5db2664ff378 627 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 628 unsigned int pushsw_get( void )
TetsuyaKonno 0:5db2664ff378 629 {
TetsuyaKonno 0:5db2664ff378 630 return (~push_sw) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:5db2664ff378 631 }
TetsuyaKonno 0:5db2664ff378 632
TetsuyaKonno 0:5db2664ff378 633 //motor speed control(PWM)
TetsuyaKonno 0:5db2664ff378 634 //Arguments: motor:-100 to 100
TetsuyaKonno 0:5db2664ff378 635 //Here, 0 is stop, 100 is forward, -100 is reverse
TetsuyaKonno 0:5db2664ff378 636 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 637 void motor( int accele_l, int accele_r )
TetsuyaKonno 0:5db2664ff378 638 {
TetsuyaKonno 0:5db2664ff378 639 accele_l = ( accele_l * MAX_SPEED ) / 100;
TetsuyaKonno 0:5db2664ff378 640 accele_r = ( accele_r * MAX_SPEED ) / 100;
TetsuyaKonno 0:5db2664ff378 641
TetsuyaKonno 0:5db2664ff378 642 /* Left Motor Control */
TetsuyaKonno 0:5db2664ff378 643 if( accele_l >= 0 ) {
TetsuyaKonno 0:5db2664ff378 644 /* forward */
TetsuyaKonno 0:5db2664ff378 645 Left_motor_signal = 0;
TetsuyaKonno 0:5db2664ff378 646 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
TetsuyaKonno 0:5db2664ff378 647 } else {
TetsuyaKonno 0:5db2664ff378 648 /* reverse */
TetsuyaKonno 0:5db2664ff378 649 Left_motor_signal = 1;
TetsuyaKonno 0:5db2664ff378 650 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
TetsuyaKonno 0:5db2664ff378 651 }
TetsuyaKonno 0:5db2664ff378 652
TetsuyaKonno 0:5db2664ff378 653 /* Right Motor Control */
TetsuyaKonno 0:5db2664ff378 654 if( accele_r >= 0 ) {
TetsuyaKonno 0:5db2664ff378 655 /* forward */
TetsuyaKonno 0:5db2664ff378 656 Right_motor_signal = 0;
TetsuyaKonno 0:5db2664ff378 657 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
TetsuyaKonno 0:5db2664ff378 658 } else {
TetsuyaKonno 0:5db2664ff378 659 /* reverse */
TetsuyaKonno 0:5db2664ff378 660 Right_motor_signal = 1;
TetsuyaKonno 0:5db2664ff378 661 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
TetsuyaKonno 0:5db2664ff378 662 }
TetsuyaKonno 0:5db2664ff378 663 }
TetsuyaKonno 0:5db2664ff378 664
TetsuyaKonno 0:5db2664ff378 665 //Handle fanction
TetsuyaKonno 0:5db2664ff378 666 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 667 void handle( int angle )
TetsuyaKonno 0:5db2664ff378 668 {
TetsuyaKonno 0:5db2664ff378 669 handle_buff = angle;
TetsuyaKonno 0:5db2664ff378 670 /* When the servo move from left to right in reverse, replace "-" with "+" */
TetsuyaKonno 0:5db2664ff378 671 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
TetsuyaKonno 0:5db2664ff378 672 }
TetsuyaKonno 0:5db2664ff378 673
TetsuyaKonno 0:5db2664ff378 674 //Deff fanction
TetsuyaKonno 0:5db2664ff378 675 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 676 int diff( int pwm )
TetsuyaKonno 0:5db2664ff378 677 {
TetsuyaKonno 0:5db2664ff378 678 int i, ret;
TetsuyaKonno 0:5db2664ff378 679
TetsuyaKonno 0:5db2664ff378 680 i = handle_buff;
TetsuyaKonno 0:5db2664ff378 681 if( i < 0 ) i = -i;
TetsuyaKonno 0:5db2664ff378 682 if( i > 45 ) i = 45;
TetsuyaKonno 0:5db2664ff378 683 ret = revolution_difference[i] * pwm / 100;
TetsuyaKonno 0:5db2664ff378 684
TetsuyaKonno 0:5db2664ff378 685 return ret;
TetsuyaKonno 0:5db2664ff378 686 }
TetsuyaKonno 0:5db2664ff378 687
TetsuyaKonno 0:5db2664ff378 688 //ServoControl_process
TetsuyaKonno 0:5db2664ff378 689 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 690 void ServoControl_process( void )
TetsuyaKonno 0:5db2664ff378 691 {
TetsuyaKonno 0:5db2664ff378 692 static int iSensorPattern = 0;
TetsuyaKonno 0:5db2664ff378 693
TetsuyaKonno 0:5db2664ff378 694 /* -3° ～ +3° */
TetsuyaKonno 0:5db2664ff378 695 iServo = image_sensorAnalog_get() / ANLOG_STEP;
TetsuyaKonno 0:5db2664ff378 696
TetsuyaKonno 0:5db2664ff378 697 if( !sensor_mode ) {
TetsuyaKonno 0:5db2664ff378 698 switch( iSensorPattern ) {
TetsuyaKonno 0:5db2664ff378 699 case 0:
TetsuyaKonno 0:5db2664ff378 700 switch( image_digital_sensor()&0x0f ) {
TetsuyaKonno 0:5db2664ff378 701 case 0x03:
TetsuyaKonno 0:5db2664ff378 702 iServo = 14;
TetsuyaKonno 0:5db2664ff378 703 break;
TetsuyaKonno 0:5db2664ff378 704 case 0x01:
TetsuyaKonno 0:5db2664ff378 705 iServo = 20;
TetsuyaKonno 0:5db2664ff378 706 iSensorPattern = 11;
TetsuyaKonno 0:5db2664ff378 707 break;
TetsuyaKonno 0:5db2664ff378 708 case 0x0c:
TetsuyaKonno 0:5db2664ff378 709 iServo = -14;
TetsuyaKonno 0:5db2664ff378 710 break;
TetsuyaKonno 0:5db2664ff378 711 case 0x08:
TetsuyaKonno 0:5db2664ff378 712 iServo = -20;
TetsuyaKonno 0:5db2664ff378 713 iSensorPattern = 12;
TetsuyaKonno 0:5db2664ff378 714 break;
TetsuyaKonno 0:5db2664ff378 715 default:
TetsuyaKonno 0:5db2664ff378 716 break;
TetsuyaKonno 0:5db2664ff378 717 }
TetsuyaKonno 0:5db2664ff378 718 break;
TetsuyaKonno 0:5db2664ff378 719
TetsuyaKonno 0:5db2664ff378 720 case 11:
TetsuyaKonno 0:5db2664ff378 721 /* Left side */
TetsuyaKonno 0:5db2664ff378 722 iServo = 23;
TetsuyaKonno 0:5db2664ff378 723 if( (image_digital_sensor()&0x0f) == 0x03 ) {
TetsuyaKonno 0:5db2664ff378 724 iSensorPattern = 0;
TetsuyaKonno 0:5db2664ff378 725 }
TetsuyaKonno 0:5db2664ff378 726 break;
TetsuyaKonno 0:5db2664ff378 727
TetsuyaKonno 0:5db2664ff378 728 case 12:
TetsuyaKonno 0:5db2664ff378 729 /* right side */
TetsuyaKonno 0:5db2664ff378 730 iServo = -23;
TetsuyaKonno 0:5db2664ff378 731 if( (image_digital_sensor()&0x0f) == 0x0c ) {
TetsuyaKonno 0:5db2664ff378 732 iSensorPattern = 0;
TetsuyaKonno 0:5db2664ff378 733 }
TetsuyaKonno 0:5db2664ff378 734 break;
TetsuyaKonno 0:5db2664ff378 735 }
TetsuyaKonno 0:5db2664ff378 736 }
TetsuyaKonno 0:5db2664ff378 737 }
TetsuyaKonno 0:5db2664ff378 738
TetsuyaKonno 0:5db2664ff378 739 //Image Data Output( for the Excel )
TetsuyaKonno 0:5db2664ff378 740 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 741 void ImageData_Serial_Out( unsigned char *Data_Y, int Width )
TetsuyaKonno 0:5db2664ff378 742 {
TetsuyaKonno 0:5db2664ff378 743 int Xp, Yp, inc, Height;
TetsuyaKonno 0:5db2664ff378 744
TetsuyaKonno 0:5db2664ff378 745 Height = (Width / (double)4) * 3;
TetsuyaKonno 0:5db2664ff378 746 for( Yp = 0, inc = 0; Yp < Height; Yp++ ) {
TetsuyaKonno 0:5db2664ff378 747 for( Xp = 0; Xp < Width; Xp++, inc++ ) {
TetsuyaKonno 0:5db2664ff378 748 pc.printf( "%d,", Data_Y[ inc ] );
TetsuyaKonno 0:5db2664ff378 749 }
TetsuyaKonno 0:5db2664ff378 750 pc.printf("\n\r");
TetsuyaKonno 0:5db2664ff378 751 }
TetsuyaKonno 0:5db2664ff378 752 }
TetsuyaKonno 0:5db2664ff378 753
TetsuyaKonno 0:5db2664ff378 754 //Image Data Output2( for TeraTerm )
TetsuyaKonno 0:5db2664ff378 755 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 756 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width )
TetsuyaKonno 0:5db2664ff378 757 {
TetsuyaKonno 0:5db2664ff378 758 int Xp, Yp, Height;
TetsuyaKonno 0:5db2664ff378 759
TetsuyaKonno 0:5db2664ff378 760 Height = (Width / (double)4) * 3;
TetsuyaKonno 0:5db2664ff378 761 for( Yp = 0; Yp < Height; Yp++ ) {
TetsuyaKonno 0:5db2664ff378 762 for( Xp = 0; Xp < Width; Xp++ ) {
TetsuyaKonno 0:5db2664ff378 763 pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] );
TetsuyaKonno 0:5db2664ff378 764 }
TetsuyaKonno 0:5db2664ff378 765 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 766 }
TetsuyaKonno 0:5db2664ff378 767 pc.printf( "\033[%dA" , Height );
TetsuyaKonno 0:5db2664ff378 768 }
TetsuyaKonno 0:5db2664ff378 769
TetsuyaKonno 0:5db2664ff378 770 //Image Data Output3( for the converter ( csv --> jpg ) )
TetsuyaKonno 0:5db2664ff378 771 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 772 void ImageData_Serial_Out3( void )
TetsuyaKonno 0:5db2664ff378 773 {
TetsuyaKonno 0:5db2664ff378 774 int Xp, Yp, x, y;
TetsuyaKonno 0:5db2664ff378 775
TetsuyaKonno 0:5db2664ff378 776 /* Camera module test process */
TetsuyaKonno 0:5db2664ff378 777 pc.printf( "//,X-Size,Y-Size" );
TetsuyaKonno 0:5db2664ff378 778 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 779 pc.printf( "#SIZE,320,240" );
TetsuyaKonno 0:5db2664ff378 780 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 781 pc.printf( "//,X-Point,Y-Point" );
TetsuyaKonno 0:5db2664ff378 782 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 783
TetsuyaKonno 0:5db2664ff378 784 for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){
TetsuyaKonno 0:5db2664ff378 785 for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){
TetsuyaKonno 0:5db2664ff378 786 pc.printf( "#YCbCr," );
TetsuyaKonno 0:5db2664ff378 787 /*Xp*/pc.printf( "%d,", x);
TetsuyaKonno 0:5db2664ff378 788 /*Yp*/pc.printf( "%d,", y);
TetsuyaKonno 0:5db2664ff378 789 /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6
TetsuyaKonno 0:5db2664ff378 790 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
TetsuyaKonno 0:5db2664ff378 791 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
TetsuyaKonno 0:5db2664ff378 792 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 793
TetsuyaKonno 0:5db2664ff378 794 pc.printf( "#YCbCr," );
TetsuyaKonno 0:5db2664ff378 795 /*Xp*/pc.printf( "%d,", x+1);
TetsuyaKonno 0:5db2664ff378 796 /*Yp*/pc.printf( "%d,", y);
TetsuyaKonno 0:5db2664ff378 797 /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4
TetsuyaKonno 0:5db2664ff378 798 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
TetsuyaKonno 0:5db2664ff378 799 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
TetsuyaKonno 0:5db2664ff378 800 pc.printf( "\n\r" );
TetsuyaKonno 0:5db2664ff378 801 }
TetsuyaKonno 0:5db2664ff378 802 }
TetsuyaKonno 0:5db2664ff378 803 }
TetsuyaKonno 0:5db2664ff378 804
TetsuyaKonno 0:5db2664ff378 805 //Change FrameBuffer Process
TetsuyaKonno 0:5db2664ff378 806 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 807 void change_framebuffer_process( void )
TetsuyaKonno 0:5db2664ff378 808 {
TetsuyaKonno 0:5db2664ff378 809 DisplayBase::graphics_error_t error;
TetsuyaKonno 0:5db2664ff378 810 DisplayBase Display;
TetsuyaKonno 0:5db2664ff378 811
TetsuyaKonno 0:5db2664ff378 812 /* Change address buffer */
TetsuyaKonno 0:5db2664ff378 813 if (write_buff_addr == FrameBuffer_Video_A) {
TetsuyaKonno 0:5db2664ff378 814 write_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:5db2664ff378 815 save_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:5db2664ff378 816 } else {
TetsuyaKonno 0:5db2664ff378 817 write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:5db2664ff378 818 save_buff_addr = FrameBuffer_Video_B;
TetsuyaKonno 0:5db2664ff378 819 }
TetsuyaKonno 0:5db2664ff378 820
TetsuyaKonno 0:5db2664ff378 821 /* Change write buffer */
TetsuyaKonno 0:5db2664ff378 822 error = Display.Video_Write_Change(
TetsuyaKonno 0:5db2664ff378 823 VIDEO_INPUT_CH,
TetsuyaKonno 0:5db2664ff378 824 write_buff_addr,
TetsuyaKonno 0:5db2664ff378 825 VIDEO_BUFFER_STRIDE);
TetsuyaKonno 0:5db2664ff378 826 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:5db2664ff378 827 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:5db2664ff378 828 while (1);
TetsuyaKonno 0:5db2664ff378 829 }
TetsuyaKonno 0:5db2664ff378 830 }
TetsuyaKonno 0:5db2664ff378 831
TetsuyaKonno 0:5db2664ff378 832 //center_line_corrective_process
TetsuyaKonno 0:5db2664ff378 833 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 834 int center_line_corrective( void )
TetsuyaKonno 0:5db2664ff378 835 {
TetsuyaKonno 0:5db2664ff378 836 static int process_No = 0;
TetsuyaKonno 0:5db2664ff378 837 static int im_cnt = 0;
TetsuyaKonno 0:5db2664ff378 838 static int value = 0;
TetsuyaKonno 0:5db2664ff378 839 int iRet = 0;
TetsuyaKonno 0:5db2664ff378 840 int image_sensor_value;
TetsuyaKonno 0:5db2664ff378 841
TetsuyaKonno 0:5db2664ff378 842 image_sensor_value = image_sensorAnalog_get();
TetsuyaKonno 0:5db2664ff378 843
TetsuyaKonno 0:5db2664ff378 844 switch( process_No ) {
TetsuyaKonno 0:5db2664ff378 845 case 0:
TetsuyaKonno 0:5db2664ff378 846 /* center_line_corrective */
TetsuyaKonno 0:5db2664ff378 847 if( (value > image_sensor_value) && (image_sensor_value > -value) ) {
TetsuyaKonno 0:5db2664ff378 848 cnt1 = 0;
TetsuyaKonno 0:5db2664ff378 849 process_No = 1;
TetsuyaKonno 0:5db2664ff378 850 }
TetsuyaKonno 0:5db2664ff378 851 if( cnt1 >= 34 ) process_No = 3;
TetsuyaKonno 0:5db2664ff378 852 break;
TetsuyaKonno 0:5db2664ff378 853
TetsuyaKonno 0:5db2664ff378 854 case 1:
TetsuyaKonno 0:5db2664ff378 855 /* Retrial after 100ms */
TetsuyaKonno 0:5db2664ff378 856 if( cnt1 >= 68 ) {
TetsuyaKonno 0:5db2664ff378 857 if( (value > image_sensor_value) && (image_sensor_value > -value) ) {
TetsuyaKonno 0:5db2664ff378 858 cnt1 = 0;
TetsuyaKonno 0:5db2664ff378 859 process_No = 2;
TetsuyaKonno 0:5db2664ff378 860 } else {
TetsuyaKonno 0:5db2664ff378 861 process_No = 3;
TetsuyaKonno 0:5db2664ff378 862 }
TetsuyaKonno 0:5db2664ff378 863 }
TetsuyaKonno 0:5db2664ff378 864 break;
TetsuyaKonno 0:5db2664ff378 865
TetsuyaKonno 0:5db2664ff378 866 case 2:
TetsuyaKonno 0:5db2664ff378 867 iRet = 1;
TetsuyaKonno 0:5db2664ff378 868 break;
TetsuyaKonno 0:5db2664ff378 869
TetsuyaKonno 0:5db2664ff378 870 case 3:
TetsuyaKonno 0:5db2664ff378 871 im_offset_x += 2;
TetsuyaKonno 0:5db2664ff378 872 if( im_offset_x > ( ((160 - IM_GAP_SET) + (IM_GAP_SET * 2)) * 2 ) ) {
TetsuyaKonno 0:5db2664ff378 873 im_offset_x = 160 - IM_GAP_SET;
TetsuyaKonno 0:5db2664ff378 874 im_cnt++;
TetsuyaKonno 0:5db2664ff378 875 }
TetsuyaKonno 0:5db2664ff378 876 if( im_cnt >= 1 ) {
TetsuyaKonno 0:5db2664ff378 877 im_cnt = 0;
TetsuyaKonno 0:5db2664ff378 878 value += 2;
TetsuyaKonno 0:5db2664ff378 879 if( value >= 6 ) {
TetsuyaKonno 0:5db2664ff378 880 value = 6;
TetsuyaKonno 0:5db2664ff378 881 iRet = -1;
TetsuyaKonno 0:5db2664ff378 882 }
TetsuyaKonno 0:5db2664ff378 883 }
TetsuyaKonno 0:5db2664ff378 884 cnt1 = 0;
TetsuyaKonno 0:5db2664ff378 885 process_No = 0;
TetsuyaKonno 0:5db2664ff378 886 break;
TetsuyaKonno 0:5db2664ff378 887
TetsuyaKonno 0:5db2664ff378 888 default:
TetsuyaKonno 0:5db2664ff378 889 break;
TetsuyaKonno 0:5db2664ff378 890 }
TetsuyaKonno 0:5db2664ff378 891 return iRet;
TetsuyaKonno 0:5db2664ff378 892 }
TetsuyaKonno 0:5db2664ff378 893
TetsuyaKonno 0:5db2664ff378 894 //digital_sensor_corrective
TetsuyaKonno 0:5db2664ff378 895 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 896 void digital_sensor_corrective( void )
TetsuyaKonno 0:5db2664ff378 897 {
TetsuyaKonno 0:5db2664ff378 898 int Ypix;
TetsuyaKonno 0:5db2664ff378 899 int Xpix;
TetsuyaKonno 0:5db2664ff378 900 int pix_value;
TetsuyaKonno 0:5db2664ff378 901
TetsuyaKonno 0:5db2664ff378 902 black = 127;
TetsuyaKonno 0:5db2664ff378 903 white = 127;
TetsuyaKonno 0:5db2664ff378 904
TetsuyaKonno 0:5db2664ff378 905 /* Maximum value of the black and white */
TetsuyaKonno 0:5db2664ff378 906 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 907 for( Xpix = (im_offset_x - 192); Xpix < (im_offset_x + 192); Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 908 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 909 if( black >= pix_value ) black = pix_value;
TetsuyaKonno 0:5db2664ff378 910 if( white <= pix_value ) white = pix_value;
TetsuyaKonno 0:5db2664ff378 911
TetsuyaKonno 0:5db2664ff378 912 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 913 if( black >= pix_value ) black = pix_value;
TetsuyaKonno 0:5db2664ff378 914 if( white <= pix_value ) white = pix_value;
TetsuyaKonno 0:5db2664ff378 915 }
TetsuyaKonno 0:5db2664ff378 916
TetsuyaKonno 0:5db2664ff378 917 /* threshold value */
TetsuyaKonno 0:5db2664ff378 918 digital_sensor_threshold = ( white - black ) / 2;
TetsuyaKonno 0:5db2664ff378 919
TetsuyaKonno 0:5db2664ff378 920 /* X3 */
TetsuyaKonno 0:5db2664ff378 921 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 922 for( Xpix = (im_offset_x - 192); Xpix < im_offset_x; Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 923 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 924 pix_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 925 pix_value /= 2;
TetsuyaKonno 0:5db2664ff378 926
TetsuyaKonno 0:5db2664ff378 927 if( pix_value > digital_sensor_threshold ) {
TetsuyaKonno 0:5db2664ff378 928 sensor_x[3] = Xpix;
TetsuyaKonno 0:5db2664ff378 929 break;
TetsuyaKonno 0:5db2664ff378 930 }
TetsuyaKonno 0:5db2664ff378 931 }
TetsuyaKonno 0:5db2664ff378 932
TetsuyaKonno 0:5db2664ff378 933 /* X2 */
TetsuyaKonno 0:5db2664ff378 934 sensor_x[2] = im_offset_x;
TetsuyaKonno 0:5db2664ff378 935
TetsuyaKonno 0:5db2664ff378 936 /* X1 */
TetsuyaKonno 0:5db2664ff378 937 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 938 for( Xpix = im_offset_x; Xpix < (im_offset_x + 192); Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 939 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 940 pix_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 941 pix_value /= 2;
TetsuyaKonno 0:5db2664ff378 942
TetsuyaKonno 0:5db2664ff378 943 if( pix_value < digital_sensor_threshold ) {
TetsuyaKonno 0:5db2664ff378 944 sensor_x[1] = Xpix;
TetsuyaKonno 0:5db2664ff378 945 break;
TetsuyaKonno 0:5db2664ff378 946 }
TetsuyaKonno 0:5db2664ff378 947 }
TetsuyaKonno 0:5db2664ff378 948
TetsuyaKonno 0:5db2664ff378 949 /* X4 */
TetsuyaKonno 0:5db2664ff378 950 sensor_x[4] = sensor_x[3] - (sensor_x[2] - sensor_x[3]);
TetsuyaKonno 0:5db2664ff378 951
TetsuyaKonno 0:5db2664ff378 952 /* X0 */
TetsuyaKonno 0:5db2664ff378 953 sensor_x[0] = (sensor_x[1] - sensor_x[2]) + sensor_x[1];
TetsuyaKonno 0:5db2664ff378 954
TetsuyaKonno 0:5db2664ff378 955 pc.printf( "x4 = %03d, x3 = %03d, x2 = %03d, x1 = %03d, x0 = %03d \n\n\r", sensor_x[4], sensor_x[3], sensor_x[2], sensor_x[1], sensor_x[0] );
TetsuyaKonno 0:5db2664ff378 956 }
TetsuyaKonno 0:5db2664ff378 957
TetsuyaKonno 0:5db2664ff378 958 //image_SensorAnalog_process
TetsuyaKonno 0:5db2664ff378 959 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 960 void image_SensorAnalog_process( int timer33 )
TetsuyaKonno 0:5db2664ff378 961 {
TetsuyaKonno 0:5db2664ff378 962 int Ypix;
TetsuyaKonno 0:5db2664ff378 963 int Xpix;
TetsuyaKonno 0:5db2664ff378 964 int left_value;
TetsuyaKonno 0:5db2664ff378 965 int right_value;
TetsuyaKonno 0:5db2664ff378 966
TetsuyaKonno 0:5db2664ff378 967 /* Initialization */
TetsuyaKonno 0:5db2664ff378 968 left_value = 0;
TetsuyaKonno 0:5db2664ff378 969 right_value = 0;
TetsuyaKonno 0:5db2664ff378 970
TetsuyaKonno 0:5db2664ff378 971 /* left image value ( 64pix to 160pix ) */
TetsuyaKonno 0:5db2664ff378 972 Ypix = IMAGE_LINE - (timer33);//109 to 75 line
TetsuyaKonno 0:5db2664ff378 973 for( Xpix = (im_offset_x - 192); Xpix < im_offset_x; Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 974 left_value += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 975 left_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 976 }
TetsuyaKonno 0:5db2664ff378 977 /* right image value ( 161pix to 256pix ) */
TetsuyaKonno 0:5db2664ff378 978 Ypix = IMAGE_LINE - (timer33);//109 to 75 line
TetsuyaKonno 0:5db2664ff378 979 for( Xpix = im_offset_x; Xpix < ( im_offset_x + 192 ); Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 980 right_value += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 981 right_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 982 }
TetsuyaKonno 0:5db2664ff378 983
TetsuyaKonno 0:5db2664ff378 984 Left_sensor = left_value / 10;
TetsuyaKonno 0:5db2664ff378 985 Right_sensor = right_value / 10;
TetsuyaKonno 0:5db2664ff378 986 }
TetsuyaKonno 0:5db2664ff378 987
TetsuyaKonno 0:5db2664ff378 988 //image_sensorAnalog_get
TetsuyaKonno 0:5db2664ff378 989 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 990 int image_sensorAnalog_get( void )
TetsuyaKonno 0:5db2664ff378 991 {
TetsuyaKonno 0:5db2664ff378 992 int iRet;
TetsuyaKonno 0:5db2664ff378 993
TetsuyaKonno 0:5db2664ff378 994 iRet = Left_sensor - Right_sensor;
TetsuyaKonno 0:5db2664ff378 995
TetsuyaKonno 0:5db2664ff378 996 /* Reverse of the positive and negative. */
TetsuyaKonno 0:5db2664ff378 997 iRet *= -1;
TetsuyaKonno 0:5db2664ff378 998
TetsuyaKonno 0:5db2664ff378 999 if( iRet <= -900 ) iRet = -900;//Left side
TetsuyaKonno 0:5db2664ff378 1000 if( iRet >= 900 ) iRet = 900;//right side
TetsuyaKonno 0:5db2664ff378 1001
TetsuyaKonno 0:5db2664ff378 1002 return iRet;
TetsuyaKonno 0:5db2664ff378 1003 }
TetsuyaKonno 0:5db2664ff378 1004
TetsuyaKonno 0:5db2664ff378 1005
TetsuyaKonno 0:5db2664ff378 1006 //image_digital_sensor
TetsuyaKonno 0:5db2664ff378 1007 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1008 unsigned char image_digital_sensor( void )
TetsuyaKonno 0:5db2664ff378 1009 {
TetsuyaKonno 0:5db2664ff378 1010 int Ypix;
TetsuyaKonno 0:5db2664ff378 1011 int Xpix;
TetsuyaKonno 0:5db2664ff378 1012 int data;
TetsuyaKonno 0:5db2664ff378 1013 unsigned char sensor;
TetsuyaKonno 0:5db2664ff378 1014
TetsuyaKonno 0:5db2664ff378 1015 sensor = 0;
TetsuyaKonno 0:5db2664ff378 1016
TetsuyaKonno 0:5db2664ff378 1017 /* sensor 2 (center) */
TetsuyaKonno 0:5db2664ff378 1018 data = 0;
TetsuyaKonno 0:5db2664ff378 1019 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 1020 for( Xpix = (sensor_x[2] - 8); Xpix < (sensor_x[2] + 8); Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 1021 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 1022 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 1023 }
TetsuyaKonno 0:5db2664ff378 1024 data /= 8;
TetsuyaKonno 0:5db2664ff378 1025 if( data <= digital_sensor_threshold ) data = 0;
TetsuyaKonno 0:5db2664ff378 1026 else data = 1;
TetsuyaKonno 0:5db2664ff378 1027 sensor |= (data << 4) & 0x10;
TetsuyaKonno 0:5db2664ff378 1028
TetsuyaKonno 0:5db2664ff378 1029 /* sensor 4 (left side) */
TetsuyaKonno 0:5db2664ff378 1030 data = 0;
TetsuyaKonno 0:5db2664ff378 1031 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 1032 for( Xpix = (sensor_x[4] - 16); Xpix < sensor_x[4]; Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 1033 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 1034 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 1035 }
TetsuyaKonno 0:5db2664ff378 1036 data /= 8;
TetsuyaKonno 0:5db2664ff378 1037 if( data <= digital_sensor_threshold ) data = 0;
TetsuyaKonno 0:5db2664ff378 1038 else data = 1;
TetsuyaKonno 0:5db2664ff378 1039 sensor |= (data << 3) & 0x08;
TetsuyaKonno 0:5db2664ff378 1040
TetsuyaKonno 0:5db2664ff378 1041 /* sensor 3 (left side) */
TetsuyaKonno 0:5db2664ff378 1042 data = 0;
TetsuyaKonno 0:5db2664ff378 1043 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 1044 for( Xpix = (sensor_x[3] - 16); Xpix < sensor_x[3]; Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 1045 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 1046 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 1047 }
TetsuyaKonno 0:5db2664ff378 1048 data /= 8;
TetsuyaKonno 0:5db2664ff378 1049 if( data <= digital_sensor_threshold ) data = 0;
TetsuyaKonno 0:5db2664ff378 1050 else data = 1;
TetsuyaKonno 0:5db2664ff378 1051 sensor |= (data << 2) & 0x04;
TetsuyaKonno 0:5db2664ff378 1052
TetsuyaKonno 0:5db2664ff378 1053 /* sensor 1 (right side) */
TetsuyaKonno 0:5db2664ff378 1054 data = 0;
TetsuyaKonno 0:5db2664ff378 1055 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 1056 for( Xpix = sensor_x[1]; Xpix < (sensor_x[1] + 16); Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 1057 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 1058 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 1059 }
TetsuyaKonno 0:5db2664ff378 1060 data /= 8;
TetsuyaKonno 0:5db2664ff378 1061 if( data <= digital_sensor_threshold ) data = 0;
TetsuyaKonno 0:5db2664ff378 1062 else data = 1;
TetsuyaKonno 0:5db2664ff378 1063 sensor |= (data << 1) & 0x02;
TetsuyaKonno 0:5db2664ff378 1064
TetsuyaKonno 0:5db2664ff378 1065 /* sensor 0 (right side) */
TetsuyaKonno 0:5db2664ff378 1066 data = 0;
TetsuyaKonno 0:5db2664ff378 1067 Ypix = IMAGE_LINE;
TetsuyaKonno 0:5db2664ff378 1068 for( Xpix = sensor_x[0]; Xpix < (sensor_x[0] + 16); Xpix+=4 ) {
TetsuyaKonno 0:5db2664ff378 1069 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */
TetsuyaKonno 0:5db2664ff378 1070 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */
TetsuyaKonno 0:5db2664ff378 1071 }
TetsuyaKonno 0:5db2664ff378 1072 data /= 8;
TetsuyaKonno 0:5db2664ff378 1073 if( data <= digital_sensor_threshold ) data = 0;
TetsuyaKonno 0:5db2664ff378 1074 else data = 1;
TetsuyaKonno 0:5db2664ff378 1075 sensor |= (data) & 0x01;
TetsuyaKonno 0:5db2664ff378 1076
TetsuyaKonno 0:5db2664ff378 1077 sensor &= 0x1f;
TetsuyaKonno 0:5db2664ff378 1078
TetsuyaKonno 0:5db2664ff378 1079 return sensor;
TetsuyaKonno 0:5db2664ff378 1080 }
TetsuyaKonno 0:5db2664ff378 1081
TetsuyaKonno 0:5db2664ff378 1082 //Image Data YCbCr -> Y(320*240pix)
TetsuyaKonno 0:5db2664ff378 1083 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1084 void Image_Extraction( unsigned char *Data_Y )
TetsuyaKonno 0:5db2664ff378 1085 {
TetsuyaKonno 0:5db2664ff378 1086 static int Xp, Yp, inc;
TetsuyaKonno 0:5db2664ff378 1087 static int counter = 0;
TetsuyaKonno 0:5db2664ff378 1088 #if 0
TetsuyaKonno 0:5db2664ff378 1089 // Function
TetsuyaKonno 0:5db2664ff378 1090 for( Yp = 0, inc = 0; Yp < 240; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1091 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1092 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1093 }
TetsuyaKonno 0:5db2664ff378 1094 }
TetsuyaKonno 0:5db2664ff378 1095 #else
TetsuyaKonno 0:5db2664ff378 1096 // Distributed processing
TetsuyaKonno 0:5db2664ff378 1097 switch( counter++ ) {
TetsuyaKonno 0:5db2664ff378 1098 case 0:
TetsuyaKonno 0:5db2664ff378 1099 for( Yp = 0, inc = 0; Yp < 30; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1100 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1101 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1102 }
TetsuyaKonno 0:5db2664ff378 1103 }
TetsuyaKonno 0:5db2664ff378 1104 break;
TetsuyaKonno 0:5db2664ff378 1105 case 1:
TetsuyaKonno 0:5db2664ff378 1106 for( ; Yp < 60; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1107 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1108 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1109 }
TetsuyaKonno 0:5db2664ff378 1110 }
TetsuyaKonno 0:5db2664ff378 1111 break;
TetsuyaKonno 0:5db2664ff378 1112 case 2:
TetsuyaKonno 0:5db2664ff378 1113 for( ; Yp < 90; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1114 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1115 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1116 }
TetsuyaKonno 0:5db2664ff378 1117 }
TetsuyaKonno 0:5db2664ff378 1118 break;
TetsuyaKonno 0:5db2664ff378 1119 case 3:
TetsuyaKonno 0:5db2664ff378 1120 for( ; Yp < 120; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1121 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1122 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1123 }
TetsuyaKonno 0:5db2664ff378 1124 }
TetsuyaKonno 0:5db2664ff378 1125 break;
TetsuyaKonno 0:5db2664ff378 1126 case 4:
TetsuyaKonno 0:5db2664ff378 1127 for( ; Yp < 150; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1128 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1129 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1130 }
TetsuyaKonno 0:5db2664ff378 1131 }
TetsuyaKonno 0:5db2664ff378 1132 break;
TetsuyaKonno 0:5db2664ff378 1133 case 5:
TetsuyaKonno 0:5db2664ff378 1134 for( ; Yp < 180; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1135 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1136 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1137 }
TetsuyaKonno 0:5db2664ff378 1138 }
TetsuyaKonno 0:5db2664ff378 1139 break;
TetsuyaKonno 0:5db2664ff378 1140 case 6:
TetsuyaKonno 0:5db2664ff378 1141 for( ; Yp < 210; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1142 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1143 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1144 }
TetsuyaKonno 0:5db2664ff378 1145 }
TetsuyaKonno 0:5db2664ff378 1146 break;
TetsuyaKonno 0:5db2664ff378 1147 case 7:
TetsuyaKonno 0:5db2664ff378 1148 for( ; Yp < 240; Yp++ ){
TetsuyaKonno 0:5db2664ff378 1149 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
TetsuyaKonno 0:5db2664ff378 1150 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
TetsuyaKonno 0:5db2664ff378 1151 }
TetsuyaKonno 0:5db2664ff378 1152 }
TetsuyaKonno 0:5db2664ff378 1153 counter = 0;
TetsuyaKonno 0:5db2664ff378 1154 break;
TetsuyaKonno 0:5db2664ff378 1155 default:
TetsuyaKonno 0:5db2664ff378 1156 break;
TetsuyaKonno 0:5db2664ff378 1157 }
TetsuyaKonno 0:5db2664ff378 1158 #endif
TetsuyaKonno 0:5db2664ff378 1159
TetsuyaKonno 0:5db2664ff378 1160 }
TetsuyaKonno 0:5db2664ff378 1161
TetsuyaKonno 0:5db2664ff378 1162 //Image_Compression Y ( Thinning processing )
TetsuyaKonno 0:5db2664ff378 1163 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1164 void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W )
TetsuyaKonno 0:5db2664ff378 1165 {
TetsuyaKonno 0:5db2664ff378 1166 int Data_H, Comp_H, Step_W, Step_H;
TetsuyaKonno 0:5db2664ff378 1167 int Xp, Yp, inc;
TetsuyaKonno 0:5db2664ff378 1168
TetsuyaKonno 0:5db2664ff378 1169 Data_H = (Data_W / (double)4) * 3;
TetsuyaKonno 0:5db2664ff378 1170 Comp_H = (Comp_W / (double)4) * 3;
TetsuyaKonno 0:5db2664ff378 1171 Step_W = Data_W / Comp_W;
TetsuyaKonno 0:5db2664ff378 1172 Step_H = Data_H / Comp_H;
TetsuyaKonno 0:5db2664ff378 1173
TetsuyaKonno 0:5db2664ff378 1174 for( Yp = 0, inc = 0; Yp < Comp_H; Yp++ ) {
TetsuyaKonno 0:5db2664ff378 1175 for( Xp = 0; Xp < Comp_W; Xp++, inc++ ) {
TetsuyaKonno 0:5db2664ff378 1176 Comp_Y[inc] = Data_Y[(Yp * (Data_W * Step_H)) + (Xp * Step_W)];
TetsuyaKonno 0:5db2664ff378 1177 }
TetsuyaKonno 0:5db2664ff378 1178 }
TetsuyaKonno 0:5db2664ff378 1179 }
TetsuyaKonno 0:5db2664ff378 1180
TetsuyaKonno 0:5db2664ff378 1181 //Image_Compression2 Y ( Averaging processing )
TetsuyaKonno 0:5db2664ff378 1182 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1183 void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M )
TetsuyaKonno 0:5db2664ff378 1184 {
TetsuyaKonno 0:5db2664ff378 1185 int Data_H, Pixel_T, Pixel_D;
TetsuyaKonno 0:5db2664ff378 1186 int x, y;
TetsuyaKonno 0:5db2664ff378 1187 static int Xp, Yp, inc;
TetsuyaKonno 0:5db2664ff378 1188 static int counter = 0;
TetsuyaKonno 0:5db2664ff378 1189
TetsuyaKonno 0:5db2664ff378 1190 Data_H = (Data_W / (double)4) * 3;
TetsuyaKonno 0:5db2664ff378 1191 Pixel_D = Comp_M * Comp_M;
TetsuyaKonno 0:5db2664ff378 1192 #if 0
TetsuyaKonno 0:5db2664ff378 1193 // Function
TetsuyaKonno 0:5db2664ff378 1194 for( Yp = 0, inc = 0; Yp < Data_H; Yp+=Comp_M ){
TetsuyaKonno 0:5db2664ff378 1195 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:5db2664ff378 1196 Pixel_T = 0;
TetsuyaKonno 0:5db2664ff378 1197 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:5db2664ff378 1198 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:5db2664ff378 1199 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:5db2664ff378 1200 }
TetsuyaKonno 0:5db2664ff378 1201 }
TetsuyaKonno 0:5db2664ff378 1202 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:5db2664ff378 1203 }
TetsuyaKonno 0:5db2664ff378 1204 }
TetsuyaKonno 0:5db2664ff378 1205 #else
TetsuyaKonno 0:5db2664ff378 1206 // Distributed processing
TetsuyaKonno 0:5db2664ff378 1207 switch( counter++ ) {
TetsuyaKonno 0:5db2664ff378 1208 case 0:
TetsuyaKonno 0:5db2664ff378 1209 for( Yp = 0, inc = 0; Yp < (Data_H / 2); Yp+=Comp_M ){
TetsuyaKonno 0:5db2664ff378 1210 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:5db2664ff378 1211 Pixel_T = 0;
TetsuyaKonno 0:5db2664ff378 1212 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:5db2664ff378 1213 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:5db2664ff378 1214 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:5db2664ff378 1215 }
TetsuyaKonno 0:5db2664ff378 1216 }
TetsuyaKonno 0:5db2664ff378 1217 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:5db2664ff378 1218 }
TetsuyaKonno 0:5db2664ff378 1219 }
TetsuyaKonno 0:5db2664ff378 1220 break;
TetsuyaKonno 0:5db2664ff378 1221 case 1:
TetsuyaKonno 0:5db2664ff378 1222 for( ; Yp < Data_H; Yp+=Comp_M ){
TetsuyaKonno 0:5db2664ff378 1223 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:5db2664ff378 1224 Pixel_T = 0;
TetsuyaKonno 0:5db2664ff378 1225 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:5db2664ff378 1226 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:5db2664ff378 1227 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:5db2664ff378 1228 }
TetsuyaKonno 0:5db2664ff378 1229 }
TetsuyaKonno 0:5db2664ff378 1230 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:5db2664ff378 1231 }
TetsuyaKonno 0:5db2664ff378 1232 }
TetsuyaKonno 0:5db2664ff378 1233 counter = 0;
TetsuyaKonno 0:5db2664ff378 1234 break;
TetsuyaKonno 0:5db2664ff378 1235 default:
TetsuyaKonno 0:5db2664ff378 1236 break;
TetsuyaKonno 0:5db2664ff378 1237 }
TetsuyaKonno 0:5db2664ff378 1238 #endif
TetsuyaKonno 0:5db2664ff378 1239 }
TetsuyaKonno 0:5db2664ff378 1240
TetsuyaKonno 0:5db2664ff378 1241 // Binarization_process
TetsuyaKonno 0:5db2664ff378 1242 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1243 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items )
TetsuyaKonno 0:5db2664ff378 1244 {
TetsuyaKonno 0:5db2664ff378 1245 int i, threshold;
TetsuyaKonno 0:5db2664ff378 1246
TetsuyaKonno 0:5db2664ff378 1247 threshold = 150;
TetsuyaKonno 0:5db2664ff378 1248 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:5db2664ff378 1249 if( Comp_Y[i] >= threshold ) Binary[i] = 1;
TetsuyaKonno 0:5db2664ff378 1250 else Binary[i] = 0;
TetsuyaKonno 0:5db2664ff378 1251 }
TetsuyaKonno 0:5db2664ff378 1252 }
TetsuyaKonno 0:5db2664ff378 1253
TetsuyaKonno 0:5db2664ff378 1254 // Extract_Image
TetsuyaKonno 0:5db2664ff378 1255 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1256 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size )
TetsuyaKonno 0:5db2664ff378 1257 {
TetsuyaKonno 0:5db2664ff378 1258 int x, y;
TetsuyaKonno 0:5db2664ff378 1259 for( y = 0; y < y_size; y++ ) {
TetsuyaKonno 0:5db2664ff378 1260 for( x = 0; x < x_size; x++ ) {
TetsuyaKonno 0:5db2664ff378 1261 Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ];
TetsuyaKonno 0:5db2664ff378 1262 }
TetsuyaKonno 0:5db2664ff378 1263 }
TetsuyaKonno 0:5db2664ff378 1264 }
TetsuyaKonno 0:5db2664ff378 1265
TetsuyaKonno 0:5db2664ff378 1266 // Standard deviation
TetsuyaKonno 0:5db2664ff378 1267 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1268 double Standard_Deviation( unsigned char *data, double *Devi, int items )
TetsuyaKonno 0:5db2664ff378 1269 {
TetsuyaKonno 0:5db2664ff378 1270 int i;
TetsuyaKonno 0:5db2664ff378 1271 double iRet_A, iRet_C, iRet_D;
TetsuyaKonno 0:5db2664ff378 1272
TetsuyaKonno 0:5db2664ff378 1273 /* A 合計値　平均化 */
TetsuyaKonno 0:5db2664ff378 1274 iRet_A = 0;
TetsuyaKonno 0:5db2664ff378 1275 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:5db2664ff378 1276 iRet_A += data[i];
TetsuyaKonno 0:5db2664ff378 1277 }
TetsuyaKonno 0:5db2664ff378 1278 iRet_A /= items;
TetsuyaKonno 0:5db2664ff378 1279
TetsuyaKonno 0:5db2664ff378 1280 /* B 偏差値 */
TetsuyaKonno 0:5db2664ff378 1281 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:5db2664ff378 1282 Devi[i] = data[i] - iRet_A;
TetsuyaKonno 0:5db2664ff378 1283 }
TetsuyaKonno 0:5db2664ff378 1284
TetsuyaKonno 0:5db2664ff378 1285 /* C 分散 */
TetsuyaKonno 0:5db2664ff378 1286 iRet_C = 0;
TetsuyaKonno 0:5db2664ff378 1287 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:5db2664ff378 1288 iRet_C += ( Devi[i] * Devi[i] );
TetsuyaKonno 0:5db2664ff378 1289 }
TetsuyaKonno 0:5db2664ff378 1290 iRet_C /= items;
TetsuyaKonno 0:5db2664ff378 1291
TetsuyaKonno 0:5db2664ff378 1292 /* D 標準偏差 */
TetsuyaKonno 0:5db2664ff378 1293 iRet_D = sqrt( iRet_C );
TetsuyaKonno 0:5db2664ff378 1294
TetsuyaKonno 0:5db2664ff378 1295 return iRet_D;
TetsuyaKonno 0:5db2664ff378 1296 }
TetsuyaKonno 0:5db2664ff378 1297
TetsuyaKonno 0:5db2664ff378 1298 // Covariance
TetsuyaKonno 0:5db2664ff378 1299 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1300 double Covariance( double *Devi_A, double *Devi_B, int items )
TetsuyaKonno 0:5db2664ff378 1301 {
TetsuyaKonno 0:5db2664ff378 1302 int i;
TetsuyaKonno 0:5db2664ff378 1303 double iRet, iRet_buff;
TetsuyaKonno 0:5db2664ff378 1304
TetsuyaKonno 0:5db2664ff378 1305 iRet = 0;
TetsuyaKonno 0:5db2664ff378 1306 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:5db2664ff378 1307 iRet_buff = Devi_A[i] * Devi_B[i];
TetsuyaKonno 0:5db2664ff378 1308 iRet += iRet_buff;
TetsuyaKonno 0:5db2664ff378 1309 }
TetsuyaKonno 0:5db2664ff378 1310 iRet /= items;
TetsuyaKonno 0:5db2664ff378 1311
TetsuyaKonno 0:5db2664ff378 1312 return iRet;
TetsuyaKonno 0:5db2664ff378 1313 }
TetsuyaKonno 0:5db2664ff378 1314
TetsuyaKonno 0:5db2664ff378 1315 // Judgement_ImageMatching
TetsuyaKonno 0:5db2664ff378 1316 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1317 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B )
TetsuyaKonno 0:5db2664ff378 1318 {
TetsuyaKonno 0:5db2664ff378 1319 int iRet;
TetsuyaKonno 0:5db2664ff378 1320
TetsuyaKonno 0:5db2664ff378 1321 iRet = ( covari * 100 ) / ( SDevi_A * SDevi_B );
TetsuyaKonno 0:5db2664ff378 1322
TetsuyaKonno 0:5db2664ff378 1323 return iRet;
TetsuyaKonno 0:5db2664ff378 1324 }
TetsuyaKonno 0:5db2664ff378 1325
TetsuyaKonno 0:5db2664ff378 1326 //******************************************************************//
TetsuyaKonno 0:5db2664ff378 1327 // @brief Interrupt callback function
TetsuyaKonno 0:5db2664ff378 1328 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:5db2664ff378 1329 // @retval None
TetsuyaKonno 0:5db2664ff378 1330 //*******************************************************************/
TetsuyaKonno 0:5db2664ff378 1331 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:5db2664ff378 1332 {
TetsuyaKonno 0:5db2664ff378 1333 if (vfield_count > 0) {
TetsuyaKonno 0:5db2664ff378 1334 vfield_count--;
TetsuyaKonno 0:5db2664ff378 1335 }
TetsuyaKonno 0:5db2664ff378 1336 }
TetsuyaKonno 0:5db2664ff378 1337
TetsuyaKonno 0:5db2664ff378 1338 //******************************************************************//
TetsuyaKonno 0:5db2664ff378 1339 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:5db2664ff378 1340 // @param[in] wait_count : Wait count
TetsuyaKonno 0:5db2664ff378 1341 // @retval None
TetsuyaKonno 0:5db2664ff378 1342 //*******************************************************************/
TetsuyaKonno 0:5db2664ff378 1343 static void WaitVfield(const int32_t wait_count)
TetsuyaKonno 0:5db2664ff378 1344 {
TetsuyaKonno 0:5db2664ff378 1345 vfield_count = wait_count;
TetsuyaKonno 0:5db2664ff378 1346 while (vfield_count > 0) {
TetsuyaKonno 0:5db2664ff378 1347 /* Do nothing */
TetsuyaKonno 0:5db2664ff378 1348 }
TetsuyaKonno 0:5db2664ff378 1349 }
TetsuyaKonno 0:5db2664ff378 1350
TetsuyaKonno 0:5db2664ff378 1351 //******************************************************************//
TetsuyaKonno 0:5db2664ff378 1352 // @brief Interrupt callback function for Vsync interruption
TetsuyaKonno 0:5db2664ff378 1353 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:5db2664ff378 1354 // @retval None
TetsuyaKonno 0:5db2664ff378 1355 //*******************************************************************/
TetsuyaKonno 0:5db2664ff378 1356 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:5db2664ff378 1357 {
TetsuyaKonno 0:5db2664ff378 1358 if (vsync_count > 0) {
TetsuyaKonno 0:5db2664ff378 1359 vsync_count--;
TetsuyaKonno 0:5db2664ff378 1360 }
TetsuyaKonno 0:5db2664ff378 1361 }
TetsuyaKonno 0:5db2664ff378 1362
TetsuyaKonno 0:5db2664ff378 1363 //******************************************************************//
TetsuyaKonno 0:5db2664ff378 1364 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:5db2664ff378 1365 // @param[in] wait_count : Wait count
TetsuyaKonno 0:5db2664ff378 1366 // @retval None
TetsuyaKonno 0:5db2664ff378 1367 //*******************************************************************/
TetsuyaKonno 0:5db2664ff378 1368 static void WaitVsync(const int32_t wait_count)
TetsuyaKonno 0:5db2664ff378 1369 {
TetsuyaKonno 0:5db2664ff378 1370 vsync_count = wait_count;
TetsuyaKonno 0:5db2664ff378 1371 while (vsync_count > 0) {
TetsuyaKonno 0:5db2664ff378 1372 /* Do nothing */
TetsuyaKonno 0:5db2664ff378 1373 }
TetsuyaKonno 0:5db2664ff378 1374 }
TetsuyaKonno 0:5db2664ff378 1375
TetsuyaKonno 0:5db2664ff378 1376 //------------------------------------------------------------------//
TetsuyaKonno 0:5db2664ff378 1377 // End of file
TetsuyaKonno 0:5db2664ff378 1378 //------------------------------------------------------------------//