NXT Measuring wheel

Here is a measuring wheel I made using the NXT and a mindsensors GlideWheel-AS. The NXT displays traveled distance in mm, cm, M, in, ft, or yd, and will automatically show the proper number of significant figures.


The only thing you have to modify in the code, is WHEEL_DIA (wheel diameter in mm), on line six. Note that nearly all (if not all) Lego technic tires have their size molded into the sidewall, in mm, in the format “diameter x width”; so the “RCX wheels” are “81.6 x 15″.

You can optionally modify AS_PORT and AS_ADDR as needed.

You will need the mindsensors library for the sensor, called “AngleSensor-lib.nxc”.

Here is the NXC program for the measuring wheel:

#include "AngleSensor-lib.nxc"

#define AS_PORT S1
#define AS_ADDR 0x30

#define WHEEL_DIA 81.6 // wheel diameter in mm
#define WHEEL_CIR (WHEEL_DIA * PI)

#define TICKS_PER_ROTATION 720

#define DISTANCE_PER_TICK (WHEEL_CIR / TICKS_PER_ROTATION)

long raw;
float Rotations;
float Distance_mm;
float Distance_cm;
float Distance_mt;
float Distance_in;
float Distance_ft;
float Distance_yd;
float Distance_Display;

#define UNITS 6
char unit = 1;

string Value_String;
byte digit;
byte x;
byte Display_Precision;
float Unit_Distance_Per_Tick;

