Aapo Rantalainen's blog

Experiences with Information Technology and Open source

NXT finds kitten

Posted by Aapo Rantalainen on March 29, 2010

I just passed course about Robotic programming on University of Helsinki. I made couple of nice robots and this is story of one of those.

If you want just check video:

Glossary

NXT Mindstorm =  Lego Robot by Lego. Read more: http://en.wikipedia.org/wiki/Lego_Mindstorms_NXT

Robot finds kitten (rfk) = Game or zen simulation. You can’t lose it. Just play and win. Hero of the game is robot, who moves on screen and tries to find kitten by touching unidentified objects. Each not kitten item has some funny message telling it is not kitten. When I talk about robot I mean that robot in the game not  physical NXT.

Less-technical

Lego NXT robot is programmable lego device, containing bluetooth, usb, 3 output ports for motors and 4 input ports for sensors. If you buy one from stock you get CD-rom containing some graphical drag-and-drop programming tool (it is meant for 10+ year children). Lego has released firmware of robot as Open Source and today there are many way to program NXT, including Java,Python,Ruby, Lua, Ada, ansi C and Not eXactly C.

E.g. programming java you need to flash java virtual machine (lejos) to the device. With GNU-arm compiler it is possible to produce firmware-like binaries or regular rxe-binaries which can be run on top of official firmware.

I decided to use Not eXactly C (nxc), which is very similar than C, but it has some NXT related functions built in. From same author there are also Extended Firmware which is boosted version of official firmware containing native multi dimensional arrays and ability to stop threads outside of thread. I also decided to use extended firmware (version 1.07) but my code can be compiled with little modifications for Lego’s official firmware.

Rfk has been ported many platforms and device but not on NXT until now. Game needs character display and some user input mechanism to move robot. NXT motors works also tachometers so I used two of them to move robot. (Look picture) Left wheel moves robot horizontally and right moves vertically on the screen. I used touch sensor for button which is pressed when messages are read. NXT has 100 x 64 pixel LCD, which is natively used for 16*8 characters.

More technical

My first task was make library to split arbitrary messages to NXT screen. I made simple ‘pager.c’. You can read it online and it is also in package.

/*
Pager
=====

Typesetting / composing library for NXT Robot written in nxc.
License: LGPL
Author: Aapo Rantalainen <aapo.rantalainen(at)gmail.com>

Usage: compose(string orig);

It will clear screen and show string 'typesetted'.
 -Only long words are splitted between rows.
 -It is users responsibility to not pass too long strings (last row will be messed then)

Then it waits user to push touch sensor on port 4.
(Can be easily changed waiting 'center'-button. Code is ready)
*/

/*
This helper returns LCD_LINEX macroname for X.
*/
int row(int a){
 switch(a)
 {
 case 1: return LCD_LINE1;
 case 2: return LCD_LINE2;
 case 3: return LCD_LINE3;
 case 4: return LCD_LINE4;
 case 5: return LCD_LINE5;
 case 6: return LCD_LINE6;
 case 7: return LCD_LINE7;
 case 8: return LCD_LINE8;
 }
return 0;
}

/*
This writes given string to given position.
Column is something between 0...15
Row is something between 1...8 (because this is handled via LCD_LINEs)
*/
void print(int column, int row_number, string s){
 TextOut(column*6,row(row_number),s);
}

