I2C Compass

[NovaOne]

I am currently experimenting using a CMPS03 Devantech compass on my RN1. -mounted on the head away from the servo motors.
Its quite large compared with the sparkfun HMC6352 but they both use I2C, so the code might be useful to someone ?
The code is still under development and for byte reading only.
-There are alot of 1ms delays, many may not be required
-I believe the i2c port is for program EEPROMs only QED
-Therefore do not use on the i2c port

WARNING DO NOT USE THIS CODE.......SEE NEXT POST

So here it is:

Code: Select all

'***************************************************
'                      I2C routine for CMPS03               
'Based on code by Gerald Coe and KurtE . Thanks guys!
'                                July 2007
'***************************************************

CONST SDA = 10     'spare port not used by a servo
CONST SCL = 5       'spare port not used by a servo
DIM I2cBuf AS BYTE
DIM I2cAddr AS BYTE
DIM I2cReg AS BYTE
DIM I2cData AS BYTE

DIM ServoH AS BYTE
DIM I2cBit AS BYTE
DIM I2cTx AS BYTE
DIM I2cRx AS BYTE
LCDINIT
TEMPO 230
MUSIC "EDC"

Main:
   I2cAddr = &Hc0
   I2cReg = 1
   GOSUB I2cByteRead
   CLS
   PRINT "Bearing(0-255BRAD's)"
   PRINT FORMAT (I2cData, DEC)   
GOTO Main

I2cByteWrite:
GOSUB I2cStart
I2cBuf = I2cAddr
GOSUB I2cOutByte
I2cBuf = I2cReg
GOSUB I2cOutByte
I2cBuf = I2cData
GOSUB I2cOutByte
GOSUB I2cStop
RETURN

I2cByteRead:
GOSUB I2cStart
I2cBuf = I2cAddr
GOSUB I2cOutByte
I2cBuf = I2cReg
GOSUB I2cOutByte

GOSUB I2cStart         
I2cBuf = I2cAddr OR 1
GOSUB I2cOutByte

GOSUB I2cInByte
I2cData = I2cBuf
GOSUB I2cStop
RETURN

I2cOutByte:

FOR I2cBit = 0 TO 7
   I2cTx = I2cBuf AND &H80
   IF I2cTx <> 0 THEN
         OUT SDA,1
   ELSE
         OUT SDA,0
   ENDIF
   
   DELAY 1
   OUT SCL, 1
   DELAY 1
   OUT SCL, 0
   DELAY 1
   I2cBuf = I2cBuf << 1
NEXT I2cBit

OUT SDA, 0
DELAY 1
OUT SCL, 1
DELAY 1
OUT SCL, 0
RETURN

I2cInByte:

I2cBuf =0
OUT SDA, 1

FOR I2cBit = 0 TO 7
   OUT SCL, 1
   DELAY 1
   I2cRx = IN(SDA)
   OUT SCL, 0
   DELAY 1
   I2cBuf = I2cBuf << 1
   IF I2cRx <> 0 THEN
   I2cBuf = I2cBuf +1
   ENDIF
   DELAY 1
NEXT I2cBit

OUT SDA, 0
DELAY 1
OUT SCL, 1
DELAY 1
OUT SCL, 0
DELAY 1
RETURN

I2cStart:
OUT SDA,1
DELAY 1
OUT SCL,1
DELAY 1
OUT SDA,0
DELAY 1
OUT SCL,0
DELAY 1
RETURN

I2cStop:
OUT SCL,1
DELAY 1
OUT SDA,1
RETURN

I realise to some of you this is just "basic" stuff, but its my first time with i2c at this level.
When I've got more time I'll test it and fully comment and upload the .bas file

Chris

[NovaOne]

Sorry for mistake in the above code, I've just found it. I forgot to include the first I2cRx = IN(SDA) line in the I2cInByte routine, which resulted in readings only being from 127 to 255 only.
I've added comments for all the I2C beginners out there like me.

Code: Select all

'***************************************************
'*           I2C routine for CMPS03                *
'Based on code by Gerald Coe and KurtE. Thanks guys!
'                  25th July 2007
'***************************************************

CONST SDA = 10         'Use port 10 for Data, constant SDA
CONST SCL = 5         'Use port 5 for Clock, constant SCL
DIM I2cBuf AS BYTE      'Buffer Byte
DIM I2cAddr AS BYTE      'Address Byte
DIM I2cReg AS BYTE      'Register Byte
DIM I2cData AS BYTE      'Data Byte


DIM I2cBit AS BYTE      'Bit index for shifting loops
DIM I2cTx AS BYTE      'Transmitted Byte
DIM I2cRx AS BYTE      'Recieved Byte
LCDINIT
TEMPO 230
MUSIC "EDC"

