From 01e6df602faff5c164f51f342d0dc5ebf37b2a45 Mon Sep 17 00:00:00 2001 From: Andrewiski Date: Sat, 25 Nov 2017 14:29:06 -0500 Subject: [PATCH 1/5] Attempt to Add FTC Compatible I2C Calls --- src/device/build_this_first/CopyFiles.cmd | 6 + src/device/main_m0/Release/main_m0_Release.ld | 217 +++--------------- src/device/main_m4/inc/serial.h | 1 + src/device/main_m4/src/serial.cpp | 206 ++++++++++++++++- 4 files changed, 249 insertions(+), 181 deletions(-) create mode 100644 src/device/build_this_first/CopyFiles.cmd diff --git a/src/device/build_this_first/CopyFiles.cmd b/src/device/build_this_first/CopyFiles.cmd new file mode 100644 index 00000000..b219f234 --- /dev/null +++ b/src/device/build_this_first/CopyFiles.cmd @@ -0,0 +1,6 @@ +@Echo Off +set sourcedir="D:\Users\adevries\sources\GitHub\pixy\src" +copy %sourcedir%\device\common\src\*.* %sourcedir%\device\libpixy_m0\src +copy %sourcedir%\device\common\src\*.* %sourcedir%\device\libpixy_m4\src +copy %sourcedir%\common\src\*.* %sourcedir%\device\libpixy_m4\src +pause \ No newline at end of file diff --git a/src/device/main_m0/Release/main_m0_Release.ld b/src/device/main_m0/Release/main_m0_Release.ld index 1aa5f8e7..324ba818 100755 --- a/src/device/main_m0/Release/main_m0_Release.ld +++ b/src/device/main_m0/Release/main_m0_Release.ld @@ -1,207 +1,87 @@ /* * GENERATED FILE - DO NOT EDIT - * (c) Code Red Technologies Ltd, 2008-13 - * (c) NXP Semiconductors 2013-2015 + * (c) Code Red Technologies Ltd, 2008-2013 + * (c) NXP Semiconductors 2013-2017 * Generated linker script file for LPC4330-M0 - * Created from generic_c.ld (LPCXpresso v7.6 (2 [Build 326] [2015-02-02] )) - * By LPCXpresso v7.6.2 [Build 326] [2015-02-02] on Wed Apr 01 17:57:00 CDT 2015 + * Created from linkscript.ldt by FMCreateLinkLibraries + * Using Freemarker v2.3.23 + * LPCXpresso v8.2.2 [Build 650] [2016-09-09] on Nov 24, 2017 2:21:33 PM */ - -INCLUDE "main_m0_Release_lib.ld" -INCLUDE "main_m0_Release_mem.ld" +INCLUDE "main_m0_Release_library.ld" +INCLUDE "main_m0_Release_memory.ld" ENTRY(ResetISR) SECTIONS { - - /* MAIN TEXT SECTION */ + /* MAIN TEXT SECTION */ .text : ALIGN(4) { FILL(0xff) __vectors_start__ = ABSOLUTE(.) ; KEEP(*(.isr_vector)) - /* Global Section Table */ - . = ALIGN(4) ; + . = ALIGN(4) ; __section_table_start = .; __data_section_table = .; LONG(LOADADDR(.data)); LONG( ADDR(.data)); LONG( SIZEOF(.data)); - LONG(LOADADDR(.data_RAM2)); - LONG( ADDR(.data_RAM2)); - LONG( SIZEOF(.data_RAM2)); - LONG(LOADADDR(.data_RAM3)); - LONG( ADDR(.data_RAM3)); - LONG( SIZEOF(.data_RAM3)); - LONG(LOADADDR(.data_RAM4)); - LONG( ADDR(.data_RAM4)); - LONG( SIZEOF(.data_RAM4)); - LONG(LOADADDR(.data_RAM5)); - LONG( ADDR(.data_RAM5)); - LONG( SIZEOF(.data_RAM5)); __data_section_table_end = .; __bss_section_table = .; LONG( ADDR(.bss)); LONG( SIZEOF(.bss)); - LONG( ADDR(.bss_RAM2)); - LONG( SIZEOF(.bss_RAM2)); - LONG( ADDR(.bss_RAM3)); - LONG( SIZEOF(.bss_RAM3)); - LONG( ADDR(.bss_RAM4)); - LONG( SIZEOF(.bss_RAM4)); - LONG( ADDR(.bss_RAM5)); - LONG( SIZEOF(.bss_RAM5)); __bss_section_table_end = .; __section_table_end = . ; - /* End of Global Section Table */ - + /* End of Global Section Table */ *(.after_vectors*) - + } >RamAHB16 - + .text : ALIGN(4) { - *(.text*) + *(.text*) *(.rodata .rodata.* .constdata .constdata.*) . = ALIGN(4); - } > RamAHB16 - /* * for exception handling/unwind - some Newlib functions (in common * with C++ and STDC++) use this. */ - .ARM.extab : ALIGN(4) + .ARM.extab : ALIGN(4) { - *(.ARM.extab* .gnu.linkonce.armextab.*) + *(.ARM.extab* .gnu.linkonce.armextab.*) } > RamAHB16 __exidx_start = .; - + .ARM.exidx : ALIGN(4) { - *(.ARM.exidx* .gnu.linkonce.armexidx.*) + *(.ARM.exidx* .gnu.linkonce.armexidx.*) } > RamAHB16 __exidx_end = .; - + _etext = .; - - /* DATA section for RamAHB32 */ - .data_RAM2 : ALIGN(4) - { - FILL(0xff) - PROVIDE(__start_data_RAM2 = .) ; - *(.ramfunc.$RAM2) - *(.ramfunc.$RamAHB32) - *(.data.$RAM2*) - *(.data.$RamAHB32*) - . = ALIGN(4) ; - PROVIDE(__end_data_RAM2 = .) ; - } > RamAHB32 AT>RamAHB16 - - /* DATA section for RamLoc128 */ - .data_RAM3 : ALIGN(4) - { - FILL(0xff) - PROVIDE(__start_data_RAM3 = .) ; - *(.ramfunc.$RAM3) - *(.ramfunc.$RamLoc128) - *(.data.$RAM3*) - *(.data.$RamLoc128*) - . = ALIGN(4) ; - PROVIDE(__end_data_RAM3 = .) ; - } > RamLoc128 AT>RamAHB16 - - /* DATA section for RamLoc72 */ - .data_RAM4 : ALIGN(4) - { - FILL(0xff) - PROVIDE(__start_data_RAM4 = .) ; - *(.ramfunc.$RAM4) - *(.ramfunc.$RamLoc72) - *(.data.$RAM4*) - *(.data.$RamLoc72*) - . = ALIGN(4) ; - PROVIDE(__end_data_RAM4 = .) ; - } > RamLoc72 AT>RamAHB16 - - /* DATA section for RamAHB_ETB16 */ - .data_RAM5 : ALIGN(4) - { - FILL(0xff) - PROVIDE(__start_data_RAM5 = .) ; - *(.ramfunc.$RAM5) - *(.ramfunc.$RamAHB_ETB16) - *(.data.$RAM5*) - *(.data.$RamAHB_ETB16*) - . = ALIGN(4) ; - PROVIDE(__end_data_RAM5 = .) ; - } > RamAHB_ETB16 AT>RamAHB16 - /* MAIN DATA SECTION */ - - .uninit_RESERVED : ALIGN(4) { KEEP(*(.bss.$RESERVED*)) . = ALIGN(4) ; _end_uninit_RESERVED = .; } > RamAHB16 - - - /* Main DATA section (RamAHB16) */ - .data : ALIGN(4) - { - FILL(0xff) - _data = . ; - *(vtable) - *(.ramfunc*) - *(.data*) - . = ALIGN(4) ; - _edata = . ; - } > RamAHB16 AT>RamAHB16 - - /* BSS section for RamAHB32 */ - .bss_RAM2 : ALIGN(4) - { - PROVIDE(__start_bss_RAM2 = .) ; - *(.bss.$RAM2*) - *(.bss.$RamAHB32*) - . = ALIGN(4) ; - PROVIDE(__end_bss_RAM2 = .) ; - } > RamAHB32 - /* BSS section for RamLoc128 */ - .bss_RAM3 : ALIGN(4) + /* Main DATA section (RamAHB16) */ + .data : ALIGN(4) { - PROVIDE(__start_bss_RAM3 = .) ; - *(.bss.$RAM3*) - *(.bss.$RamLoc128*) - . = ALIGN(4) ; - PROVIDE(__end_bss_RAM3 = .) ; - } > RamLoc128 - /* BSS section for RamLoc72 */ - .bss_RAM4 : ALIGN(4) - { - PROVIDE(__start_bss_RAM4 = .) ; - *(.bss.$RAM4*) - *(.bss.$RamLoc72*) - . = ALIGN(4) ; - PROVIDE(__end_bss_RAM4 = .) ; - } > RamLoc72 - /* BSS section for RamAHB_ETB16 */ - .bss_RAM5 : ALIGN(4) - { - PROVIDE(__start_bss_RAM5 = .) ; - *(.bss.$RAM5*) - *(.bss.$RamAHB_ETB16*) + FILL(0xff) + _data = . ; + *(vtable) + *(.ramfunc*) + *(.data*) . = ALIGN(4) ; - PROVIDE(__end_bss_RAM5 = .) ; - } > RamAHB_ETB16 - + _edata = . ; + } > RamAHB16 AT>RamAHB16 /* MAIN BSS SECTION */ .bss : ALIGN(4) { @@ -212,36 +92,6 @@ SECTIONS _ebss = .; PROVIDE(end = .); } > RamAHB16 - - /* NOINIT section for RamAHB32 */ - .noinit_RAM2 (NOLOAD) : ALIGN(4) - { - *(.noinit.$RAM2*) - *(.noinit.$RamAHB32*) - . = ALIGN(4) ; - } > RamAHB32 - /* NOINIT section for RamLoc128 */ - .noinit_RAM3 (NOLOAD) : ALIGN(4) - { - *(.noinit.$RAM3*) - *(.noinit.$RamLoc128*) - . = ALIGN(4) ; - } > RamLoc128 - /* NOINIT section for RamLoc72 */ - .noinit_RAM4 (NOLOAD) : ALIGN(4) - { - *(.noinit.$RAM4*) - *(.noinit.$RamLoc72*) - . = ALIGN(4) ; - } > RamLoc72 - /* NOINIT section for RamAHB_ETB16 */ - .noinit_RAM5 (NOLOAD) : ALIGN(4) - { - *(.noinit.$RAM5*) - *(.noinit.$RamAHB_ETB16*) - . = ALIGN(4) ; - } > RamAHB_ETB16 - /* DEFAULT NOINIT SECTION */ .noinit (NOLOAD): ALIGN(4) { @@ -250,7 +100,16 @@ SECTIONS . = ALIGN(4) ; _end_noinit = .; } > RamAHB16 - + PROVIDE(_pvHeapStart = DEFINED(__user_heap_base) ? __user_heap_base : .); PROVIDE(_vStackTop = DEFINED(__user_stack_top) ? __user_stack_top : __top_RamAHB16 - 0); -} + + /* ## Create checksum value (used in startup) ## */ + PROVIDE(__valid_user_code_checksum = 0 - + (_vStackTop + + (ResetISR + 1) + + (( DEFINED(NMI_Handler) ? NMI_Handler : M0_NMI_Handler ) + 1) + + (( DEFINED(HardFault_Handler) ? HardFault_Handler : M0_HardFault_Handler ) + 1) + ) + ); +} \ No newline at end of file diff --git a/src/device/main_m4/inc/serial.h b/src/device/main_m4/inc/serial.h index abaa4100..617955e8 100644 --- a/src/device/main_m4/inc/serial.h +++ b/src/device/main_m4/inc/serial.h @@ -25,6 +25,7 @@ #define SER_INTERFACE_ADX 4 #define SER_INTERFACE_ADY 5 #define SER_INTERFACE_LEGO 6 +#define SER_INTERFACE_FTC 7 int ser_init(); diff --git a/src/device/main_m4/src/serial.cpp b/src/device/main_m4/src/serial.cpp index b993d534..822ba48a 100644 --- a/src/device/main_m4/src/serial.cpp +++ b/src/device/main_m4/src/serial.cpp @@ -203,11 +203,213 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen) } } +uint16_t ftc_getData(uint8_t *buf, uint32_t buflen) +{ + uint8_t c; + uint16_t d; + uint16_t numBlobs; + uint16_t numBlobsMax; + uint32_t temp, width, height; + Iserial *serial = ser_getSerial(); + + if (serial->receive(&c, 1)==0) + return 0; + +#if 1 + if (c==0x00) + { + //printf("0\n"); + char *str = "V2.1"; + strcpy((char *)buf, str); + return 5; + //return strlen((char *)str); + } + if (c==0x08) + { + //printf("8\n"); + char *str = "Pixy"; + strcpy((char *)buf, str); + return 5; + //return strlen((char *)str); + } + else if (c==0x10) + { + //printf("10\n"); + char *str = "FTC"; + strcpy((char *)buf, str); + return 4; + //return strlen((char *)str); + } + else +#endif + if (c==0x50) + { + BlobB *max; +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; + buf[6] = 7; + buf[7] = 8; +#else + max = (BlobB *)g_blobs->getMaxBlob(); + if (max==0) + memset(buf, 0, 7); + else if (max==(BlobB *)-1) + memset(buf, -1, 7); + else + { + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + *(uint16_t *)buf = max->m_model; // signature + temp = ((max->m_left + width/2)*819)>>10; + buf[2] = temp; // x + buf[3] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[4] = temp; // width + buf[5] = height; // height + if (max->m_model>CL_NUM_SIGNATURES) + { + temp = ((int32_t)max->m_angle*91)>>7; + g_angle = temp; + }else{ + g_angle = 255; + } + buf[6] = g_angle; + } +#endif + return 7; + } + else if (c==0x60) + { + buf[0] = g_angle; + return 1; + } + else if (c>=0x51 && c<=0x57) + { +#if 0 + buf[0] = 1; + buf[1] = 1; + buf[2] = 2; + buf[3] = 3; + buf[4] = 4; + buf[5] = 1; + buf[6] = 2; + buf[7] = 3; + buf[8] = 4; + buf[9] = 1; + buf[10] = 2; + buf[11] = 3; + buf[12] = 4; + buf[13] = 1; + buf[14] = 2; + buf[15] = 3; + buf[16] = 4; + buf[17] = 1; + buf[18] = 2; + buf[19] = 3; + buf[20] = 4; + buf[21] = 1; + buf[22] = 2; + buf[23] = 3; + buf[24] = 4; + +#else + numBlobsMax = 6; //we have a 27 byte buffer on the MR Dim so only return top 6 + BlobA *max; + max = g_blobs->getMaxBlob(c-0x50, &numBlobs); + if (max==0) + memset(buf, 0, 24); + else if (max==(BlobA *)-1) + memset(buf, -1, 24); + else + { + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + buf[0] = numBlobs; // number of blocks that match signature + temp = ((max->m_left + width/2)*819)>>10; + buf[1] = temp; // x + buf[2] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[3] = temp; // width + buf[4] = height; // height + } +#endif + return 5; + } + else if (c==0x58) + { + BlobB *max; + if (serial->receive((uint8_t *)&d, 2)<2) // receive cc signature to look for + return 0; +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; +#else + max = (BlobB *)g_blobs->getMaxBlob(d, &numBlobs); + if (max==0) + memset(buf, 0, 6); + else if (max==(BlobB *)-1) + memset(buf, -1, 6); + else + { + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + buf[0] = numBlobs; // number of cc blocks that match + temp = ((max->m_left + width/2)*819)>>10; + buf[1] = temp; // x + buf[2] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[3] = temp; // width + buf[4] = height; // height + temp = ((int32_t)max->m_angle*91)>>7; + buf[5] = temp; // angle + } +#endif + return 6; + } + else + { +#if 0 + static uint8_t c = 0; + + buf[0] = c++; +#else + //printf("%x\n", c); + + if (c==0x42) // this works in port view mode on the ev3's LCD + { + BlobA *max; + max = g_blobs->getMaxBlob(); + if (max==0 || max==(BlobA *)-1) + buf[0] = 0; + else + { + width = max->m_right - max->m_left; + temp = ((max->m_left + width/2)*819)>>10; + buf[0] = temp; + } + } + else + buf[0] = 1; // need to return nonzero value for other inquiries or LEGO brick will think we're an analog sensor + +#endif + return 1; + } +} + uint32_t callback(uint8_t *data, uint32_t len) { if (g_interface==SER_INTERFACE_LEGO) - return lego_getData(data, len); + return ftc_getData(data, len); else return g_blobs->getBlock(data, len); } @@ -290,7 +492,7 @@ int ser_setInterface(uint8_t interface) case SER_INTERFACE_LEGO: g_serial = g_i2c0; - g_i2c0->setSlaveAddr(0x01); + //g_i2c0->setSlaveAddr(0x01); g_i2c0->setFlags(true, false); break; From d7dfd3130b0c83ff8b4b9d437d0edac89f665b9a Mon Sep 17 00:00:00 2001 From: Andrewiski Date: Thu, 30 Nov 2017 11:15:48 -0500 Subject: [PATCH 2/5] Added i2c 0x70 Added i2c 0x70 request for Multiple Signatures in one call not just greatest, If testing is success ful will add for all signature types fot FTC 27 byte limit. --- src/common/inc/blobs.h | 4 + src/common/src/blobs.cpp | 2270 +++++++++++++++-------------- src/device/main_m4/src/serial.cpp | 272 +--- 3 files changed, 1259 insertions(+), 1287 deletions(-) diff --git a/src/common/inc/blobs.h b/src/common/inc/blobs.h index a9069f99..16fb25f8 100644 --- a/src/common/inc/blobs.h +++ b/src/common/inc/blobs.h @@ -20,6 +20,7 @@ #include "pixytypes.h" #include "colorlut.h" #include "qqueue.h" +#include //added to get sort by area functionality #define MAX_BLOBS 100 #define MAX_BLOBS_PER_MODEL 20 @@ -29,6 +30,7 @@ #define MAX_CODED_DIST 8 #define MAX_COLOR_CODE_MODELS 5 + #define BL_BEGIN_MARKER 0xaa55 #define BL_BEGIN_MARKER_CC 0xaa56 @@ -49,6 +51,7 @@ class Blobs uint16_t getBlock(uint8_t *buf, uint32_t buflen); uint16_t getCCBlock(uint8_t *buf, uint32_t buflen); BlobA *getMaxBlob(uint16_t signature=0, uint16_t *numBlobs=NULL); + void getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlobs, uint16_t *numBlobs); void getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen); int setParams(uint16_t maxBlobs, uint16_t maxBlobsPerModel, uint32_t minArea, ColorCodeMode ccMode); int runlengthAnalysis(); @@ -69,6 +72,7 @@ class Blobs bool closeby(BlobA *blob0, BlobA *blob1); int16_t distance(BlobA *blob0, BlobA *blob1); void sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz); + //static int compareBlobAreas(const void *a, const void *b); int16_t angle(BlobA *blob0, BlobA *blob1); int16_t distance(BlobA *blob0, BlobA *blob1, bool horiz); void processCC(); diff --git a/src/common/src/blobs.cpp b/src/common/src/blobs.cpp index 8e7451d6..e9ca8bc1 100644 --- a/src/common/src/blobs.cpp +++ b/src/common/src/blobs.cpp @@ -1,1118 +1,1216 @@ -// -// begin license header -// -// This file is part of Pixy CMUcam5 or "Pixy" for short -// -// All Pixy source code is provided under the terms of the -// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). -// Those wishing to use Pixy source code, software and/or -// technologies under different licensing terms should contact us at -// cmucam@cs.cmu.edu. Such licensing terms are available for -// all portions of the Pixy codebase presented here. -// -// end license header -// - -#ifndef PIXY -#include "debug.h" -#else -#include "pixy_init.h" -#endif - -#include "blobs.h" - -#define CC_SIGNATURE(s) (m_ccMode==CC_ONLY || m_clut.getType(s)==CL_MODEL_TYPE_COLORCODE) - -Blobs::Blobs(Qqueue *qq, uint8_t *lut) : m_clut(lut) -{ - int i; - - m_mutex = false; - m_minArea = MIN_AREA; - m_maxBlobs = MAX_BLOBS; - m_maxBlobsPerModel = MAX_BLOBS_PER_MODEL; - m_mergeDist = MAX_MERGE_DIST; - m_maxBlob = NULL; - - m_qq = qq; -#ifdef PIXY - m_maxCodedDist = MAX_CODED_DIST; -#else - m_maxCodedDist = MAX_CODED_DIST/2; - m_qvals = new uint32_t[0x8000]; -#endif - m_ccMode = DISABLED; - - m_blobs = new uint16_t[MAX_BLOBS*5]; - m_numBlobs = 0; - m_blobReadIndex = 0; - m_ccBlobReadIndex = 0; - - // reset blob assemblers - for (i=0; idequeue(&qval)==0); - if (qval.m_col>=0xfffe) - break; - if (res<0) - continue; - if (qval.m_col==0) - { - prevStartCol = 0xffff; - prevSig = 0; - if (segmentSig) - { - res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); - segmentSig = 0; - } - row++; -#ifndef PIXY - m_qvals[m_numQvals++] = 0; -#else - if (icount++==5) // an interleave of every 5 lines or about every 175us seems good - { - g_chirpUsb->service(); - icount = 0; - } -#endif - continue; - } - - sig = qval.m_col&0x07; - - u = qval.m_u; - v = qval.m_v; - - u <<= CL_LUT_ENTRY_SCALE; - v <<= CL_LUT_ENTRY_SCALE; - c = qval.m_y; - if (c==0) - c = 1; - u /= c; - v /= c; - - if (m_clut.m_runtimeSigs[sig-1].m_uMin=(int32_t)m_clut.m_miny) - { - qval.m_col >>= 3; - startCol = qval.m_col; - merge = startCol-prevStartCol<=5 && prevSig==sig; - if (segmentSig==0 && merge) - { - segmentSig = sig; - segmentStartCol = prevStartCol; - } - else if (segmentSig!=0 && (segmentSig!=sig || !merge)) - { - res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); - segmentSig = 0; - } - - if (segmentSig!=0 && merge) - segmentEndCol = startCol; - else if (segmentSig==0 && !merge) - res = handleSegment(sig, row, startCol-1, 2); - prevSig = sig; - prevStartCol = startCol; - } - else if (segmentSig!=0) - { - res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); - segmentSig = 0; - } - } - endFrame(); - - if (qval.m_col==0xfffe) // error code, queue overrun - return -1; - return 0; -} - -int Blobs::blobify() -{ - uint32_t i, j, k; - bool colorCode; - CBlob *blob; - uint16_t *blobsStart; - uint16_t numBlobsStart, invalid, invalid2; - uint16_t left, top, right, bottom; - //uint32_t timer, timer2=0; - - if (runlengthAnalysis()<0) - { - for (i=0; inext, k++) - { - if ((colorCode && blob->GetArea()GetArea()<(int)m_minArea)) - continue; - blob->getBBox((short &)left, (short &)top, (short &)right, (short &)bottom); - if (bottom-top<=1) // blobs that are 1 line tall - continue; - m_blobs[j + 0] = i+1; - m_blobs[j + 1] = left; - m_blobs[j + 2] = right; - m_blobs[j + 3] = top; - m_blobs[j + 4] = bottom; - m_numBlobs++; - j += 5; - - } - //setTimer(&timer); - if (!colorCode) // do not combine color code models - { - while(1) - { - invalid2 = combine2(blobsStart, m_numBlobs-numBlobsStart); - if (invalid2==0) - break; - invalid += invalid2; - } - } - //timer2 += getTimer(timer); - } - //setTimer(&timer); - invalid += combine(m_blobs, m_numBlobs); - if (m_ccMode!=DISABLED) - { - m_ccBlobs = (BlobB *)(m_blobs + m_numBlobs*5); - // calculate number of codedblobs left - processCC(); - } - if (invalid || m_ccMode!=DISABLED) - { - invalid2 = compress(m_blobs, m_numBlobs); - m_numBlobs -= invalid2; - } - //timer2 += getTimer(timer); - //cprintf("time=%d\n", timer2); // never seen this greater than 200us. or 1% of frame period - - // reset read indexes-- new frame - m_blobReadIndex = 0; - m_ccBlobReadIndex = 0; - m_mutex = false; - - // free memory - for (i=0; i0) - cprintf("%d: blobs %d %d %d %d %d\n", frame, m_numBlobs, m_blobs[1], m_blobs[2], m_blobs[3], m_blobs[4]); - else - cprintf("%d: blobs 0\n", frame); - frame++; -#endif - return 0; -} - -#ifndef PIXY -void Blobs::getRunlengths(uint32_t **qvals, uint32_t *len) -{ - *qvals = m_qvals; - *len = m_numQvals; -} -#endif - -uint16_t Blobs::getCCBlock(uint8_t *buf, uint32_t buflen) -{ - uint16_t *buf16 = (uint16_t *)buf; - uint16_t temp, width, height; - uint16_t checksum; - uint16_t len = 8; // default - - if (buflen<9*sizeof(uint16_t)) - return 0; - +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// + +#ifndef PIXY +#include "debug.h" +#else +#include "pixy_init.h" +#endif + +#include "blobs.h" +#include //added to get sort by area functionality + +#define CC_SIGNATURE(s) (m_ccMode==CC_ONLY || m_clut.getType(s)==CL_MODEL_TYPE_COLORCODE) + +Blobs::Blobs(Qqueue *qq, uint8_t *lut) : m_clut(lut) +{ + int i; + + m_mutex = false; + m_minArea = MIN_AREA; + m_maxBlobs = MAX_BLOBS; + m_maxBlobsPerModel = MAX_BLOBS_PER_MODEL; + m_mergeDist = MAX_MERGE_DIST; + m_maxBlob = NULL; + + m_qq = qq; +#ifdef PIXY + m_maxCodedDist = MAX_CODED_DIST; +#else + m_maxCodedDist = MAX_CODED_DIST/2; + m_qvals = new uint32_t[0x8000]; +#endif + m_ccMode = DISABLED; + + m_blobs = new uint16_t[MAX_BLOBS*5]; + m_numBlobs = 0; + m_blobReadIndex = 0; + m_ccBlobReadIndex = 0; + + // reset blob assemblers + for (i=0; idequeue(&qval)==0); + if (qval.m_col>=0xfffe) + break; + if (res<0) + continue; + if (qval.m_col==0) + { + prevStartCol = 0xffff; + prevSig = 0; + if (segmentSig) + { + res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); + segmentSig = 0; + } + row++; +#ifndef PIXY + m_qvals[m_numQvals++] = 0; +#else + if (icount++==5) // an interleave of every 5 lines or about every 175us seems good + { + g_chirpUsb->service(); + icount = 0; + } +#endif + continue; + } + + sig = qval.m_col&0x07; + + u = qval.m_u; + v = qval.m_v; + + u <<= CL_LUT_ENTRY_SCALE; + v <<= CL_LUT_ENTRY_SCALE; + c = qval.m_y; + if (c==0) + c = 1; + u /= c; + v /= c; + + if (m_clut.m_runtimeSigs[sig-1].m_uMin=(int32_t)m_clut.m_miny) + { + qval.m_col >>= 3; + startCol = qval.m_col; + merge = startCol-prevStartCol<=5 && prevSig==sig; + if (segmentSig==0 && merge) + { + segmentSig = sig; + segmentStartCol = prevStartCol; + } + else if (segmentSig!=0 && (segmentSig!=sig || !merge)) + { + res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); + segmentSig = 0; + } + + if (segmentSig!=0 && merge) + segmentEndCol = startCol; + else if (segmentSig==0 && !merge) + res = handleSegment(sig, row, startCol-1, 2); + prevSig = sig; + prevStartCol = startCol; + } + else if (segmentSig!=0) + { + res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); + segmentSig = 0; + } + } + endFrame(); + + if (qval.m_col==0xfffe) // error code, queue overrun + return -1; + return 0; +} + +int Blobs::blobify() +{ + uint32_t i, j, k; + bool colorCode; + CBlob *blob; + uint16_t *blobsStart; + uint16_t numBlobsStart, invalid, invalid2; + uint16_t left, top, right, bottom; + //uint32_t timer, timer2=0; + + if (runlengthAnalysis()<0) + { + for (i=0; inext, k++) + { + if ((colorCode && blob->GetArea()GetArea()<(int)m_minArea)) + continue; + blob->getBBox((short &)left, (short &)top, (short &)right, (short &)bottom); + if (bottom-top<=1) // blobs that are 1 line tall + continue; + m_blobs[j + 0] = i+1; + m_blobs[j + 1] = left; + m_blobs[j + 2] = right; + m_blobs[j + 3] = top; + m_blobs[j + 4] = bottom; + m_numBlobs++; + j += 5; + + } + //setTimer(&timer); + if (!colorCode) // do not combine color code models + { + while(1) + { + invalid2 = combine2(blobsStart, m_numBlobs-numBlobsStart); + if (invalid2==0) + break; + invalid += invalid2; + } + } + //timer2 += getTimer(timer); + } + //setTimer(&timer); + invalid += combine(m_blobs, m_numBlobs); + if (m_ccMode!=DISABLED) + { + m_ccBlobs = (BlobB *)(m_blobs + m_numBlobs*5); + // calculate number of codedblobs left + processCC(); + } + if (invalid || m_ccMode!=DISABLED) + { + invalid2 = compress(m_blobs, m_numBlobs); + m_numBlobs -= invalid2; + } + //timer2 += getTimer(timer); + //cprintf("time=%d\n", timer2); // never seen this greater than 200us. or 1% of frame period + + // reset read indexes-- new frame + m_blobReadIndex = 0; + m_ccBlobReadIndex = 0; + m_mutex = false; + + // free memory + for (i=0; i0) + cprintf("%d: blobs %d %d %d %d %d\n", frame, m_numBlobs, m_blobs[1], m_blobs[2], m_blobs[3], m_blobs[4]); + else + cprintf("%d: blobs 0\n", frame); + frame++; +#endif + return 0; +} + +#ifndef PIXY +void Blobs::getRunlengths(uint32_t **qvals, uint32_t *len) +{ + *qvals = m_qvals; + *len = m_numQvals; +} +#endif + +uint16_t Blobs::getCCBlock(uint8_t *buf, uint32_t buflen) +{ + uint16_t *buf16 = (uint16_t *)buf; + uint16_t temp, width, height; + uint16_t checksum; + uint16_t len = 8; // default + + if (buflen<9*sizeof(uint16_t)) + return 0; + if (m_mutex || m_ccBlobReadIndex>=m_numCCBlobs) // we're copying, so no CC blocks for now.... - return 0; - - if (m_blobReadIndex==0 && m_ccBlobReadIndex==0) // beginning of frame, mark it with empty block - { - buf16[0] = BL_BEGIN_MARKER; - len++; - buf16++; - } - - // beginning of block - buf16[0] = BL_BEGIN_MARKER_CC; - - // model - temp = m_ccBlobs[m_ccBlobReadIndex].m_model; - checksum = temp; - buf16[2] = temp; - - // width - width = m_ccBlobs[m_ccBlobReadIndex].m_right - m_ccBlobs[m_ccBlobReadIndex].m_left; - checksum += width; - buf16[5] = width; - - // height - height = m_ccBlobs[m_ccBlobReadIndex].m_bottom - m_ccBlobs[m_ccBlobReadIndex].m_top; - checksum += height; - buf16[6] = height; - - // x center - temp = m_ccBlobs[m_ccBlobReadIndex].m_left + width/2; - checksum += temp; - buf16[3] = temp; - - // y center - temp = m_ccBlobs[m_ccBlobReadIndex].m_top + height/2; - checksum += temp; - buf16[4] = temp; - - temp = m_ccBlobs[m_ccBlobReadIndex].m_angle; - checksum += temp; - buf16[7] = temp; - - buf16[1] = checksum; - - // next blob - m_ccBlobReadIndex++; - - return len*sizeof(uint16_t); -} - - -uint16_t Blobs::getBlock(uint8_t *buf, uint32_t buflen) -{ - uint16_t *buf16 = (uint16_t *)buf; - uint16_t temp, width, height; - uint16_t checksum; - uint16_t len = 7; // default - int i = m_blobReadIndex*5; - - if (buflen<8*sizeof(uint16_t)) - return 0; - - if (m_blobReadIndex>=m_numBlobs && m_ccMode!=DISABLED) - return getCCBlock(buf, buflen); - - if (m_mutex || m_blobReadIndex>=m_numBlobs) // we're copying, so no blocks for now.... return 0; - - if (m_blobReadIndex==0) // beginning of frame, mark it with empty block - { - buf16[0] = BL_BEGIN_MARKER; - len++; - buf16++; - } - - // beginning of block - buf16[0] = BL_BEGIN_MARKER; - - // model - temp = m_blobs[i]; - checksum = temp; - buf16[2] = temp; - - // width - width = m_blobs[i+2] - m_blobs[i+1]; - checksum += width; - buf16[5] = width; - - // height - height = m_blobs[i+4] - m_blobs[i+3]; - checksum += height; - buf16[6] = height; - - // x center - temp = m_blobs[i+1] + width/2; - checksum += temp; - buf16[3] = temp; - - // y center - temp = m_blobs[i+3] + height/2; - checksum += temp; - buf16[4] = temp; - - buf16[1] = checksum; - - // next blob - m_blobReadIndex++; - - return len*sizeof(uint16_t); -} - - -BlobA *Blobs::getMaxBlob(uint16_t signature, uint16_t *numBlobs) -{ + + if (m_blobReadIndex==0 && m_ccBlobReadIndex==0) // beginning of frame, mark it with empty block + { + buf16[0] = BL_BEGIN_MARKER; + len++; + buf16++; + } + + // beginning of block + buf16[0] = BL_BEGIN_MARKER_CC; + + // model + temp = m_ccBlobs[m_ccBlobReadIndex].m_model; + checksum = temp; + buf16[2] = temp; + + // width + width = m_ccBlobs[m_ccBlobReadIndex].m_right - m_ccBlobs[m_ccBlobReadIndex].m_left; + checksum += width; + buf16[5] = width; + + // height + height = m_ccBlobs[m_ccBlobReadIndex].m_bottom - m_ccBlobs[m_ccBlobReadIndex].m_top; + checksum += height; + buf16[6] = height; + + // x center + temp = m_ccBlobs[m_ccBlobReadIndex].m_left + width/2; + checksum += temp; + buf16[3] = temp; + + // y center + temp = m_ccBlobs[m_ccBlobReadIndex].m_top + height/2; + checksum += temp; + buf16[4] = temp; + + temp = m_ccBlobs[m_ccBlobReadIndex].m_angle; + checksum += temp; + buf16[7] = temp; + + buf16[1] = checksum; + + // next blob + m_ccBlobReadIndex++; + + return len*sizeof(uint16_t); +} + + +uint16_t Blobs::getBlock(uint8_t *buf, uint32_t buflen) +{ + uint16_t *buf16 = (uint16_t *)buf; + uint16_t temp, width, height; + uint16_t checksum; + uint16_t len = 7; // default + int i = m_blobReadIndex*5; + + if (buflen<8*sizeof(uint16_t)) + return 0; + + if (m_blobReadIndex>=m_numBlobs && m_ccMode!=DISABLED) + return getCCBlock(buf, buflen); + + if (m_mutex || m_blobReadIndex>=m_numBlobs) // we're copying, so no blocks for now.... + return 0; + + if (m_blobReadIndex==0) // beginning of frame, mark it with empty block + { + buf16[0] = BL_BEGIN_MARKER; + len++; + buf16++; + } + + // beginning of block + buf16[0] = BL_BEGIN_MARKER; + + // model + temp = m_blobs[i]; + checksum = temp; + buf16[2] = temp; + + // width + width = m_blobs[i+2] - m_blobs[i+1]; + checksum += width; + buf16[5] = width; + + // height + height = m_blobs[i+4] - m_blobs[i+3]; + checksum += height; + buf16[6] = height; + + // x center + temp = m_blobs[i+1] + width/2; + checksum += temp; + buf16[3] = temp; + + // y center + temp = m_blobs[i+3] + height/2; + checksum += temp; + buf16[4] = temp; + + buf16[1] = checksum; + + // next blob + m_blobReadIndex++; + + return len*sizeof(uint16_t); +} + + +BlobA *Blobs::getMaxBlob(uint16_t signature, uint16_t *numBlobs) +{ int i; - uint16_t blobs; - uint32_t area, maxArea; - BlobA *blob, *maxBlob; + uint16_t blobs; + uint32_t area, maxArea; + BlobA *blob, *maxBlob; BlobB *ccBlob; - + if (m_mutex) return (BlobA *)-1; // busy! - - if (signature==0) // 0 means return the biggest regardless of signature number + + if (signature==0) // 0 means return the biggest regardless of signature number { if (numBlobs) *numBlobs = 1; // not really used in this mode, so return 1 - - // if we've already found it, return it - if (m_maxBlob) - return m_maxBlob; - - // look through all blobs looking for the blob with the biggest area - for (i=0, maxArea=0; im_right - blob->m_left)*(blob->m_bottom - blob->m_top); - if (area>maxArea) - { - maxArea = area; - m_maxBlob = blob; - } - } - for (i=0; im_right - ccBlob->m_left)*(ccBlob->m_bottom - ccBlob->m_top); - if (area>maxArea) - { - maxArea = area; - m_maxBlob = (BlobA *)ccBlob; - } - } - return m_maxBlob; + + // if we've already found it, return it + if (m_maxBlob) + return m_maxBlob; + + // look through all blobs looking for the blob with the biggest area + for (i=0, maxArea=0; im_right - blob->m_left)*(blob->m_bottom - blob->m_top); + if (area>maxArea) + { + maxArea = area; + m_maxBlob = blob; + } + } + for (i=0; im_right - ccBlob->m_left)*(ccBlob->m_bottom - ccBlob->m_top); + if (area>maxArea) + { + maxArea = area; + m_maxBlob = (BlobA *)ccBlob; + } + } + return m_maxBlob; } - // look for a specific signature - else + // look for a specific signature + else { // regular signature if (signature<=CL_NUM_SIGNATURES) - { - for (i=0, blobs=0, maxBlob=NULL; im_model==signature) { if (!maxBlob) maxBlob = blob; blobs++; // count - } + } } } // color code else { for (i=0, blobs=0, maxBlob=NULL; im_model==signature) { if (!maxBlob) maxBlob = (BlobA *)ccBlob; blobs++; // count } - } - } + } + } if (numBlobs) *numBlobs = blobs; - return maxBlob; - } - - return NULL; // no blobs... -} - -void Blobs::getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen) -{ - *blobs = (BlobA *)m_blobs; - *len = m_numBlobs; - - *ccBlobs = m_ccBlobs; - *ccLen = m_numCCBlobs; -} - - - -uint16_t Blobs::compress(uint16_t *blobs, uint16_t numBlobs) -{ - uint16_t i, ii; - uint16_t *destination, invalid; - - // compress list - for (i=0, ii=0, destination=NULL, invalid=0; i=right && top0<=top && bottom0>=bottom) - { - blobs[jj+0] = 0; // invalidate - invalid++; - } - else if (left<=left0 && right>=right0 && top<=top0 && bottom>=bottom0) - { - blobs[ii+0] = 0; // invalidate - invalid++; - } - } - } - - return invalid; -} - -uint16_t Blobs::combine2(uint16_t *blobs, uint16_t numBlobs) -{ - uint16_t i, j, ii, jj, left0, right0, top0, bottom0; - uint16_t left, right, top, bottom; - uint16_t invalid; - - for (i=0, ii=0, invalid=0; i=right0 && left-right0<=m_mergeDist && - ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0))) - { - blobs[ii+2] = right; - blobs[jj+0] = 0; // invalidate - invalid++; - } - else if (top<=top0 && top0-bottom<=m_mergeDist && - ((left0<=left && left<=right0) || (left0<=right && right<=right0))) - { - blobs[ii+3] = top; - blobs[jj+0] = 0; // invalidate - invalid++; - } - else if (bottom>=bottom0 && top-bottom0<=m_mergeDist && - ((left0<=left && left<=right0) || (left0<=right && right<=right0))) - { - blobs[ii+4] = bottom; - blobs[jj+0] = 0; // invalidate - invalid++; - } -#else // at least half of a side (the smaller adjacent side) has to overlap - if (left<=left0 && left0-right<=m_mergeDist && - ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom))) - { - blobs[ii+1] = left; - blobs[jj+0] = 0; // invalidate - invalid++; - } - else if (right>=right0 && left-right0<=m_mergeDist && - ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom))) - { - blobs[ii+2] = right; - blobs[jj+0] = 0; // invalidate - invalid++; - } - else if (top<=top0 && top0-bottom<=m_mergeDist && - ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right))) - { - blobs[ii+3] = top; - blobs[jj+0] = 0; // invalidate - invalid++; - } - else if (bottom>=bottom0 && top-bottom0<=m_mergeDist && - ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right))) - { - blobs[ii+4] = bottom; - blobs[jj+0] = 0; // invalidate - invalid++; - } -#endif - } - } - - return invalid; -} - -int16_t Blobs::distance(BlobA *blob0, BlobA *blob1) -{ - int16_t left0, right0, top0, bottom0; - int16_t left1, right1, top1, bottom1; - - left0 = blob0->m_left; - right0 = blob0->m_right; - top0 = blob0->m_top; - bottom0 = blob0->m_bottom; - left1 = blob1->m_left; - right1 = blob1->m_right; - top1 = blob1->m_top; - bottom1 = blob1->m_bottom; - - if (left0>=left1 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0)))) - return left0-right1; - - if (left1>=left0 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0)))) - return left1-right0; - - if (top0>=top1 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0)))) - return top0-bottom1; - - if (top1>=top0 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0)))) - return top1-bottom0; - - return 0x7fff; // return a large number -} - -bool Blobs::closeby(BlobA *blob0, BlobA *blob1) -{ - // check to see if blobs are invalid or equal - if (blob0->m_model==0 || blob1->m_model==0 || blob0->m_model==blob1->m_model) - return false; - // check to see that the blobs are from color code models. If they aren't both - // color code blobs, we return false - if (!CC_SIGNATURE(blob0->m_model&0x07) || !CC_SIGNATURE(blob1->m_model&0x07)) - return false; - - return distance(blob0, blob1)<=m_maxCodedDist; -} - -int16_t Blobs::distance(BlobA *blob0, BlobA *blob1, bool horiz) -{ - int16_t dist; - - if (horiz) - dist = (blob0->m_right+blob0->m_left)/2 - (blob1->m_right+blob1->m_left)/2; - else - dist = (blob0->m_bottom+blob0->m_top)/2 - (blob1->m_bottom+blob1->m_top)/2; - - if (dist<0) - return -dist; - else - return dist; -} - -int16_t Blobs::angle(BlobA *blob0, BlobA *blob1) -{ - int acx, acy, bcx, bcy; - float res; - - acx = (blob0->m_right + blob0->m_left)/2; - acy = (blob0->m_bottom + blob0->m_top)/2; - bcx = (blob1->m_right + blob1->m_left)/2; - bcy = (blob1->m_bottom + blob1->m_top)/2; - - res = atan2((float)(acy-bcy), (float)(bcx-acx))*180/3.1415f; - - return (int16_t)res; -} - -void Blobs::sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz) -{ - uint16_t i, td, distances[MAX_COLOR_CODE_MODELS*2]; - bool done; - BlobA *tb; - - // create list of distances - for (i=0; idistances[i]) - { - // swap distances - td = distances[i]; - distances[i] = distances[i-1]; - distances[i-1] = td; - // swap blobs - tb = blobs[i]; - blobs[i] = blobs[i-1]; - blobs[i-1] = tb; - - done = false; - } - } - if (done) - break; - } -} - -bool Blobs::analyzeDistances(BlobA *blobs0[], int16_t numBlobs0, BlobA *blobs[], int16_t numBlobs, BlobA **blobA, BlobA **blobB) -{ - bool skip; - bool result = false; - int16_t dist, minDist, i, j, k; - - for (i=0, minDist=0x7fff; im_model&0x07)==(blobs[j]->m_model&0x07)) - { - skip = true; - break; - } - } - if (skip) - continue; - dist = distance(blobs0[i], blobs[j]); - if (distm_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top); - lowerArea = (area0*100)/(100+TOL); - upperArea = area0 + (area0*TOL)/100; - - for (j=0, numEqual=0; j<*numBlobs; j++) - { - if (i==j) - continue; - area1 = (blobs[j]->m_right-blobs[j]->m_left) * (blobs[j]->m_bottom-blobs[j]->m_top); - if (lowerArea<=area1 && area1<=upperArea) - numEqual++; - } - if (numEqual>maxEqual) - { - maxEqual = numEqual; - maxEqualArea = area0; - set = true; - } - } - - if (!set) - *numBlobs = 0; - - for (i=0, numNewBlobs=0; i<*numBlobs && numNewBlobsm_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top); - lowerArea = (area0*100)/(100+TOL); - upperArea = area0 + (area0*TOL)/100; - if (lowerArea<=maxEqualArea && maxEqualArea<=upperArea) - newBlobs[numNewBlobs++] = blobs[i]; -#ifndef PIXY - else if (*numBlobs>=5 && (blobs[i]->m_model&0x07)==2) - DBG("eliminated!"); -#endif - } - - // copy new blobs over - for (i=0; im_model&0x07)==(blobs[i]->m_model&0x07)) - set = true; - else - break; - } - } - if (set) - { - // copy new blobs over - for (i=0; im_model<=CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES) - { - count++; - scount = count<<3; - blob0->m_model |= scount; - blob1->m_model |= scount; - } - else if (blob0->m_model>CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES) - { - scount = blob0->m_model & ~0x07; - blob1->m_model |= scount; - } - else if (blob1->m_model>CL_NUM_SIGNATURES && blob0->m_model<=CL_NUM_SIGNATURES) - { - scount = blob1->m_model & ~0x07; - blob0->m_model |= scount; - } - } - } - } - -#if 1 - // 2nd pass: merge blob clumps - for (blob0=(BlobA *)m_blobs; blob0m_model<=CL_NUM_SIGNATURES) // skip normal blobs - continue; - scount = blob0->m_model&~0x07; - for (blob1=(BlobA *)blob0+1; blob1m_model<=CL_NUM_SIGNATURES) - continue; - - scount1 = blob1->m_model&~0x07; - if (scount!=scount1 && closeby(blob0, blob1)) - mergeClumps(scount, scount1); - } - } -#endif - - // 3rd and final pass, find each blob clean it up and add it to the table - endBlobB = (BlobB *)((BlobA *)m_blobs + MAX_BLOBS)-1; - for (i=1, codedBlob = m_ccBlobs, m_numCCBlobs=0; i<=count && codedBlobm_model&~0x07)==scount) - blobs[j++] = blob0; - } - -#if 1 - // cleanup blobs, deal with cases where there are more blobs than models - cleanup(blobs, &j); -#endif - - if (j<2) - continue; - - // find left, right, top, bottom of color coded block - for (k=0, left=right=top=bottom=avgWidth=avgHeight=0; km_model, blobs[k]->m_left, blobs[k]->m_right, blobs[k]->m_top, blobs[k]->m_bottom); - if (blobs[left]->m_left > blobs[k]->m_left) - left = k; - if (blobs[top]->m_top > blobs[k]->m_top) - top = k; - if (blobs[right]->m_right < blobs[k]->m_right) - right = k; - if (blobs[bottom]->m_bottom < blobs[k]->m_bottom) - bottom = k; - avgWidth += blobs[k]->m_right - blobs[k]->m_left; - avgHeight += blobs[k]->m_bottom - blobs[k]->m_top; - } - avgWidth /= j; - avgHeight /= j; - codedBlob->m_left = blobs[left]->m_left; - codedBlob->m_right = blobs[right]->m_right; - codedBlob->m_top = blobs[top]->m_top; - codedBlob->m_bottom = blobs[bottom]->m_bottom; - -#if 1 - // is it more horizontal than vertical? - width = (blobs[right]->m_right - blobs[left]->m_left)*100; - width /= avgWidth; // scale by average width because our swatches might not be square - height = (blobs[bottom]->m_bottom - blobs[top]->m_top)*100; - height /= avgHeight; // scale by average height because our swatches might not be square - - if (width > height) - sort(blobs, j, blobs[left], true); - else - sort(blobs, j, blobs[top], false); - -#if 1 - cleanup2(blobs, &j); - if (j<2) - continue; - else if (j>5) - j = 5; -#endif - // create new blob, compare the coded models, pick the smaller one - for (k=0, codedModel0=0; km_model&0x07; - } - for (k=j-1, codedModel=0; k>=0; k--) - { - codedModel <<= 3; - codedModel |= blobs[k]->m_model&0x07; - blobs[k]->m_model = 0; // invalidate - } - - if (codedModel0m_model = codedModel0; - codedBlob->m_angle = angle(blobs[0], blobs[j-1]); - } - else - { - codedBlob->m_model = codedModel; - codedBlob->m_angle = angle(blobs[j-1], blobs[0]); - } -#endif - //DBG("cc %d %d %d %d %d", m_numCCBlobs, codedBlob->m_left, codedBlob->m_right, codedBlob->m_top, codedBlob->m_bottom); - codedBlob++; - m_numCCBlobs++; - } - - // 3rd pass, invalidate blobs - for (blob0=(BlobA *)m_blobs; blob0m_model>CL_NUM_SIGNATURES) - blob0->m_model = 0; - } - else if (blob0->m_model>CL_NUM_SIGNATURES || CC_SIGNATURE(blob0->m_model)) - blob0->m_model = 0; // invalidate-- not part of a color code - } -} - -void Blobs::endFrame() -{ - int i; - for (i=0; im_right - ia->m_left)*(ia->m_bottom - ia->m_top); + int areaB = (ib->m_right - ib->m_left)*(ib->m_bottom - ib->m_top); + if (areaA < areaB) + return -1; + else if (areaA > areaB) + return 1; + else + return 0; + +} + +void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlobs, uint16_t *numBlobs) +{ + int i; + uint16_t blobs; + BlobA *blob; + BlobB *ccBlob; + + if (m_mutex){ + *numBlobs = 0; + return; + } + + if (signature==0) // 0 means return the top maxNumBlobs largest area of and signature number <= 7 + { + //sort by Area + qsort(m_blobs, m_numBlobs, sizeof (BlobA), &compareBlobAreas); + for (i=0; i 7 + { + //sort by Area + qsort(m_ccBlobs, m_numCCBlobs, sizeof (BlobB), &compareBlobAreas); + for (i=0; im_model==signature) + { + if (blobs < maxNumBlobs) + maxBlobs[blobs] = blob; + blobs++; // count + } + } + + } + // color code + else + { + qsort(m_ccBlobs, m_numCCBlobs, sizeof (BlobB), &compareBlobAreas); + for (i=0, blobs=0; im_model==signature) + { + if (blobs < maxNumBlobs) + maxBlobs[blobs] =(BlobA *)ccBlob; + blobs++; // count + } + } + } + + *numBlobs = blobs; + return; + } + + + *numBlobs = 0; + return; +} + + + +void Blobs::getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen) +{ + *blobs = (BlobA *)m_blobs; + *len = m_numBlobs; + + *ccBlobs = m_ccBlobs; + *ccLen = m_numCCBlobs; +} + + + +uint16_t Blobs::compress(uint16_t *blobs, uint16_t numBlobs) +{ + uint16_t i, ii; + uint16_t *destination, invalid; + + // compress list + for (i=0, ii=0, destination=NULL, invalid=0; i=right && top0<=top && bottom0>=bottom) + { + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (left<=left0 && right>=right0 && top<=top0 && bottom>=bottom0) + { + blobs[ii+0] = 0; // invalidate + invalid++; + } + } + } + + return invalid; +} + +uint16_t Blobs::combine2(uint16_t *blobs, uint16_t numBlobs) +{ + uint16_t i, j, ii, jj, left0, right0, top0, bottom0; + uint16_t left, right, top, bottom; + uint16_t invalid; + + for (i=0, ii=0, invalid=0; i=right0 && left-right0<=m_mergeDist && + ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0))) + { + blobs[ii+2] = right; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (top<=top0 && top0-bottom<=m_mergeDist && + ((left0<=left && left<=right0) || (left0<=right && right<=right0))) + { + blobs[ii+3] = top; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (bottom>=bottom0 && top-bottom0<=m_mergeDist && + ((left0<=left && left<=right0) || (left0<=right && right<=right0))) + { + blobs[ii+4] = bottom; + blobs[jj+0] = 0; // invalidate + invalid++; + } +#else // at least half of a side (the smaller adjacent side) has to overlap + if (left<=left0 && left0-right<=m_mergeDist && + ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom))) + { + blobs[ii+1] = left; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (right>=right0 && left-right0<=m_mergeDist && + ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom))) + { + blobs[ii+2] = right; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (top<=top0 && top0-bottom<=m_mergeDist && + ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right))) + { + blobs[ii+3] = top; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (bottom>=bottom0 && top-bottom0<=m_mergeDist && + ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right))) + { + blobs[ii+4] = bottom; + blobs[jj+0] = 0; // invalidate + invalid++; + } +#endif + } + } + + return invalid; +} + +int16_t Blobs::distance(BlobA *blob0, BlobA *blob1) +{ + int16_t left0, right0, top0, bottom0; + int16_t left1, right1, top1, bottom1; + + left0 = blob0->m_left; + right0 = blob0->m_right; + top0 = blob0->m_top; + bottom0 = blob0->m_bottom; + left1 = blob1->m_left; + right1 = blob1->m_right; + top1 = blob1->m_top; + bottom1 = blob1->m_bottom; + + if (left0>=left1 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0)))) + return left0-right1; + + if (left1>=left0 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0)))) + return left1-right0; + + if (top0>=top1 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0)))) + return top0-bottom1; + + if (top1>=top0 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0)))) + return top1-bottom0; + + return 0x7fff; // return a large number +} + +bool Blobs::closeby(BlobA *blob0, BlobA *blob1) +{ + // check to see if blobs are invalid or equal + if (blob0->m_model==0 || blob1->m_model==0 || blob0->m_model==blob1->m_model) + return false; + // check to see that the blobs are from color code models. If they aren't both + // color code blobs, we return false + if (!CC_SIGNATURE(blob0->m_model&0x07) || !CC_SIGNATURE(blob1->m_model&0x07)) + return false; + + return distance(blob0, blob1)<=m_maxCodedDist; +} + +int16_t Blobs::distance(BlobA *blob0, BlobA *blob1, bool horiz) +{ + int16_t dist; + + if (horiz) + dist = (blob0->m_right+blob0->m_left)/2 - (blob1->m_right+blob1->m_left)/2; + else + dist = (blob0->m_bottom+blob0->m_top)/2 - (blob1->m_bottom+blob1->m_top)/2; + + if (dist<0) + return -dist; + else + return dist; +} + +int16_t Blobs::angle(BlobA *blob0, BlobA *blob1) +{ + int acx, acy, bcx, bcy; + float res; + + acx = (blob0->m_right + blob0->m_left)/2; + acy = (blob0->m_bottom + blob0->m_top)/2; + bcx = (blob1->m_right + blob1->m_left)/2; + bcy = (blob1->m_bottom + blob1->m_top)/2; + + res = atan2((float)(acy-bcy), (float)(bcx-acx))*180/3.1415f; + + return (int16_t)res; +} + +void Blobs::sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz) +{ + uint16_t i, td, distances[MAX_COLOR_CODE_MODELS*2]; + bool done; + BlobA *tb; + + // create list of distances + for (i=0; idistances[i]) + { + // swap distances + td = distances[i]; + distances[i] = distances[i-1]; + distances[i-1] = td; + // swap blobs + tb = blobs[i]; + blobs[i] = blobs[i-1]; + blobs[i-1] = tb; + + done = false; + } + } + if (done) + break; + } +} + +bool Blobs::analyzeDistances(BlobA *blobs0[], int16_t numBlobs0, BlobA *blobs[], int16_t numBlobs, BlobA **blobA, BlobA **blobB) +{ + bool skip; + bool result = false; + int16_t dist, minDist, i, j, k; + + for (i=0, minDist=0x7fff; im_model&0x07)==(blobs[j]->m_model&0x07)) + { + skip = true; + break; + } + } + if (skip) + continue; + dist = distance(blobs0[i], blobs[j]); + if (distm_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top); + lowerArea = (area0*100)/(100+TOL); + upperArea = area0 + (area0*TOL)/100; + + for (j=0, numEqual=0; j<*numBlobs; j++) + { + if (i==j) + continue; + area1 = (blobs[j]->m_right-blobs[j]->m_left) * (blobs[j]->m_bottom-blobs[j]->m_top); + if (lowerArea<=area1 && area1<=upperArea) + numEqual++; + } + if (numEqual>maxEqual) + { + maxEqual = numEqual; + maxEqualArea = area0; + set = true; + } + } + + if (!set) + *numBlobs = 0; + + for (i=0, numNewBlobs=0; i<*numBlobs && numNewBlobsm_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top); + lowerArea = (area0*100)/(100+TOL); + upperArea = area0 + (area0*TOL)/100; + if (lowerArea<=maxEqualArea && maxEqualArea<=upperArea) + newBlobs[numNewBlobs++] = blobs[i]; +#ifndef PIXY + else if (*numBlobs>=5 && (blobs[i]->m_model&0x07)==2) + DBG("eliminated!"); +#endif + } + + // copy new blobs over + for (i=0; im_model&0x07)==(blobs[i]->m_model&0x07)) + set = true; + else + break; + } + } + if (set) + { + // copy new blobs over + for (i=0; im_model<=CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES) + { + count++; + scount = count<<3; + blob0->m_model |= scount; + blob1->m_model |= scount; + } + else if (blob0->m_model>CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES) + { + scount = blob0->m_model & ~0x07; + blob1->m_model |= scount; + } + else if (blob1->m_model>CL_NUM_SIGNATURES && blob0->m_model<=CL_NUM_SIGNATURES) + { + scount = blob1->m_model & ~0x07; + blob0->m_model |= scount; + } + } + } + } + +#if 1 + // 2nd pass: merge blob clumps + for (blob0=(BlobA *)m_blobs; blob0m_model<=CL_NUM_SIGNATURES) // skip normal blobs + continue; + scount = blob0->m_model&~0x07; + for (blob1=(BlobA *)blob0+1; blob1m_model<=CL_NUM_SIGNATURES) + continue; + + scount1 = blob1->m_model&~0x07; + if (scount!=scount1 && closeby(blob0, blob1)) + mergeClumps(scount, scount1); + } + } +#endif + + // 3rd and final pass, find each blob clean it up and add it to the table + endBlobB = (BlobB *)((BlobA *)m_blobs + MAX_BLOBS)-1; + for (i=1, codedBlob = m_ccBlobs, m_numCCBlobs=0; i<=count && codedBlobm_model&~0x07)==scount) + blobs[j++] = blob0; + } + +#if 1 + // cleanup blobs, deal with cases where there are more blobs than models + cleanup(blobs, &j); +#endif + + if (j<2) + continue; + + // find left, right, top, bottom of color coded block + for (k=0, left=right=top=bottom=avgWidth=avgHeight=0; km_model, blobs[k]->m_left, blobs[k]->m_right, blobs[k]->m_top, blobs[k]->m_bottom); + if (blobs[left]->m_left > blobs[k]->m_left) + left = k; + if (blobs[top]->m_top > blobs[k]->m_top) + top = k; + if (blobs[right]->m_right < blobs[k]->m_right) + right = k; + if (blobs[bottom]->m_bottom < blobs[k]->m_bottom) + bottom = k; + avgWidth += blobs[k]->m_right - blobs[k]->m_left; + avgHeight += blobs[k]->m_bottom - blobs[k]->m_top; + } + avgWidth /= j; + avgHeight /= j; + codedBlob->m_left = blobs[left]->m_left; + codedBlob->m_right = blobs[right]->m_right; + codedBlob->m_top = blobs[top]->m_top; + codedBlob->m_bottom = blobs[bottom]->m_bottom; + +#if 1 + // is it more horizontal than vertical? + width = (blobs[right]->m_right - blobs[left]->m_left)*100; + width /= avgWidth; // scale by average width because our swatches might not be square + height = (blobs[bottom]->m_bottom - blobs[top]->m_top)*100; + height /= avgHeight; // scale by average height because our swatches might not be square + + if (width > height) + sort(blobs, j, blobs[left], true); + else + sort(blobs, j, blobs[top], false); + +#if 1 + cleanup2(blobs, &j); + if (j<2) + continue; + else if (j>5) + j = 5; +#endif + // create new blob, compare the coded models, pick the smaller one + for (k=0, codedModel0=0; km_model&0x07; + } + for (k=j-1, codedModel=0; k>=0; k--) + { + codedModel <<= 3; + codedModel |= blobs[k]->m_model&0x07; + blobs[k]->m_model = 0; // invalidate + } + + if (codedModel0m_model = codedModel0; + codedBlob->m_angle = angle(blobs[0], blobs[j-1]); + } + else + { + codedBlob->m_model = codedModel; + codedBlob->m_angle = angle(blobs[j-1], blobs[0]); + } +#endif + //DBG("cc %d %d %d %d %d", m_numCCBlobs, codedBlob->m_left, codedBlob->m_right, codedBlob->m_top, codedBlob->m_bottom); + codedBlob++; + m_numCCBlobs++; + } + + // 3rd pass, invalidate blobs + for (blob0=(BlobA *)m_blobs; blob0m_model>CL_NUM_SIGNATURES) + blob0->m_model = 0; + } + else if (blob0->m_model>CL_NUM_SIGNATURES || CC_SIGNATURE(blob0->m_model)) + blob0->m_model = 0; // invalidate-- not part of a color code + } +} + +void Blobs::endFrame() +{ + int i; + for (i=0; igetMaxBlob(); - if (max==0 || max==(BlobA *)-1) - buf[0] = 0; + BlobA *maxBlobs; + #if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; + buf[6] = 7; + buf[7] = 8; + buf[8] = 9; + buf[9] = 10; + buf[10] = 11; + buf[11] = 12; + buf[12] = 13; + buf[13] = 14; + buf[14] = 15; + buf[15] = 16; + buf[16] = 17; + buf[17] = 18; + buf[18] = 19; + buf[19] = 20; + buf[20] = 21; + buf[21] = 22; + buf[22] = 23; + buf[23] = 24; + buf[24] = 25; + //buf[25] = 26; + //buf[26] = 27; + //buf[27] = 28; + #else + g_blobs->getMaxBlobs(0, 4, &maxBlobs, &numBlobs); + if (numBlobs==0) + memset(buf, 0, 25); +// else if (maxBlobs[0]==(BlobA *)-1) +// memset(buf, -1, 25); else { - width = max->m_right - max->m_left; - temp = ((max->m_left + width/2)*819)>>10; - buf[0] = temp; + buf[0] = numBlobs; // number of total blocks in view + for(uint8_t i=0; i < 4; i++){ + *max = maxBlobs[i]; + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + uint8_t buffOffset = (i*4); + temp = ((max->m_left + width/2)*819)>>10; + buf[buffOffset + 1] = temp; // x + buf[buffOffset + 2] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[buffOffset + 3] = temp; // width + buf[buffOffset + 4] = height; // height + //temp = ((int32_t)max->m_angle*91)>>7; + //buf[buffOffset + 5] = temp; // angle + } } + #endif + return 25; } - else - buf[0] = 1; // need to return nonzero value for other inquiries or LEGO brick will think we're an analog sensor - -#endif - return 1; - } -} -uint16_t ftc_getData(uint8_t *buf, uint32_t buflen) -{ - uint8_t c; - uint16_t d; - uint16_t numBlobs; - uint16_t numBlobsMax; - uint32_t temp, width, height; - Iserial *serial = ser_getSerial(); - - if (serial->receive(&c, 1)==0) - return 0; - -#if 1 - if (c==0x00) - { - //printf("0\n"); - char *str = "V2.1"; - strcpy((char *)buf, str); - return 5; - //return strlen((char *)str); - } - if (c==0x08) - { - //printf("8\n"); - char *str = "Pixy"; - strcpy((char *)buf, str); - return 5; - //return strlen((char *)str); - } - else if (c==0x10) - { - //printf("10\n"); - char *str = "FTC"; - strcpy((char *)buf, str); - return 4; - //return strlen((char *)str); - } - else -#endif - if (c==0x50) - { - BlobB *max; -#if 0 - buf[0] = 1; - buf[1] = 2; - buf[2] = 3; - buf[3] = 4; - buf[4] = 5; - buf[5] = 6; - buf[6] = 7; - buf[7] = 8; -#else - max = (BlobB *)g_blobs->getMaxBlob(); - if (max==0) - memset(buf, 0, 7); - else if (max==(BlobB *)-1) - memset(buf, -1, 7); - else - { - width = max->m_right - max->m_left; - height = max->m_bottom - max->m_top; - *(uint16_t *)buf = max->m_model; // signature - temp = ((max->m_left + width/2)*819)>>10; - buf[2] = temp; // x - buf[3] = max->m_top + height/2; // y - temp = (width*819)>>10; - buf[4] = temp; // width - buf[5] = height; // height - if (max->m_model>CL_NUM_SIGNATURES) - { - temp = ((int32_t)max->m_angle*91)>>7; - g_angle = temp; - }else{ - g_angle = 255; - } - buf[6] = g_angle; - } -#endif - return 7; - } - else if (c==0x60) - { - buf[0] = g_angle; - return 1; - } - else if (c>=0x51 && c<=0x57) - { -#if 0 - buf[0] = 1; - buf[1] = 1; - buf[2] = 2; - buf[3] = 3; - buf[4] = 4; - buf[5] = 1; - buf[6] = 2; - buf[7] = 3; - buf[8] = 4; - buf[9] = 1; - buf[10] = 2; - buf[11] = 3; - buf[12] = 4; - buf[13] = 1; - buf[14] = 2; - buf[15] = 3; - buf[16] = 4; - buf[17] = 1; - buf[18] = 2; - buf[19] = 3; - buf[20] = 4; - buf[21] = 1; - buf[22] = 2; - buf[23] = 3; - buf[24] = 4; - -#else - numBlobsMax = 6; //we have a 27 byte buffer on the MR Dim so only return top 6 - BlobA *max; - max = g_blobs->getMaxBlob(c-0x50, &numBlobs); - if (max==0) - memset(buf, 0, 24); - else if (max==(BlobA *)-1) - memset(buf, -1, 24); - else - { - width = max->m_right - max->m_left; - height = max->m_bottom - max->m_top; - buf[0] = numBlobs; // number of blocks that match signature - temp = ((max->m_left + width/2)*819)>>10; - buf[1] = temp; // x - buf[2] = max->m_top + height/2; // y - temp = (width*819)>>10; - buf[3] = temp; // width - buf[4] = height; // height - } -#endif - return 5; - } - else if (c==0x58) - { - BlobB *max; - if (serial->receive((uint8_t *)&d, 2)<2) // receive cc signature to look for - return 0; -#if 0 - buf[0] = 1; - buf[1] = 2; - buf[2] = 3; - buf[3] = 4; - buf[4] = 5; - buf[5] = 6; -#else - max = (BlobB *)g_blobs->getMaxBlob(d, &numBlobs); - if (max==0) - memset(buf, 0, 6); - else if (max==(BlobB *)-1) - memset(buf, -1, 6); - else - { - width = max->m_right - max->m_left; - height = max->m_bottom - max->m_top; - buf[0] = numBlobs; // number of cc blocks that match - temp = ((max->m_left + width/2)*819)>>10; - buf[1] = temp; // x - buf[2] = max->m_top + height/2; // y - temp = (width*819)>>10; - buf[3] = temp; // width - buf[4] = height; // height - temp = ((int32_t)max->m_angle*91)>>7; - buf[5] = temp; // angle - } -#endif - return 6; - } - else + else { #if 0 static uint8_t c = 0; buf[0] = c++; #else - //printf("%x\n", c); + //printf("%x\n", c); if (c==0x42) // this works in port view mode on the ev3's LCD { @@ -406,10 +265,16 @@ uint16_t ftc_getData(uint8_t *buf, uint32_t buflen) } + + uint32_t callback(uint8_t *data, uint32_t len) { - if (g_interface==SER_INTERFACE_LEGO) - return ftc_getData(data, len); + if (g_interface==SER_INTERFACE_LEGO){ + return lego_getData(data, len); + } + else if (g_interface==SER_INTERFACE_FTC){ + return lego_getData(data, len); + } else return g_blobs->getBlock(data, len); } @@ -492,10 +357,15 @@ int ser_setInterface(uint8_t interface) case SER_INTERFACE_LEGO: g_serial = g_i2c0; - //g_i2c0->setSlaveAddr(0x01); + // For now comment this out tell I get to changing pixymon to pass the FTC Parmeter below g_i2c0->setSlaveAddr(0x01); g_i2c0->setFlags(true, false); break; - + case SER_INTERFACE_FTC: + g_serial = g_i2c0; + //g_i2c0->setSlaveAddr(0x01); + g_i2c0->setFlags(true, false); + break; + default: case SER_INTERFACE_ARDUINO_SPI: g_serial = g_spi; From d8019788d6ae321ebb12c64cf6346b9a7f1774e8 Mon Sep 17 00:00:00 2001 From: Andrewiski Date: Tue, 5 Dec 2017 00:16:24 -0500 Subject: [PATCH 3/5] FTC Changes Use Lego I2c --- src/common/inc/blobs.h | 5 +- src/common/src/blobs.cpp | 45 +++++-- src/device/main_m4/src/serial.cpp | 196 +++++++++++++++++++++++++++--- 3 files changed, 217 insertions(+), 29 deletions(-) diff --git a/src/common/inc/blobs.h b/src/common/inc/blobs.h index 16fb25f8..bc881051 100644 --- a/src/common/inc/blobs.h +++ b/src/common/inc/blobs.h @@ -29,7 +29,7 @@ #define MIN_COLOR_CODE_AREA 10 #define MAX_CODED_DIST 8 #define MAX_COLOR_CODE_MODELS 5 - +#define MAX_FTCBLOBS 5 #define BL_BEGIN_MARKER 0xaa55 #define BL_BEGIN_MARKER_CC 0xaa56 @@ -88,6 +88,9 @@ class Blobs uint16_t *m_blobs; uint16_t m_numBlobs; + + uint16_t *m_ftcblobs; + BlobB *m_ccBlobs; uint16_t m_numCCBlobs; diff --git a/src/common/src/blobs.cpp b/src/common/src/blobs.cpp index e9ca8bc1..5225ffda 100644 --- a/src/common/src/blobs.cpp +++ b/src/common/src/blobs.cpp @@ -45,6 +45,7 @@ Blobs::Blobs(Qqueue *qq, uint8_t *lut) : m_clut(lut) m_ccMode = DISABLED; m_blobs = new uint16_t[MAX_BLOBS*5]; + m_ftcblobs = new uint16_t[MAX_FTCBLOBS*5]; m_numBlobs = 0; m_blobReadIndex = 0; m_ccBlobReadIndex = 0; @@ -526,8 +527,10 @@ void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlo uint16_t blobs; BlobA *blob; BlobB *ccBlob; + BlobA *ftcblob; if (m_mutex){ + *maxBlobs = (BlobA *)-1; *numBlobs = 0; return; } @@ -539,8 +542,16 @@ void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlo for (i=0; im_model = blob->m_model; + ftcblob->m_left = blob->m_left; + ftcblob->m_right = blob->m_right; + ftcblob->m_top = blob->m_top; + ftcblob->m_bottom = blob->m_bottom; + } + + *maxBlobs = (BlobA *)m_ftcblobs; *numBlobs = m_numBlobs; return; }else if (signature==8) // 8 means return the top maxNumBlobs largest area of all cc signature number > 7 @@ -551,8 +562,14 @@ void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlo { //ccBlob = (BlobB *)m_ccBlobs + i; blob = (BlobA *)m_ccBlobs + i; - maxBlobs[i] = blob; + ftcblob = (BlobA *)m_ftcblobs + i; + ftcblob->m_model = blob->m_model; + ftcblob->m_left = blob->m_left; + ftcblob->m_right = blob->m_right; + ftcblob->m_top = blob->m_top; + ftcblob->m_bottom = blob->m_bottom; } + *maxBlobs = (BlobA *)m_ftcblobs; *numBlobs = m_numCCBlobs; return; } @@ -568,8 +585,14 @@ void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlo blob = (BlobA *)m_blobs + i; if (blob->m_model==signature) { - if (blobs < maxNumBlobs) - maxBlobs[blobs] = blob; + if (blobs < maxNumBlobs){ + ftcblob = (BlobA *)m_ftcblobs + i; + ftcblob->m_model = blob->m_model; + ftcblob->m_left = blob->m_left; + ftcblob->m_right = blob->m_right; + ftcblob->m_top = blob->m_top; + ftcblob->m_bottom = blob->m_bottom; + } blobs++; // count } } @@ -584,18 +607,24 @@ void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlo ccBlob = (BlobB *)m_ccBlobs + i; if (ccBlob->m_model==signature) { - if (blobs < maxNumBlobs) - maxBlobs[blobs] =(BlobA *)ccBlob; + if (blobs < maxNumBlobs){ + ftcblob = (BlobA *)m_ftcblobs + i; + ftcblob->m_model = ccBlob->m_model; + ftcblob->m_left = ccBlob->m_left; + ftcblob->m_right = ccBlob->m_right; + ftcblob->m_top = ccBlob->m_top; + ftcblob->m_bottom = ccBlob->m_bottom; + } blobs++; // count } } } - + *maxBlobs = (BlobA *)m_ftcblobs; *numBlobs = blobs; return; } - + *maxBlobs = (BlobA *)0; *numBlobs = 0; return; } diff --git a/src/device/main_m4/src/serial.cpp b/src/device/main_m4/src/serial.cpp index 05e7d681..2292a1ce 100644 --- a/src/device/main_m4/src/serial.cpp +++ b/src/device/main_m4/src/serial.cpp @@ -28,11 +28,13 @@ uint8_t g_interface = 0; int8_t g_angle = 0; static Iserial *g_serial = 0; + uint16_t lego_getData(uint8_t *buf, uint32_t buflen) { uint8_t c; uint16_t d; uint16_t numBlobs; + BlobA* ftcBlobs; uint32_t temp, width, height; Iserial *serial = ser_getSerial(); @@ -173,10 +175,10 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen) #endif return 6; } - else if (c==0x70) //FTC Extension return top 4 Largest Signatures 27 byte limit need 6 bytes per sig 6 * 4 = 24 + NumBlocks in view = 25 + else if (c==0x70) //FTC Extension return top 5 Largest Signatures 27 byte limit need 5 bytes per sig 5 * 5 = 25 + NumBlocks in view = 26 { - BlobA *max; - BlobA *maxBlobs; + + #if 0 buf[0] = 1; buf[1] = 2; @@ -203,39 +205,193 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen) buf[22] = 23; buf[23] = 24; buf[24] = 25; - //buf[25] = 26; - //buf[26] = 27; - //buf[27] = 28; + buf[25] = 26; #else - g_blobs->getMaxBlobs(0, 4, &maxBlobs, &numBlobs); + + g_blobs->getMaxBlobs(0, 5, &ftcBlobs, &numBlobs); if (numBlobs==0) - memset(buf, 0, 25); + memset(buf, 0, 26); // else if (maxBlobs[0]==(BlobA *)-1) // memset(buf, -1, 25); else { + if(numBlobs > 255){ + buf[0] = 255; // Max is 255 for a single byte + }else{ + buf[0] = numBlobs; // number of total blocks in view + } + for(uint8_t i=0; i < 5; i++){ + uint8_t buffOffset = (i*5) + 1; + if(i < numBlobs){ + BlobA *max = (BlobA *) ftcBlobs + i; + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + buf[buffOffset] = max->m_model; // signature 1-7 only need one byte + temp = ((max->m_left + width/2)*819)>>10; + buf[buffOffset + 1] = temp; // x + buf[buffOffset + 2] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[buffOffset + 3] = temp; // width + buf[buffOffset + 4] = height; // height + }else{ + buf[buffOffset] = 0; // signature 1-7 only need one byte + buf[buffOffset + 1] = 0; // x + buf[buffOffset + 2] = 0; // y + buf[buffOffset + 3] = 0; // width + buf[buffOffset + 4] = 0; //height + } + + } + } + #endif + return 26; + } + + else if (c>=0x71 && c<=0x77) //FTC Extension return top 6 Largest Signatures 27 byte limit need 4 bytes per sig 6 * 4 = 24 + NumBlocks in view = 25 + { + + +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; + buf[6] = 7; + buf[7] = 8; + buf[8] = 9; + buf[9] = 10; + buf[10] = 11; + buf[11] = 12; + buf[12] = 13; + buf[13] = 14; + buf[14] = 15; + buf[15] = 16; + buf[16] = 17; + buf[17] = 18; + buf[18] = 19; + buf[19] = 20; + buf[20] = 21; + buf[21] = 22; + buf[22] = 23; + buf[23] = 24; + buf[24] = 25; + +#else + + g_blobs->getMaxBlobs(c-0x70, 6, &ftcBlobs, &numBlobs); + if (numBlobs==0) + memset(buf, 0, 25); +// else if (maxBlobs[0]==(BlobA *)-1) +// memset(buf, -1, 25); + else + { + if(numBlobs > 255){ + buf[0] = 255; // Max is 255 for a single byte + }else{ buf[0] = numBlobs; // number of total blocks in view - for(uint8_t i=0; i < 4; i++){ - *max = maxBlobs[i]; + } + for(uint8_t i=0; i < 6; i++){ + uint8_t buffOffset = (i*4) + 1; + if(i < numBlobs){ + BlobA *max = (BlobA *) ftcBlobs + i; width = max->m_right - max->m_left; height = max->m_bottom - max->m_top; - uint8_t buffOffset = (i*4); temp = ((max->m_left + width/2)*819)>>10; - buf[buffOffset + 1] = temp; // x - buf[buffOffset + 2] = max->m_top + height/2; // y + buf[buffOffset] = temp; // x + buf[buffOffset + 1] = max->m_top + height/2; // y temp = (width*819)>>10; - buf[buffOffset + 3] = temp; // width - buf[buffOffset + 4] = height; // height - //temp = ((int32_t)max->m_angle*91)>>7; - //buf[buffOffset + 5] = temp; // angle + buf[buffOffset + 2] = temp; // width + buf[buffOffset + 3] = height; // height + }else{ + buf[buffOffset] = 0; // x + buf[buffOffset + 1] = 0; // y + buf[buffOffset + 2] = 0; // width + buf[buffOffset + 3] = 0; // height } } - #endif - return 25; } +#endif + return 25; +} + +else if (c==0x78) //FTC Extension return top 5 Largest Color Code Signatures 27 byte limit need 6 bytes per sig 4 * 6 = 24 + NumBlocks in view = 25 +{ + + +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; + buf[6] = 7; + buf[7] = 8; + buf[8] = 9; + buf[9] = 10; + buf[10] = 11; + buf[11] = 12; + buf[12] = 13; + buf[13] = 14; + buf[14] = 15; + buf[15] = 16; + buf[16] = 17; + buf[17] = 18; + buf[18] = 19; + buf[19] = 20; + buf[20] = 21; + buf[21] = 22; + buf[22] = 23; + buf[23] = 24; + buf[24] = 25; + +#else - else + g_blobs->getMaxBlobs(8, 4, &ftcBlobs, &numBlobs); + if (numBlobs==0) + memset(buf, 0, 25); +// else if (maxBlobs[0]==(BlobA *)-1) +// memset(buf, -1, 25); + else { + if(numBlobs > 255){ + buf[0] = 255; // Max is 255 for a single byte + }else{ + buf[0] = numBlobs; // number of total blocks in view + } + for(uint8_t i=0; i < 4; i++){ + uint8_t buffOffset = (i*6) + 1; + if(i < numBlobs){ + BlobA *max = (BlobA *) ftcBlobs + i; + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + buf[buffOffset] = (max->m_model & 0xFF); // cc signature need two bytes Lower + buf[buffOffset+1] = ((max->m_model & 0xFF00) >> 8); // cc signature need two bytes Higher + temp = ((max->m_left + width/2)*819)>>10; + buf[buffOffset + 2] = temp; // x + buf[buffOffset + 3] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[buffOffset + 4] = temp; // width + buf[buffOffset + 5] = height; // height + //temp = ((int32_t)max->m_angle*91)>>7; + //buf[buffOffset + 5] = temp; // angle + }else{ + buf[buffOffset] = 0; // signature lb + buf[buffOffset + 1] = 0; // signature hb + buf[buffOffset + 2] = 0; // x + buf[buffOffset + 3] = 0; // y + buf[buffOffset + 4] = 0; // width + buf[buffOffset + 5] = 0; // height + } + } + } +#endif + return 25; +} + +else{ #if 0 static uint8_t c = 0; From 8e359961d4b89a519220b14a944914ca241109fa Mon Sep 17 00:00:00 2001 From: Andrewiski Date: Thu, 21 Dec 2017 10:57:11 -0500 Subject: [PATCH 4/5] Added FTC I2C as PixyMon Option --- src/device/main_m4/src/serial.cpp | 193 ++++++++++++++++++++++++++++-- 1 file changed, 184 insertions(+), 9 deletions(-) diff --git a/src/device/main_m4/src/serial.cpp b/src/device/main_m4/src/serial.cpp index 2292a1ce..73e4f0d6 100644 --- a/src/device/main_m4/src/serial.cpp +++ b/src/device/main_m4/src/serial.cpp @@ -175,6 +175,183 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen) #endif return 6; } + + +else{ +#if 0 + static uint8_t c = 0; + + buf[0] = c++; +#else + //printf("%x\n", c); + + if (c==0x42) // this works in port view mode on the ev3's LCD + { + BlobA *max; + max = g_blobs->getMaxBlob(); + if (max==0 || max==(BlobA *)-1) + buf[0] = 0; + else + { + width = max->m_right - max->m_left; + temp = ((max->m_left + width/2)*819)>>10; + buf[0] = temp; + } + } + else + buf[0] = 1; // need to return nonzero value for other inquiries or LEGO brick will think we're an analog sensor + +#endif + return 1; + } +} + +uint16_t ftc_getData(uint8_t *buf, uint32_t buflen) +{ + uint8_t c; + uint16_t d; + uint16_t numBlobs; + BlobA* ftcBlobs; + uint32_t temp, width, height; + Iserial *serial = ser_getSerial(); + + if (serial->receive(&c, 1)==0) + return 0; + +#if 1 + if (c==0x00) + { + //printf("0\n"); + char *str = "V0.1"; + strcpy((char *)buf, str); + return 5; + //return strlen((char *)str); + } + if (c==0x08) + { + //printf("8\n"); + char *str = "Pixy"; + strcpy((char *)buf, str); + return 5; + //return strlen((char *)str); + } + else if (c==0x10) + { + //printf("10\n"); + char *str = "Pixy"; + strcpy((char *)buf, str); + return 5; + //return strlen((char *)str); + } + else +#endif + if (c==0x50) + { + BlobB *max; +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; + buf[6] = 7; +#else + max = (BlobB *)g_blobs->getMaxBlob(); + if (max==0) + memset(buf, 0, 7); + else if (max==(BlobB *)-1) + memset(buf, -1, 7); + else + { + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + *(uint16_t *)buf = max->m_model; // signature + temp = ((max->m_left + width/2)*819)>>10; + buf[2] = temp; // x + buf[3] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[4] = temp; // width + buf[5] = height; // height + if (max->m_model>CL_NUM_SIGNATURES) + { + temp = ((int32_t)max->m_angle*91)>>7; + g_angle = temp; + } + } +#endif + return 6; + } + else if (c==0x60) + { + buf[0] = g_angle; + return 1; + } + else if (c>=0x51 && c<=0x57) + { +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; +#else + BlobA *max; + max = g_blobs->getMaxBlob(c-0x50, &numBlobs); + if (max==0) + memset(buf, 0, 5); + else if (max==(BlobA *)-1) + memset(buf, -1, 5); + else + { + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + buf[0] = numBlobs; // number of blocks that match signature + temp = ((max->m_left + width/2)*819)>>10; + buf[1] = temp; // x + buf[2] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[3] = temp; // width + buf[4] = height; // height + } +#endif + return 5; + } + else if (c==0x58) + { + BlobB *max; + if (serial->receive((uint8_t *)&d, 2)<2) // receive cc signature to look for + return 0; +#if 0 + buf[0] = 1; + buf[1] = 2; + buf[2] = 3; + buf[3] = 4; + buf[4] = 5; + buf[5] = 6; +#else + max = (BlobB *)g_blobs->getMaxBlob(d, &numBlobs); + if (max==0) + memset(buf, 0, 6); + else if (max==(BlobB *)-1) + memset(buf, -1, 6); + else + { + width = max->m_right - max->m_left; + height = max->m_bottom - max->m_top; + buf[0] = numBlobs; // number of cc blocks that match + temp = ((max->m_left + width/2)*819)>>10; + buf[1] = temp; // x + buf[2] = max->m_top + height/2; // y + temp = (width*819)>>10; + buf[3] = temp; // width + buf[4] = height; // height + temp = ((int32_t)max->m_angle*91)>>7; + buf[5] = temp; // angle + } +#endif + return 6; + } else if (c==0x70) //FTC Extension return top 5 Largest Signatures 27 byte limit need 5 bytes per sig 5 * 5 = 25 + NumBlocks in view = 26 { @@ -421,15 +598,13 @@ else{ } - - uint32_t callback(uint8_t *data, uint32_t len) { if (g_interface==SER_INTERFACE_LEGO){ return lego_getData(data, len); } else if (g_interface==SER_INTERFACE_FTC){ - return lego_getData(data, len); + return ftc_getData(data, len); } else return g_blobs->getBlock(data, len); @@ -452,7 +627,7 @@ void ser_loadParams() { #ifndef LEGO prm_add("Data out port", 0, - "Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y @s 6=LEGO_I2C", UINT8(0), END); + "Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y @s 6=LEGO_I2C @s 7=FTC_I2C", UINT8(0), END); prm_add("I2C address", PRM_FLAG_HEX_FORMAT, "@c Interface Sets the I2C address if you are using I2C data out port. (default 0x54)", UINT8(I2C_DEFAULT_SLAVE_ADDR), END); prm_add("UART baudrate", 0, @@ -513,14 +688,14 @@ int ser_setInterface(uint8_t interface) case SER_INTERFACE_LEGO: g_serial = g_i2c0; - // For now comment this out tell I get to changing pixymon to pass the FTC Parmeter below g_i2c0->setSlaveAddr(0x01); + g_i2c0->setSlaveAddr(0x01); g_i2c0->setFlags(true, false); break; case SER_INTERFACE_FTC: - g_serial = g_i2c0; - //g_i2c0->setSlaveAddr(0x01); - g_i2c0->setFlags(true, false); - break; + g_serial = g_i2c0; + // Use the Address set via PixyMon g_i2c0->setSlaveAddr(0x01); + g_i2c0->setFlags(true, false); + break; default: case SER_INTERFACE_ARDUINO_SPI: From 0b863da8d164584ed96a621f48d48fd8278c83ef Mon Sep 17 00:00:00 2001 From: Andrewiski Date: Thu, 21 Dec 2017 15:03:07 -0500 Subject: [PATCH 5/5] FTC I2C Added as an Serial Protocol Option, Firmware Bump to 2.0.20 --- src/device/main_m4/inc/exec.h | 2 +- src/device/main_m4/src/serial.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/device/main_m4/inc/exec.h b/src/device/main_m4/inc/exec.h index 5674c130..e6c77aac 100644 --- a/src/device/main_m4/inc/exec.h +++ b/src/device/main_m4/inc/exec.h @@ -23,7 +23,7 @@ #define FW_MAJOR_VER 2 #define FW_MINOR_VER 0 -#define FW_BUILD_VER 19 +#define FW_BUILD_VER 20 #ifdef LEGO #define FW_TYPE "LEGO" #else diff --git a/src/device/main_m4/src/serial.cpp b/src/device/main_m4/src/serial.cpp index 73e4f0d6..a5bf763a 100644 --- a/src/device/main_m4/src/serial.cpp +++ b/src/device/main_m4/src/serial.cpp @@ -238,9 +238,9 @@ uint16_t ftc_getData(uint8_t *buf, uint32_t buflen) else if (c==0x10) { //printf("10\n"); - char *str = "Pixy"; + char *str = "FTC"; strcpy((char *)buf, str); - return 5; + return 4; //return strlen((char *)str); } else