查看: 7460|回复: 3

Pixy CMUcam5图像识别传感器教程（二）：与PIXY对话

（二）与PIXY对话

Pixy CMUcam5图像识别传感器教程（一）：教PIXY识别物体
Pixy CMUcam5图像识别传感器教程（二）：与PIXY对话
Pixy CMUcam5图像识别传感器教程（三）：连接PIXY与Arduino
Pixy CMUcam5图像识别传感器教程（四）：连接PIXY与树莓派
Pixy CMUcam5图像识别传感器教程（五）：摄像头云台安装

原文链接：
https://docs.pixycam.com/wiki/doku.php?id=wiki:v1:porting_guide

本教程详细说明PIXY如何与Arduino进行通信

如何与Pixy对话

Pixy目前对以下微控制器提供软件支持：

如果主控器不在上面的列表里，可以采用下面三种方式与PIXY通信：

Serial串口：包括SPI，I2C和UART接口。串行接口使用简化的协议，代码量小，内存占用也少，代码很容易移植到不同的微控制器。 Arduino也是采取这类接口与PIXY通信。使用串行协议Pixy发送检测到的内容给主控板，并接受主控发来的简单命令控制摄像头云台转动。
USB接口：USB接口适用于具有更多内存资源（RAM和闪存）的微控制器。 USB协议的代码占用内存较多。 Raspberry Pi和BeagleBone Black采取这种方式与PIXY通信，PixyMon也使用USB接口来传输实时视频和读/写配置信息。
Analog/digital模拟数字引脚：这是最简单的接口，甚至都不需要协议。

选择合适的接口

首先我们要确定以何种方式与Pixy通信。Arduino的SPI时钟速率设置为1 MHz（可以设置得更高），Pixy通过SPI接口以1 Mbits /秒的速率向Arduino发送块信息，这表明Pixy每秒可以发送超过6000个检测到的对象（每帧发送135个，Pixy以每秒50帧的速度进行处理）。但是，如果串行链接太慢，还没有等这一帧等所有对象传送完毕，下一帧已准备好发送。串口波特率为19200时会经常出现这种情况（没有发送完毕），即便波特率达到115000也仍然有可能发生。这种情况下，PIXY会丢弃前一帧中未发送完的对象块，继续发送来自新帧的新对象块。最新信息被优先处理并始终发送。对象按大小排序，对象越大，它获得的优先级也越高。下面是接口选择的一些准则：

如果控制器是基于Linux并且有USB接口，可以使用 libpixyusb，易于移植，而且USB是速度最快的接口。
如果控制器不支持USB但支持SPI，可以使用SPI接口与PIXY通信，它通常比I2C和UART更快
如果控制器不支持上述接口但支持I2C，可以使用I2C接口与PIXY通信，它与UART串行速度大致相同但更灵活
如果控制器不支持上述接口但支持UART串口，可以使用UART接口与PIXY通信
如果控制器不支持以上任何接口，可以使用模拟和数字输出接口，它们是最简单的接口！

设置接口

通过软件PixyMon可以对接口进行设置。在“Interface”选项卡中的“Data out port”选项可以对接口进行设置。

注：如果运行的是乐高固件，在PixyMon里是没有接口选项卡的。如果希望在非乐高主控上运行Pixy Lego，可以参考此页。

接口说明：

Arduino ICSP SPI - 这是使用3针数据线的默认接口，接在PIXY上的是1、3、4引脚，用于通过ICSP连接器与Arduino通信。此版本的SPI不使用从选择信号。
带SS的SPI - 和Arduino ICSP SPI相同，不同之处在于它通过引脚7（SPI SS）支持从选择。在发送/接收每个字节之前，需要将SPI SS驱动为低电平。
I2C - 这是一个多点2针接口，接在PIXY上的5和9引脚上，允许单个主机与最多127个从站（127个Pixys）通信。可以通过接口选项卡中的“I2C Address”来配置I2C地址。
UART - 这是常见的“串行接口”，接在PIXY上的1和4引脚上。 Pixy通过引脚1（输入引脚）接收数据，并通过引脚4（输出引脚）输出数据。可以通过接口选项卡中的“UART Baudrate”来设置UART的波特率。
模拟/数字引脚 x - 引脚3输出模拟信号，代表PIXY获取的最大检测对象的x值，范围为0到3.3V。引脚1输出数字信号，判断是否检测到物体。
模拟/数字引脚 y - 引脚3输出模拟信号，代表PIXY获取的最大检测对象的Y值，范围为0到3.3V。引脚1输出数字信号，判断是否检测到物体。
LEGO I2C - LEGO Mindstorms EV3或NXP控制器使用这种模式。虽然是I2C接口但使用的是LEGO协议。