/*
This is poor typesetting/composing algorithm.
It gets one long string and parses it to screen (width=16).
It goes one word a time. If word doesn't fit in current line, it is
dropped to next line. But if it doesn't fit in the next line (meaning it is
word containing more than 16 letters), take it back to this line and split it.

This function is useful if there are not too big messages to parse.
It doesn't check if string is too big to fit in 8 rows.
*/
void compose(string orig){
 ClearScreen();

 int row_counter=1;                //count current row
 int column_counter=0;             //count current column

 int length_of_string = StrLen(orig);  //this is total length of whole given string
 int previous_index=0;                 //cursor for previous word
 int len=0;                           //length of current word
 int i;
 for (i=0;i<length_of_string+1;i++)              //loop over all characters. Note one additional round!
 {
 int currentCharacter = StrIndex(orig, i);
 if (currentCharacter!=32 && i!=length_of_string) //when whitespace founded or we are in last round, jump over this
 {
 len++;    //increase length of current word
 continue; //...and keep rolling
 }
 //so now we found space -> one word (or we reach end of string -> one word)

 string word=SubStr(orig, previous_index, len);  //make that word

 //Special case:
 if (len>16)               //this is special case. We do not split any other word, only those extra lengthy.
 {
 while (len>16) //when we enter this loop, column_counter can be anything, but after 1st round it is 0.
 {
 print (column_counter,row_counter,SubStr(word,0,16-column_counter)); //print as much there are space.
 previous_index=previous_index+(16-column_counter);                   //move previous_index marker
 word = SubStr(word,16-column_counter,len);                           //this current word has something left
 len = len - (16-column_counter);                                     //fix length of this word
 row_counter+=1;                                                      //change row
 column_counter=0;                                                    //and carriage return
 }
 //And now there are only small (<16) tail of word. And we are beginning of the row
 print (0,row_counter,word);      //print what are left
 column_counter=len+1;            //+1 is space after word,
 }

 //Normal case:
 else
 {
 if (column_counter+len<16)                   //check can we add this word in this row
 {
 print (column_counter,row_counter,word);   //add it
 column_counter+=len+1;                     //move column_counter. +1 is space after word,
 }
 else                                        //no, it doesn't fit (do not split it)
 {
 row_counter+=1;                           //next row
 print (0,row_counter,word);               //starting beginning of the row
 column_counter=len+1;                     //and that space (we can trust len(i)<=16)
 }
 }

 previous_index=previous_index+len+1; //move previous_index marker
 len=0;//reset len
 }

 //Show message until sensor_2 is clicked (pressed and released)
 SetSensorTouch(IN_4);
 while (SENSOR_4 != 1) Wait(10);
 while (SENSOR_4 == 1) Wait(10);

/*
 If we want use center-button to ACK messages:
 while(true)
 {
 if (ButtonPressed (BTNCENTER, true)) //last true means we clear press count after press
 {
 while(ButtonPressed(BTNCENTER, true)){}
 break;
 }
 Wait(100);
 }
 */
}

//#define TEST_PAGER_NXC 1
#ifdef TEST_PAGER_NXC
task main()  {
 compose("Lorem ipsum dolor sit amet, consectetur adipiscing elit."); //this will use 5 rows

 //7th row is 'hendrerit erat'
 //8th row is messed: 'neque.ravidalin' (Last row is wrote several times)
 compose("Maecenas at metus justo. Pellentesque blandit blandit posuere. Fusce suscipit tortor hendrerit erat faucibus cursus. Nam velit purus, commodo eget commodo non, aliquam eget neque. Vestibulum urna dolor, sagittis vitae convallis sed, gravida in neque.");

}
#endif

//End of file
<code>

After that writing game itself was straightforward task, I followed structure of libcurse rfk by Leonard Richardson. There is matrix (thanks for multi dimensional arrays) containing all blocks of screen. And each of them are empty or occupied by robot, kitten, or bogus. Original game contains over 400 funny messages for non kitten items, but it is too much for NXT’s memory, so I used only first 60. It is possible to store them to text file or even many files and read some of them on runtime but I didn’t have time to implement it.

Here is code for online reading. It is also in package.

/*
This is NXT port for Robot finds kitten game (or Zen simulation).

This code based Leonard Richardson's libcurse-rfk.

See more:

http://robotfindskitten.org

Copyright (C) 2010 Aapo Rantalainen
 aapo.rantalainen(at)gmail.com

 *   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 EXISTANCE OF KITTEN.  See the GNU General
 *   Public License for more details.
 *
 *   http://www.gnu.org/copyleft/gpl.html
*/

//Size of grid.
#define WIDTH 16
#define HEIGHT 8

typedef struct
{
 int x;
 int y;
 string character;
} screen_object;