task main(){
  SetSensorLowspeed(AS_PORT);
  AS_ResetAngle(S1, 0x30);
  while(true){

    if ( ButtonPressed(BTNCENTER, true)){
      while (ButtonPressed(BTNCENTER, true));
      AS_ResetAngle(S1, 0x30);
    }
    
    if ( ButtonPressed(BTNLEFT, true)){
      while (ButtonPressed(BTNLEFT, true));
      unit--;
      if(unit < 1)unit = UNITS;
    }

    if ( ButtonPressed(BTNRIGHT, true)){
      while (ButtonPressed(BTNRIGHT, true));
      unit++;
      if(unit > UNITS)unit = 1;
    }
    
    switch(unit){
      case 1:
        Unit_Distance_Per_Tick = DISTANCE_PER_TICK;
        TextOut(84, LCD_LINE1, "mm");
      break;
      case 2:
        Unit_Distance_Per_Tick = DISTANCE_PER_TICK / 10;
        TextOut(84, LCD_LINE1, "cm");
      break;
      case 3:
        Unit_Distance_Per_Tick = DISTANCE_PER_TICK / 1000;
        TextOut(84, LCD_LINE1, "M ");
      break;
      case 4:
        Unit_Distance_Per_Tick = DISTANCE_PER_TICK / 25.4;
        TextOut(84, LCD_LINE1, "In");
      break;
      case 5:
        Unit_Distance_Per_Tick = DISTANCE_PER_TICK / 304.8;
        TextOut(84, LCD_LINE1, "Ft");
      break;
      case 6:
        Unit_Distance_Per_Tick = DISTANCE_PER_TICK / 914.4;
        TextOut(84, LCD_LINE1, "Yd");
      break;
    }

    raw = AS_ReadRawValue(AS_PORT, AS_ADDR);
    
    Distance_Display = raw * Unit_Distance_Per_Tick;

    if(Unit_Distance_Per_Tick >= 1){
      Display_Precision = 0;
    }
    else{
      if(Unit_Distance_Per_Tick >= 0.1){
        Display_Precision = 1;
      }
      else{
        if(Unit_Distance_Per_Tick >= 0.01){
          Display_Precision = 2;
        }
        else{
          if(Unit_Distance_Per_Tick >= 0.001){
            Display_Precision = 3;
          }
          else{
            Display_Precision = 4;
          }
        }
      }
    }

    x = 0;
    if(Distance_Display >= 0){
      TextOut(x, LCD_LINE1, " ");
    }
    else{
      TextOut(x, LCD_LINE1, "-");
      Distance_Display *= (-1);
    }

    if(Distance_Display >= 10000000 && unit < 3){
      unit++;
    }

    x += 6;
    if(Distance_Display >= 1000000){
      digit = (Distance_Display / 1000000) %10;
      NumOut(x, LCD_LINE1, digit);
    }
    else{
      TextOut(x, LCD_LINE1, " ");
    }

    x += 6;
    if(Distance_Display >= 100000){
      digit = (Distance_Display / 100000) %10;
      NumOut(x, LCD_LINE1, digit);
    }
    else{
      TextOut(x, LCD_LINE1, " ");
    }

    x += 6;
    if(Distance_Display >= 10000){
      digit = (Distance_Display / 10000) %10;
      NumOut(x, LCD_LINE1, digit);
    }
    else{
      TextOut(x, LCD_LINE1, " ");
    }

    x += 6;
    if(Distance_Display >= 1000){
      digit = (Distance_Display / 1000) %10;
      NumOut(x, LCD_LINE1, digit);
    }
    else{
      TextOut(x, LCD_LINE1, " ");
    }
    
    x += 6;
    if(Distance_Display >= 100){
      digit = (Distance_Display / 100) %10;
      NumOut(x, LCD_LINE1, digit);
    }
    else{
      TextOut(x, LCD_LINE1, " ");
    }
    
    x += 6;
    if(Distance_Display >= 10){
      digit = (Distance_Display / 10) %10;
      NumOut(x, LCD_LINE1, digit);
    }
    else{
      TextOut(x, LCD_LINE1, " ");
    }
    
    x += 6;
    digit = Distance_Display %10;
    NumOut(x, LCD_LINE1, digit);

    x += 6;
    if(Display_Precision){
      TextOut(x, LCD_LINE1, ".");
      x += 6;
      digit = (Distance_Display * 10) %10;
      NumOut(x, LCD_LINE1, digit);

      x += 6;
      if(Display_Precision >= 2){

        digit = (Distance_Display * 100) %10;
        NumOut(x, LCD_LINE1, digit);
        
        x += 6;
        if(Display_Precision >= 3){
          digit = (Distance_Display * 1000) %10;
          NumOut(x, LCD_LINE1, digit);

          x += 6;
          if(Display_Precision >= 4){
            digit = (Distance_Display * 10000) %10;
            NumOut(x, LCD_LINE1, digit);
          }
          else{
            TextOut(x, LCD_LINE1, " ");
          }
        }
        else{
          TextOut(x, LCD_LINE1, "  ");
        }
      }
      else{
        TextOut(x, LCD_LINE1, "   ");
      }
    }
    else{
      TextOut(x, LCD_LINE1, "     ");
    }
  }
}
Posted in Electronics, Mindsensors, Mindstorms, NXT | Leave a comment

NXTChuck pong game

Here is a pong game I made for the NXT. It uses the NXTChuck to read a Wii Nunchuk, and uses the values as the user input to the game.

In the video I was using two NXTChucks and Nunchuks, in two-player game mode. If you want to play single-player, you can change just one line of the program so that it compiles for single-player mode.

You need to have the NXC NXTChuck library, available from Dexter Industries’ downloads page.

Lines 3 through 16 are constants that determine the characteristics of the game. Most of them are fairly self-explanatory. Try changing them, and don’t worry, I have listed all the original values in the program, so you can always change it back really easily. Line 16 tells the compiler to compile for 1 or for 2 controllers.

#include "NXTChuck lib.nxc"

#define SpeedUpAmount      5  // 5
#define InitialSpeed       50 // 50
#define InitialBallSize    4  // 4
#define InitialPaddleWidth 10 // 10

#define MinX 1                // 1
#define MaxX 98               // 98
#define MinY 1                // 0
#define MaxY 55               // 55

