Diff: main.cpp

Revision:

7:3ad9c948bc06

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jan 27 11:15:26 2013 +0000
@@ -0,0 +1,153 @@
+// BaseJpegDecode_example/main.cpp 2013/1/27
+// simple color tracking
+//
+#include "mbed.h"
+#include "rtos.h"
+#include "BaseUsbHost.h"
+#include "UvcCam.h"
+#include "BaseJpegDecode.h"
+#include "Terminal.h"
+#include "MyThread.h"
+
+#define WIDTH  160
+#define HEIGHT 120
+
+#define THRESHOLD 200
+
+DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
+Terminal term(USBTX, USBRX);
+
+class CalcCenter : public MyThread, public BaseJpegDecode {
+public:
+    int y_center, x_center;
+    int m_x_sum, m_y_sum, m_sum;
+    uint32_t EOI_count;
+    int16_t buf_Cb[WIDTH/16*HEIGHT/8]; // debug
+    int16_t buf_Cr[WIDTH/16*HEIGHT/8]; // debug
+    CalcCenter(BaseUvc* cam) {
+        m_cam = cam;
+        m_cam->setOnResult(this, &CalcCenter::callback_motion_jpeg);
+        EOI_count = 0;
+    }
+protected:
+    void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
+        input(buf+12, len-12);
+        if (buf[1]&1) { // FID
+            led1 = !led1;
+        }
+    }
+    
+    virtual void outputDC(int mcu, int block, int value) {
+        if (mcu >= (WIDTH/16*HEIGHT/8)) {
+            return;
+        }
+        if (block == 2) {
+            buf_Cb[mcu] = value * qt[1][0];
+        } else if (block == 3) { // 0-1:Y 2:Cb 3:Cr
+            buf_Cr[mcu] = value * qt[1][0];
+            value *= qt[1][0];
+            if (value >= THRESHOLD) { // red
+                m_x_sum += value*(mcu%(WIDTH/16));
+                m_y_sum += value*(mcu/(WIDTH/16));
+                m_sum += value;
+            }
+        }
+    }
+    virtual void outputAC(int mcu, int block, int scan, int value){};
+    virtual void outputMARK(uint8_t c){
+        if (c == 0xd9) { // EOI
+            if(m_sum == 0) {
+                x_center = y_center = -1; // not found
+            } else {
+                x_center = m_x_sum / m_sum;
+                y_center = m_y_sum / m_sum;
+            }
+            m_x_sum = m_y_sum = m_sum = 0; // reset
+            EOI_count++;
+            led3 = !led3;
+        }
+    }
+
+    virtual void run() {
+        while(true) {
+            if (m_cam) {
+                m_cam->poll();
+            }
+        }
+    }
+    BaseUvc* m_cam;
+};
+
+BaseUvc* cam = NULL;
+CalcCenter* calc = NULL;
+
+class display_Thread : public MyThread {
+    virtual void run() {
+        term.cls();
+        int fg, old_fg = 0xffffff;
+        while(1) {
+            int column = 0;
+            for(int y = 0; y < HEIGHT/8; y++) {
+                term.locate(0, column++);
+                for(int x = 0; x < WIDTH/16; x++) {
+                    int value = calc->buf_Cr[y*WIDTH/16+x];
+                    if (value >= THRESHOLD) {
+                        fg = 0xff0000; // red
+                    } else {
+                        fg = 0xffffff; // white
+                    }
+                    if (fg != old_fg) {
+                        term.foreground(fg);
+                        old_fg = fg;
+                    }
+                    term.printf("%+4d,", value);
+                    Thread::yield();
+                }
+            }
+            term.locate(0, column++);
+            term.printf("Cr:(%d,%d)", calc->x_center, calc->y_center);
+
+            term.locate(0, column++);
+            term.printf("width=%d height=%d yblock=%d EOI: %u]\nCC:", 
+                         calc->width, calc->height, calc->yblock, calc->EOI_count);
+            for(int i = 0; i < 16; i++) {
+                term.printf(" %u", cam->report_cc_count[i]); 
+            }
+            term.printf("]\nPS:"); 
+            for(int i = 0; i < 16; i++) {
+                term.printf(" %u", cam->report_ps_cc_count[i]); 
+            }
+            term.printf("]\n");
+            Thread::wait(150);
+            led2 = !led2;
+        }
+    }
+};
+
+void no_memory () {
+  error("Failed to allocate memory!\n");
+}
+
+int main() {
+    term.baud(921600);
+    term.printf("%s\n", __FILE__);
+    set_new_handler(no_memory);
+
+    BaseUsbHost* UsbHost = new BaseUsbHost;
+    ControlEp* ctlEp = new ControlEp; // root hub
+    if (UsbHub::check(ctlEp)) { // hub?
+        UsbHub* hub = new UsbHub(ctlEp);
+        ctlEp = hub->search<UvcCam>();
+    }
+    if (!UvcCam::check(ctlEp)) {
+        error("UVC camera is not connected\n");
+    }
+    cam = new UvcCam(UVC_MJPEG, UVC_160x120, _15FPS, ctlEp); 
+    calc = new CalcCenter(cam);
+    calc->start();
+
+    display_Thread* display_th = new display_Thread;
+    display_th->start(osPriorityBelowNormal);
+
+    Thread::wait(osWaitForever);
+}