注：USB接口及其协议始终处于启用状态，而上面这些接口在给定时间内只能启用一个。

各种接口说明

下图是PixyI / O连接器的引脚图，所有串行（SPI，I2C，UART）和模拟/数字接口都可以使用。

Pixy背面的 I/O 端口的引脚按以下顺序排列，左上角是引脚1：

1 2
3 4
5 6
7 8
9 10

引脚1：SPI MISO，UART RX，GPIO0引脚2：5V（输入或输出）
引脚3：SPI SCK，DAC输出，GPIO1
引脚4：SPI MOSI，UART TX，GPIO2
引脚5：I2C SCL
引脚6：GND
引脚7：SPI SS，ADC输入，GPIO3
引脚8：GND
引脚9：I2C SDA
引脚10：Vin（6~10V）（上图标注有误，见下图）

SPI

ICSP SPI接口作为SPI从器件来使用。该接口围绕Arduino的ICSP端口设计，Arduino的这个端口没有从选择信号。默认数据速率为1M bits/秒，可以通过修改Pixy Arduino库中的Pixy.h文件来增加此速率。该协议具有校验和来处理位错误。特别提醒，SPI线缆是没有被屏蔽的！ Pixy支持的特定SPI类型有：

ICSP SPI接口作为从属SPI进行操作。它是围绕Arduino的ICSP端口设计的，该端口没有从机选择信号。默认的数据速率是1 Mbits/秒，但是可以通过修改Pixy Arduino 库中的Pixy.h文件来提高这个速率。协议有校验和来处理位错误，但是要记住带状电缆没有被屏蔽！Pixy支持的特定类型的SPI有:

数据首先发送最重要的位空闲时SPI SCK为低电平数据位被锁定在SPI SCK的上升边缘从机选择为低电平有效3.3V输出，5V耐压

Pixy还支持具有从选择的SPI（带SS的SPI，slave select）。

如果没有PIXY提供的Arduino SPI线缆，可以用下面的方法将将PIXY通过SPI连接到主控板：

引脚10➜控制器的GND
引脚1（SPI MISO）➜控制器的SPI MISO信号
引脚4（SPI MOSI）➜控制器的SPI MOSI信号
引脚3（SPI SCK）➜控制器的SPI SCK信号
引脚7（SPI SS）➜控制器的SPI SS信号（如果使用带SS的SPI）

I2C

I2C接口作为I2C从器件工作，需要轮询。
通过R14和R15电阻，SDA和SCL信号上有低至4.7K的电阻将电压上拉至5V。
I2C信号具有5V容限。
可以在PixyMon的“Configure Parameters”对话框的“Interface”选项卡中配置I2C地址
Arduino中有使用I2C的示例。在Arduino IDE中选择File➜Examples➜I2C来运行。需要使用杜邦线来连接各个引脚。

控制器的I2C与Pixy连接方法：

引脚10➜控制器的GND
引脚9（I2C SDA）➜控制器的I2C SDA信号
引脚5（I2C SCL）➜控制器的I2C SCL信号

注意，当通过I2C与多个Pixy通信时，需要为每个Pixy配置不同的I2C地址，这样才不会引起冲突。可以制作“多重压合电缆”（multi-crimp cable）：使用一条10芯的带状电缆并将其压接到N个10针的IDC连接器上并插入N个Pixy的引脚。也就是说，当选择I2C作为接口时，Pixy的I/O连接器上的所有信号都会进入高阻态，不会相互干扰、浪费电力等。

UART

UART接口有8个数据位，1个停止位，无奇偶校验，无握手
可以在PixyMon的Configure对话框的“Interface”选项卡中配置波特率
RX信号（引脚1）为5V限压输入
TX信号（引脚4）为0至3.3V信号输出
支持高达230 k的波特率。