//Original messages.h contains 405 funny messages for bogus objects
//But it is too much for NXT, so there are smaller version with 60
//Maybe it can handle 99?
#include "messages2.h"

//This contains routine for drawing screen_objects in screen
#include "draw.nxc"

//These are used for tachometers.
int last_x;
int last_y;

//This is matrix for each grid in screen
//It can't be initializez here.
int screen[][];

//Screen can have some of those values:
#define EMPTY -1
#define ROBOT 0
#define KITTEN 1

//These screen_objects are whole point of game
screen_object robot;
screen_object kitten;
screen_object bogus[NUMBER_OF_MESSAGES]; //We make them all, but use only couple of them

//This is the number of bogus objects in game.
#define NUM_BOGUS 6

//This keeps track what message points where
int bogus_messages[NUMBER_OF_MESSAGES];
bool used_messages[NUMBER_OF_MESSAGES];

/*
Array initialization.
*/
void initialize_arrays()
{
 int counter;
 screen_object empty;

 //this is the way how multidimensional arrays are initialized
 int one_column[];
 ArrayInit(one_column, EMPTY, HEIGHT); // make one column of EMPTIes
 ArrayInit(screen, one_column, WIDTH); // And make WIDTH*columns

 /*Initialize the empty object.*/
 empty.x = -1;
 empty.y = -1;
 empty.character = " ";

 /*Initialize the other arrays.*/
 for (counter = 0; counter < NUMBER_OF_MESSAGES; counter++)
 {
 used_messages[counter] = FALSE;
 bogus_messages[counter] = 0;
 bogus[counter] = empty;
 }
}

/*Robot initialization.*/
void initialize_robot()
{
 /*Assign a position to the robot.*/
 robot.x = Random(WIDTH);
 robot.y = Random(HEIGHT);

 robot.character = "#";
 screen[robot.x][robot.y] = ROBOT;
}

/*Returns one random character for kitten or bogus*/
string randchar() {
 string choise="+-%&()=?*_:;>[]\\qwertyuiopasdfghjklzxcvbnm";
 int r = Random(StrLen(choise));
 return SubStr(choise,r,1);
}

/*Kitten initialization.*/
void initialize_kitten()
{
 /*Assign the kitten a unique position.*/
 do
 {
 kitten.x = Random(WIDTH);
 kitten.y = Random(HEIGHT);
 } while (screen[kitten.x][kitten.y] != EMPTY);

 /*Assign the kitten a character*/
 kitten.character = randchar();
 screen[kitten.x][kitten.y] = KITTEN;
}

/*initialize_bogus initializes all non-kitten objects to be used in this run.*/
void initialize_bogus()
{
 int counter, index;
 for (counter = 0; counter < NUM_BOGUS; counter++)
 {
 /*Give it a character.*/
 bogus[counter].character = randchar();

 /*Give it a unique position.*/
 do
 {
 bogus[counter].x = Random(WIDTH);
 bogus[counter].y = Random(HEIGHT);
 } while (screen[bogus[counter].x][bogus[counter].y] != EMPTY);

 //screen[][] contains ints representing #define ROBOT 0 and #define KITTEN 1
 //So we store this id-number of bogus to this square. (added 2).
 screen[bogus[counter].x][bogus[counter].y] = counter+2;

 /*Find a unique message for this object.*/
 do {
 index = Random(NUMBER_OF_MESSAGES);
 } while (used_messages[index] != FALSE);

 bogus_messages[counter] = index;
 used_messages[index] = TRUE;
 }
}

/*
Draw all bogus, kitten and robot.
*/
void draw_all()
{
 ClearScreen();
 int counter;
 for (counter = 0; counter < NUM_BOGUS; counter++)
 {
 draw(bogus[counter]);
 }
 draw(kitten);
 draw(robot);
}

/*
play_game waits in a loop getting input and processing it.

return FALSE when game ends
 */