#define NXTChuck_1_PORT S1    // S1
#define NXTChuck_2_PORT S2    // S2

#define Controllers 2         // 2

byte N1_SX, N1_SY;
int N1_AX, N1_AY, N1_AZ;
byte N1_B;

#if Controllers == 2
byte N2_SX, N2_SY;
int N2_AX, N2_AY, N2_AZ;
byte N2_B;
#endif

long LeftScore, RightScore, TotalScore;     // scores

float TotalSpeed;                      // ball speed
byte BallRadius;                       // ball size
byte PaddleHalfWidth;                  // paddle size

int PaddleL, PaddleR;                  // paddle positions

int BallXspeedBase, BallYspeedBase;    // base speed, expenced to be in the range of 0-100
char BallXdir, BallYdir;               // ball travle direction
float BallXPos, BallYPos;              // current position
int BallXlast, BallYlast;    // was a byte, and should be fine as such         // last position, for erasing the last frame

unsigned long LastTick, ThisTick;

#define Clip(in,min,max) (in<min?min:(in>max?max:in))

inline long Round(float Value){
  long Result = Value > 0 ? Value + 0.5 : Value - 0.5 ;
  return Result;
}

float ScaleRange(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)
{
  return (DesiredMax - DesiredMin) * (Value - ValueMin) / (ValueMax - ValueMin) + DesiredMin;
}

int ScaleRangeInt(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)
{
  return (DesiredMax - DesiredMin) * (Value - ValueMin) / (ValueMax - ValueMin) + DesiredMin;
}

