Pixy Cam = CMU Cam5 für Objekterkennung

By | 9. November 2014

Die Pixy Cam kann per RS485 am NXT angeschlossen und dann in RobotC programmiert werden.

Etwas Bastelarbeiten sind erforderlich. Siehe:

http://robotics.benedettelli.com/connect-pixy-camera-nxt/  (in englisch)

Bastelarbeit Pixy Cam

Hier ein Video:

Der Programmcode zu der Cam:

/*
   // 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.
   
   This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
   
   Adapted from Arduino Library source code by 
   Daniele Benedettelli - September 2014
   
*/



typedef unsigned int uint16_t;

#define PIXY_ARRAYSIZE      30
#define PIXY_START_WORD             0x55AA
#define PIXY_START_WORD_CC          0x56AA
#define PIXY_START_WORDX            0xAA55
#define SERIAL_SPEED                        57600

#define MAX_W 320
#define MAX_H 200

enum BlockType
{
   NORMAL_BLOCK,
   CC_BLOCK
};

enum BlockField
{
   SIGNATURE=0,
   X,
   Y,
   WIDTH,
   HEIGHT,
   ANGLE
};

struct Block {
   uint16_t signature;
  uint16_t x;
  uint16_t y;
  uint16_t width;
  uint16_t height;
  int angle;
};

Block blocks[PIXY_ARRAYSIZE];
bool  skipStart;
BlockType blockType;
uint16_t blockCount;
uint16_t blockArraySize;

void decodeSignature(uint16_t signature, string &sign) {
   int i, j;
   char sig[6], d;
   bool flag = false;
   j = 0;
   for (i=12; i>=0; i-=3) {
      d = (signature>>i)&0x07;
      if (d>0 && !flag)
         flag = true;
      if (flag)
         sig[j++] = d + '0';
   }
   sig[j] = '\0';
   stringFromChars(sign,sig);
}

void pixyPrint(Block b, int line = 0) {
   string s;
   decodeSignature(b.signature,s);
   writeDebugStream("S:%s X:%d Y:%d W:%d H:%d A:%d\n", s, b.x, b.y, b.width, b.height, b.angle);
   if (line!=0) displayTextLine(line, "S:%s X:%d Y:%d W:%d H:%d A:%d\n", s, b.x, b.y, b.width, b.height, b.angle);
}

void pixyDraw(Block b, bool displaySignature = false) {
   string s;
   decodeSignature(b.signature,s);
   int l = 100*(b.x-b.width/2)/MAX_W;
   int r = 100*(b.x+b.width/2)/MAX_W;
   int t = 64-64*(b.y+b.height/2)/MAX_H;
   int d = 64-64*(b.y-b.height/2)/MAX_H;
   drawRect(l,t,r,d);
   if (displaySignature) {
      displayStringAt(l-10,d+3,"%s",s);
      if (b.signature>7) {
         displayStringAt(r+5,d+3,"%d",b.angle);
      }
   }
}

void pixyInit() {
  nxtEnableHSPort();
   nxtSetHSBaudRate(SERIAL_SPEED);
   nxtHS_Mode = hsRawMode;
   skipStart = false;
  blockCount = 0;
  blockArraySize = PIXY_ARRAYSIZE;
}

uint16_t getStartWord() {
  ubyte data[2];
  while(nxtGetAvailHSBytes()<2) sleep(1);
  //if(nxtGetAvailHSBytes()) {
     if(nxtReadRawHS(data, 2)) {
        return (data[0]<<8)|(data[1]); // little Endian
     }
  //}
  return 0;
}

int getWord() {
  ubyte data[2];
  while(nxtGetAvailHSBytes()<2) sleep(1);
  //if(nxtGetAvailHSBytes()) {
     if(nxtReadRawHS(data, 2)) {
        return (data[1]<<8)|(data[0]); // big Endian
     }
  //}
  return 0;
}

ubyte getByte() {
  ubyte u;
   while(nxtGetAvailHSBytes()==0) sleep(1);
   if(nxtReadRawHS(&u, 1))
      return u;
   else return 0;
}