Arduino中有一个使用UART串行的示例。在Arduino IDE中选择File➜Examples➜uart来运行。需要制作一根特殊的连接线缆。

以下是如何将控制器的UART连接到Pixy：

引脚10➜控制器的接地信号
引脚1（UART RX）➜控制器的UART TX输出
引脚4（UART TX）➜控制器的UART RX输入

模拟和数字输出

Pixy具有单独的模拟（DAC）输出，因此其模拟/数字输出有两种模式。
模式4将最大检测对象中心的x值输出到 I/O 连接器的引脚3。
模式5将最大检测对象中心的y值输出到 I/O 连接器的引脚3。
检测到物体时，引脚1为高电平（3.3V），未检测到物体时，引脚1为低电平（0V）。
Pixy的数字输出为0至3.3V逻辑值，可以提供/吸收5 mA电流。
Pixy的模拟（DAC）输出范围在0到3.3V之间，阻抗大约为200欧姆。
Pixy的模拟（DAC）输出电压直接与图像中的物体位置成线性比例（取决于模式）
在模式4（x模式）下，在PixyMon透视图中，如果对象位于图像的最左侧，则模拟输出为0V，如果对象位于图像的最右侧，则模拟输出为3.3V。
在模式5（y模式）下，在PixyMon透视图中，如果对象位于图像的最底部，则模拟输出为0V，如果对象位于图像的最上方，则模拟输出为3.3V。

以下是将控制器的ADC和数字I / O连接到Pixy的方法：

引脚10➜控制器的接地信号
引脚3（DAC 输出）➜控制器的一个ADC输入信号
引脚1（GPIO0）➜控制器的一个数字输入信号
注意，Pixy上的所有数字输出信号都是3.3V CMOS逻辑值。 Pixy上的所有数字输入信号均为5V限压。

串行协议
无论使用的是SPI、I2C还是UART串行，协议都是完全相同的。

该协议是数据有效的二进制。
每个帧中的对象按大小排序，最大的对象优先发送。
可以配置每个图像帧发送的最大对象数（参数为“Max blocks”）。
SPI和I2C工作在“从模式”，依靠轮询接收更新。
没有检测到对象（无数据）时，如果接口是SPI或I2C，Pixy会发送0（因为Pixy是从属地位，必须发送一些东西）。
每个对象都在“对象块object block”中发送（见下表）。
对象块中的所有值都是16位字，从最小字节开始发送。例如当发送同步字0xaa55时，Pixy先发送0x55（第1个字节）再发送0xaa（第2个字节）。

对象的块格式
字节    16-位字                   描述
----------------------------------------------------------------
0, 1          是          同步字：0xaa55代表检测到的是普通对象（单色），0xaa56是颜色编码对象（多色）
2, 3         是          校验和：（第4~13字节所有16位字的和）
4, 5       是          颜色特征号
6, 7         是          对象中心的x值
8, 9       是          对象中心的y值
10, 11    是          对象的宽度
12, 13      是          对象的高度

帧与帧之间通过插入一个额外的同步字（0xaa55）来标记。下面任一项表明检测到了新图像：

两个同步字背靠背发送（0xaa55,0xaa55）（即连续接收到两个0xaa55）
普通对象同步字（0xaa55）后跟颜色代码同步字（0xaa56）。

因此，解析串行流的典型方法是等待两个同步字后再开始解析对象块，使用同步字指示下一个对象块的开始，依此类推。

将控制数据发送到Pixy

将控制数据发送到Pixy来控制云台的移动、调整摄像机亮度及设置LED颜色。每个16位字都从小的字节（最先发送最小有效字节）发送。

控制云台舵机转动
字节    16-位字                   描述
----------------------------------------------------------------
0, 1         是                舵机0（水平方向）同步字
2, 3         是             舵机0（水平）的位置，值介于0~1000
4, 5    是                  舵机1（垂直）的位置，值介于0~1000

摄像头亮度（曝光度）控制
字节    16-位字                   描述
----------------------------------------------------------------
0, 1    是               亮度同步字，值为0xfe00
2          否                  亮度值

