Wednesday, August 5, 2009

Arduino+Accelerometer=Computerized Level Vial?

What?
Ok! So I hooked up Arduino Pro Mini, an LIS302DL 3 axis accelerometer and Nokia 5110 (PCD8544 based) LCD. And What did I do with all of this? A vial level meter emulation! Right you are, not a very useful application of such a pile of technology. But so much fun! Besides, It was easier to do it this way rather then trying to melt some sand into a glass vial...



Really?
Yep! Both the accelerometer and LCD are SPI devices so they share MOSI, SCK and MISO (LCD does not have output). Then there are a few RESETS and SELECTs pins and this is pretty much it (plus an old cellphone battery and a charger circuit). The glue is the code.

Here!
The Arduino sketch simulates some physics rules. The LCD displays a "ball rolling in a bowl" an object rolling in a indented surface shaped like a shallow bowl. By inclining the contraption in earth gravity field, the accelerometer pick ups the motion, its readings converted into change vector acting on the virtual ball, as if someone was tilting a real bowl and hence moving the ball around. I have tossed Newton laws of motion and some mass and friction into the mix.

Pardon my CODE
Highly experimental. The code uses 2 libraries one for accelerometer and one for LCD. Warning: Since I hate to waste space and CPU cycles, I diverged from classic Arduino singleton-object style (like in the "Serial" code-space killer). With each call to such object, there is a hidden parameter passed: the "this" pointer (and pointers on Avrs are 2 bytes). And then each function code must obey such pointer when it accesses its data members but there is never ever going to be another instance of such class (and hence another value of "this" pointer), so it is a pure waste. Granted, the source code looks more like C# or Java (Serial.println())....but I am a purist when it comes to execution...So instead I have created a static classes with only a header file to include (no cpp), perhaps I should call them "includaries"? I only wish Arduino was help while it comes to #includes, intead it is so much more (pain)!

Accelerate!

acm.h

////////////////////////////////////////////////////////////////////////////////
// ACceleroMeter LIS302DL
////////////////////////////////////////////////////////////////////////////////

#ifdef PREDEFINED_PINS
// SPI bus pins
#define PIN_SSEL 10
#define PIN_SDIN 11
#define PIN_SDOU 12
#define PIN_SCLK 13

// ACCELEROMETER
#define PIN_ACM_CE 7

#define SPITRANSFER(data) { SPDR = data; while (!(SPSR & (1<<SPIF))) ; }
#define SPIINDATA SPDR

void SPIInit()
{
// set direction of pins
pinMode(PIN_SDIN, OUTPUT);
pinMode(PIN_SCLK, OUTPUT);
pinMode(PIN_SDOU, INPUT);
pinMode(PIN_SSEL, OUTPUT);

// SPCR = 01010000
// Set the SPCR register to 01010000
//interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
//sample on leading edge of clk,system clock/4 rate
SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPOL)|(1<<CPHA);
byte clr;
clr=SPSR;
clr=SPDR;
}

#endif

class ACM
{
public:

static inline char ID() { return ReadReg(0x0f); }
static inline byte STATE() { return (byte)ReadReg(0x27); }
static inline char X() { return ReadReg(0x29); }
static inline char Y() { return ReadReg(0x2b); }
static inline char Z() { return ReadReg(0x2d); }

// write to a register
static void WriteReg(byte reg, byte data)
{
// SS is active low
digitalWrite(PIN_ACM_CE, LOW);
// send the address of the register we want to write
SPITRANSFER(reg);
// send the data we're writing
SPITRANSFER(data);
// unselect the device
digitalWrite(PIN_ACM_CE, HIGH);
}

// reads a register
static char ReadReg(byte reg)
{
WriteReg(reg|128,0);
return SPIINDATA;
}

static void Init()
{
// CE pin, disable device
pinMode(PIN_ACM_CE,OUTPUT);
digitalWrite(PIN_ACM_CE,HIGH);
// start up the device
// this essentially activates the device, powers it on, enables all axes, and turn off the self test
// CTRL_REG1 set to 01000111
WriteReg(0x20, 0x47);
delay(250);
}

}; // ACM


Show!

lcd.h

#ifdef LCD_PREDEFINED_PINS
#define PIN_LCD_SCE 3
#define PIN_LCD_RESET 4
#define PIN_LCD_DC 6

// SPI bus pins
#define PIN_SSEL 10
#define PIN_SDIN 11
#define PIN_SDOU 12
#define PIN_SCLK 13

#ifdef SPI
#define SPITRANSFER(data) { while (!(SPSR & (1<<SPIF))); SPDR = data; }
#define SPIINDATA SPDR
#else
#define SPITRANSFER(data) shiftOut(PIN_SDIN, PIN_SCLK, MSBFIRST, data)
#define SPIINDATA 0
#endif

#endif

