Mindsensors LightSensorArray scanner

This is a scanner I made, using the new mindsensors LightSensorArray. The LSA (LightSensorArray) has 8 light sensors spaced about 6.5mm apart. I used a sideways moving scan head, so that I essentially double the scan width resolution (take a reading, move about 3.3mm sideways, and take another reading). At the end of the scan, you can upload the image (as a monochrome .bmp) to the computer, and open it using a photo viewer. The image is 16 pixels tall, and as many pixels wide as the number of rows you told the scanner to scan.

Here is a video of it scanning:

Here is the actual image it scanned while I was taking the video:

Scanned image (enlarged 6 times, and reformatted to png for uploading to the blog)

And here are a few pictures of the scanner:

Lego NXT LSA Scanner – Right

Lego NXT LSA Scanner – Left

Lego NXT LSA Scanner – Bottom

You can download the LSA library (called “LSA-lib.nxc”) from mindsensors.com.

Here’s the NXC program the scanner is running:

#include "LSA-lib.nxc"

#include "File BMP lib.nxc"

#define DISPLAY 0

#define CM 35

#define DARK_THRESHOLD 40

#define LSA_PORT S1
#define LSA_ADDR 0x14
#define DRIVE_MOTOR OUT_A
#define HEAD_MOTOR OUT_B

#define HEAD_MOVE_WAIT 50

byte LSAValues[8];

#define HEAD_LEFT 1
#define HEAD_RIGHT 2
#define HEAD_DEFAULT 0

/*
  DRIVE_ADVANCE_TICKS =
    Distance required / (Wheel diameter * PI) * gear reduction * 360
    3.28125 / (43.2 * PI) * 3 * 3 * 360 = 78.334073553042235573747477284909
    For non-floating point
    3 * 3 * 360 * 3.28125 / (PI * 43.2)
*/

#define WHEEL_SIZE_MM 43.2
#define GEAR_REDUCTION 3*3
#define PIXEL_SPACING_MM 3.28125
#define TICKS_PER_ROTATION 360
//#define DRIVE_ADVANCE_TICKS (PIXEL_SPACING_MM / (WHEEL_SIZE_MM * PI) * GEAR_REDUCTION * TICKS_PER_ROTATION)      //78.334073553042235573747477284912
#define DRIVE_ADVANCE_TICKS (PIXEL_SPACING_MM * GEAR_REDUCTION * TICKS_PER_ROTATION / (WHEEL_SIZE_MM * PI))

#define ROWS ((CM * 10 / PIXEL_SPACING_MM) & 0xFFFFFFFF)

float DrivePosition;
long DrivePositionLong;

bool DriveArrive(byte Ths){
  long CurrentPosition = MotorTachoCount(DRIVE_MOTOR);
  return(((CurrentPosition < DrivePositionLong + Ths) && (CurrentPosition > DrivePositionLong - Ths)));
}

long Rows;

void MoveHead(byte dir = HEAD_DEFAULT){
  switch(dir){
    case HEAD_LEFT:
      OnFwd(HEAD_MOTOR, 100);
      Wait(HEAD_MOVE_WAIT);
      Off(HEAD_MOTOR);
      Wait(HEAD_MOVE_WAIT);
    break;
    case HEAD_RIGHT:
      OnRev(HEAD_MOTOR, 100);
      Wait(HEAD_MOVE_WAIT);
      Off(HEAD_MOTOR);
      Wait(HEAD_MOVE_WAIT);
    break;
    case HEAD_DEFAULT:

    break;
  }
}

byte Row[16];

byte LastLeft = 0;

void GetRow(){
  until(DriveArrive(2));
  LSA_ReadRaw_Calibrated (LSA_PORT, LSA_ADDR, LSAValues);
  for(byte i = 0; i < 8; i++){
    Row[(i*2)+LastLeft] = LSAValues[i];
  }
  if(LastLeft){
    MoveHead(HEAD_RIGHT);
    LastLeft = 0;
  }
  else{
    MoveHead(HEAD_LEFT);
    LastLeft = 1;
  }
  LSA_ReadRaw_Calibrated (LSA_PORT, LSA_ADDR, LSAValues);
  for(byte i = 0; i < 8; i++){
    Row[(i*2)+LastLeft] = LSAValues[i];
  }
}