LED control LED控制
字节    16-位字                   描述
----------------------------------------------------------------
0, 1         是               LED灯同步字，值为0xfd00
2             否                   红色值
3             否                   绿色值
4             否                   蓝色值

编写代码

先解析同步字以分离帧。然后计算在1秒内获得的帧数，帧数应该是50。下面是示例代码：

[mw_shl_code=cpp,true]#define PIXY_START_WORD          0xaa55
#define PIXY_START_WORD_CC       0xaa56
#define PIXY_START_WORDX          0x55aa

typedef enum
{
  NORMAL_BLOCK,
  CC_BLOCK // color code block
} BlockType;

static BlockType g_blockType; // use this to remember the next object block type between function calls

int getStart(void)
{
  uint16_t w, lastw;

  lastw = 0xffff; // some inconsequential initial value 将lastw初始化为一个在检测中不可能出现的值

  while(1)
  {
w = getWord();
if (w==0 && lastw==0)
   return 0; // in I2C and SPI modes this means no data, so return immediately 在I2C和SPI模式下如果第0和第1个字节的值都是0，表示未检测到对象
else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD)
{
   g_blockType = NORMAL_BLOCK; // remember block type 保存块类型
   return 1; // code found!检测到普通对象（单色）
}
else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD)
{
   g_blockType = CC_BLOCK; // found color code block检测到颜色编码对象（多色）
   return 1;
}
else if (w==PIXY_START_WORDX) // this is important, we might be juxtaposed
   getByte(); // we're out of sync! (backwards) 未同步（回退）
lastw = w; // save
  }
}[/mw_shl_code]

上面的代码缺少的例程是getWord()，定义如下：

[mw_shl_code=cpp,true]extern uint8_t getByte(void); // external, does the right things for your interface

uint16_t getWord(void)
{
  // this routine assumes little endian
  uint16_t w;
  uint8_t c;
  c = getByte();
  w = getByte();
  w <<= 8;
  w |= c;
  return w;
}[/mw_shl_code]

因此可以用下面的方法来验证每秒是否获得了50帧：

[mw_shl_code=cpp,true]int main()
{
  int i=0, curr, prev=0;

  // look for two start codes back to back
  while(1)
  {
curr = getStart());
if (prev && curr) // two start codes means start of new frame
   printf("%d", i++);
prev = curr;
  }
}[/mw_shl_code]

但是SPI有一个重要的例外，如果使用SPI接口，请阅读后面“最易混淆的SPI” “SPI tries its best to confuse things”

解析剩余的对象块比较简单，代码如下：

[mw_shl_code=cpp,true]#define PIXY_ARRAYSIZE             100

typedef struct
{
  uint16_t signature;
  uint16_t x;
  uint16_t y;
  uint16_t width;
  uint16_t height;
  uint16_t angle; // angle is only available for color coded blocks
} Block;

static int g_skipStart = 0;
static Block *g_blocks;

uint16_t getBlocks(uint16_t maxBlocks)
{
  uint8_t i;
  uint16_t w, blockCount, checksum, sum;
  Block *block;

  if (!g_skipStart)
  {
if (getStart()==0)
   return 0;
  }
  else
g_skipStart = 0;

  for(blockCount=0; blockCount<maxBlocks && blockCount<PIXY_ARRAYSIZE;)
  {
checksum = getWord();
if (checksum==PIXY_START_WORD) // we've reached the beginning of the next frame 到达下一帧的开始位置
{
   g_skipStart = 1;
   g_blockType = NORMAL_BLOCK;
   return blockCount;
}
else if (checksum==PIXY_START_WORD_CC)
{
   g_skipStart = 1;
   g_blockType = CC_BLOCK;
   return blockCount;
}
else if (checksum==0)
   return blockCount;

block = g_blocks + blockCount;

for (i=0, sum=0; i<sizeof(Block)/sizeof(uint16_t); i++)
{
   if (g_blockType==NORMAL_BLOCK && i>=5) // no angle for normal block
   {
      block->angle = 0;
      break;
   }
   w = getWord();
   sum += w;
   *((uint16_t *)block + i) = w;
}

// check checksum
if (checksum==sum)
   blockCount++;
else
   printf("checksum error!\n");

w = getWord();
if (w==PIXY_START_WORD)
   g_blockType = NORMAL_BLOCK;
else if (w==PIXY_START_WORD_CC)
   g_blockType = CC_BLOCK;
else
   return blockCount;
  }
}[/mw_shl_code]