void InitializeValues(bool First){
  LeftScore = 0;
  RightScore = 0;
  TotalScore = 0;

  TotalSpeed = InitialSpeed;
  BallRadius = InitialBallSize;
  PaddleHalfWidth = InitialPaddleWidth;

  BallXspeedBase = ScaleRange(Random(), -32768, 32767, 45, 55);
  BallYspeedBase = 100 - BallXspeedBase;

  BallXdir = Random()&1?1:-1;
  Wait(Random(25));
  BallYdir = Random()&1?1:-1;

  BallXPos = (MaxX - MinX) / 2;
  BallYPos = (MaxY - MinY) / 2;

  if(First){
    ClearScreen();
    TextOut(12, LCD_LINE1, "Initializing");
  
    until(NXTChuckInit(NXTChuck_1_PORT, false) == NXTChuck_COM_SUCCESS);
    until(NXTChuckIdent(NXTChuck_1_PORT) == NXTChuck_DEVICE_NUNCHUK);

#if Controllers == 2
    until(NXTChuckInit(NXTChuck_2_PORT, false) == NXTChuck_COM_SUCCESS);
    until(NXTChuckIdent(NXTChuck_2_PORT) == NXTChuck_DEVICE_NUNCHUK);
#endif

    ClearScreen();
    TextOut(23, LCD_LINE1, "Press z");
    TextOut(23, LCD_LINE2, "to start.");
  }
  else{
    TextOut(17, LCD_LINE7, "Press z to");
    TextOut(23, LCD_LINE8, "restart.");
  }

#if Controllers == 2
  while(!(N1_B&N2_B&NXTChuck_N_BTN_Z)){
    NXTChuckReadNunchuk(NXTChuck_2_PORT, N2_SX, N2_SY, N2_AX, N2_AY, N2_AZ, N2_B);
#else
  while(!(N1_B&NXTChuck_N_BTN_Z)){
#endif
    NXTChuckReadNunchuk(NXTChuck_1_PORT, N1_SX, N1_SY, N1_AX, N1_AY, N1_AZ, N1_B);

    Wait(10);
  }
  
  ClearScreen();
  LastTick = CurrentTick();
}

void UpdateDisplay(){
  CircleOut(BallXlast, BallYlast, BallRadius, 0x36);
  BallXlast = Round(BallXPos);
  BallYlast = Round(BallYPos);
  CircleOut(BallXlast, BallYlast, BallRadius, 0x32);

  LineOut(MinX - 1, PaddleL-PaddleHalfWidth, MinX - 1, PaddleL + PaddleHalfWidth);
  LineOut(MaxX + 1, PaddleR-PaddleHalfWidth, MaxX + 1, PaddleR + PaddleHalfWidth);
  
  LineOut(0, 55, 99, 55);
  LineOut(0, 0, 99, 0);

  ClearLine(LCD_LINE1);
  NumOut(37, LCD_LINE1, TotalScore);
  NumOut(0, LCD_LINE1, LeftScore);
  NumOut(75, LCD_LINE1, RightScore);
}

void ChangeBaseSpeeds(){
  char OffsetFromEven = BallXspeedBase - 50;
  BallXspeedBase += Round(ScaleRange(Random(), -32768, 32767, OffsetFromEven>0?-5:-4, OffsetFromEven<0?5:4));
  BallYspeedBase = 100 - BallXspeedBase;

  if(BallXspeedBase > 90){
    BallXspeedBase = 90;
    BallYspeedBase = 10;
  }
  else if(BallXspeedBase < 10){
    BallXspeedBase = 10;
    BallYspeedBase = 90;
  }
}

char CheckEdge(){
  if(BallYPos < (MinY + BallRadius)){
    BallYdir = -BallYdir;
    BallYPos = MinY + BallRadius;
    ChangeBaseSpeeds();
  }
  if(BallYPos > (MaxY - BallRadius)){
    BallYdir = -BallYdir;
    BallYPos = MaxY - BallRadius;
    ChangeBaseSpeeds();
  }

  char result = 0;
  
  if(BallXPos < (MinX + BallRadius)){
    if(Round(BallYPos) > (PaddleL + PaddleHalfWidth) || Round(BallYPos) < (PaddleL - PaddleHalfWidth)){
      result = -1;
    }
    else{
      result = 1;
    }
    BallXdir = -BallXdir;
    BallXPos = MinX + BallRadius;
    ChangeBaseSpeeds();
  }
  
  if(BallXPos > (MaxX - BallRadius)){
    if(Round(BallYPos) > (PaddleR + PaddleHalfWidth) || Round(BallYPos) < (PaddleR - PaddleHalfWidth)){
      result = -2;
    }
    else{
      result = 2;
    }
    BallXdir = -BallXdir;
    BallXPos = MaxX - BallRadius;
    ChangeBaseSpeeds();
  }
  
  return result;
}

char Edge;

task Game(){
  InitializeValues(true);
  while(true){

    NXTChuckReadNunchuk(NXTChuck_1_PORT, N1_SX, N1_SY, N1_AX, N1_AY, N1_AZ, N1_B);
    PaddleL = Clip(ScaleRangeInt(N1_SY, 0, 255, MinY + PaddleHalfWidth, MaxY - PaddleHalfWidth), MinY + PaddleHalfWidth, MaxY - PaddleHalfWidth);

#if Controllers == 2
    NXTChuckReadNunchuk(NXTChuck_2_PORT, N2_SX, N2_SY, N2_AX, N2_AY, N2_AZ, N1_B);
    PaddleR = Clip(ScaleRangeInt(N2_SY, 0, 255, MinY + PaddleHalfWidth, MaxY - PaddleHalfWidth), MinY + PaddleHalfWidth, MaxY - PaddleHalfWidth);
#else
    PaddleR = PaddleL;
#endif

    ThisTick = CurrentTick();
    
    BallXPos += (ScaleRange((BallXspeedBase * BallXdir), -100, 100, -TotalSpeed, TotalSpeed) * (ThisTick - LastTick) / 1000 );
    BallYPos += (ScaleRange((BallYspeedBase * BallYdir), -100, 100, -TotalSpeed, TotalSpeed) * (ThisTick - LastTick) / 1000 );
    
    LastTick = ThisTick;

    Edge = CheckEdge();
    if(Edge == 0){
    
    }
    else if(Edge == 1){
      LeftScore++;
      TotalScore++;
      TotalSpeed += SpeedUpAmount;
    }
    else if(Edge == 2){
      RightScore++;
      TotalScore++;
      TotalSpeed += SpeedUpAmount;
    }
    else if(Edge == -1 || Edge == -2){
      ClearScreen();
      NumOut(37, LCD_LINE1, TotalScore);
      NumOut(0, LCD_LINE1, LeftScore);
      NumOut(75, LCD_LINE1, RightScore);
      TextOut(18, LCD_LINE2, "Game Over,");
      if(Edge == -1){
        TextOut(18, LCD_LINE3, "Left Lost.");
      }
      if(Edge == -2){
        TextOut(15, LCD_LINE3, "Right Lost.");
      }
      TextOut(0, LCD_LINE5, "Speed Was =");
      NumOut(72, LCD_LINE5, TotalSpeed);

      InitializeValues(false);
    }

    UpdateDisplay();
    
    Wait(10);
  }
}

task main(){
  start Game;
}
Posted in Dexter Industries, Mindstorms, NXC, NXT | Tagged , , , , | 3 Comments

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;
}
Posted in Mindsensors, Mindstorms, NXC, NXT | Tagged , , , , | Leave a comment