#if DISPLAY
byte image[ROWS][16];

#define DISPLAY_PIXEL_SIZE 4
#define DISPLAY_PIXELS_WIDTH 100/DISPLAY_PIXEL_SIZE

void Display(){
  if(Rows < DISPLAY_PIXELS_WIDTH){
    for(byte i = 0; i < 16; i ++){
      if(Row[i] < DARK_THRESHOLD){
        RectOut(Rows * DISPLAY_PIXEL_SIZE, i * DISPLAY_PIXEL_SIZE, DISPLAY_PIXEL_SIZE - 1, DISPLAY_PIXEL_SIZE - 1, 0x20);
      }
      else{
        RectOut(Rows * DISPLAY_PIXEL_SIZE, i * DISPLAY_PIXEL_SIZE, DISPLAY_PIXEL_SIZE - 1, DISPLAY_PIXEL_SIZE - 1, 0x24);
      }
    }
  }
  else{
    for(byte x = 0; x < DISPLAY_PIXELS_WIDTH; x ++){
      for(byte y = 0; y < 16; y ++){
        if(image[Rows - (DISPLAY_PIXELS_WIDTH - x)][y] < DARK_THRESHOLD){
          RectOut(x * DISPLAY_PIXEL_SIZE, y * DISPLAY_PIXEL_SIZE, DISPLAY_PIXEL_SIZE - 1, DISPLAY_PIXEL_SIZE - 1, 0x20);
        }
        else{
          RectOut(x * DISPLAY_PIXEL_SIZE, y * DISPLAY_PIXEL_SIZE, DISPLAY_PIXEL_SIZE - 1, DISPLAY_PIXEL_SIZE - 1, 0x24);
        }
      }
    }
  }
}
#endif

task main(){
  MoveHead(HEAD_RIGHT);

  SetSensorLowspeed(LSA_PORT);
  LSA_ReadRaw_Calibrated (LSA_PORT, LSA_ADDR, LSAValues);

  SetMotorRegulationTime(5);             //Set the update speed (from my personal experience, this doesn't seem necessary, or even dynamic.
  PosRegEnable (DRIVE_MOTOR);            //Start the APR control
  PosRegSetMax (DRIVE_MOTOR, 0, 0);      //Set the max parameters (speed and acceleration).

  byte BMP_handle;
  BMP_handle = AcquireBitmapHandle();
  if (BMP_handle)
  {
    SetupBitmap(BMP_handle, ROWS, 16);
  
    while(true){
      GetRow();
      DrivePosition += DRIVE_ADVANCE_TICKS;
      DrivePositionLong = -DrivePosition;
      PosRegSetAngle(DRIVE_MOTOR, DrivePositionLong);

      for(byte i = 0; i < 16; i ++){
#if DISPLAY
        image[Rows][i] = Row[i];
#endif
        if(Row[i] < DARK_THRESHOLD){
          PointOutBitmap(BMP_handle, Rows, i, DRAW_OPT_NORMAL);
        }
      }

#if DISPLAY
      Display();
#endif

      Rows++;
      if(Rows >= ROWS)break;
      
      Wait(50);
    }
    SaveBitmapToFile(BMP_handle, "Scan.bmp");
    ReleaseBitmapHandle(BMP_handle);
  }
}

And here is the .bmp library:

FileReadWriteType __bmp_pixel_data;

byte __bmp_handle = 0;
unsigned int __bmp_width = 128;
unsigned int __bmp_height = 128;

byte __Bytes_Per_Line;

#define BMP_PIXEL_DATA(_handle) __bmp_pixel_data.Buffer

inline bool ValidBitmapHandle(byte handle) {
  return (handle && (handle == __bmp_handle));
}

inline unsigned long __getBitmapPixelDataSize()
{
  unsigned long result;
  byte delta = __bmp_width % 32;
  if (delta){
    result = (__bmp_width + 32 - delta) * __bmp_height;
  }
  else{
    result = __bmp_width * __bmp_height;
  }
  result /= 8;
  return result;
}