上面这段代码假定同步字在被调用时已被读取。代码中唯一需要解释的可能是gskipStart变量。之所以设定这个变量是因为如果读取的是最后一个块，我们可能会读取同步字而不是校验和。 g_skipStart变量告诉我们是否已经读过同步字。

上面的代码复制了读入g_blocks数组的块，需要对这些块进行初始化：

[mw_shl_code=cpp,true]void init()
{
g_blocks = (Block *)malloc(sizeof(Block)*PIXY_ARRAYSIZE);
}[/mw_shl_code]

上面的代码处理了来自Pixy的对象数据，对Pixy的控制数据要如何发送？如控制云台舵机的移动、设置摄像机的亮度、设置LED颜色等：

[mw_shl_code=cpp,true]#define PIXY_SERVO_SYNC          0xff
#define PIXY_CAM_BRIGHTNESS_SYNC 0xfe
#define PIXY_LED_SYNC             0xfd

extern int sendByte(uint8_t byte);

int send(uint8_t *data, int len)
{
  int i;
  for (i=0; i<len; i++)
sendByte(data);

  return len;
}

int setServos(uint16_t s0, uint16_t s1)
{
  uint8_t outBuf[6];

  outBuf[0] = 0x00;
  outBuf[1] = PIXY_SERVO_SYNC;
  *(uint16_t *)(outBuf + 2) = s0;
  *(uint16_t *)(outBuf + 4) = s1;

  return send(outBuf, 6);
}

int setBrightness(uint8_t brightness)
{
  uint8_t outBuf[3];

  outBuf[0] = 0x00;
  outBuf[1] = PIXY_CAM_BRIGHTNESS_SYNC;
  outBuf[2] = brightness;

  return send(outBuf, 3);
}

int setLED(uint8_t r, uint8_t g, uint8_t b)
{
  uint8_t outBuf[5];

  outBuf[0] = 0x00;
  outBuf[1] = PIXY_LED_SYNC;
  outBuf[2] = r;
  outBuf[3] = g;
  outBuf[4] = b;

  return send(outBuf, 5);
}[/mw_shl_code]

和getByte()一样，sendByte()例程是一个外部例程，为接口做正确的事情。

SPI tries its best to confuse things最易混淆的SPI

上面那些操作都挺合理的，除了Pixy上的SPI有让人郁闷的地方：

SPI is a simultaneous send/receive interface, so our getByte() routine instead of just returning a received data byte, needs to accept an output data byte too.
To save CPU, Pixy configures its SPI controller with 16-bit words instead of 8. This works great, but the 16-bit words are sent big-endian instead of little-endian.
Pixy relies on sync bytes sent to it to make sure it has good bit-sync, so you need to send a sync byte every other byte when talking to Pixy over SPI. This also solves the data imbalance problem with Pixy and SPI – that is, there's a lot more data being sent by Pixy than being received by Pixy. The sync bytes allow Pixy to separate the filler data from the valid data.
SPI是一个同步发送/接收接口，因此getByte()例程不仅仅返回接收到的数据字节，还需要接受输出的数据字节。
为了节省CPU，Pixy用16位字而不是8位来配置SPI控制器。但是16位字是以大端big-endian（最大的字节位）而不是小端little-endian方式被发送。
Pixy依赖于发送给它的同步字节以确保它具有良好的位同步，因此在通过SPI与Pixy通信时需要每隔一个字节发送一个同步字节。这也解决了Pixy和SPI的数据不平衡问题 - 也就是说，Pixy发送的数据比Pixy接收的数据要多得多。同步字节允许Pixy从有效数据中分割填充数据

下面是考虑了上述SPI情况的代码：

[mw_shl_code=cpp,true]#define PIXY_SYNC_BYTE             0x5a  // to sync SPI data
#define PIXY_SYNC_BYTE_DATA       0x5b  // to sync/indicate SPI send data
#define PIXY_OUTBUF_SIZE          64