class LCD
{
public:
enum
{
XM=84,
YM=48
};

static void Fill(byte pattern, int count)
{
digitalWrite(PIN_LCD_SCE, LOW);
while(count-->0)
{
SPITRANSFER(pattern);
}
digitalWrite(PIN_LCD_SCE, HIGH);
}

static void Clear()
{
Fill(0, XM * YM / 8);
}

static void Char(byte character)
{
static const byte ASCII[][5] =
{
{0x00, 0x00, 0x00, 0x00, 0x00} // 20
,{0x00, 0x00, 0x5f, 0x00, 0x00} // 21 !
,{0x00, 0x07, 0x00, 0x07, 0x00} // 22 "
,{0x14, 0x7f, 0x14, 0x7f, 0x14} // 23 #
,{0x24, 0x2a, 0x7f, 0x2a, 0x12} // 24 $
,{0x23, 0x13, 0x08, 0x64, 0x62} // 25 %
,{0x36, 0x49, 0x55, 0x22, 0x50} // 26 &
,{0x00, 0x05, 0x03, 0x00, 0x00} // 27 '
,{0x00, 0x1c, 0x22, 0x41, 0x00} // 28 (
,{0x00, 0x41, 0x22, 0x1c, 0x00} // 29 )
,{0x14, 0x08, 0x3e, 0x08, 0x14} // 2a *
,{0x08, 0x08, 0x3e, 0x08, 0x08} // 2b +
,{0x00, 0x50, 0x30, 0x00, 0x00} // 2c ,
,{0x08, 0x08, 0x08, 0x08, 0x08} // 2d -
,{0x00, 0x60, 0x60, 0x00, 0x00} // 2e .
,{0x20, 0x10, 0x08, 0x04, 0x02} // 2f /
,{0x3e, 0x51, 0x49, 0x45, 0x3e} // 30 0
,{0x00, 0x42, 0x7f, 0x40, 0x00} // 31 1
,{0x42, 0x61, 0x51, 0x49, 0x46} // 32 2
,{0x21, 0x41, 0x45, 0x4b, 0x31} // 33 3
,{0x18, 0x14, 0x12, 0x7f, 0x10} // 34 4
,{0x27, 0x45, 0x45, 0x45, 0x39} // 35 5
,{0x3c, 0x4a, 0x49, 0x49, 0x30} // 36 6
,{0x01, 0x71, 0x09, 0x05, 0x03} // 37 7
,{0x36, 0x49, 0x49, 0x49, 0x36} // 38 8
,{0x06, 0x49, 0x49, 0x29, 0x1e} // 39 9
,{0x00, 0x36, 0x36, 0x00, 0x00} // 3a :
,{0x00, 0x56, 0x36, 0x00, 0x00} // 3b ;
,{0x08, 0x14, 0x22, 0x41, 0x00} // 3c <
,{0x14, 0x14, 0x14, 0x14, 0x14} // 3d =
,{0x00, 0x41, 0x22, 0x14, 0x08} // 3e >
,{0x02, 0x01, 0x51, 0x09, 0x06} // 3f ?
,{0x32, 0x49, 0x79, 0x41, 0x3e} // 40 @
,{0x7e, 0x11, 0x11, 0x11, 0x7e} // 41 A
,{0x7f, 0x49, 0x49, 0x49, 0x36} // 42 B
,{0x3e, 0x41, 0x41, 0x41, 0x22} // 43 C
,{0x7f, 0x41, 0x41, 0x22, 0x1c} // 44 D
,{0x7f, 0x49, 0x49, 0x49, 0x41} // 45 E
,{0x7f, 0x09, 0x09, 0x09, 0x01} // 46 F
,{0x3e, 0x41, 0x49, 0x49, 0x7a} // 47 G
,{0x7f, 0x08, 0x08, 0x08, 0x7f} // 48 H
,{0x00, 0x41, 0x7f, 0x41, 0x00} // 49 I
,{0x20, 0x40, 0x41, 0x3f, 0x01} // 4a J
,{0x7f, 0x08, 0x14, 0x22, 0x41} // 4b K
,{0x7f, 0x40, 0x40, 0x40, 0x40} // 4c L
,{0x7f, 0x02, 0x0c, 0x02, 0x7f} // 4d M
,{0x7f, 0x04, 0x08, 0x10, 0x7f} // 4e N
,{0x3e, 0x41, 0x41, 0x41, 0x3e} // 4f O
,{0x7f, 0x09, 0x09, 0x09, 0x06} // 50 P
,{0x3e, 0x41, 0x51, 0x21, 0x5e} // 51 Q
,{0x7f, 0x09, 0x19, 0x29, 0x46} // 52 R
,{0x46, 0x49, 0x49, 0x49, 0x31} // 53 S
,{0x01, 0x01, 0x7f, 0x01, 0x01} // 54 T
,{0x3f, 0x40, 0x40, 0x40, 0x3f} // 55 U
,{0x1f, 0x20, 0x40, 0x20, 0x1f} // 56 V
,{0x3f, 0x40, 0x38, 0x40, 0x3f} // 57 W
,{0x63, 0x14, 0x08, 0x14, 0x63} // 58 X
,{0x07, 0x08, 0x70, 0x08, 0x07} // 59 Y
,{0x61, 0x51, 0x49, 0x45, 0x43} // 5a Z
,{0x00, 0x7f, 0x41, 0x41, 0x00} // 5b [
,{0x02, 0x04, 0x08, 0x10, 0x20} // 5c ¥
,{0x00, 0x41, 0x41, 0x7f, 0x00} // 5d ]
,{0x04, 0x02, 0x01, 0x02, 0x04} // 5e ^
,{0x40, 0x40, 0x40, 0x40, 0x40} // 5f _
,{0x00, 0x01, 0x02, 0x04, 0x00} // 60 `
,{0x20, 0x54, 0x54, 0x54, 0x78} // 61 a
,{0x7f, 0x48, 0x44, 0x44, 0x38} // 62 b
,{0x38, 0x44, 0x44, 0x44, 0x20} // 63 c
,{0x38, 0x44, 0x44, 0x48, 0x7f} // 64 d
,{0x38, 0x54, 0x54, 0x54, 0x18} // 65 e
,{0x08, 0x7e, 0x09, 0x01, 0x02} // 66 f
,{0x0c, 0x52, 0x52, 0x52, 0x3e} // 67 g
,{0x7f, 0x08, 0x04, 0x04, 0x78} // 68 h
,{0x00, 0x44, 0x7d, 0x40, 0x00} // 69 i
,{0x20, 0x40, 0x44, 0x3d, 0x00} // 6a j
,{0x7f, 0x10, 0x28, 0x44, 0x00} // 6b k
,{0x00, 0x41, 0x7f, 0x40, 0x00} // 6c l
,{0x7c, 0x04, 0x18, 0x04, 0x78} // 6d m
,{0x7c, 0x08, 0x04, 0x04, 0x78} // 6e n
,{0x38, 0x44, 0x44, 0x44, 0x38} // 6f o
,{0x7c, 0x14, 0x14, 0x14, 0x08} // 70 p
,{0x08, 0x14, 0x14, 0x18, 0x7c} // 71 q
,{0x7c, 0x08, 0x04, 0x04, 0x08} // 72 r
,{0x48, 0x54, 0x54, 0x54, 0x20} // 73 s
,{0x04, 0x3f, 0x44, 0x40, 0x20} // 74 t
,{0x3c, 0x40, 0x40, 0x20, 0x7c} // 75 u
,{0x1c, 0x20, 0x40, 0x20, 0x1c} // 76 v
,{0x3c, 0x40, 0x30, 0x40, 0x3c} // 77 w
,{0x44, 0x28, 0x10, 0x28, 0x44} // 78 x
,{0x0c, 0x50, 0x50, 0x50, 0x3c} // 79 y
,{0x44, 0x64, 0x54, 0x4c, 0x44} // 7a z
,{0x00, 0x08, 0x36, 0x41, 0x00} // 7b {
,{0x00, 0x00, 0x7f, 0x00, 0x00} // 7c |
,{0x00, 0x41, 0x36, 0x08, 0x00} // 7d }
,{0x10, 0x08, 0x08, 0x10, 0x08} // 7e ?
,{0x78, 0x46, 0x41, 0x46, 0x78} // 7f ?
,{0xff, 0x81, 0x81, 0x81, 0xff} // 80 frame
,{0x18, 0x24, 0x42, 0x24, 0x18} // 81 diamont
};

digitalWrite(PIN_LCD_SCE, LOW);
if( character < 0x20 || character-0x20 >= sizeof(ASCII)/5 )
{
for (byte index = 5+2; index > 0 ; index--)
{
SPITRANSFER(character);
}
}
else
{
SPITRANSFER(0x00);
for (byte index = 0; index < 5; index++)
{
byte data = ASCII[character - 0x20][index];
SPITRANSFER(data);
}
SPITRANSFER(0x00);
}
digitalWrite(PIN_LCD_SCE, HIGH);
}

static void Init(void)
{
pinMode(PIN_LCD_SCE, OUTPUT);
pinMode(PIN_LCD_RESET, OUTPUT);
pinMode(PIN_LCD_DC, OUTPUT);
digitalWrite(PIN_LCD_DC,HIGH);
digitalWrite(PIN_LCD_SCE,HIGH);
digitalWrite(PIN_LCD_RESET, LOW);
digitalWrite(PIN_LCD_RESET, HIGH);
delay(10);
#if 1
Cmd( 0x21 ); // LCD Extended Commands.
Cmd( 0xC8 ); // Set LCD Vop (Contrast).
Cmd( 0x06 ); // Set Temp coefficent.
Cmd( 0x13 ); // LCD bias mode 1:48.
#endif
Cmd( 0x20 ); // LCD Standard Commands, Horizontal addressing mode.
Cmd( 0x0C ); // LCD in normal mode.
// Cmd( 0x0D ); // LCD in reverse mode
}

static void Goto(byte row,byte col)
{
digitalWrite(PIN_LCD_DC, LOW);
digitalWrite(PIN_LCD_SCE, LOW);
SPITRANSFER(0x40|row); //Cmd(0x40|row);
SPITRANSFER(0x80|col); // Cmd(0x80|col);
digitalWrite(PIN_LCD_SCE, HIGH);
digitalWrite(PIN_LCD_DC, HIGH);
}

static void Str(char *characters)
{
while (*characters)
{
Char(*characters++);
}
}

template<unsigned base>
static void Num(unsigned num,char digits=0)
{
unsigned div = 1;
// find biggest power of base that is not greater then num
// keep going when more digits are requested
while( true )
{
unsigned newdiv = div*base;
if( newdiv < div ) break; // overflow
digits--;
if( newdiv > num && digits<=0) break; // end
div = newdiv;
}
while( div > 0 )
{
unsigned dig = num / div;
char c = dig < 10 ? '0'+dig : 'A'+dig-10;
Char(c);
num-=dig*div;
div/=base;
}
}

template<unsigned base>
static void Num(int num,char digits)
{
if( num<0 )
{
Char('-');
num=-num;
digits--;
}
Num<base>((unsigned)num,digits);
}

private:
static void Cmd(byte cmd)
{
digitalWrite(PIN_LCD_DC, LOW);
digitalWrite(PIN_LCD_SCE, LOW);
SPITRANSFER(cmd);
digitalWrite(PIN_LCD_SCE, HIGH);
digitalWrite(PIN_LCD_DC, HIGH);
}
};

