Micon Car Rally / Mbed 2 deprecated Trace_Program_60fps

This program is line trace program by image processing(NTSC 60fps).

Dependencies:   GR-PEACH_video mbed

main.cpp@1:51650f3f8d4f, 2017-01-10 (annotated)

Committer:
TetsuyaKonno
Date:
Tue Jan 10 04:57:12 2017 +0000
Revision:
1:51650f3f8d4f
Parent:
0:b20adb7baebc 
We changed the contents of the "intTimer" function. but there is no design changes of the function.

Who changed what in which revision?

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