// SPI sends as it receives so we need a getByte routine that
// takes an output data argument
extern uint8_t getByte(uint8_t out);

// variables for a little circular queue for SPI output data
static uint8_t g_outBuf[PIXY_OUTBUF_SIZE];
static uint8_t g_outLen = 0;
static uint8_t g_outWriteIndex = 0;
static uint8_t g_outReadIndex = 0;

uint16_t getWord()
{
  // ordering is big endian because Pixy is sending 16 bits through SPI
  uint16_t w;
  uint8_t c, cout = 0;

  if (g_outLen)
  {
w = getByte(PIXY_SYNC_BYTE_DATA);
cout = g_outBuf[g_outReadIndex++];
g_outLen--;
if (g_outReadIndex==PIXY_OUTBUF_SIZE)
   g_outReadIndex = 0;
  }
  else
w = getByte(PIXY_SYNC_BYTE); // send out sync byte
  w <<= 8;
  c = getByte(cout); // send out data byte
  w |= c;

  return w;
}

int send(uint8_t *data, int len)
{
  int i;

  // check to see if we have enough space in our circular queue
  if (g_outLen+len>PIXY_OUTBUF_SIZE)
return -1;

  g_outLen += len;
  for (i=0; i<len; i++)
  {
g_outBuf[g_outWriteIndex++] = data;
if (g_outWriteIndex==PIXY_OUTBUF_SIZE)
   g_outWriteIndex = 0;
  }
  return len;
}[/mw_shl_code]

上面的代码我们为发送的数据实现了一个小的循环队列，这是因为接收和发送是绑定在一起。

以下是完整的代码，供参考：

[mw_shl_code=cpp,true]#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>

// Are you using an SPI interface?  if so, uncomment this line
#define SPI

#define PIXY_ARRAYSIZE             100
#define PIXY_START_WORD          0xaa55
#define PIXY_START_WORD_CC       0xaa56
#define PIXY_START_WORDX          0x55aa
#define PIXY_SERVO_SYNC          0xff
#define PIXY_CAM_BRIGHTNESS_SYNC 0xfe
#define PIXY_LED_SYNC             0xfd
#define PIXY_OUTBUF_SIZE          64

#define PIXY_SYNC_BYTE             0x5a
#define PIXY_SYNC_BYTE_DATA       0x5b

// the routines
void init();
int getStart(void);
uint16_t getBlocks(uint16_t maxBlocks);
int setServos(uint16_t s0, uint16_t s1);
int setBrightness(uint8_t brightness);
int setLED(uint8_t r, uint8_t g, uint8_t b);

// data types
typedef enum
{
NORMAL_BLOCK,
CC_BLOCK // color code block
} BlockType;

typedef struct
{
  uint16_t signature;
  uint16_t x;
  uint16_t y;
  uint16_t width;
  uint16_t height;
  uint16_t angle; // angle is only available for color coded blocks
} Block;

// communication routines
static uint16_t getWord(void);
static int send(uint8_t *data, int len);

#ifndef SPI //////////// for I2C and UART

extern uint8_t getByte(void);
extern int sendByte(uint8_t byte);

uint16_t getWord(void)
{
  // this routine assumes little endian
  uint16_t w;
  uint8_t c;
  c = getByte();
  w = getByte();
  w <<= 8;
  w |= c;
  return w;
}

int send(uint8_t *data, int len)
{
  int i;
  for (i=0; i<len; i++)
sendByte(data);

  return len;
}

#else ///////////// SPI routines

// SPI sends as it receives so we need a getByte routine that
// takes an output data argument
extern uint8_t getByte(uint8_t out);

// variables for a little circular queue
static uint8_t g_outBuf[PIXY_OUTBUF_SIZE];
static uint8_t g_outLen = 0;
static uint8_t g_outWriteIndex = 0;
static uint8_t g_outReadIndex = 0;