The MAIN feature

// SPI bus pins
#define PIN_SSEL 10
#define PIN_SDIN 11
#define PIN_SDOU 12
#define PIN_SCLK 13

#define SPITRANSFER(data) { SPDR = data; while (!(SPSR & (1<<SPIF))) ; }
#define SPIINDATA SPDR

////////////////////////////////////////////////////////////////////////////////

void SPIInit()
{
// set direction of pins
pinMode(PIN_SDIN, OUTPUT);
pinMode(PIN_SCLK, OUTPUT);
pinMode(PIN_SDOU, INPUT);
pinMode(PIN_SSEL, OUTPUT);

// SPCR = 01010000
// Set the SPCR register to 01010000
//interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
//sample on leading edge of clk,system clock/4 rate
SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPOL)|(1<<CPHA);
byte clr;
clr=SPSR;
clr=SPDR;
}

// ACCELEROMETER
#define PIN_ACM_CE 7
#include "{yourpathhere}\accelerometer.h"

// LCD
#define PIN_LCD_SCE 3
#define PIN_LCD_RESET 4
#define PIN_LCD_DC 6
#include "{yourpathhere}\lcd.h"


////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////

void setup(void)
{
Serial.begin(9600);

// set direction of pins
pinMode(PIN_SDIN, OUTPUT);
pinMode(PIN_SCLK, OUTPUT);
pinMode(PIN_SDOU, INPUT);
pinMode(PIN_SSEL, OUTPUT);

pinMode(PIN_ACM_CE,OUTPUT);
digitalWrite(PIN_ACM_CE, HIGH);
pinMode(PIN_LCD_SCE,OUTPUT);
digitalWrite(PIN_LCD_SCE, HIGH);

SPIInit();

LCD::Init();
LCD::Clear();
ACM::Init();
Serial.println(ACM::ID(),HEX);
Serial.println(ACM::STATE(),HEX);
Serial.println("=============");
}


const byte BALLSIZE = 4;

void DrawBall(byte x, byte y, byte erase)
{
byte row = y / 8;
byte shift = y % 8;
unsigned pattern = (1<<BALLSIZE)-1;
pattern <<= shift;
byte f;
f = pattern & 0xFF;
if( f )
{
LCD::Goto(row,x);
LCD::Fill(erase ? 0 : f, BALLSIZE);
}
f = pattern >> 8;
if( f )
{
LCD::Goto(row+1,x);
LCD::Fill(erase ? 0 : f, BALLSIZE);
}
}