Main:
   I2cAddr = &Hc0      'Default address of CMPS03 compass
   I2cReg = 1         'Register containing Compass bearing as a single Byte
   GOSUB I2cByteRead   
   CLS
   PRINT "Bearing(0-255BRAD's)"
   PRINT FORMAT (I2cData, DEC)   
GOTO Main

I2cByteWrite:
GOSUB I2cStart         'Send start bit
I2cBuf = I2cAddr      
GOSUB I2cOutByte      'Send module address
I2cBuf = I2cReg
GOSUB I2cOutByte      'Send the register number you wish to read
I2cBuf = I2cData
GOSUB I2cOutByte      'Send Data
GOSUB I2cStop         'Send stop bit
RETURN

I2cByteRead:   
GOSUB I2cStart         'Send start bit
I2cBuf = I2cAddr
GOSUB I2cOutByte      'Send module address
I2cBuf = I2cReg
GOSUB I2cOutByte      'Send the register number you wish to read

GOSUB I2cStart         'Send the required repeated start bit
I2cBuf = I2cAddr OR 1   'set write enable bit
GOSUB I2cOutByte      'Send the module address with write enable bit set

GOSUB I2cInByte         'Read required data
I2cData = I2cBuf      'copy data from buffer Byte
GOSUB I2cStop         'Send stop bit
RETURN

I2cOutByte:

FOR I2cBit = 0 TO 7
   I2cTx = I2cBuf AND &H80      'Mask off all but the Most significant bit
   IF I2cTx <> 0 THEN          'Test the BSB and set up data ie
         OUT SDA,1         'data = 1
   ELSE
         OUT SDA,0         'data = 0
   ENDIF
   OUT SCL, 1
   OUT SCL, 0               'Clock the data out
   I2cBuf = I2cBuf << 1
NEXT I2cBit

OUT SDA, 0
OUT SCL, 1
OUT SCL, 0      'clock in the Ack' bit, and ignore it.
RETURN

I2cInByte:

I2cBuf =0                  'clear input buffer byte
OUT SDA, 1                  
I2cRx = IN(SDA)               'release data line for input
FOR I2cBit = 0 TO 7
      OUT SCL, 1            
      I2cRx = IN(SDA)         'Read one bit of the byte
      OUT SCL, 0            'Pull clock low, ready for next bit
      I2cBuf = I2cBuf << 1   'Shift the Data buffer one bit left
      IF I2cRx <> 0 THEN
         I2cBuf = I2cBuf +1   'if the data is a one set the Least Significant bit in the buffer
      ENDIF
NEXT I2cBit

OUT SDA, 0      
OUT SCL, 1
OUT SCL, 0      'clock in the Ack' bit, and ignore it.
RETURN

I2cStart:
OUT SDA,1      'Ensure data and clock lines are both high
OUT SCL,1
OUT SDA,0      'Sent start bit, ie lower data line while clock line is high
OUT SCL,0      'Lower clock line ready to start clocking data.
RETURN

I2cStop:
OUT SCL,1      
OUT SDA,1      'Sent stop bit, ie raise data line while clock high
RETURN

I hope some one finds this useful
Next step...Keeping RN walking in a staight line.

[BillB]

Unfortunatley I do not have a compass - so I am unable to try your code.

However - if you have success in walking in a straight line a compass will be at the top of my shopping list. Please keep us posted with your progress.

[NovaOne]

I hope my idea of straight line walking is the same as yours?

Initially my next objective was to check the compass after one or two steps, then correct with a small turn. Then repeat as required.
I hope to have completed this by the end of the weekend?
I'll post it Sunday evening.

I think ideally straight line walking needs steps that include the turn, ie one leg moving further than the other.
That's too big a step for me yet, I'm just making small steps (so is RN).

Chris

[Pev]

I hope my idea of straight line walking is the same as yours?

Initially my next objective was to check the compass after one or two steps, then correct with a small turn. Then repeat as required.
I hope to have completed this by the end of the weekend?
I'll post it Sunday evening.

I think ideally straight line walking needs steps that include the turn, ie one leg moving further than the other.
That's too big a step for me yet, I'm just making small steps (so is RN).

Chris

Chris,

Good luck and one word of advice - do remember to calibrate it. The instructions are on the robot-electronics website. I had terrible trouble with mine until I calibrated it.

Pev

[NovaOne]

So this weekend I integrated the I2c compass code into my RN basic template program, to get RN to walk in a straight line:

I tried to issue a MOTOROFF 15 and MOTOROFF 16 commands to free up the ports to use for I2c,However the MOTOROFF [servo number] command did not seem work after a MOTOR G24, so I turned on the required servos individually with the MOTOR [servo number] command.

The code for the relevant routines etc can be found here
Compass code in PDF form

Its far from perfect, but this code does keep RN walking in generally the required heading. Hope I haven't forgotten to copy and paste any code?

Sorry for the number of edits the code would not paste in correctly

[NovaOne]

These are photos of my compass fitted inside my RN head:


Wire colours as follows:
Purple +5v
Grey SCL
White SDA
Black 0v

[BillB]

Looks Great - a high quality job.

Is that a stereo camera I can see in its head ?

[NovaOne]

Yeh. Thats my next project:

They are M64282FP Artificial Retina's.....

I know what your thinking, but thats what they are called.
Basically they are the camera modules used in they original Gameboy Camera.

..... they are only 128x128 greyscale analogue output devices

My aim is to keep all processing onboard, ie use another uC for higher level vision, sound and crude AI stuff.

I'll keep you updated in a few months when I get round to making a PCB and testing them.

Chris

[NovaOne]

I finally figured out how to do 16bit word reads from my CMPS03,

Although i-bot has made my code redundant, i suppose my posting this is better late than never? It was Ack and Nack (or Nak if you prefer) that confused me.

NOTE: The word write routine is still untested, I will try to test it over the next few days, with another device


Sorry some of the formatting didn't survive the cut and paste.

Code: Select all

'***************************************************
'*           I2C routine for CMPS03                *
'Based on code by Gerald Coe and KurtE. Thanks guys!
'                  September 2007
'***************************************************

CONST SDA = 15         'Use port 10 for Data, constant SDA
CONST SCL = 16         'Use port 5 for Clock, constant SCL
DIM I2cBuf AS BYTE         'Buffer Byte
DIM I2cAddr AS BYTE         'Address Byte
DIM I2cReg AS BYTE         'Register Byte
DIM I2cData AS BYTE         'Data Byte
DIM I2cDataWord AS INTEGER      '16 bit word data
DIM I2cAckBit AS BYTE         'only need 1 bit for Acknowledge Flag
DIM I2cBit AS BYTE         'Bit index for shifting loops
DIM I2cTx AS BYTE         'Transmitted Byte
DIM I2cRx AS BYTE         'Recieved Byte
LCDINIT
TEMPO 230
MUSIC "EDE"
I2cAckBit = 0
Main:
   I2cAddr = &Hc0         'Default address of CMPS03 compass
   I2cReg = 2         'Register containing Compass bearing as a word
   GOSUB I2cWordRead   
   'I2cReg = 1         Register containing Compass bearing as a byte
   'GOSUB I2cByteRead
   CLS
   PRINT " Bearing: - "
   PRINT FORMAT (I2cDataWord, DEC)   'Display word bearing
   'PRINT FORMAT (I2cData, DEC)   'Display byte bearing
   
GOTO Main

I2cByteWrite:
   GOSUB I2cStart         'Send start bit
   I2cBuf = I2cAddr      
   GOSUB I2cOutByte      'Send module address
   I2cBuf = I2cReg
   GOSUB I2cOutByte      'Send the register number you wish to write
      I2cBuf = I2cData
      GOSUB I2cOutByte   'Send Data
   GOSUB I2cStop         'Send stop bit
RETURN

I2cWordWrite:
   GOSUB I2cStart         'Send start bit
   I2cBuf = I2cAddr      
   GOSUB I2cOutByte      'Send module address
   I2cBuf = I2cReg         'I2cReg must be a 16 bit register
   GOSUB I2cOutByte      'Send the register number you wish to write
      I2cData = I2cDataWord >> 8   'shift high order byte down 8 bits
      I2cBuf = I2cData
      GOSUB I2cOutByte      'Send high byte
      I2cData = I2cDataWord AND &H00FF   'mask off high order byte
      I2cBuf = I2cData
      GOSUB I2cOutByte      'Send low byte
   GOSUB I2cStop            'Send stop bit
RETURN

I2cByteRead:   
   GOSUB I2cStart         'Send start bit
   I2cBuf = I2cAddr
   GOSUB I2cOutByte      'Send module address
   I2cBuf = I2cReg
   GOSUB I2cOutByte      'Send the register number you wish to read
   GOSUB I2cStart         'Send the required repeated start bit
   I2cBuf = I2cAddr OR 1   'set write enable bit
   GOSUB I2cOutByte      'Send the module address with write enable bit set
      I2cAckBit = 1            '
      GOSUB I2cInByte      'Read required data
      I2cData = I2cBuf   'copy data from buffer Byte
   GOSUB I2cStop         'Send stop bit