inline bool ValidBitmapPixelData(byte handle) {
  bool vbpd_result = false;
  if (ValidBitmapHandle(handle))
    vbpd_result = (ArrayLen(BMP_PIXEL_DATA(handle)) == __getBitmapPixelDataSize());
  return vbpd_result;
}

inline unsigned int GetBitmapWidth(byte handle) {
  if (ValidBitmapHandle(handle))
    return __bmp_width;
  else
    return 0;
}

inline void SetBytesPerLine(byte handle){
  if (!ValidBitmapHandle(handle))return;
  __Bytes_Per_Line = (GetBitmapWidth(handle) + 7) / 8;     // Get the minimum number of bytes used.
  byte delta = __Bytes_Per_Line % 4;
  if (delta) __Bytes_Per_Line += 4-delta;
}

inline unsigned int GetBitmapHeight(byte handle) {
  if (ValidBitmapHandle(handle))
    return __bmp_height;
  else
    return 0;
}

inline void SetBitmapWidth(byte handle, unsigned int w) {
  if (ValidBitmapHandle(handle))
    __bmp_width = w;
}

inline void SetBitmapHeight(byte handle, unsigned int h) {
  if (ValidBitmapHandle(handle))
    __bmp_height = h;
}

inline void ClearBitmap(byte handle){
  if (ValidBitmapHandle(handle)) {
    unsigned long size = __getBitmapPixelDataSize();
    ArrayInit(BMP_PIXEL_DATA(handle), 0xFF, size);
  }
}

#define AllocateBitmap(_handle) ClearBitmap(_handle)

inline void SetupBitmap(byte handle, unsigned int width, unsigned int height){
  SetBitmapWidth(handle, width);
  SetBitmapHeight(handle, height);
  SetBytesPerLine(handle);
  AllocateBitmap(handle);
}

void PointOutBitmap(byte handle, unsigned int x, unsigned int y, unsigned int options = DRAW_OPT_NORMAL)
{
  if (!ValidBitmapPixelData(handle)) return;

  unsigned int Y_Offset = y * __Bytes_Per_Line;
  unsigned int X_Offset = x / 8;
  unsigned int data_pointer = Y_Offset + X_Offset;
  byte mask = 0x80 >> (x % 8);
  switch (options&0x1C){
    case DRAW_OPT_NORMAL:
      BMP_PIXEL_DATA(handle)[data_pointer] &= (~mask);
    break;
    case DRAW_OPT_CLEAR:
      BMP_PIXEL_DATA(handle)[data_pointer] |= (0xFF&mask);
    break;
  }
}

//  C:\Users\Family\Desktop\Matt\My Dropbox\computer sharing\Lego\NXT\NXT Firmware\Enhanced\John's\EFW source 1.32\AT91SAM7S256\Source\c_cmd_drawing.inc
void LineOutBitmap(byte handle, unsigned int x1, unsigned int y1, unsigned int x2, unsigned int y2, unsigned int options = DRAW_OPT_NORMAL)
{
  unsigned int height = GetBitmapHeight(handle);
  unsigned int width  = GetBitmapWidth(handle);

  long tx, ty;
  long dx, dy;

  dx = x2-x1;
  dy = y2-y1;

  //Clip line ends vertically - easier if y1<y2:
  if (y1 > y2) {tx=x1; x1=x2; x2=tx;
                ty=y1; y1=y2; y2=ty;}

  //Is line completely off screen?
  if (y2<0 || y1>=height) return;

  //Trim y1 end:
  if (y1 < 0)
  {
    if (dx && dy)
      x1 = x1 + (((0-y1)*dx)/dy);
    y1 = 0;
  }
  //Trim y2 end:
  if (y2 > height-1)
  {
    if (dx && dy)
      x2 = x2 - (((y2-(height-1))*dx)/dy);
    y2 = height-1;
  }

  //Clip horizontally - easier if x1<x2
  if (x1 > x2) {tx=x1; x1=x2; x2=tx;
                ty=y1; y1=y2; y2=ty;}

  //Is line completely off screen?
  if (x2<0 || x1>=width) return;

  //Trim x1 end:
  if (x1 < 0)
    {
      if (dx && dy)
        y1 = y1 + (((0-x1)*dy)/dx);
      x1 = 0;
    }
  //Trim x2 end:
  if (x2 > width-1)
    {
      if (dx && dy)
        y2 = y2 - (((x2-(width-1))*dy)/dx);
      x2 = width-1;
    }

  dx = x2-x1;
  dy = y2-y1;

  if (x1 == x2) {
    // single point
    if (y1 == y2)
      PointOutBitmap(handle, x1, y1, options);
    // vertical line
    else
    {
      for(long i = 0; i<(dy+1); i++){
        PointOutBitmap(handle, x1, y1 + i, options);
      }
    }
  }
    // horzontal line
  else if (y1 == y2) {
      for(long i = 0; i<(dx+1); i++){
        PointOutBitmap(handle, x1 + i, y1, options);
      }
  }
  else {
    long d,x,y,ax,ay,sx,sy;
    // Initialize variables
    dx = x2-x1;  ax = abs(dx)<<1;  sx = sign(dx);
    dy = y2-y1;  ay = abs(dy)<<1;  sy = sign(dy);
    x = x1;
    y = y1;
    if (ax>ay)
    {    // x dominant
      d = ay-(ax>>1);
      for (;;)
      {
        PointOutBitmap(handle, x, y, options);
        if (x==x2)
          return;
        if (d>=0)
        {
          y += sy;
          d -= ax;
        }
        x += sx;
        d += ay;
      }
    }
    else
    {      // y dominant
      d = ax-(ay>>1);
      for (;;)
      {
        PointOutBitmap(handle, x, y, options);
        if (y==y2)
          return;
        if (d>=0)
        {
          x += sx;
          d -= ay;
        }
        y += sy;
        d += ax;
      }
    }
  }
}