static byte curx = 0;
static byte cury = 0;

void MoveBall(byte x, byte y)
{
if( curx != x || cury != y )
{
DrawBall(curx,cury,1);
curx = x; cury=y;
DrawBall(curx,cury,0);
}
}

//////////////////////////////////////////////////////////////////////////////////////////

// all units are SI : meters, seconds etc.

// G force (earth gravity)
const float G = 9.81; // m/s2

// pixel size
const float pixsize = 3e-2/LCD::XM; // in meters =~ width(3cm)/numpixX
// ball size
const float ballsize = BALLSIZE*pixsize;
const float xsize = LCD::XM * pixsize;
const float ysize = LCD::YM * pixsize;
float x=float(LCD::XM/2)*pixsize;
float y=float(LCD::YM/2)*pixsize;
float px=float(LCD::XM/2)*pixsize;
float py=float(LCD::YM/2)*pixsize;
float vx=0;
float vy=0;
float prevTime = 0;

float elapsed()
{
if( prevTime == 0 ) prevTime = float(1e-3*millis());
float time = float(1e-3*millis());
float delta = time - prevTime;
prevTime = time;
return delta;
}

inline float sign(float f)
{
return f > 0 ? 1 : (f < 0 ? -1 : 0);
}


float ax = 0;
float ay = 0;
float az = 0;

float azx = 0;
float azy = 0;
float azz = 0;


void zero()
{
ax=ay=az=0;
azx=azy=azz=0;
for( byte n = 0; n < 4; n++ )
{
measure();
}
azx=ax;
azy=ay;
azz=az;
}

void measure()
{
const float filtercoef = 0.25;
ax = filtercoef*(G/64.0)*float(ACM::X())+(1-filtercoef)*ax;
ay = filtercoef*(G/64.0)*float(ACM::Y())+(1-filtercoef)*ay;
az = filtercoef*(G/64.0)*float(ACM::Z())+(1-filtercoef)*az;
}

void loop(void)
{
zero();
while(1)
{
#if 0
// LIS302DL
char ax = ACM::X();
char ay = ACM::Y();
char az = ACM::Z();
char accx = map(ax,-64,64,0,LCD::XM);
LCD::Goto(3,0);LCD::Fill(0,accx-2);LCD::Fill(0xFF,4);LCD::Fill(0,LCD::XM-accx-4);
char accy = map(ay,-64,64,0,LCD::XM);
LCD::Goto(4,0);LCD::Fill(0,accy-2);LCD::Fill(0xFF,4);LCD::Fill(0,LCD::XM-accy-4);
char accz = map(az,-64,64,0,LCD::XM);
LCD::Goto(5,0);LCD::Fill(0,accz-2);LCD::Fill(0xFF,4);LCD::Fill(0,LCD::XM-accz-4);
#endif

float dt = elapsed();

#if 0
const float refrat = -1;
if( vx == 0 ) vx = 10*pixsize/1; // 10 pixels per second
if( vy == 0 ) vy = 10*pixsize/1; // 10 pixels per second
x += vx * dt;
y += vy * dt;
if( x < 0 ) { x = 0; vx = refrat * vx; }
if( x >= xsize-ballsize ) { x = xsize-ballsize; vx = refrat * vx; }
if( y < 0 ) { y = 0; vy = refrat * vy; }
if( y >= ysize-ballsize ) { y = ysize-ballsize; vy = refrat * vy; }
#else
measure();
// apply gravity the display's y axis is a z axis of acceleromiter due to mounting direction
vx += (ax-azx)*dt;
vy += (az-azz)*dt;
// apply center pull this emulates a hiperbol shape bowl-like dish in which the ball rolls
const float fudge = 0.2;
vx += -fudge*G*(x-(xsize/2-ballsize/2))/xsize*dt;
vy += -fudge*G*(y-(ysize/2-ballsize/2))/ysize*dt;
// vx += -0.5*(x>xsize/2?1:-1)*dt;
// vy += -0.5*(y>ysize/2?1:-1)*dt;
// apply friction
vx += -0.1*vx;
vy += -0.1*vy;
// move with speed
x += vx * dt;
y += vy * dt;
// bounce off walls
const float refrat = 0.9; // bounce reflection ratio (== 1 for perfect bounce)
if( x < 0 ) { x = 0; vx = refrat * vx; }
if( x >= xsize-ballsize ) { x = xsize-ballsize; vx = refrat * vx; }
if( y < 0 ) { y = 0; vy = refrat * vy; }
if( y >= ysize-ballsize ) { y = ysize-ballsize; vy = refrat * vy; }
// zero speed if we are trully not moving
if( abs(px - x) < 1e-10 ) vx = 0;
if( abs(py - y) < 1e-10 ) vy = 0;
px = x;
py = y;
#endif

byte bx = (byte)(x/pixsize);
byte by = (byte)(y/pixsize);
MoveBall(bx,by);
#if 1
Serial.println(dt);
Serial.println(ax);
Serial.println(az);
Serial.println(vx);
Serial.println(vy);
Serial.println(G*(x-(xsize/2-ballsize/2))/xsize*dt);
Serial.println(G*(y-(ysize/2-ballsize/2))/ysize*dt);
Serial.println();
#endif
// LCD::Goto(0,0);LCD::Num<10>(bx);
// LCD::Goto(0,LCD::XM/2);LCD::Num<10>(by);
delay(20);
}
}

Sunday, August 2, 2009

Arduino IR Receiver with Interrupts



Infra Red Receiver with the use of Pin Change Interrupt

Infrared remote control receiver implemented using pin change interrupt. This implementation allows the main loop to perform other tasks while the receiver code collects incoming IR message bits in the background. This method was used to receive IR control message send to the iSOBOT robot from its remote. It is runnin on Arduino Mini Pro with AVR m168 CPU.

Problem Definition

The IR remote receiver is observing output of the IR sensor/demodulator which signals presents or absents of modulated IR. The output of the sensor is connected to a pin of the CPU which is then read by the software and interpreted by as bits of the message. To recognize the beginning of the message and to interpret the waveform generated by, the code has to measure time between signal transition. Beginning of the message is signaled as 2.5ms burst of IR while 0 and 1 are coded as length of silence between 0.5ms IR bursts (0- 0.5 ms silence while 1 is 1ms silence). See IR waveforms and protocol description.

Classic Approach

The code below is uses a polling approach. When it waits for the signal change it simply reads (polls) the signal until a change is observed. While this is straight forward and very simply, it occupies the CPU in 100%. Here is an implementation of example:



// helper functions

unsigned long elapsedSince(unsigned long since, unsigned long now)
{
return since &lh; now ? now-since : 0xFFFFFFFFUL - (now - since);
}

unsigned long elapsedSince(unsigned long since)
{
return elapsedSince( since, micros() );
}

// iSOBOT IR protocol timing
#define TimeStart 2500
#define TimeZero 500
#define TimeOne 1000
#define TimeOff 500

// check if the time was in range +/- 25%
#define IS_X(t,x) ((t > 3*(x)/4) && (t < 5*(x)/4))
#define IS_0(t) IS_X(t,TimeZero)
#define IS_1(t) IS_X(t,TimeOne)
#define IS_S(t) IS_X(t,TimeStart)
#define IS_O(t) IS_X(t,TimeOff)

//////////////////////////////////////////////////////////////////////
// polling/blocking version of IR receiver
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
// waits for IR carrier transitions (on->off & off->on)
// returns duration of the specified state (HIGH->IR transmitting; LOW->IR off)
// returns 0 if timeout
unsigned irRecvSignal(byte waitFor)
{
while(digitalRead(PIN_IR)==(waitFor==LOW?HIGH:LOW)) {};
unsigned long start = micros();
while(digitalRead(PIN_IR)==waitFor) {};
return elapsedSince(start);
}

////////////////////////////////////////////////////////////////////////
// receives all 22 bits of the messagge
long irRecv()
{
Serial.println("Waiting...");
unsigned time = irRecvSignal(LOW);
if( !IS_S(time) )
{
Serial.println("False Start");
Serial.println(time);
return 0;
}
long bits = 0;
byte len = 22;
for(int i = 0; i < len; i++ )
{
bits <<= 1;
time = irRecvSignal(HIGH);
if( IS_1(time) )
{
bits |= 1;
}
else if( IS_0(time) )
{
bits |= 0;
}
else
{
Serial.println("Bad Bit");
Serial.println(time);
return 0;
}
// check type of message (when recved) to guess the message length (either 22 ot 30 bits)
if( bit == 3 )
{
byte msgtype = bits & 0x3;
if( msgtype == 0 ) len = 30;
}
}
return bits;
}


The jest of the polling happens in the loops in irRecvSignal(byte waitFor) function.
The time duration is measured with Arduino's time function micros() returning number of microseconds since the CPU was started. Note that the number of microseconds will wrap around, which I am compensating for in elapsedSince() function.

When I want to receive a message I just call irRecv().


void loop()
{
...
long msg = irRecv();
...
}


This call will block until a message is received. Hence, nothing else in the main loop will be executed while I wait for IR message (ie wait for the irRecv() to return).

Interrupts

So, I have one CPU here but I'd like to perform more then one task at a time. One way is to "distract" or "interrupt" the main code and "switch" to something else for a moment is to you interrupts. Since the IR receiver cares only about changes in the IR signal and these changes are relatively infrequent, the cost of interrupting the main task will be small. At the worst case the IR signal changes every 500us which on 16MHz CPU AVR allows for about 8000 instructions to be executed in between. Granted, there is some overhead cost of interrupting (multitasking is hard for humans as well) but it should be rather small esp. since in this case the IR message are also sent infrequently.

Solution

Ok, so here is the code I came up with. It is more complicated than the polling version. Since the interrupt code is/should be active only for brief moments, the state of receiving a message must be preserved between its actions. I used a state machine to implement this functionality (by "state machine" I mean that one single function is invoke on IR signal change but this function "switches" into different state depending on its current state and elapsed time since last invocation).



// pin state change interrupt based IR receiver
// observing the IR pin changes, measure time between interrupts
// use state machine to decide what to do

enum
{
ISR_IDLE, // nothing is/was happening (quiet)
ISR_START, // start of sequence, was waiting for a header signal
ISR_BIT_ON, // transsmitting a bit (IR carrier turned on)
ISR_BIT_OFF // in an OFF bit slot (IR carrier turned off)
}
isrState = ISR_IDLE;

unsigned long isrLastTimeStamp;
unsigned long isrRcvCmd;
unsigned long isrNewCmd;
byte isrBitLen = 22;
byte isrBitCnt;


ISR( PCINT2_vect )
{
// PIN_IR == #2 --> PD2;
// receiving a modulated IR signal makes the pin go low (active low)
byte transmitting = (PIND & (1<<2)) == 0;

// compute elapsed time since last change
unsigned elapsed;
{
unsigned long timeStamp = micros();
elapsed = elapsedSince(isrLastTimeStamp,timeStamp);
isrLastTimeStamp = timeStamp;
}
switch( isrState )
{
case ISR_IDLE :
if( transmitting ) isrState = ISR_START;
break;
case ISR_START:
isrBitCnt = 0;
isrNewCmd = 0;
isrBitLen = 22;
if( !transmitting && IS_S(elapsed) )
isrState = ISR_BIT_ON; // bits are now rolling
else
isrState = ISR_IDLE; // wrong timing of start or pin state
break;
case ISR_BIT_ON:
if( transmitting )
{
isrState = ISR_BIT_OFF;
isrNewCmd <<= 1;
isrBitCnt ++;
if( IS_1(elapsed) )
{
isrNewCmd |= 1;
}
else if( IS_0(elapsed) )
{
// isrNewCmd |= 0;
}
else
isrState = ISR_START; // bad timing, start over
if( isrBitCnt == 7 ) // we have received 6 bit header (now expecting 7th bit)
{
isrBitLen = (isrNewCmd & (3<<3)) == 0 ? 30 : 22; // 2 vs 3 byte commands
}
}
else isrState = ISR_IDLE; // bad state (should never get here...)
break;
case ISR_BIT_OFF:
if( !transmitting && IS_O(elapsed) )
{
if( isrBitCnt == isrBitLen ) // is this the end?
{
isrState = ISR_IDLE;
isrRcvCmd = isrNewCmd;
}
else
isrState = ISR_BIT_ON; // keep bits rolling
}
else
if( IS_S(elapsed) )
isrState = ISR_START;
else
isrState = ISR_IDLE;
break;
}
}