Mindsensors Light Sensor Array

Mindsensors is working on a new NXT sensor, called a Light Sensor Array. It’s similar in form factor to the LineLeader that they have sold previously, but it doesn’t have a PID algorithm built in. Instead, it just gives you access to the individual light sensors, and their binary states. The reason for this, is that the LineLeader was disqualified from many competitions, since it was too much of a “canned” line follower (it did all the necessary calculations on board).

I made a function that you can use to determine where a line is compared to the sensor. It returns a number from 10-80 based on where it “sees” the line. 45 is the middle, so you can compare the position to 45 to determine the offset from being centered on the line. 0 represents no line detected. Use this in conjunction with the LSA library provided by mindsensors.

float ScaleRange(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)
{
  return (DesiredMax - DesiredMin) * (Value - ValueMin) / (ValueMax - ValueMin) + DesiredMin;
}

float LSA_Get_Average(byte port, byte addr, byte & Bool_LSA_Values, byte & LSA_Array_Values[], byte LSA_Threshold = 40){
  Bool_LSA_Values = 0;                                                          // Reset from last state
  int bit_index_i = 0;                                                          // The bit it's working on
  int averaging_weight_i = 10;                                                  // The weight to add the the Weighted_Position, if the bit is 1
  byte total_bits = 0;                                                          // The number of bits that are true. This is needed for the division at the end
  bool set_bit;                                                                 // The bit it's working on
  char low_bit = 0;                                                             // The bit just below the lowest 1
  char high_bit = 7;                                                            // The bit just above the highest 1

  LSA_ReadRaw_Calibrated(port, addr, LSA_Array_Values);                         // Fill LL_Array_Values with the calibrated values of the 8 sensors

  for (byte i = 0; i<8; i++){                                                   // Build a byte with the bits set to the boolean state of the sensors
    Bool_LSA_Values = Bool_LSA_Values | ((LSA_Array_Values[i]<=LSA_Threshold?1:0)<<i);
  }

  float Weighted_Position = 0;                                                  // The value that will contain the overall weighted avarage
  float Weighted_Position_Adder = 0;                                            // The value that will contain the average adder, based on the effected, but not true sensors
  float Absolute_Average = 0;

  repeat(8){
    set_bit = Bool_LSA_Values >> bit_index_i & 0x01;                            // Seperate the bits out of the byte
    Weighted_Position += set_bit * averaging_weight_i;                          // Add the 1's to the value
    total_bits += set_bit;                                                      // If the bit was 1, add 1 to the counter (divide by the sum at the end).
    averaging_weight_i+=10;                                                     // Set the variables for the next round
    bit_index_i++;                                                              //                    ''
  }

  until((Bool_LSA_Values >> high_bit) & 0x01){                                  // Determine the bit just above the highest set bit
    high_bit--;
    if(high_bit==-1)break;
  }
  high_bit++;

  until((Bool_LSA_Values >> low_bit) & 0x01){                                   // Determine the bit just below the lowest set bit
    low_bit++;
    if(low_bit==8)break;
  }
  low_bit--;

  if (0 < high_bit && high_bit < 8 && -1 < low_bit && low_bit < 7){             // Both ends can be used
    Weighted_Position_Adder = (ScaleRange(LSA_Array_Values[low_bit], LSA_Threshold, 100, 0, 100) - ScaleRange(LSA_Array_Values[high_bit], LSA_Threshold, 100, 0, 100)) / 20;
  }
  if(high_bit == 8 && low_bit == -1){                                           // According to logic, blacked out totally
    return 45;
  }
  else{
    if(high_bit==8){                                                            // Black off the high side, but not totally blacked out
      Weighted_Position_Adder = ScaleRange(LSA_Array_Values[low_bit], LSA_Threshold, 100, -2.5, 2.5);//(100/(100-Threshold)*lower/20)-5;
    }
    if(low_bit==-1){                                                            // Black off the low side, but not totally blacked out
      Weighted_Position_Adder = ScaleRange(LSA_Array_Values[high_bit], LSA_Threshold, 100, 5, 0);//((100/(100-Threshold)*higher/20)-5) *(-1);
    }
  }
  if(low_bit==7&&high_bit==0){                                                  // According to logic, whited out completely
    if(LSA_Array_Values[0]<LSA_Array_Values[7]){                                // More detection on the low side
      Weighted_Position_Adder = ScaleRange(LSA_Array_Values[0], LSA_Threshold, 100, 10, 5);
    }
    else{
      if (LSA_Array_Values[0]>LSA_Array_Values[7]){                             // More detection on the high side
        Weighted_Position_Adder = ScaleRange(LSA_Array_Values[7], LSA_Threshold, 100, 0, 5) + 82;
      }
    }
  }

  //NumOut(54, LCD_LINE5, Weighted_Position_Adder);                             // The average's adder calculated by the closest non-true bytes

  if (Weighted_Position/total_bits+Weighted_Position_Adder){ //Not equal to 0
    Absolute_Average = ScaleRange(Weighted_Position/total_bits+Weighted_Position_Adder, 5, 86, 10, 80); // Avaragetotal divided by the number of bits high, plus the adder. Then scale this from 0-82 to 0-80 so it is the same as the LL native average.
  }
  return Absolute_Average;
}
Posted in Drivers, Mindsensors, Mindstorms, NXC, NXT | Tagged , , , , , | Leave a comment

NXShield Lego Arduino Drivers V1.12 released

A while ago I removed a work-around that had been necessary, but I forgot to update the online files. It is now fixes in this V1.12 release (available here).

Posted in Arduino, Dexter Industries, Drivers, Mindsensors | Tagged , , , , | Leave a comment

NXShield Lego Arduino Drivers V1.11 released

I released V1.1 earlier, but soon after realized I had made a mistake in the calculation of ClassNXTSound::SoundLevel. I fixed the mistake in this V1.11 release (available here).

Posted in Arduino, Dexter Industries, Drivers, Mindsensors | Tagged , , , , | 8 Comments

NXShield Lego Arduino Drivers V1.1 released

I just released NXShield Lego Arduino Drivers V1.1.

I added support for the Dexter Industries dIMU, and the Lego Sound sensor. Go to the NXShield Driver Suite page for a complete supported devices list, and change log.

Posted in Arduino, Dexter Industries, Drivers, Mindsensors | Tagged , , , , | Leave a comment