void RectOutBitmap(byte handle, unsigned int left, unsigned int bottom, unsigned int width, unsigned int height, unsigned int options = DRAW_OPT_NORMAL)
{
  unsigned int x1, y1;
  unsigned int x2, y2;

  x1 = left;
  x2 = left + width;

  y1 = bottom;
  y2 = bottom + height;

  if (y2 == y1 || x2 == x1) {
    // height == 0 so draw a single pixel horizontal line OR
    // width == 0 so draw a single pixel vertical line
    LineOutBitmap(handle, x1, y1, x2, y2, options);
    return;
  }
  // rectangle has abs(width) or abs(height) >= 1
  if (options & DRAW_OPT_FILL_SHAPE)
  {
    if (x1>__bmp_width-1 || y1>__bmp_height-1) return;
    if (x2>__bmp_width-1) x2=__bmp_width-1;
    if (y2>__bmp_height-1) y2=__bmp_height-1;
    for(unsigned int i = 0; i<(width+1); i++){
      //LineOutBitmap(handle, x1+i, y1, x1+i, y2, options);        // same function, but slower
      for(unsigned int ii = 0; ii < (height+1); ii++){
        PointOutBitmap(handle, left + i, bottom + ii, options);
      }
    }
  }
  else
  {
    //Use the full line drawing functions rather than horizontal/vertical
    //functions so these get clipped properly.  These will fall straight
    //through to the faster functions anyway.
    //Also don't re-draw parts of slim rectangles since XOR might be on.

    LineOutBitmap(handle, x1, y1, x2, y1, options);
    if (y2>y1)
    {
      LineOutBitmap(handle, x1, y2, x2, y2, options);
      if (y2 > y1+1)
      {
        LineOutBitmap(handle, x2, y1+1, x2, y2-1, options);
        if (x2>x1)
          LineOutBitmap(handle, x1, y1+1, x1, y2-1, options);
      }
    }
  }
}

/*

  definately needs work for the DRAW_OPT_FILL_SHAPE mode.

*/

#if 0

