Micon Car Rally / Mbed 2 deprecated TraceMark_Program_60fps_Ver2

Added program for mark detection (Cross line, Left half line and Right half line).

Dependencies:   GR-PEACH_video mbed

Fork of TraceMark_Program_60fps by Micon Car Rally

main.cpp@2:af4db183ee76, 2017-03-17 (annotated)

Committer:
TetsuyaKonno
Date:
Fri Mar 17 03:53:01 2017 +0000
Revision:
2:af4db183ee76
Parent:
1:4a94c87a7d04 
Added program for mark detection (Cross line, Left half line and Right half line).

Who changed what in which revision?

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