uint16_t getWord()
{
  // ordering is big endian because Pixy is sending 16 bits through SPI
  uint16_t w;
  uint8_t c, cout = 0;

  if (g_outLen)
  {
w = getByte(PIXY_SYNC_BYTE_DATA);
cout = g_outBuf[g_outReadIndex++];
g_outLen--;
if (g_outReadIndex==PIXY_OUTBUF_SIZE)
   g_outReadIndex = 0;
  }
  else
w = getByte(PIXY_SYNC_BYTE); // send out sync byte
  w <<= 8;
  c = getByte(cout); // send out data byte
  w |= c;

  return w;
}

int send(uint8_t *data, int len)
{
  int i;

  // check to see if we have enough space in our circular queue
  if (g_outLen+len>PIXY_OUTBUF_SIZE)
return -1;

  g_outLen += len;
  for (i=0; i<len; i++)
  {
g_outBuf[g_outWriteIndex++] = data;
if (g_outWriteIndex==PIXY_OUTBUF_SIZE)
   g_outWriteIndex = 0;
  }
  return len;
}

#endif //////////////// end SPI routines

static int g_skipStart = 0;
static BlockType g_blockType;
static Block *g_blocks;

void init()
{
  g_blocks = (Block *)malloc(sizeof(Block)*PIXY_ARRAYSIZE);
}

int getStart(void)
{
  uint16_t w, lastw;

  lastw = 0xffff;

  while(1)
  {
w = getWord();
if (w==0 && lastw==0)
   return 0; // no start code
else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD)
{
   g_blockType = NORMAL_BLOCK;
   return 1; // code found!
}
else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD)
{
   g_blockType = CC_BLOCK; // found color code block
   return 1;
}
else if (w==PIXY_START_WORDX)
#ifdef SPI
   getByte(0); // we're out of sync! (backwards)
#else
   getByte(); // we're out of sync! (backwards)
#endif
lastw = w;
  }
}

uint16_t getBlocks(uint16_t maxBlocks)
{
  uint8_t i;
  uint16_t w, blockCount, checksum, sum;
  Block *block;

  if (!g_skipStart)
  {
if (getStart()==0)
   return 0;
  }
  else
g_skipStart = 0;

  for(blockCount=0; blockCount<maxBlocks && blockCount<PIXY_ARRAYSIZE;)
  {
checksum = getWord();
if (checksum==PIXY_START_WORD) // we've reached the beginning of the next frame
{
   g_skipStart = 1;
   g_blockType = NORMAL_BLOCK;
   return blockCount;
}
else if (checksum==PIXY_START_WORD_CC)
{
   g_skipStart = 1;
   g_blockType = CC_BLOCK;
   return blockCount;
}
else if (checksum==0)
   return blockCount;

block = g_blocks + blockCount;

for (i=0, sum=0; i<sizeof(Block)/sizeof(uint16_t); i++)
{
   if (g_blockType==NORMAL_BLOCK && i>=5) // no angle for normal block
   {
      block->angle = 0;
      break;
   }
   w = getWord();
   sum += w;
   *((uint16_t *)block + i) = w;
}

// check checksum
if (checksum==sum)
   blockCount++;
else
   printf("checksum error!\n");

w = getWord();
if (w==PIXY_START_WORD)
   g_blockType = NORMAL_BLOCK;
else if (w==PIXY_START_WORD_CC)
   g_blockType = CC_BLOCK;
else
   return blockCount;
  }
}

int setServos(uint16_t s0, uint16_t s1)
{
  uint8_t outBuf[6];

  outBuf[0] = 0x00;
  outBuf[1] = PIXY_SERVO_SYNC;
  *(uint16_t *)(outBuf + 2) = s0;
  *(uint16_t *)(outBuf + 4) = s1;

  return send(outBuf, 6);
}

int setBrightness(uint8_t brightness)
{
  uint8_t outBuf[3];

  outBuf[0] = 0x00;
  outBuf[1] = PIXY_CAM_BRIGHTNESS_SYNC;
  outBuf[2] = brightness;

  return send(outBuf, 3);
}

int setLED(uint8_t r, uint8_t g, uint8_t b)
{
  uint8_t outBuf[5];

  outBuf[0] = 0x00;
  outBuf[1] = PIXY_LED_SYNC;
  outBuf[2] = r;
  outBuf[3] = g;
  outBuf[4] = b;

  return send(outBuf, 5);
}[/mw_shl_code]