void EllipseOutBitmap(byte handle, unsigned int xc, unsigned int yc, unsigned int a, unsigned int b, unsigned int options = DRAW_OPT_NORMAL)
{
  unsigned int x = 0, y = b;
  unsigned int rx = x, ry = y;
  unsigned int width = 1;
  unsigned int height = 1;
  long a2 = a*a;
  long b2 = b*b;
  long crit1 = -(a2/4 + a%2 + b2);
  long crit2 = -(b2/4 + b%2 + a2);
  long crit3 = -(b2/4 + b%2);
  long t = -a2*y;
  long dxt = 2*b2*x, dyt = -2*a2*y;
  long d2xt = 2*b2, d2yt = 2*a2;

  if (b == 0) {
    RectOutBitmap(handle, xc-a, yc, 2*a, 0, options);
    return;
  }
  if (a == 0) {
    RectOutBitmap(handle, xc, yc-b, 0, 2*b, options);
    return;
  }

  while (y>=0 && x<=a)
  {
    if (!(options & DRAW_OPT_FILL_SHAPE))
    {
      PointOutBitmap(handle, xc+x, yc+y, options);
      if (x!=0 || y!=0)
        PointOutBitmap(handle, xc-x, yc-y, options);
      if (x!=0 && y!=0)
      {
        PointOutBitmap(handle, xc+x, yc-y, options);
        PointOutBitmap(handle, xc-x, yc+y, options);
      }
    }
    if (t + b2*x <= crit1 ||
        t + a2*y <= crit3)
    {
      if (options & DRAW_OPT_FILL_SHAPE)
      {
        if (height == 1)
          ; /* draw nothing */
        else if (ry*2+1 > (height-1)*2)
        {
          RectOutBitmap(handle, xc-rx, yc-ry, width-1, height-1, options);
          RectOutBitmap(handle, xc-rx, yc+ry, width-1, -(height-1), options);
          ry -= height-1;
          height = 1;
        }
        else
        {
          RectOutBitmap(handle, xc-rx, yc-ry, width-1, ry*2, options);
          ry -= ry;
          height = 1;
        }
        rx++;
        width += 2;
      }
      x++;
      dxt += d2xt;
      t += dxt;
    }
    else if (t - a2*y > crit2)   /* e(x+1/2,y-1) > 0 */
    {
      y--;
      dyt += d2yt;
      t += dyt;
      if (options & DRAW_OPT_FILL_SHAPE)
        height++;
    }
    else
    {
      if (options & DRAW_OPT_FILL_SHAPE)
      {
        if (ry*2+1 > height*2)
        {
          RectOutBitmap(handle, xc-rx, yc-ry, width-1, height-1, options);
          RectOutBitmap(handle, xc-rx, yc+ry, width-1, -(height-1), options);
        }
        else
        {
          RectOutBitmap(handle, xc-rx, yc-ry, width-1, ry*2, options);
        }
        width += 2;
        ry -= height;
        height = 1;
        rx++;
      }
      x++;
      dxt += d2xt;
      t += dxt;
      y--;
      dyt += d2yt;
      t += dyt;
    }
  }
  if (options & DRAW_OPT_FILL_SHAPE)
  {
    if (ry > height) {
      RectOutBitmap(handle, xc-rx, yc-ry, width-1, height-1, options);
      RectOutBitmap(handle, xc-rx, yc+ry, width-1, -(height-1), options);
    }
    else {
      RectOutBitmap(handle, xc-rx, yc-ry, width-1, ry*2, options);
    }
  }
}

#else

void EllipseOutBitmap(byte handle, unsigned int xc, unsigned int yc, unsigned int width, unsigned int height, unsigned int options = DRAW_OPT_NORMAL)
{
  unsigned int LargeRadius = width>height?width:height;                         // Determine the number of points needed, and set the advance number accordinly.
  float AdvSize = 360 / ((LargeRadius * PI)*2);
//  float radius;
  float lx, ly;
  for (float angle = 0; angle < 360; angle += AdvSize){
    float x = width  * sind(angle);
    float y = height * cosd(angle);
//    radius = sqrt(x*x + y*y);        // (A squared) + (B squared) = (C squared)
//    AdvSize = 360 / ((radius*PI)*2);

    x += -sign(x)*0.3;
    y += -sign(y)*0.3;
    
    unsigned int t = x;
    x = t;
    t = y;
    y = t;
    
    if(abs(x)>=abs(y)){      // x is prominant
      if(!((x != lx) && (ly == y))){
        PointOutBitmap(handle, x + xc, y + yc, options);
      }
    }
    else if(abs(y)>abs(x)){  // y is prominant
      if(!((y != ly) && (lx == x))){
        PointOutBitmap(handle, x + xc, y + yc, options);
      }
    }
    
    lx = x;
    ly = y;
  }
}