bool play_game()  {
 draw_all(); //Draw current situation
 int input=1;
 int potential_new_x = robot.x;
 int potential_new_y = robot.y;

//Read until some MotorTachoCount differs
//There must 10 angle difference (it is too sensitive otherway)
while(true)
 {
 int x = MotorTachoCount(OUT_A);
 int y = MotorTachoCount(OUT_C);

 if (x>last_x+10)
 {
 potential_new_x--;
 last_x=x;
 break;
 }
 else if (x<last_x-10)
 {
 potential_new_x++;
 last_x=x;
 break;
 }

 else if (y>last_y+10)
 {
 potential_new_y--;
 last_y=y;
 break;
 }
 else if (y<last_y-10)
 {
 potential_new_y++;
 last_y=y;
 break;
 }
 }

 //If we move inside boundaries, go check collision, otherwise jump to next round
 if (potential_new_y >= 0 && potential_new_y < HEIGHT && potential_new_x >= 0 && potential_new_x < WIDTH)
 {
 /*Check for collision*/
 switch (screen[potential_new_x][potential_new_y])
 {
 case ROBOT: //This is robot starting place. We could also move ROBOT, and then we will never encounter this.
 case EMPTY: //We can move to here
 robot.x = potential_new_x;
 robot.y = potential_new_y;
 break;
 case KITTEN: //Found it!
 message("You found kitten! Way to go, robot!");
 return FALSE;
 default: //We hit a bogus object; print its message
 message(messages[bogus_messages[screen[potential_new_x][potential_new_y]-2]]);
 }
 }

return TRUE;
}

/*
Prints two screen instructions and manual.
*/
void instructions()
{
 ClearScreen();
 TextOut (0,LCD_LINE1, "Move robot:     ");
 TextOut (0,LCD_LINE2, "Motor A is      ");
 TextOut (0,LCD_LINE3, " for horizontal.");
 TextOut (0,LCD_LINE4, "Motor C is      ");
 TextOut (0,LCD_LINE5, " for vertical.  ");
 TextOut (0,LCD_LINE6, "                ");
 TextOut (0,LCD_LINE7, "Button is input ");
 TextOut (0,LCD_LINE8, " 4. (Touch)     ");

 //Wait for button.
 SetSensorTouch(IN_4);
 while (SENSOR_4 != 1) Wait(10);
 while (SENSOR_4 == 1) Wait(10);

 ClearScreen();
 TextOut (0,LCD_LINE1, "In this game,   ");
 TextOut (0,LCD_LINE2, "you are robot #.");
 TextOut (0,LCD_LINE3, "Your job is to  ");
 TextOut (0,LCD_LINE4, "find kitten.    ");
 TextOut (0,LCD_LINE5, "Robot must touch");
 TextOut (0,LCD_LINE6, "items to solve  ");
 TextOut (0,LCD_LINE7, "if they are     ");
 TextOut (0,LCD_LINE8, "kitten or not.  ");

 SetSensorTouch(IN_4);
 while (SENSOR_4 != 1) Wait(10);
 while (SENSOR_4 == 1) Wait(10);
 return;
}

task main()
{
 initialize_arrays();

 initialize_robot();
 initialize_kitten();
 initialize_bogus();

 instructions();

 //Init globals for tachometers
 last_x = MotorTachoCount(OUT_A);
 last_y = MotorTachoCount(OUT_C);

 //start game loop
 while(play_game()); //This returns FALSE, when game ends.

 //Game is ended: Play some melody.
 int VOL =3;
 PlayToneEx(262,400,VOL,FALSE);  Wait(500);
 PlayToneEx(294,400,VOL,FALSE);  Wait(500);
 PlayToneEx(330,400,VOL,FALSE);  Wait(500);
 PlayToneEx(294,400,VOL,FALSE);  Wait(500);
 PlayToneEx(262,1600,VOL,FALSE); Wait(2000);
}

//End of file

Some images of building NXT finds kitten:

All source codes (GPLv3) and precompiled binary for extended firmware: nxt_finds_kitten.tar.gz

About these ads

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

 
Follow

Get every new post delivered to your Inbox.

%d bloggers like this: