main.cpp

00001 #include "mbed.h"
00002 #include "opencv.hpp"
00003 #include "EasyAttach_CameraAndLCD.h"
00004 #include "dcache-control.h"
00005 #include "AsciiFont.h"
00006 #include "hal/trng_api.h"
00007 #include "oekaki_theme.h"
00008 
00009 
00010 #define PLOT_INTERVAL          (30)
00011 #define DIST_SCALE_FACTOR_X    (6.0)
00012 #define DIST_SCALE_FACTOR_Y    (6.0)
00013 
00014 #define STRING_DISP_TEST       (1)
00015 #define DRAW_POINT (3)
00016 #define ERASER_POINT (8)
00017 #define TOUCH_BUFFER_BYTE_PER_PIXEL (2u)
00018 #define TOUCH_BUFFER_STRIDE           (((LCD_PIXEL_WIDTH * TOUCH_BUFFER_BYTE_PER_PIXEL) + 31u) & ~31u)
00019 #define COLOR_BLUE cv::Scalar(0,255,0)
00020 
00021 
00022 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
00023     in accordance with the frame buffer burst transfer mode. */
00024 #define VIDEO_PIXEL_HW         (160u)  /* HQVGA */
00025 #define VIDEO_PIXEL_VW         (120u)  /* HQVGA */
00026 
00027 #define FRAME_BUFFER_STRIDE    (((VIDEO_PIXEL_HW * 2) + 31u) & ~31u)
00028 #define FRAME_BUFFER_HEIGHT    (VIDEO_PIXEL_VW)
00029 
00030 #if defined(__ICCARM__)
00031 #pragma data_alignment=32
00032 static uint8_t user_frame_buffer0[FRAME_BUFFER_STRIDE * FRAME_BUFFER_HEIGHT]@ ".mirrorram";
00033 static uint8_t user_frame_buffer1[TOUCH_BUFFER_STRIDE * LCD_PIXEL_HEIGHT]@ ".mirrorram";
00034 #else
00035 static uint8_t user_frame_buffer0[FRAME_BUFFER_STRIDE * FRAME_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(32)));
00036 static uint8_t user_frame_buffer1[TOUCH_BUFFER_STRIDE * LCD_PIXEL_HEIGHT]__attribute((section("NC_BSS"),aligned(32)));//cameraみたい
00037 #endif
00038 static volatile int Vfield_Int_Cnt = 0;
00039 
00040 static InterruptIn skip_btn0(USER_BUTTON0);
00041 static InterruptIn skip_btn1(USER_BUTTON1);
00042 static int sta=0;//描画のon,off switch
00043 static int clr=0;//カラー選択(0=青、 1=黒(消しゴム))
00044 static int layer2flg=0;
00045 static int firstflg=0;
00046 static int kesiflg=0;//1のとき、消しゴムを使ったあと。
00047 static int16_t RangeCorr(int16_t target ,int16_t uplimit,int16_t lowlimit);
00048 static uint8_t get_random(void);
00049 
00050 DisplayBase Display;
00051 DigitalOut  led1(LED1);
00052 static Thread mainTask(osPriorityNormal, 1024 * 16);
00053 
00054 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) {
00055     if (Vfield_Int_Cnt > 0) {
00056         Vfield_Int_Cnt--;
00057     }
00058 }
00059 
00060 static void wait_new_image(void) {
00061     Vfield_Int_Cnt = 1;
00062     while (Vfield_Int_Cnt > 0) {
00063         ThisThread::sleep_for(1);
00064     }
00065 }
00066 
00067 
00068 static void Start_Video_Camera(void) {
00069     // Field end signal for recording function in scaler 0
00070     Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VFIELD, 0, IntCallbackFunc_Vfield);
00071 
00072     // Video capture setting (progressive form fixed)
00073     Display.Video_Write_Setting(
00074         DisplayBase::VIDEO_INPUT_CHANNEL_0,
00075         DisplayBase::COL_SYS_NTSC_358,
00076         (void *)user_frame_buffer0,
00077         FRAME_BUFFER_STRIDE,
00078         DisplayBase::VIDEO_FORMAT_YCBCR422,
00079         DisplayBase::WR_RD_WRSWA_32_16BIT,
00080         VIDEO_PIXEL_VW,
00081         VIDEO_PIXEL_HW
00082     );
00083     EasyAttach_CameraStart(Display, DisplayBase::VIDEO_INPUT_CHANNEL_0);
00084 }
00085 
00086 #if MBED_CONF_APP_LCD
00087 static void Start_LCD_Display(void) {
00088     DisplayBase::rect_t rect;
00089 
00090     /* The layer by which the touch panel location is drawn */
00091     memset(user_frame_buffer1, 0, sizeof(user_frame_buffer1));//framebufferの名前を変えた、
00092     dcache_clean(user_frame_buffer1, sizeof(user_frame_buffer1));//framebufferの名前を変えた、
00093     rect.vs = 0;
00094     rect.vw = LCD_PIXEL_HEIGHT;
00095     rect.hs = 0;
00096     rect.hw = LCD_PIXEL_WIDTH;
00097     Display.Graphics_Read_Setting(
00098         DisplayBase::GRAPHICS_LAYER_0,
00099         (void *)user_frame_buffer1,
00100         TOUCH_BUFFER_STRIDE,
00101         DisplayBase::GRAPHICS_FORMAT_ARGB4444,
00102         DisplayBase::WR_RD_WRSWA_32_16BIT,
00103         &rect
00104     );
00105     Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_0);
00106 
00107     ThisThread::sleep_for(50);
00108     EasyAttach_LcdBacklight(true);
00109 }
00110 
00111 #if STRING_DISP_TEST
00112 //文字列出力
00113 #define STRING_PIXEL_HW               (129)
00114 #define STRING_PIXEL_VM               (24)
00115 #define STRING_BUFFER_BYTE_PER_PIXEL  (2u)
00116 #define STRING_BUFFER_STRIDE          (((LCD_PIXEL_WIDTH * STRING_BUFFER_BYTE_PER_PIXEL) + 31u) & ~31u)
00117 
00118 #if defined(__ICCARM__)
00119 #pragma data_alignment=32
00120 static uint8_t user_frame_buffer_string[STRING_BUFFER_STRIDE * STRING_PIXEL_VM];
00121 #else
00122 static uint8_t user_frame_buffer_string[STRING_BUFFER_STRIDE * STRING_PIXEL_VM]__attribute((aligned(32)));
00123 #endif
00124 
00125 static void string_task(void){
00126   DisplayBase::rect_t rect;
00127 
00128   memset(user_frame_buffer_string,0,sizeof(user_frame_buffer_string));
00129   dcache_clean(user_frame_buffer_string,sizeof(user_frame_buffer_string));
00130 
00131   rect.vs = LCD_PIXEL_HEIGHT - STRING_PIXEL_VM - 10;
00132   rect.vw = STRING_PIXEL_VM;
00133   rect.hs = LCD_PIXEL_WIDTH - STRING_PIXEL_HW-10;
00134   rect.hw = STRING_PIXEL_HW;
00135   Display.Graphics_Read_Setting(
00136     DisplayBase::GRAPHICS_LAYER_2,
00137     (void *)user_frame_buffer_string,
00138     STRING_BUFFER_STRIDE,
00139     DisplayBase::GRAPHICS_FORMAT_ARGB4444,
00140     DisplayBase::WR_RD_WRSWA_32_16BIT,
00141     &rect
00142   );
00143   Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_2);
00144 
00145   AsciiFont ascii_font(user_frame_buffer_string,STRING_PIXEL_HW,STRING_PIXEL_VM,
00146     STRING_BUFFER_STRIDE,STRING_BUFFER_BYTE_PER_PIXEL);
00147 
00148             //(文字列, x, y, colour,font_size)
00149     int num=get_random();
00150     int layer2flg_last = -1;
00151     while(1){
00152     if(layer2flg_last!=layer2flg){
00153       if(layer2flg==0){
00154         ascii_font.DrawStr(ChooseTheme(num), 0, 0 , 0x0000ffff, 2);//白
00155       }else{
00156         memset(user_frame_buffer_string,0,sizeof(user_frame_buffer_string));
00157       }
00158       dcache_clean(user_frame_buffer_string,sizeof(user_frame_buffer_string));
00159       layer2flg_last=layer2flg;
00160     }
00161     ThisThread::sleep_for(50);
00162   }
00163 }
00164 #endif
00165 
00166 static void draw_touch_pos(uint8_t * p_buf, int id, int drawpoint,int x, int y) { //バッファ,色,線の太さ,x座標,y座標
00167     int idx_base;
00168     int wk_idx;
00169     int i;
00170     int j;
00171     uint8_t coller_pix[TOUCH_BUFFER_BYTE_PER_PIXEL];  /* ARGB4444 */
00172 
00173     /* A coordinate in the upper left is calculated from a central coordinate. */
00174     if ((x - (drawpoint / 2)) >= 0) {
00175         x -= (drawpoint / 2);
00176     }
00177     if (x > ((int)LCD_PIXEL_WIDTH - drawpoint)) {
00178         x = ((int)LCD_PIXEL_WIDTH - drawpoint);
00179     }
00180     if ((y - (drawpoint / 2)) >= 0) {
00181         y -= (drawpoint / 2);
00182     }
00183     if (y > ((int)LCD_PIXEL_HEIGHT - drawpoint)) {
00184         y = ((int)LCD_PIXEL_HEIGHT - drawpoint);
00185     }
00186     idx_base = (x + ((int)LCD_PIXEL_WIDTH * y)) * TOUCH_BUFFER_BYTE_PER_PIXEL;
00187 
00188     /* Select color */
00189     if (id == 0) {
00190         /* Blue */
00191         coller_pix[0] = 0x0F;  /* 4:Green 4:Blue */
00192         coller_pix[1] = 0xF0;  /* 4:Alpha 4:Red  */
00193     } else if (id == 1){
00194 
00195         /* black */
00196         coller_pix[0] = 0x00;  /* 4:Green 4:Blue */
00197         coller_pix[1] = 0x00;  /* 4:Alpha 4:Red  */
00198     } else {
00199         /* red */
00200         coller_pix[0] = 0x01;
00201         coller_pix[1] = 0xFF;
00202       }
00203 
00204     /* Drawing */
00205     for (i = 0; i < drawpoint; i++) {
00206         wk_idx = idx_base + ((int)LCD_PIXEL_WIDTH * TOUCH_BUFFER_BYTE_PER_PIXEL * i);
00207         for (j = 0; j < drawpoint; j++) {
00208             p_buf[wk_idx++] = coller_pix[0];
00209             p_buf[wk_idx++] = coller_pix[1];
00210         }
00211     }
00212 }
00213 #endif
00214 
00215 
00216 static void DrawLine(int32_t x1,int32_t y1,int32_t  x2, int32_t y2,int drawpoint){
00217   int32_t dx=x2-x1;
00218   int32_t dy=y2-y1;
00219   int32_t sx=1;
00220   int32_t sy=1;
00221   int32_t i;
00222   int32_t de;
00223 
00224   if(dx<0){
00225     dx *= -1;
00226     sx *= -1;
00227   }
00228 
00229   if(dy<0){
00230     dy*=-1;
00231     sy*=-1;
00232   }
00233   draw_touch_pos(user_frame_buffer1,clr,drawpoint,x1,y1);
00234 
00235   if(dx>dy){
00236       for(i=dx,de=i/2;i;i--){
00237         x1+=sx;
00238         de+=dy;
00239         if(de>dx){
00240           de-=dx;
00241           y1+=sy;
00242         }
00243         draw_touch_pos(user_frame_buffer1,clr,drawpoint,x1,y1);
00244       }
00245     }else{
00246       for(i=dy,de=i/2;i;i--){
00247         y1+=sy;
00248         de+=dx;
00249         if(de>dy){
00250           de-=dy;
00251           x1+=sx;
00252         }
00253         draw_touch_pos(user_frame_buffer1,clr,drawpoint,x1,y1);
00254       }
00255     }
00256   }
00257 
00258 
00259 void skip_btn_fall0(void){
00260   //sta=(sta+1)%2; //on=1,off=0;
00261 
00262   if(firstflg==0 || (layer2flg==1 && sta==0)){
00263     layer2flg=0;
00264     sta=1;
00265     firstflg=1;
00266   }else{
00267     layer2flg=1;//非表示
00268     sta=0;//off
00269   }
00270 }
00271 
00272 void skip_btn_fall1(void){
00273   clr=(clr+1)%2; //on=1,off=0; //消しゴム(黒色)と青色色分け
00274 }
00275 
00276 static void main_task(void) {
00277     cv::Mat prev_image;
00278     cv::Mat curr_image;
00279     std::vector<cv::Point2f> prev_pts;
00280     std::vector<cv::Point2f> curr_pts;
00281     cv::Point2f point;
00282     int16_t  x = 0;
00283     int16_t  y = 0;
00284 
00285 
00286     EasyAttach_Init(Display);
00287     Start_Video_Camera();
00288 #if MBED_CONF_APP_LCD
00289     Start_LCD_Display();
00290 #endif
00291 
00292 #if STRING_DISP_TEST
00293     //string
00294     Thread stringTask;
00295     stringTask.start(callback(string_task));
00296 #endif // STRING_DISP_TEST
00297 
00298     // Initialization of optical flow
00299     point.y = (VIDEO_PIXEL_VW / 2) + (PLOT_INTERVAL * 1);
00300     for (int32_t i = 0; i < 3; i++) {
00301         point.x = (VIDEO_PIXEL_HW / 2) - (PLOT_INTERVAL * 1);
00302         for (int32_t j = 0; j < 3; j++) {
00303             prev_pts.push_back(point);
00304             point.x += PLOT_INTERVAL;
00305         }
00306         point.y -= PLOT_INTERVAL;
00307     }
00308     skip_btn0.fall(&skip_btn_fall0);
00309     skip_btn1.fall(&skip_btn_fall1);
00310 
00311 #if MBED_CONF_APP_LCD
00312             int16_t posx=LCD_PIXEL_WIDTH/2;
00313             int16_t posy=LCD_PIXEL_HEIGHT/2;
00314             int16_t prex=0;
00315             int16_t prey=0;
00316             int16_t akax=0;
00317             int16_t akay=0;
00318 
00319     while (1) {
00320         // Wait for image input
00321         wait_new_image();
00322 
00323         // Convert from YUV422 to grayscale
00324         cv::Mat img_yuv(VIDEO_PIXEL_VW, VIDEO_PIXEL_HW, CV_8UC2, user_frame_buffer0);
00325         cv::cvtColor(img_yuv, curr_image, cv::COLOR_YUV2GRAY_YUY2);
00326 
00327         point = cv::Point2f(0, 0);
00328         if ((!curr_image.empty()) && (!prev_image.empty())) {
00329             // Optical flow
00330             std::vector<uchar> status;
00331             std::vector<float> err;
00332             cv::calcOpticalFlowPyrLK(prev_image, curr_image, prev_pts, curr_pts, status, err, cv::Size(21, 21), 0);
00333 
00334             // Setting movement distance of feature point
00335             std::vector<cv::Scalar> samples;
00336             for (size_t i = 0; i < (size_t)status.size(); i++) {
00337                 if (status[i]) {
00338                     cv::Point2f vec = curr_pts[i] - prev_pts[i];
00339                     cv::Scalar sample = cv::Scalar(vec.x, vec.y);
00340                     samples.push_back(sample);
00341                 }
00342             }
00343 
00344             // Mean and standard deviation
00345             if (samples.size() >= 6) {
00346                 cv::Scalar mean;
00347                 cv::Scalar stddev;
00348                 cv::meanStdDev((cv::InputArray)samples, mean, stddev);
00349                 //printf("%d,  stddev=%lf, %lf\r\n", samples.size(), stddev[0], stddev[1]);  // for debug
00350                 if ((stddev[0] < 10.0) && (stddev[1] < 10.0)) {
00351                     point.x = mean[0];
00352                     point.y = mean[1];
00353                 }
00354             }
00355         }
00356         cv::swap(prev_image, curr_image);
00357 
00358         x = (int16_t)(point.x * DIST_SCALE_FACTOR_X) *- 1;
00359         y = (int16_t)(point.y * DIST_SCALE_FACTOR_Y) * -1;
00360 
00361         if(sta==1){
00362           //xとyは移動量なので、現在地を算出する
00363           //その前に現在地を保存
00364           prex=posx;//移動前
00365           prey=posy;//移動前
00366           posx+=x;//移動後
00367           posy+=y;//移動後
00368         }
00369         //displayの外にはみでていたらdisplayの枠に値を矯正
00370         posx=RangeCorr(posx,(int)LCD_PIXEL_WIDTH,0);
00371         posy=RangeCorr(posy,(int)LCD_PIXEL_HEIGHT,0);
00372 
00373 #endif
00374         if ((x != 0) || (y != 0)) {
00375             led1 = 1;
00376             //printf("x=%d, y=%d\r\n", x, y);  // for debug
00377 #if MBED_CONF_APP_LCD
00378           if(sta==1){//ub0のボタンが奇数回(1,,3, 5,・・・)押下されたら、線を描画する。(on,offスイッチ)
00379             if(clr==1){
00380              //円を描画して消す
00381              draw_touch_pos(user_frame_buffer1,clr,ERASER_POINT,prex,prey); //前の座標を消去
00382              DrawLine(prex,prey,posx,posy,ERASER_POINT);
00383              draw_touch_pos(user_frame_buffer1,2,ERASER_POINT,posx,posy);
00384              akax=posx;
00385              akay=posy;
00386 
00387              kesiflg=1;
00388 
00389              //ThisThread::sleep_for(5);
00390              //draw_touch_pos(user_frame_buffer1,clr,ERASER_POINT,posx,posy); //消しゴム
00391              }else{
00392                if(kesiflg==1){
00393                  draw_touch_pos(user_frame_buffer1,1,ERASER_POINT,akax,akay);
00394                  kesiflg=0;
00395                }
00396              draw_touch_pos(user_frame_buffer1,clr,DRAW_POINT,posx,posy); //ペン
00397              DrawLine(prex,prey,posx,posy,DRAW_POINT);
00398            }
00399          }
00400 
00401 
00402 #endif
00403         } else {
00404             led1 = 0;
00405         }
00406     }
00407 }
00408 
00409 #if MBED_CONF_APP_LCD
00410 //範囲の修正
00411 int16_t RangeCorr(int16_t target ,int16_t uplimit,int16_t lowlimit){
00412   if(target>=lowlimit &&  target<=uplimit){ //もし範囲内だったら
00413     return target;//そのまま返す
00414   }else if(target>uplimit){ //上限より値が大きかったら
00415     return uplimit; //上限の値を返す
00416   }else{ //下限より小さかったら
00417     return lowlimit; //下限の値を返す
00418   }
00419 }
00420 #endif
00421 
00422 
00423 uint8_t get_random(void){
00424     size_t olen;
00425     uint8_t data;
00426     trng_t trng_obj;
00427     static bool init_flg = false;
00428 
00429     trng_init(&trng_obj);
00430     if (init_flg == false) {
00431         init_flg = true;
00432         trng_get_bytes(&trng_obj, &data, 1, &olen);
00433     }
00434     trng_get_bytes(&trng_obj, &data, 1, &olen);
00435     trng_free(&trng_obj);
00436    return data;
00437 }
00438 
00439 int main(void) {
00440     mainTask.start(callback(main_task));
00441     mainTask.join();
00442 
00443 }
00444