To link a function to an interrupt I used
ISR( PCINT2_vect )
(assigns it to the INT2 interrupt vector which is linked to the pin I used for IR signal).

And the rest of it

Here is a function returning a new message:


long irRecv()
{
byte oldSREG = SREG;
cli();
long cmd = isrRcvCmd;
isrRcvCmd = 0;
SREG = oldSREG;
return cmd;
}


This is a non-blocking function. If there was nothing, it returns 0 (there is no message that would result in a 0). When a message code is read, a 0 is stuffed back into isrRcvCmd to clear it so next time I read it I do not get a repeat. I also turn off interrupts for the duration of the "read and zero" of isrRcvCmd so I do not conflict with the interrupt code "interrupting" my read. Note that:
long cmd = isrRcvCmd;
is not "atomic" ie it consists of several CPU instructions and hence it may be interrupted in the middle and ended up returning an inconsistent value.

So now I can really do more stuff in main loop:


void loop
{
unsigned long rcv = irRecv();
if(rcv)
{
// got a message
Serial.print("recv ");
Serial.println(rcv,HEX);
}
Serial.print('.');
... do whatever you like
}


Configuring Interrupt
I have connected the IR sensor to pin D2 (#2 in Arduino convention). The pin change interrupts are not enabled by default so I needed to configure it in setup:


void setup()
{
...
PCICR |= 4; // enable PCIE2 which services PCINT18
PCMSK2 |= 4; // enable PCINT18 --> Pin Change Interrupt of PD2
...
}


Saturday, August 1, 2009

iSobot Infrared Remote Protocol Hack

The iSOBOT IR protocol details are described here. I figured out all details to be able to create complete messages,even ones not originally send by the remote (some diagnostic codes and prompts).

History
iSobot is a lot of fun. It has a bunch of preprogrammed, often very funny moves. Kudos to TOMMY enginieers. I envy them their fun at work! But, all in all, iSobot is rather dummy.It does not have any sensors to be autonomous. The only sensors it posseses are microphone for voice commands and 1 axis gyro. So, it did not take much time for the people to want to hack it.

See some iSOBOT hacking links (with thanks to authors for their work):
HACKING RESOURCES
ANATHOMY
SERVOS
IR SIGNALING
TOTAL (HEADLESS) HACK
IRCONTROLLED BACKPACK

For me the IR control was the way to go, as I did not have time to do a total hack (replacing iSOBOT's brains) and the build-in stuff is fun anyway. I just wanted to add an alter-ego personality...I ended up replacing the 3 AAA batteries with a LiIon 3.7V battery (S007 from my broken Panasonic camera) and Arduino Pro Mini CPU. A CPU pin is directly connected to the iSOBOT IR receiver output (as the IR receiver device has open-collector output it was rather simple).

IR Signaling
The iSobot IR remote is using fairly simple protocol. In short, the IR is modulated with 38kHz. Each message starts with a longer IR burst of ~2.5ms, followed by actual payload bits. The bit value is coded as length of silence period between the 0.5ms IR 38KHz bursts. 1 is a 1ms silence and 0 is 0.5 ms silence. Initially I used Arduino "logic analyzer" to read the bits and create list of messages. Next I tried to find any rhyme or reason to all the codes and here is the result.

General Message Format

[HEADER][BYTE1][BYTE2]{[BYTE3]}


[HEADER]is 6 bits of:
  • [CHANNEL:1] 0 - A; 1 - B
  • [TYPE:2] 00 - 3 byte message used for arms control; 01 - 2 byte message (used for most commands;
  • [CHECKSUM:3] checksum of the entire message (see below)

So here are the 2 types of messages:
[CH:1][TYPE:2 = 00][CHECK:3][CMD1:8][CMD2:8][CMD3:8]
[CH:1][TYPE:2 = 01][CHECK:3][CMD:8][PARAM:8]
For message type 01 (2 byte) the second byte is often zero, except when used sometimes for parameters e.g. in joystick movement command (PARAM is always 0x03, do not know what this means) and for configuration command 0xD3 where the 4 lowest bits of PARAM are Lon:1,Loff:1,S:1,V:1 where L-light, V-voice, S-sound.

The Checksum

This was the toughest part to decipher. It ended up logical and simple.
First sum all the bytes (including header stored as separate byte).
Then sum up the result, 3 bits at a time and use lower 3 bit of this sum as a CHECKSUM.

So here is a C code snaplet (Arduino):


byte computeCheckSum( byte hdr, byte cmd1, byte cmd2, byte cmd3 )
{
// first sum up all bytes
byte s = hdr + cmd1 + cmd2 + cmd3;
// then sum up the result, 3 bits at a time
s = (s & 7) + ((s >> 3) & 7) + (s >> 6) & 7;
// return 3 lower bits of the sum
return s & 7;
}

Command Notes

Observations:
  • commands are interrupt-able but each expect to start from HP (home position)
  • some commands will reposition legs, while some will not so interesting hybrids can result e.g. try EAGLE for 3 seconds and then sent AIRDRUM - iSOBOT will play drums while standing on one leg (but this will not work with AIRGUITAR as it will reset legs).

Movement (left joystick)
FWRD 0.01.101.10110111.00000011
BWRD 0.01.110.10111000.00000011
LEFT 0.01.001.10111011.00000011
RGHT 0.01.010.10111100.00000011
FWLT 0.01.111.10111001.00000011
FWRT 0.01.000.10111010.00000011
BKRT 0.01.101.10111110.00000011
BKLT 0.01.100.10111101.00000011
STOP 0.01.101.11010111.00000000 (joystick nutral)
These are repeated when joystick is pushed. Need to try to mess with the parameters...

Arms (right joystick+front buttons)

NOIMP 1.00.010.10000100.10000000.10000000
NOIMP 1.01.000.11010110.00000000 (locked)
END 1.01.001.11010111.00000000
END 1.01.001.11010111.00000000 (locked-same as normal)

RUP 1.00.000.10000111.10000010.10000000
RDW 1.00.000.10000000.10000010.10000000
RRT 1.00.110.10000100.10000000.11110000
RLT 1.00.010.10000100.10000000.10000000
LUP 1.00.110.10000100.11110000.10000000
LDW 1.00.010.10000100.00010000.10000000
LRT 1.00.101.11101100.10000000.10000000
LLT 1.00.001.00001100.10000000.10000000
NOIMP is send when joystick is not pushed. Now, any patterns here? Why these needed 3 bytes? Perhaps they drive the servos directly...

List of Commands


#define CMD_RC 0x07
#define CMD_PM 0x08
#define CMD_SA 0x09
#define CMD_VC 0x0a
#define CMD_1P 0x13
#define CMD_2P 0x14
#define CMD_3P 0x15
#define CMD_4P 0x16
#define CMD_11P 0x17
#define CMD_12P 0x18
#define CMD_13P 0x19
#define CMD_14P 0x1a
#define CMD_21P 0x1b
#define CMD_22P 0x1c
#define CMD_23P 0x1d
#define CMD_24P 0x1e
#define CMD_31P 0x1f
#define CMD_32P 0x20
#define CMD_34P 0x21
#define CMD_1K 0x22
#define CMD_2K 0x23
#define CMD_3K 0x24
#define CMD_4K 0x25
#define CMD_11K 0x26
#define CMD_12K 0x27
#define CMD_13K 0x28
#define CMD_14K 0x29
#define CMD_31K 0x2a
#define CMD_42K 0x2b
#define CMD_21K 0x2c
#define CMD_22K 0x2d
#define CMD_23K 0x2e
#define CMD_24K 0x2f
#define CMD_31K 0x30
#define CMD_34K 0x31
#define CMD_3G 0x32
#define CMD_2G 0x33
#define CMD_3G 0x34
#define CMD_4G 0x35
#define CMD_11G 0x36
#define CMD_12G 0x37
#define CMD_13G 0x38
#define CMD_14G 0x39
#define CMD_21G 0x3a
#define CMD_22G 0x3b
#define CMD_23G 0x3c
#define CMD_A 0x3d
#define CMD_B 0x3e
#define CMD_1A 0x3f
#define CMD_2A 0x40
#define CMD_2A 0x41
#define CMD_3A 0x42
#define CMD_4A 0x43
#define CMD_11A 0x44
#define CMD_12A 0x45
#define CMD_13A 0x46
#define CMD_14A 0x47
#define CMD_21A 0x48
#define CMD_22A 0x49
#define CMD_23A 0x4a
#define CMD_32A 0x4b
#define CMD_31A 0x4c
#define CMD_32A 0x4d
#define CMD_41A 0x4e
#define CMD_42A 0x4f
#define CMD_43A 0x50
#define CMD_111A 0x51
#define CMD_222A 0x52
#define CMD_333A 0x53
#define CMD_11B 0x54
#define CMD_12B 0x55
#define CMD_13B 0x56
#define CMD_14B 0x57
#define CMD_31B 0x58
#define CMD_22B 0x59
#define CMD_23B 0x5a
#define CMD_24B 0x5b
#define CMD_31B 0x5c
#define CMD_32B 0x5d
#define CMD_33B 0x5e
#define CMD_234B 0x5f
#define CMD_41B 0x60
#define CMD_42B 0x61
#define CMD_43B 0x62
#define CMD_44B 0x63
#define CMD_112A 0x65
#define CMD_113A 0x66
#define CMD_114A 0x67
#define CMD_124A 0x6b
#define CMD_131A 0x6c
#define CMD_132A 0x6d
#define CMD_113B 0x6e
#define CMD_114B 0x6f
#define CMD_121B 0x70
#define CMD_122B 0x71
#define CMD_123B 0x72
#define CMD_124B 0x73
#define CMD_131B 0x74
#define CMD_132B 0x75
#define CMD_133B 0x76
#define CMD_134B 0x77
#define CMD_141A 0x78
#define CMD_143A 0x79
#define CMD_144A 0x7b
#define CMD_211B 0x7c
#define CMD_212B 0x7d
#define CMD_213B 0x7e
#define CMD_221B 0x80
#define CMD_222B 0x81
#define CMD_223B 0x82
#define CMD_224B 0x83
#define CMD_232B 0x85
#define CMD_233B 0x86
#define CMD_241B 0x88
#define CMD_242B 0x89
#define CMD_A 0x8a
#define CMD_B 0x8b
#define CMD_AB 0x8c
#define CMD_AAA 0x8d
#define CMD_BBB 0x8e
#define CMD_BAB 0x8f
#define CMD_ABB 0x95
#define CMD_BBA 0x97
#define CMD_ABA 0x98
#define CMD_ABAB 0x99
#define CMD_AAAA 0x9a
#define CMD_FWRD 0xb7
#define CMD_BWRD 0xb8
#define CMD_FWLT 0xb9
#define CMD_FWRT 0xba
#define CMD_LEFT 0xbb
#define CMD_RGHT 0xbc
#define CMD_BKLT 0xbd
#define CMD_BKRT 0xbe
#define CMD_411A 0xc7
#define CMD_412A 0xc8
#define CMD_413A 0xc9
#define CMD_444B 0xca
#define CMD_444A 0xcb
#define CMD_LVSoff 0xd3
#define CMD_HP 0xd5
#define CMD_NOIMP 0xd6
#define CMD_END 0xd7
#define MSG_NOIMP 0x848080
#define MSG_NOIMP 0x848080
#define MSG_RUP 0x878280
#define MSG_RDW 0x808280
#define MSG_RRT 0x8480f0
#define MSG_RLT 0x848080
#define MSG_LUP 0x84f080
#define MSG_LDW 0x841080
#define MSG_LRT 0xec8080
#define MSG_LLT 0x0c8080


What NEXT?

Now that we can sent any command it is time to try sending new code. I did try to sent CMDs 0-7 and they produce the initial iSOBOT moves (initial grittings, no input frustration, greetings etc). Also, the it would be great to explore the parameters and longer-type command.

Command codes that I could not generate with the iSOBOT remote:

Dec Hex
0-6 00-06
11-11 0B-0B
13-18 0D-12
100-100 64-64
104-106 68-6A
122-122 7A-7A
127-127 7F-7F
144-148 90-94
150-150 96-96
155-182 9B-B6
191-198 BF-C6
204-210 CC-D2
212-212 D4-D4
216-235 D8-EB


Wait for more posts about code: interrupt-based IR receiver, iSOBOT boobs-job (front backpack).

Just exercised these "bonus" codes and got:

#define CMD_TURNON 0x01
#define CMD_ACTIVATED 0x02
#define CMD_READY 0x03
#define CMD_RC_CONFIRM 0x04
#define CMD_RC_PROMPT 0x05
#define CMD_MODE_PROMPT 0x06
#define CMD_IDLE_PROMPT 0x0B // 0x0C,0x0D,0x0E all the same
#define CMD_HUMMING_PROMPT 0x0F
#define CMD_COUGH_PROMPT 0x10
#define CMD_TIRED_PROMPT 0x11
#define CMD_SLEEP_PROMPT 0x12
#define CMD_FART 0x40 // 2A
#define CMD_SHOOT_RIGHT 0x64
#define CMD_SHOOT_RIGHT2 0x68
#define CMD_SHOOT2 0x69
#define CMD_BEEP 0x6a
#define CMD_BANZAI 0x7F
#define CMD_CHEER1 0x90
#define CMD_CHEER2 0x91
#define CMD_DOG 0x92
#define CMD_CAR 0x93
#define CMD_EAGLE 0x94
#define CMD_ROOSTER 0x95
#define CMD_GORILLA 0x96
#define CMD_LOOKOUT 0xA1
#define CMD_STORY1 0xA2 // knight and princess
#define CMD_STORY2 0xA3 // ready to start day
#define CMD_GREET1 0xA4 // good morning
#define CMD_GREET2 0xA5 // do somthing fun
#define CMD_POOP 0xA6 // poops his pants
#define CMD_GOOUT 0xA7 // ready to go out dancing
#define CMD_HIBUDDY 0xA8 // .. bring a round of drinks
#define CMD_INTRODUCTION 0xA9
#define CMD_ATYOURSERVICE 0xAA
#define CMD_SMELLS 0xAB
#define CMD_THATWASCLOSE 0xAC
#define CMD_WANNAPICEOFME 0xAD
#define CMD_RUNFORYOURLIFE 0xAE
#define CMD_TONEWTODIE 0xAF
// 0xB0 - nothing?
#define CMD_SWANLAKE 0xB1
#define CMD_DISCO 0xB2
#define CMD_MOONWALK 0xB3
#define CMD_REPEAT_PROMPT 0xB4
#define CMD_REPEAT_PROMPT2 0xB5
#define CMD_REPEAT_PROMPT3 0xB6
// 0xB7-0xC4 single steps in different directions
#define CMD_HEADSMASH 0xC5
#define CMD_HEADHIT 0xC6
// 0xCC-0xD2 - unknown (use param?)
// after exercising one of these I am getting only beeps instead of voice/sounds
// (looks like a tool to synchronize sound with moves)
#define CMD_HIBEEP 0xD3
// 0xD4 - unknown (use param?)
#define CMD_BEND_BACK 0xD8 // same untill 0xDB
#define CMD_SQUAT 0xDB // also 0xDC
#define CMD_BEND_FORWARD 0xDD
#define CMD_HEAD_LEFT_60 0xDE
#define CMD_HEAD_LEFT_45 0xDF
#define CMD_HEAD_LEFT_30 0xE0
#define CMD_HEAD_RIGHT_30 0xE1
#define CMD_HEAD_RIGHT_45 0xE2
#define CMD_HEAD_RIGHT_60 0xE3
// seems identical to A & B getups
#define CMD_GETUP_BELLY 0xE4
#define CMD_GETUP_BACK 0xE5
// E6 unknown
#define CMD_HEAD_SCAN_AND_BEND 0xE7
#define CMD_ARM_TEST 0xE8
#define CMD_FALL_AND_LEG_TEST 0xE9
#define CMD_THANKYOUSIR 0xEA
#define CMD_ILOVEYOU_SHORT 0xEB
#define CMD_3BEEPS 0xEC
#define CMD_FALL_DEAD 0xED
#define CMD_3BEEPS_AND_SLIDE 0xEE
// EF-FF unknown



Sunday, January 25, 2009

iRoomba battery/charger fixes, observations.

Resurection
Just picked up a used 4210 iRobot vaccum, just to check it out. It was a $30 not-working machine. The battery was a suspect so I ordered a core replacement from all-battery.com ($35). It turned out new battery did not work either. It was a charging circuit that was broken, a common failure. Thanks to the iRoomba robot enthusiasts at http://www.robotreviews.com/chat/viewtopic.php?f=1&t=3909&start=20 who reverse-engineered the circuit, I was able to pinpoint the problem. I t turned out one of the power FETs (U2) was blown so I have replaced it with a bigger one (TO220 Id=18A Usd=60V) . Now I think I could (shoud) have replaced both U2 and U4 with this single p-Fet as it appears the only reason there are two fets in series is for the power dissipation. I will do this when U4 blows out.
Motion
Now roomba does its thing but after initial exitement I am getting bored watching it obsesively cleaning the corners or areas around furniture legs, while leaving vast areas of dirt in the middle of the room untouched.
Docking
Its docking algorithm is flawed. It keeps on vacuming while searching for the base and hence wasting remaining charge. It does not appears to change its motion pattern hence it keeps the corner obsession going on. This combined with a very limited range of the home base IR beacon (about 3 feet in my experience) results in almost guaranteed dead roomba lying somewhere in a corner.
Navigation
..is what this robot needs. Dead reckoning could be used probably. Albeit unlike in transportation, there is no absolute North reference for heading but it is probably not needed (relative to the surrounding should be fine).. The wheel rotation sensors (both the caster and the optical pickup in drive wheels) should provide enough info assuming no slippage. This combined with bump events and memory could be sufficient for making out room/furniture layout and perhaps also provide location for the base. Best yet to equip roomba with ultrasonic range finder and have her do initial circle.