bool getStart() {
   unsigned int w, lastw;
   lastw = 0xffff;
   while(true) {
      w = getStartWord();
      //writeDebugStreamLine("(%04X) %04X", lastw, w);
      if (w==0 && lastw==0)   {
         sleep(1);
         return false;
      }   else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD) {
         blockType = NORMAL_BLOCK;
         return true;
      } else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD) {
         blockType = CC_BLOCK;
         return true;
      }   else if (w==PIXY_START_WORDX) {
         //writeDebugStreamLine("resync=>%02X",getByte());
         getByte();
      }
      lastw = w;
   }
}

// returns the number of blocks and populate the blocks structure
uint16_t pixyGetBlocks(uint16_t maxBlocks = 30) {
   ubyte i;
   uint16_t checksum, sum;
   short w;
   Block *block;

   if (!skipStart) {
      //writeDebugStreamLine("Waiting start");
      if (getStart()==false)
         return 0;
   } else
      skipStart = false;

   for(blockCount=0; blockCount<maxBlocks && blockCount<PIXY_ARRAYSIZE; ) {
      {
         checksum = getWord();
         //writeDebugStream("Checksum %X ",checksum);
         if (checksum==PIXY_START_WORD) { // we've reached the beginning of the next frame
            skipStart = true;
            blockType = NORMAL_BLOCK;
            //writeDebugStreamLine("normal block");
            return blockCount;
         }
         else if (checksum==PIXY_START_WORD_CC) {
            skipStart = true;
            blockType = CC_BLOCK;
            //writeDebugStreamLine("color code block");
            return blockCount;
         }
         else if (checksum==0) {
            return blockCount;
            //writeDebugStreamLine("END");
         }
         //else writeDebugStream("\n"); // checksum is ok to be used for later check

         if (blockCount>blockArraySize)
            return blockCount;

         block = &blocks[0] + blockCount; // block pointer now points to last element of blocks array

         sum = 0;
         uint16_t M = sizeof(Block)/sizeof(uint16_t); // M should be 6
         for (i=0; i<M; i++) {
            if (blockType==NORMAL_BLOCK && i>=5) {// skip
               block->angle = 0; // angle data available only for color codes blocks
               break;
            }

            w = (short)getWord();
            sum += w;
            switch(i) { //fill current block
               case SIGNATURE:
                  block->signature = w;
                  //writeDebugStreamLine("block[%i].signature = %d",blockCount, w);
                  break;
               case X:
                  block->x = w;
                  //writeDebugStreamLine("block[%i].x = %d",blockCount, w);
                  break;
               case Y:
                  block->y = w;
                  //writeDebugStreamLine("block[%i].y = %d",blockCount, w);
                  break;
               case WIDTH:
                  block->width = w;
                  //writeDebugStreamLine("block[%i].width = %d",blockCount, w);
                  break;
               case HEIGHT:
                  block->height = w;
                  //writeDebugStreamLine("block[%i].height = %d",blockCount, w);
                  break;
               case ANGLE:
                  block->angle = w;
                  //writeDebugStreamLine("block[%i].angle = %i",blockCount, w);
                  break;
            }
         }
         if (checksum==sum) {
            blockCount++;
            //writeDebugStreamLine("checksum %d OK! blockCount %d",checksum, blockCount);
         }
         //else writeDebugStreamLine("ERROR: checksum %d <> sum %d", checksum, sum);

         w = getStartWord();
         //writeDebugStreamLine("next blockType %X",w);
         if (w==PIXY_START_WORD)
            blockType = NORMAL_BLOCK;
         else if (w==PIXY_START_WORD_CC)
            blockType = CC_BLOCK;
         else {
            return blockCount;
         }
      }
   }
   return 0;
}

task main() {
   clearDebugStream();
   static int i = 0;
   uint16_t j;
   uint16_t nBlocks;
   pixyInit();
   while(1) {
      nBlocks = pixyGetBlocks(10);
      if (nBlocks!=0){
         i++;
         //if (i%10==0){ // slow down display
            displayTextLine(1,"OBJECTS:%d",nBlocks);
            writeDebugStreamLine("Detected %d:", nBlocks);
            eraseDisplay();
            for (j=0; j<nBlocks; j++) {
               writeDebugStreamLine("  block %d: ", j+1);
               pixyPrint(blocks[j]);
               pixyDraw(blocks[j],true);
            }
            writeDebugStream("\n");
         }
      //}
   }
   nxtDisableHSPort();
}