Diff: main.cpp

Revision:

0:a5b12f5693c6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 06 03:03:47 2019 +0000
@@ -0,0 +1,444 @@
+#include "mbed.h"
+#include "opencv.hpp"
+#include "EasyAttach_CameraAndLCD.h"
+#include "dcache-control.h"
+#include "AsciiFont.h"
+#include "hal/trng_api.h"
+#include "oekaki_theme.h"
+
+
+#define PLOT_INTERVAL          (30)
+#define DIST_SCALE_FACTOR_X    (6.0)
+#define DIST_SCALE_FACTOR_Y    (6.0)
+
+#define STRING_DISP_TEST       (1)
+#define DRAW_POINT (3)
+#define ERASER_POINT (8)
+#define TOUCH_BUFFER_BYTE_PER_PIXEL (2u)
+#define TOUCH_BUFFER_STRIDE           (((LCD_PIXEL_WIDTH * TOUCH_BUFFER_BYTE_PER_PIXEL) + 31u) & ~31u)
+#define COLOR_BLUE cv::Scalar(0,255,0)
+
+
+/*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
+    in accordance with the frame buffer burst transfer mode. */
+#define VIDEO_PIXEL_HW         (160u)  /* HQVGA */
+#define VIDEO_PIXEL_VW         (120u)  /* HQVGA */
+
+#define FRAME_BUFFER_STRIDE    (((VIDEO_PIXEL_HW * 2) + 31u) & ~31u)
+#define FRAME_BUFFER_HEIGHT    (VIDEO_PIXEL_VW)
+
+#if defined(__ICCARM__)
+#pragma data_alignment=32
+static uint8_t user_frame_buffer0[FRAME_BUFFER_STRIDE * FRAME_BUFFER_HEIGHT]@ ".mirrorram";
+static uint8_t user_frame_buffer1[TOUCH_BUFFER_STRIDE * LCD_PIXEL_HEIGHT]@ ".mirrorram";
+#else
+static uint8_t user_frame_buffer0[FRAME_BUFFER_STRIDE * FRAME_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(32)));
+static uint8_t user_frame_buffer1[TOUCH_BUFFER_STRIDE * LCD_PIXEL_HEIGHT]__attribute((section("NC_BSS"),aligned(32)));//cameraみたい
+#endif
+static volatile int Vfield_Int_Cnt = 0;
+
+static InterruptIn skip_btn0(USER_BUTTON0);
+static InterruptIn skip_btn1(USER_BUTTON1);
+static int sta=0;//描画のon,off switch
+static int clr=0;//カラー選択(0=青、 1=黒(消しゴム))
+static int layer2flg=0;
+static int firstflg=0;
+static int kesiflg=0;//1のとき、消しゴムを使ったあと。
+static int16_t RangeCorr(int16_t target ,int16_t uplimit,int16_t lowlimit);
+static uint8_t get_random(void);
+
+DisplayBase Display;
+DigitalOut  led1(LED1);
+static Thread mainTask(osPriorityNormal, 1024 * 16);
+
+static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) {
+    if (Vfield_Int_Cnt > 0) {
+        Vfield_Int_Cnt--;
+    }
+}
+
+static void wait_new_image(void) {
+    Vfield_Int_Cnt = 1;
+    while (Vfield_Int_Cnt > 0) {
+        ThisThread::sleep_for(1);
+    }
+}
+
+
+static void Start_Video_Camera(void) {
+    // Field end signal for recording function in scaler 0
+    Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VFIELD, 0, IntCallbackFunc_Vfield);
+
+    // Video capture setting (progressive form fixed)
+    Display.Video_Write_Setting(
+        DisplayBase::VIDEO_INPUT_CHANNEL_0,
+        DisplayBase::COL_SYS_NTSC_358,
+        (void *)user_frame_buffer0,
+        FRAME_BUFFER_STRIDE,
+        DisplayBase::VIDEO_FORMAT_YCBCR422,
+        DisplayBase::WR_RD_WRSWA_32_16BIT,
+        VIDEO_PIXEL_VW,
+        VIDEO_PIXEL_HW
+    );
+    EasyAttach_CameraStart(Display, DisplayBase::VIDEO_INPUT_CHANNEL_0);
+}
+
+#if MBED_CONF_APP_LCD
+static void Start_LCD_Display(void) {
+    DisplayBase::rect_t rect;
+
+    /* The layer by which the touch panel location is drawn */
+    memset(user_frame_buffer1, 0, sizeof(user_frame_buffer1));//framebufferの名前を変えた、
+    dcache_clean(user_frame_buffer1, sizeof(user_frame_buffer1));//framebufferの名前を変えた、
+    rect.vs = 0;
+    rect.vw = LCD_PIXEL_HEIGHT;
+    rect.hs = 0;
+    rect.hw = LCD_PIXEL_WIDTH;
+    Display.Graphics_Read_Setting(
+        DisplayBase::GRAPHICS_LAYER_0,
+        (void *)user_frame_buffer1,
+        TOUCH_BUFFER_STRIDE,
+        DisplayBase::GRAPHICS_FORMAT_ARGB4444,
+        DisplayBase::WR_RD_WRSWA_32_16BIT,
+        &rect
+    );
+    Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_0);
+
+    ThisThread::sleep_for(50);
+    EasyAttach_LcdBacklight(true);
+}
+
+#if STRING_DISP_TEST
+//文字列出力
+#define STRING_PIXEL_HW               (129)
+#define STRING_PIXEL_VM               (24)
+#define STRING_BUFFER_BYTE_PER_PIXEL  (2u)
+#define STRING_BUFFER_STRIDE          (((LCD_PIXEL_WIDTH * STRING_BUFFER_BYTE_PER_PIXEL) + 31u) & ~31u)
+
+#if defined(__ICCARM__)
+#pragma data_alignment=32
+static uint8_t user_frame_buffer_string[STRING_BUFFER_STRIDE * STRING_PIXEL_VM];
+#else
+static uint8_t user_frame_buffer_string[STRING_BUFFER_STRIDE * STRING_PIXEL_VM]__attribute((aligned(32)));
+#endif
+
+static void string_task(void){
+  DisplayBase::rect_t rect;
+
+  memset(user_frame_buffer_string,0,sizeof(user_frame_buffer_string));
+  dcache_clean(user_frame_buffer_string,sizeof(user_frame_buffer_string));
+
+  rect.vs = LCD_PIXEL_HEIGHT - STRING_PIXEL_VM - 10;
+  rect.vw = STRING_PIXEL_VM;
+  rect.hs = LCD_PIXEL_WIDTH - STRING_PIXEL_HW-10;
+  rect.hw = STRING_PIXEL_HW;
+  Display.Graphics_Read_Setting(
+    DisplayBase::GRAPHICS_LAYER_2,
+    (void *)user_frame_buffer_string,
+    STRING_BUFFER_STRIDE,
+    DisplayBase::GRAPHICS_FORMAT_ARGB4444,
+    DisplayBase::WR_RD_WRSWA_32_16BIT,
+    &rect
+  );
+  Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_2);
+
+  AsciiFont ascii_font(user_frame_buffer_string,STRING_PIXEL_HW,STRING_PIXEL_VM,
+    STRING_BUFFER_STRIDE,STRING_BUFFER_BYTE_PER_PIXEL);
+
+            //(文字列, x, y, colour,font_size)
+    int num=get_random();
+    int layer2flg_last = -1;
+    while(1){
+    if(layer2flg_last!=layer2flg){
+      if(layer2flg==0){
+        ascii_font.DrawStr(ChooseTheme(num), 0, 0 , 0x0000ffff, 2);//白
+      }else{
+        memset(user_frame_buffer_string,0,sizeof(user_frame_buffer_string));
+      }
+      dcache_clean(user_frame_buffer_string,sizeof(user_frame_buffer_string));
+      layer2flg_last=layer2flg;
+    }
+    ThisThread::sleep_for(50);
+  }
+}
+#endif
+
+static void draw_touch_pos(uint8_t * p_buf, int id, int drawpoint,int x, int y) { //バッファ,色,線の太さ,x座標,y座標
+    int idx_base;
+    int wk_idx;
+    int i;
+    int j;
+    uint8_t coller_pix[TOUCH_BUFFER_BYTE_PER_PIXEL];  /* ARGB4444 */
+
+    /* A coordinate in the upper left is calculated from a central coordinate. */
+    if ((x - (drawpoint / 2)) >= 0) {
+        x -= (drawpoint / 2);
+    }
+    if (x > ((int)LCD_PIXEL_WIDTH - drawpoint)) {
+        x = ((int)LCD_PIXEL_WIDTH - drawpoint);
+    }
+    if ((y - (drawpoint / 2)) >= 0) {
+        y -= (drawpoint / 2);
+    }
+    if (y > ((int)LCD_PIXEL_HEIGHT - drawpoint)) {
+        y = ((int)LCD_PIXEL_HEIGHT - drawpoint);
+    }
+    idx_base = (x + ((int)LCD_PIXEL_WIDTH * y)) * TOUCH_BUFFER_BYTE_PER_PIXEL;
+
+    /* Select color */
+    if (id == 0) {
+        /* Blue */
+        coller_pix[0] = 0x0F;  /* 4:Green 4:Blue */
+        coller_pix[1] = 0xF0;  /* 4:Alpha 4:Red  */
+    } else if (id == 1){
+
+        /* black */
+        coller_pix[0] = 0x00;  /* 4:Green 4:Blue */
+        coller_pix[1] = 0x00;  /* 4:Alpha 4:Red  */
+    } else {
+        /* red */
+        coller_pix[0] = 0x01;
+        coller_pix[1] = 0xFF;
+      }
+
+    /* Drawing */
+    for (i = 0; i < drawpoint; i++) {
+        wk_idx = idx_base + ((int)LCD_PIXEL_WIDTH * TOUCH_BUFFER_BYTE_PER_PIXEL * i);
+        for (j = 0; j < drawpoint; j++) {
+            p_buf[wk_idx++] = coller_pix[0];
+            p_buf[wk_idx++] = coller_pix[1];
+        }
+    }
+}
+#endif
+
+
+static void DrawLine(int32_t x1,int32_t y1,int32_t  x2, int32_t y2,int drawpoint){
+  int32_t dx=x2-x1;
+  int32_t dy=y2-y1;
+  int32_t sx=1;
+  int32_t sy=1;
+  int32_t i;
+  int32_t de;
+
+  if(dx<0){
+    dx *= -1;
+    sx *= -1;
+  }
+
+  if(dy<0){
+    dy*=-1;
+    sy*=-1;
+  }
+  draw_touch_pos(user_frame_buffer1,clr,drawpoint,x1,y1);
+
+  if(dx>dy){
+      for(i=dx,de=i/2;i;i--){
+        x1+=sx;
+        de+=dy;
+        if(de>dx){
+          de-=dx;
+          y1+=sy;
+        }
+        draw_touch_pos(user_frame_buffer1,clr,drawpoint,x1,y1);
+      }
+    }else{
+      for(i=dy,de=i/2;i;i--){
+        y1+=sy;
+        de+=dx;
+        if(de>dy){
+          de-=dy;
+          x1+=sx;
+        }
+        draw_touch_pos(user_frame_buffer1,clr,drawpoint,x1,y1);
+      }
+    }
+  }
+
+
+void skip_btn_fall0(void){
+  //sta=(sta+1)%2; //on=1,off=0;
+
+  if(firstflg==0 || (layer2flg==1 && sta==0)){
+    layer2flg=0;
+    sta=1;
+    firstflg=1;
+  }else{
+    layer2flg=1;//非表示
+    sta=0;//off
+  }
+}
+
+void skip_btn_fall1(void){
+  clr=(clr+1)%2; //on=1,off=0; //消しゴム(黒色)と青色色分け
+}
+
+static void main_task(void) {
+    cv::Mat prev_image;
+    cv::Mat curr_image;
+    std::vector<cv::Point2f> prev_pts;
+    std::vector<cv::Point2f> curr_pts;
+    cv::Point2f point;
+    int16_t  x = 0;
+    int16_t  y = 0;
+
+
+    EasyAttach_Init(Display);
+    Start_Video_Camera();
+#if MBED_CONF_APP_LCD
+    Start_LCD_Display();
+#endif
+
+#if STRING_DISP_TEST
+    //string
+    Thread stringTask;
+    stringTask.start(callback(string_task));
+#endif // STRING_DISP_TEST
+
+    // Initialization of optical flow
+    point.y = (VIDEO_PIXEL_VW / 2) + (PLOT_INTERVAL * 1);
+    for (int32_t i = 0; i < 3; i++) {
+        point.x = (VIDEO_PIXEL_HW / 2) - (PLOT_INTERVAL * 1);
+        for (int32_t j = 0; j < 3; j++) {
+            prev_pts.push_back(point);
+            point.x += PLOT_INTERVAL;
+        }
+        point.y -= PLOT_INTERVAL;
+    }
+    skip_btn0.fall(&skip_btn_fall0);
+    skip_btn1.fall(&skip_btn_fall1);
+
+#if MBED_CONF_APP_LCD
+            int16_t posx=LCD_PIXEL_WIDTH/2;
+            int16_t posy=LCD_PIXEL_HEIGHT/2;
+            int16_t prex=0;
+            int16_t prey=0;
+            int16_t akax=0;
+            int16_t akay=0;
+
+    while (1) {
+        // Wait for image input
+        wait_new_image();
+
+        // Convert from YUV422 to grayscale
+        cv::Mat img_yuv(VIDEO_PIXEL_VW, VIDEO_PIXEL_HW, CV_8UC2, user_frame_buffer0);
+        cv::cvtColor(img_yuv, curr_image, cv::COLOR_YUV2GRAY_YUY2);
+
+        point = cv::Point2f(0, 0);
+        if ((!curr_image.empty()) && (!prev_image.empty())) {
+            // Optical flow
+            std::vector<uchar> status;
+            std::vector<float> err;
+            cv::calcOpticalFlowPyrLK(prev_image, curr_image, prev_pts, curr_pts, status, err, cv::Size(21, 21), 0);
+
+            // Setting movement distance of feature point
+            std::vector<cv::Scalar> samples;
+            for (size_t i = 0; i < (size_t)status.size(); i++) {
+                if (status[i]) {
+                    cv::Point2f vec = curr_pts[i] - prev_pts[i];
+                    cv::Scalar sample = cv::Scalar(vec.x, vec.y);
+                    samples.push_back(sample);
+                }
+            }
+
+            // Mean and standard deviation
+            if (samples.size() >= 6) {
+                cv::Scalar mean;
+                cv::Scalar stddev;
+                cv::meanStdDev((cv::InputArray)samples, mean, stddev);
+                //printf("%d,  stddev=%lf, %lf\r\n", samples.size(), stddev[0], stddev[1]);  // for debug
+                if ((stddev[0] < 10.0) && (stddev[1] < 10.0)) {
+                    point.x = mean[0];
+                    point.y = mean[1];
+                }
+            }
+        }
+        cv::swap(prev_image, curr_image);
+
+        x = (int16_t)(point.x * DIST_SCALE_FACTOR_X) *- 1;
+        y = (int16_t)(point.y * DIST_SCALE_FACTOR_Y) * -1;
+
+        if(sta==1){
+          //xとyは移動量なので、現在地を算出する
+          //その前に現在地を保存
+          prex=posx;//移動前
+          prey=posy;//移動前
+          posx+=x;//移動後
+          posy+=y;//移動後
+        }
+        //displayの外にはみでていたらdisplayの枠に値を矯正
+        posx=RangeCorr(posx,(int)LCD_PIXEL_WIDTH,0);
+        posy=RangeCorr(posy,(int)LCD_PIXEL_HEIGHT,0);
+
+#endif
+        if ((x != 0) || (y != 0)) {
+            led1 = 1;
+            //printf("x=%d, y=%d\r\n", x, y);  // for debug
+#if MBED_CONF_APP_LCD
+          if(sta==1){//ub0のボタンが奇数回(1,,3, 5,・・・)押下されたら、線を描画する。(on,offスイッチ)
+            if(clr==1){
+             //円を描画して消す
+             draw_touch_pos(user_frame_buffer1,clr,ERASER_POINT,prex,prey); //前の座標を消去
+             DrawLine(prex,prey,posx,posy,ERASER_POINT);
+             draw_touch_pos(user_frame_buffer1,2,ERASER_POINT,posx,posy);
+             akax=posx;
+             akay=posy;
+
+             kesiflg=1;
+
+             //ThisThread::sleep_for(5);
+             //draw_touch_pos(user_frame_buffer1,clr,ERASER_POINT,posx,posy); //消しゴム
+             }else{
+               if(kesiflg==1){
+                 draw_touch_pos(user_frame_buffer1,1,ERASER_POINT,akax,akay);
+                 kesiflg=0;
+               }
+             draw_touch_pos(user_frame_buffer1,clr,DRAW_POINT,posx,posy); //ペン
+             DrawLine(prex,prey,posx,posy,DRAW_POINT);
+           }
+         }
+
+
+#endif
+        } else {
+            led1 = 0;
+        }
+    }
+}
+
+#if MBED_CONF_APP_LCD
+//範囲の修正
+int16_t RangeCorr(int16_t target ,int16_t uplimit,int16_t lowlimit){
+  if(target>=lowlimit &&  target<=uplimit){ //もし範囲内だったら
+    return target;//そのまま返す
+  }else if(target>uplimit){ //上限より値が大きかったら
+    return uplimit; //上限の値を返す
+  }else{ //下限より小さかったら
+    return lowlimit; //下限の値を返す
+  }
+}
+#endif
+
+
+uint8_t get_random(void){
+    size_t olen;
+    uint8_t data;
+    trng_t trng_obj;
+    static bool init_flg = false;
+
+    trng_init(&trng_obj);
+    if (init_flg == false) {
+        init_flg = true;
+        trng_get_bytes(&trng_obj, &data, 1, &olen);
+    }
+    trng_get_bytes(&trng_obj, &data, 1, &olen);
+    trng_free(&trng_obj);
+   return data;
+}
+
+int main(void) {
+    mainTask.start(callback(main_task));
+    mainTask.join();
+
+}
+