#endif

void CircleOutBitmap(byte handle, unsigned int cx, unsigned int cy, unsigned int radius, unsigned int options = DRAW_OPT_NORMAL)  //JJR
{
  EllipseOutBitmap(handle, cx, cy, radius, radius, options);
}

safecall byte AcquireBitmapHandle() {
  if (__bmp_handle) return 0; // already acquired
  __bmp_handle = 1;
  return __bmp_handle;
}

safecall void ReleaseBitmapHandle(byte handle)
{
  if (ValidBitmapHandle(handle))
    __bmp_handle = 0;
}

byte __bmp_magic[] = {0x42, 0x4D, 0x00}; // "BM"
byte __bmp_bw_color_table[] = {0x00, 0x00, 0x00, 0x00,
                               0xFF, 0xFF, 0xFF, 0x00, 0x00};

struct BITMAPFILEHEADER {
  unsigned long bfSize;
  unsigned int bfReserved1;
  unsigned int bfReserved2;
  unsigned long bfOffBits;
};

struct BITMAPINFOHEADER {
  unsigned long biSize;
  long  biWidth;
  long  biHeight;
  unsigned int  biPlanes;
  unsigned int  biBitCount;
  unsigned long biCompression;
  unsigned long biSizeImage;
  long  biXPelsPerMeter;
  long  biYPelsPerMeter;
  unsigned long biClrUsed;
  unsigned long biClrImportant;
};

struct BITMAPCOREHEADER {
  unsigned long bcSize;
  unsigned int  bcWidth;
  unsigned int  bcHeight;
  unsigned int  bcPlanes;
  unsigned int  bcBitCount;
};

enum bmp_compression_t {
  BI_RGB = 0,
  BI_RLE8,
  BI_RLE4,
  BI_BITFIELDS, //Also Huffman 1D compression for BITMAPCOREHEADER2
  BI_JPEG,      //Also RLE-24 compression for BITMAPCOREHEADER2
  BI_PNG
};

bool SaveBitmapToFile(byte handle, string filename)
{
  bool result = false;
  if (!ValidBitmapPixelData(handle))
    return result;
  
  // build the bitmap info header
  BITMAPINFOHEADER bih;
  bih.biSize = SizeOf(bih);
  bih.biWidth = __bmp_width;
  bih.biHeight = __bmp_height;
  bih.biPlanes = 1;
  bih.biBitCount = 1;
  bih.biCompression = BI_RGB;
  bih.biSizeImage = 0;
  bih.biXPelsPerMeter = 2835;
  bih.biYPelsPerMeter = 2835;
  bih.biClrUsed = 0;
  bih.biClrImportant = 0;

  // build the bitmap file header
  BITMAPFILEHEADER bfh;
  bfh.bfOffBits = StrLen(__bmp_magic);
  bfh.bfOffBits += SizeOf(bfh);
  bfh.bfOffBits += bih.biSize;
  bfh.bfOffBits += StrLen(__bmp_bw_color_table);
  bfh.bfReserved1 = 0;
  bfh.bfReserved2 = 0;
  bfh.bfSize = bfh.bfOffBits + ArrayLen(BMP_PIXEL_DATA(handle));

  // save it to a file
  remove(filename);
  byte fh;

  int res = CreateFile(filename, bfh.bfSize, fh);
  if (res == LDR_SUCCESS)
  {
    FileReadWriteType rwArgs;

    // write the magic bytes
    rwArgs.FileHandle = fh;
    rwArgs.Buffer = __bmp_magic;
    rwArgs.Length = StrLen(__bmp_magic);
    SysFileWrite(rwArgs);

    // write the file header
    Write(fh, bfh);

    // write the info header
    Write(fh, bih);

    // write the color table
    rwArgs.Buffer = __bmp_bw_color_table;
    rwArgs.Length = StrLen(__bmp_bw_color_table);
    SysFileWrite(rwArgs);

    // now write the pixel data
    __bmp_pixel_data.FileHandle = fh;
    __bmp_pixel_data.Length = ArrayLen(__bmp_pixel_data.Buffer);
    SysFileWrite(__bmp_pixel_data);

    fclose(fh);
    result = true;
  }
  return result;
}

Advertisement