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...


video
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);
}
}

2 comments:

MichWorks said...

code in github LCDandACM sketch
https://github.com/michkrom/arduino.git

MichWorks said...

code in https://github.com/michkrom/arduino.git