RETURN

I2cWordRead:   
   GOSUB I2cStart         'Send start bit
   I2cBuf = I2cAddr
   GOSUB I2cOutByte      'Send module address
   I2cBuf = I2cReg
   GOSUB I2cOutByte      'Send the register number you wish to read
   GOSUB I2cStart         'Send the required repeated start bit
   I2cBuf = I2cAddr OR 1   'set write enable bit
   GOSUB I2cOutByte      'Send the module address with write enable bit set
      I2cAckBit = 0      'NAck
      GOSUB I2cInByte      'read highorder byte
      I2cDataWord = I2cBuf << 8   'shift high order byte up by 8 bits
      I2cAckBit = 1      'Ack
      GOSUB I2cInByte      'read low order byte
      I2cDataWord = I2cDataWord + I2cBuf   'combine high and low order bytes
      
   GOSUB I2cStop         'Send stop bit
RETURN



I2cOutByte:

FOR I2cBit = 0 TO 7
   I2cTx = I2cBuf AND &H80      'Mask off all but the Most significant bit
   IF I2cTx <> 0 THEN          'Test the BSB and set up data ie
         OUT SDA,1         'data = 1
   ELSE
         OUT SDA,0         'data = 0
   ENDIF
   OUT SCL, 1
   OUT SCL, 0               'Clock the data out
   I2cBuf = I2cBuf << 1
NEXT I2cBit

OUT SDA, 0      'should read the ack bit for error detection
OUT SCL, 1
OUT SCL, 0      'clock in the Ack' bit, and ignore it.
RETURN

I2cInByte:
   I2cBuf =0                  'clear input buffer byte
   OUT SDA, 1                  
   I2cRx = IN(SDA)               'release data line for input
   FOR I2cBit = 0 TO 7
         OUT SCL, 1            
         I2cRx = IN(SDA)         'Read one bit of the byte
         OUT SCL, 0            'Pull clock low, ready for next bit
         I2cBuf = I2cBuf << 1   'Shift the Data buffer one bit left
         IF I2cRx <> 0 THEN
            I2cBuf = I2cBuf +1   'if the data is a one set the Least Significant bit in the buffer
         ENDIF
   NEXT I2cBit

   OUT SDA, I2cAckBit            'clock in the Ack (1)Or Nack (0)' bit, 1= we are reading another byte.   
   OUT SCL, 1                                                   '0= we are only reading one byte
   OUT SCL, 0      
RETURN

I2cStart:
   OUT SDA,1      'Ensure data and clock lines are both high
   OUT SCL,1
   OUT SDA,0      'Sent start bit, ie lower data line while clock line is high
   OUT SCL,0      'Lower clock line ready to start clocking data.
RETURN

I2cStop:
   OUT SCL,1      
   OUT SDA,1      'Sent stop bit, ie raise data line while clock high
RETURN

Pev, thanks for the calibration tip. The byte bearing data was fine without calibrating, but the word data was very odd until I calibrated the unit.

I think I will still only be using the byte data from the compass. I really only finished these routines for use with other devices, and for those who do not wish to re-flash their controller with i-bots new i2c code.

Chris

[Gort]

I saw these old posts and thought that the compass was a good idea. I just bought a HMC6352 compass from Sparkfun. Has anyone else worked with this compass? It looks like I should be able to use NovaOne’s code examples with very little changes. I have hooked up the compass to the Arduino with very little effort. I am hoping that the Robonova will be the same?

phpBB [media]

[Gort]

It is easier to see the compass headings when I hook up an LCD to my Arduino. I wish that I could figure out how to hook up an LCD to my Robonova!

phpBB [media]

[NovaOne]

I wish that I could figure out how to hook up an LCD to my Robonova!

I would try something like this if you already have a standard LCD module: http://www.rev-ed.co.uk/docs/FRM010.pdf

You will also need i-Bot flash upgrade to comm at 9600baud.

[i-Bot]

An LCD is pretty much essential when you are coding things like this. Alternatively use a level converter and send the LCD string to a PC, or use serial out to PC.

For the I2C, then I think using Robobasic V2.72 will help a lot with the speed of the bit banging, or use my I2C firmware for even faster.

If you want help on the LCD or I2C, let us know.

I am planning to go to Robogames, will you be there ?

[Gort]

Cool, Yes I will be at Robogames.

I like programming the Arduino so my idea was to hook my LCD to the Arduino and then hook up the Robonova to the Arduino. That way the Arduino can display sensor data on the LCD. I would also like to play around with the other sensors that Hitec has come out with for the Robonova.