Programming Help, Please (Vector Compass)
freq42
Posts: 18
·· Hello,
I am in the prose’s of setting up a vector compass electronic module from Jameco #126703 mfg#v2x
To a basic stamp 2 board with a LCD, Serial-interface-module for parallel displays from Jameco #157981 mfg#1616b-05a-100
This is my first problem, my rctime when debug'd has 5 to 6 digit's "ok" but my lcd only shows me 3 of them 0 to 256 I think it's only showing 8 bit's
And I need it to show at least 10 bit's to get 4000.
The other problem I have is the v2x output format. It has a synchronous serial port four communications. The
Output can be BCD or Binary. For BCD, the first six Bits are zeroes; for binary, the first seven bits are zeroes. After these zeroes the first bit out of the·vector is the most significant bit and the last bit out is the least significant bit {example a bcd of 359 degrees is 0000001101011001 and the binary bit's are 0000000101100111} I want to have the stamp reed from the v2x and print it on the lcd in dec.
My problem is I am new to basic stamp and don’t know how to right the cod to read the v2x 10 bit out put·and display it on the lcd.
I am a wizard @ hardware but not software. I am a ham radio operator and have countless projects under my belt
5 of them with the basic stamp2.
The code is not complete yet but this is what I have sow fare. It’s not perfect and nether is my spelling.
If you all could help me I would apresheat it.
Thank you.
··
' {$STAMP BS2}
' Program:SERIL LCD 2.BS2
' This program demonstrates fundamental techniques of driving
' PVC160204AYL serial LCDs in BASIC and a v2x vector compass chip from the BASIC Stamp II.
' It assumes·that the PVC160204AYL is connected to I/O pin P0 of the Stamp, and
' that it is set for 9600 bps.
' It assumes·that the V2X output ·is connected to I/O pin p2
' Start by defining some useful constants for the Backpack.
'
N9600 CON $4054 ' Baudmode-9600 bps inverted. Use $40F0 for BS2-SX.
I CON 254 ' Instruction prefix value.
time VAR Word
time2 VAR Word
CLR CON 1 ' LCD clear-screen instruction.
h CON 2 'move cursor to top/left character position
v CON 14 'turn on visible underline cursor
LINE2 CON 192 ' Address of 1st char of 2nd line.
L1_C7 CON 135 ' Address of line 1, character 7.
L1_c11 CON 139'··················· character 11.
L2_c11 CON 203
SEROUT 0,n9600,[noparse][[/noparse]96,i]
PAUSE 1
Again:
' Now clear the screen in case there's text left from a previous
' run of the program. Note that there's a 1-second PAUSE prior to
' sending any data to the Backpack. This gives the Backpack plenty
' of time to initialize the LCD after power up.
SEROUT 0,n9600,[noparse][[/noparse]1,2] ' Clear the LCD screen and home.
PAUSE 1
'
RCTIME---frum-mast
· HIGH 7········· 'Discharge the cap
· PAUSE 10··········· 'for·10 ms.
· RCTIME 7, 1, time
'
PrintIt Subroutine
SEROUT 0,n9600,[noparse][[/noparse]"HEADDING· ",DEC4 B2,223,CR,"AZIMUTH··· ",223]
B2 = time ' this will be the new vector compass memory in b2 four the headding.
'B3 = time2· ' set this up when the azimuth is set up using rctime.
PAUSE 100 ' Slow the loop down.
GOTO Again ' Repeat endlessly.
Post Edited By Moderator (Jon Williams) : 9/16/2004 5:54:23 PM GMT
I am in the prose’s of setting up a vector compass electronic module from Jameco #126703 mfg#v2x
To a basic stamp 2 board with a LCD, Serial-interface-module for parallel displays from Jameco #157981 mfg#1616b-05a-100
This is my first problem, my rctime when debug'd has 5 to 6 digit's "ok" but my lcd only shows me 3 of them 0 to 256 I think it's only showing 8 bit's
And I need it to show at least 10 bit's to get 4000.
The other problem I have is the v2x output format. It has a synchronous serial port four communications. The
Output can be BCD or Binary. For BCD, the first six Bits are zeroes; for binary, the first seven bits are zeroes. After these zeroes the first bit out of the·vector is the most significant bit and the last bit out is the least significant bit {example a bcd of 359 degrees is 0000001101011001 and the binary bit's are 0000000101100111} I want to have the stamp reed from the v2x and print it on the lcd in dec.
My problem is I am new to basic stamp and don’t know how to right the cod to read the v2x 10 bit out put·and display it on the lcd.
I am a wizard @ hardware but not software. I am a ham radio operator and have countless projects under my belt
5 of them with the basic stamp2.
The code is not complete yet but this is what I have sow fare. It’s not perfect and nether is my spelling.
If you all could help me I would apresheat it.
Thank you.
··' {$STAMP BS2}
' Program:SERIL LCD 2.BS2
' This program demonstrates fundamental techniques of driving
' PVC160204AYL serial LCDs in BASIC and a v2x vector compass chip from the BASIC Stamp II.
' It assumes·that the PVC160204AYL is connected to I/O pin P0 of the Stamp, and
' that it is set for 9600 bps.
' It assumes·that the V2X output ·is connected to I/O pin p2
' Start by defining some useful constants for the Backpack.
'
N9600 CON $4054 ' Baudmode-9600 bps inverted. Use $40F0 for BS2-SX.
I CON 254 ' Instruction prefix value.
time VAR Word
time2 VAR Word
CLR CON 1 ' LCD clear-screen instruction.
h CON 2 'move cursor to top/left character position
v CON 14 'turn on visible underline cursor
LINE2 CON 192 ' Address of 1st char of 2nd line.
L1_C7 CON 135 ' Address of line 1, character 7.
L1_c11 CON 139'··················· character 11.
L2_c11 CON 203
SEROUT 0,n9600,[noparse][[/noparse]96,i]
PAUSE 1
Again:
' Now clear the screen in case there's text left from a previous
' run of the program. Note that there's a 1-second PAUSE prior to
' sending any data to the Backpack. This gives the Backpack plenty
' of time to initialize the LCD after power up.
SEROUT 0,n9600,[noparse][[/noparse]1,2] ' Clear the LCD screen and home.
PAUSE 1
'
RCTIME---frum-mast
· HIGH 7········· 'Discharge the cap
· PAUSE 10··········· 'for·10 ms.
· RCTIME 7, 1, time
'
PrintIt Subroutine
SEROUT 0,n9600,[noparse][[/noparse]"HEADDING· ",DEC4 B2,223,CR,"AZIMUTH··· ",223]
B2 = time ' this will be the new vector compass memory in b2 four the headding.
'B3 = time2· ' set this up when the azimuth is set up using rctime.
PAUSE 100 ' Slow the loop down.
GOTO Again ' Repeat endlessly.
Post Edited By Moderator (Jon Williams) : 9/16/2004 5:54:23 PM GMT
Comments
You are probably destroying one of the named variables, and B2 is only eight bits when your time variable is 16 bits. Change your "DEC4 B2" section to "DEC4 time" and that will, at least, display the time variable on your LCD.
Get rid of any references to internal variables (B0 - B25, W0 - W13) in your BS2 programs and you will have a lot more success.
Hint: Be sure to include a proper subject line for your posts (I've done that for you this time) -- you will get more help because many forum members skip "No Subject" posts.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
Post Edited (Jon Williams) : 9/16/2004 5:55:15 PM GMT
When I posted it I real lizd I four got the heading.
I did what you told me and it works now I just need to find how to get the data off the V2C card.
I look't in the book that cam with it and it has no mention of baud rate in it???
This is the link four the book http://www.jameco.com/wcsstore/Jameco/Products/ProdDS/126703.PDF
I would lick it to run in master mode or slave mode. The extra lead’s hi is not a problem I would just need CAL to pin14 and RESET to pin15 four further programming bolth pin's will be tied hi and pulled low by the stamp.
It look’s like I need 7 pins four master mode or 3 four slave mode?
with a littel bit of hacking i think this will worck for me i think.
' {$STAMP BS2}
'
' Basic Stamp BS1-IC interfaced with Vector 2X Compass Module.
'
' Makes 256 compass bearing measurements at about 5 second intervals.
'
' Vector 2X is reset, _PC is brought momentarily low, _SS is brought
' low and 16 bits of data are read. Lower 10 bits are displayed in
' hexadecimal.
'
' The bearing is also converted to natural binary and displayed.
'
' Peter Anderson, MSU, Dec 31, '96
reading VAR W0
n VAR B2
m VAR B3
'base_10 CON W2
RESET CON 3
SCLK CON 2
_SS CON 1
_PC CON 0
EOC CON 7
da CON 6
DIRS=%00001111
INS=%00001111
top:
FOR n=0 TO 255
RESET=0 ' reset compass module
PAUSE 10
RESET=1
PAUSE 100
_PC=0 ' _PC brought momentarily low
PAUSE 10
_PC=1
PAUSE 100
_SS=0 ' slave select brought low
scan:
IF EOC=0 THEN scan ' wait until conversion is completed
GOSUB get_reading ' fetch 16 bit reading
reading=reading & $3ff ' only the 10 least sig bits
DEBUG reading ' display result in base 10
GOSUB convert_base_10 ' converts reading to base 10
DEBUG base_10
_SS=1 ' reading done
PAUSE 500 ' wait for next measurement
NEXT
END
get_reading:
' reads each of 16 bits from SDO, most significant bit first
reading=0
FOR m=0 TO 15
PAUSE 20 ' negative going clock pulses
SCLK=0
PAUSE 20
SCLK=1 ' DATA is read after positive clock
reading=reading * 2 + DATA ' shift left and add new bit
NEXT
RETURN
convert_base_10:
' convert BCD to natural binary
base_10 = reading / 256 * 100 ' hundreds
reading = reading // 256
base_10 = reading / 16 * 10 + base_10 ' tens
reading = reading // 16
base_10 = reading + base_10 ' units
RETURN
I was looking on www.google.com and I typed in BASIC Stamp and Vector 2x and noticed that there was quite a few topics regarding the BASIC Stamp and the Vector 2x.· I'm not sure if it has the information that you are looking for?· It's worth a try.
Dave
Dave
'
'vector.bs2 Arrick Robotics www.robotics.com
'This routine shows how to read data from the
'Vector 2x compass.
'compass signals.
select con 2 'select signal.
sdo con 3 'serial data out.
sclk con 4 'clock signal.
rst con 5 'reset signal.
'Other.
speaker con 9 'speaker pin.
heading var word 'heading variable.
setup
output select 'set pin direction.
output sclk
output rst
input sdo
low speaker 'shut off the speaker.
gosub compinit 'initialize the compass.
main
gosub getcomp 'get the compass heading.
debug dec ? heading 'Display heading.
pause 500 'wait a sec.
goto main 'loop for ever.
'
'this routine initializes the Vector 2x compass.
'
compinit
high sclk
high select 'make sure this is high.
low rst 'lower reset pin.
pause 15 'wait 15 ms.
high rst 'raise reset pin.
pause 200 'wait a bit.
return 'done.
'
'This routine gets the heading from the Vector 2x
'compass and places it in the variable heading.
'
getcomp
low select 'start the Vector 2x.
pause 200 'wait for heading.
heading = 0 'clear variable.
shiftin sdo,sclk,msbpre,[noparse][[/noparse]heading\16]
'get the data from Vector 2x.
high sclk 'reset pins.
high select
return 'done.
Dave
perhaps a lookdown and lookup is in order hear?
Headings DATA "N NEE SES SWW NW"
...
Show_Heading:
pntr = heading / 45
FOR idx = 0 TO 1
READ pntr + idx, char
DEBUG char
NEXT
RETURN
Notice that the each heading requires two characters -- hence the spaces in the DATA statement string. Of course, you can replace the DEBUG instruction with SEROUT if you're using an external display.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
evry 90% is a letter?
' {$STAMP BS2}
'
'seril lcd with v2x.bs2 Bobby Petersen http://69.86.31.158
'This routine shows how to read data from the
'Vector 2x compass.
'compass signals.
select CON 2 'select signal.
sdo CON 3 'serial data out.
sclk CON 4 'clock signal.
rst CON 5 'reset signal.
'Other.
index VAR Word
N9600 CON $4054 ' Baudmode-9600 bps inverted. Use $40F0 for BS2-SX.
I CON 254 ' Instruction prefix value.
time VAR Word
CLR CON 1 ' LCD clear-screen instruction.
L1_C7 CON 135 ' Address of line 1, character 7.
L1_c11 CON 139' , character 11.
L2_c11 CON 203
heading VAR Word 'heading variable.
HEADING2 VAR Word
SEROUT 0,n9600,[noparse][[/noparse]96,i]'set SIM to 16x2 fore the LCD
PAUSE 1
setup
OUTPUT select 'set pin direction.
OUTPUT sclk
OUTPUT rst
INPUT sdo
GOSUB compinit 'initialize the compass.
main
GOSUB getcomp 'get the compass heading.
'DEBUG DEC ? heading 'Display heading.
PAUSE 500 'wait a sec.
GOTO main 'loop for ever.
'
'this routine initializes the Vector 2x compass.
'
compinit
HIGH sclk
HIGH select 'make sure this is high.
LOW rst 'lower reset pin.
PAUSE 15 'wait 15 ms.
HIGH rst 'raise reset pin.
PAUSE 200 'wait a bit.
RETURN 'done.
'
'This routine gets the heading from the Vector 2x
'compass and places it in the variable heading.
'
getcomp
LOW select 'start the Vector 2x.
PAUSE 200 'wait for heading.
heading = 0 'clear variable.
SHIFTIN sdo,sclk,MSBPRE,[noparse][[/noparse]heading\16]'get the data from Vector 2x.
HIGH sclk 'reset pins.
HIGH select
'
' this will tack the heading and asin it leters
pntr VAR Word
idx VAR Word
Headings DATA "N E S W "
Show_Heading:
pntr = heading /90
FOR idx = 0 TO 1
READ pntr + idx, heading2
'
' This program demonstrates fundamental techniques of driving
' PVC160204AYL serial LCDs in BASIC from the BASIC Stamp II. It assumes
' that the PVC160204AYL is connected to I/O pin P0 of the Stamp, and
' that it is set for 9600 bps.
' Start by defining some useful constants for the Backpack.
'
' Now clear the screen in case there's text left from a previous
' run of the program. Note that there's a 1-second PAUSE prior to
' sending any data to the Backpack. This gives the Backpack plenty
' of time to initialize the LCD after power up.
SEROUT 0,n9600,[noparse][[/noparse]1,2] ' Clear the LCD screen.
PAUSE 1
'
RCTIME---from-mast
HIGH 7 'Discharge the cap
PAUSE 10 'for 9 ms.
RCTIME 7, 1, time
'
PrintIt Subroutine
SEROUT 0,n9600,[noparse][[/noparse]"HEADDING ",DEC heading ,223,heading2,CR,"AZIMUTH ",DEC time,223]
PAUSE 1 ' Slow the loop down.
'GOTO getcomp ' Repeat endlessly.
RETURN 'dune.
Headings DATA "NESW"
Show_Heading:
pntr = heading /90
FOR idx = 0 TO 1
READ pntr + idx, heading2
if i left it "Headings DATA "N E S W "" i wuld have to devide by 51.428571 to show the right way.
' {$STAMP BS2}
hdng··· VAR···· Word
pntr··· VAR···· Nib
idx···· VAR···· Nib
char··· VAR···· Byte
HdgStr· DATA··· "N NEE SES SWW NW"
Main:
· FOR hdng = 0 TO 359
··· DEBUG HOME, DEC3 hdng, " "
··· LOOKDOWN hdng, <=[noparse][[/noparse]22,67,112,157,202,247,292,337,360], pntr
··· pntr = pntr // 8 * 2
··· FOR idx = 0 TO 1
····· READ (HdgStr + pntr + idx), char
····· DEBUG char
··· NEXT
··· PAUSE 500
· NEXT
· GOTO Main
Run this code on its own first, just so you know it works -- then you can add the working bits into your program.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
Post Edited (Jon Williams) : 9/17/2004 4:48:46 PM GMT
thank you.
Just have to set up the GPS on pin 16 then set up the math four the maidenhead grid squares.
I still have not found a way to get the display to reed right when you first turn on the stamp, after you hit reset all works fine.
I do thank you four all your help and continuing support.
' {$STAMP BS2}
'
'seril lcd with v2x.bs2 Bobby Petersen http://69.86.31.158
'This routine shows how to read data from the
'Vector 2x compass.
'compass signals.
select CON 2 'select signal.
sdo CON 3 'serial data out.
sclk CON 4 'clock signal.
rst CON 5 'reset signal.
'Other.
GPS VAR Word
index VAR Word
N9600 CON $4054 ' Baudmode-9600 bps inverted. Use $40F0 for BS2-SX.
SEROUT 0,n9600,[noparse][[/noparse]04,"Q"]'set SIM to 16x2 fore the LCD
PAUSE 10
I CON 254 ' Instruction prefix value.
time VAR Word
CLR CON 1 ' LCD clear-screen instruction.
'L1_C0 CON 127 ' Address of line 1, character 7.
'L1_c11 CON 139' , character 11.
L2_c0 CON 192
heading VAR Word 'heading variable.
HEADING2 VAR Word
setup
OUTPUT select 'set pin direction.
OUTPUT sclk
OUTPUT rst
INPUT sdo
GOSUB compinit 'initialize the compass.
main
GOSUB getcomp 'get the compass heading.
'DEBUG DEC ? heading 'Display heading.
PAUSE 500 'wait a sec.
GOTO main 'loop for ever.
'
'this routine initializes the Vector 2x compass.
'
compinit
HIGH sclk
HIGH select 'make sure this is high.
LOW rst 'lower reset pin.
PAUSE 15 'wait 15 ms.
HIGH rst 'raise reset pin.
PAUSE 200 'wait a bit.
RETURN 'done.
'
'This routine gets the heading from the Vector 2x
'compass and places it in the variable heading.
'
getcomp
LOW select 'start the Vector 2x.
PAUSE 200 'wait for heading.
heading = 0 'clear variable.
SHIFTIN sdo,sclk,MSBPRE,[noparse][[/noparse]heading\16]'get the data from Vector 2x.
HIGH sclk 'reset pins.
HIGH select
'
'this will let you calibrate the V2X
'
' this will tack the heading and asin it leters
pntr VAR Word
idx VAR Word
Headings DATA "NESW"
Show_Heading:
pntr = heading /90
FOR idx = 0 TO 1
READ pntr + idx, heading2
'
' This program demonstrates fundamental techniques of driving
' PVC160204AYL serial LCDs in BASIC from the BASIC Stamp II. It assumes
' that the PVC160204AYL is connected to I/O pin P0 of the Stamp, and
' that it is set for 9600 bps.
' Start by defining some useful constants for the Backpack.
'
' Now clear the screen in case there's text left from a previous
' run of the program. Note that there's a 1-second PAUSE prior to
' sending any data to the Backpack. This gives the Backpack plenty
' of time to initialize the LCD after power up.
'SEROUT 0,n9600,[noparse][[/noparse]04,"q"]
SEROUT 0,n9600, ' Clear the LCD screen.
PAUSE 1
SEROUT 0,n9600,[noparse][[/noparse] 12]
PAUSE 1
'
RCTIME---from-mast
HIGH 7 'Discharge the cap
PAUSE 10 'for 9 ms.
RCTIME 7, 1, time
'
PrintIt Subroutine
SEROUT 0,n9600,[noparse][[/noparse]"HEADDING ",DEC heading ,223, heading2,10,CR,"AZIMUTH ",DEC time,223,10,CR,"GPS ",DEC5 Gps,10,CR,2]
PAUSE 1 ' Slow the loop down.
'GOTO getcomp ' Repeat endlessly.
RETURN 'dune.
I put on the kettle this morning, made some green tea, and decided to apply Parallax programming standards to your application to see if I could make it easier to work with (see the attached program -- but understand that I didn't have the parts to test it, so it may need adjusting).· You'll see that my version of your program takes advantage of PBASIC 2.5, is organized, and·uses named pins and values to make the core code easier to read.· I'd love to know if it works on your system without adjustment (that would make me smile).· Don't be concerned about the removal of some lines of code -- there are unnecessary bits of code in your program.· For example, you don't have to make a pin an output if you're going to be using HIGH and LOW with that pin, as these commands make the specified pin an output beforehand.· And you don't have to declare pins an input if they've not been previously used by other commands because the BASIC Stamp initializes everything to inputs.
We developed our programming standards for internal use and for those that write books for us that include code.· You can get a copy of that standard here:
http://www.parallax.com/dl/docs/prod/stamps/pbasicstyle.pdf
I think --·since you said you're new to the BASIC Stamp -- that you'll find the standard easy to live with and very helpful as you design projects with greater complexity.· Have fun with your projects, and do keep everyone posted on your progress.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
Post Edited (Jon Williams) : 9/19/2004 6:21:01 PM GMT
Well i'm happy to say that yes it did work! In fact I that it work't reel good four the first time on my own with out a book to guide me. "Doing all this with your homework board from radio shack open box sale”. I look’t at your code and it look's lick it should work but the heading was erratic would not stay with in 2 degrees kept reading 585,876,27,650,66, xed, xed but it did salve my power up ruteen thaw. Ill goes back over the program I did, and with yours to generate a new code.
I'm nearly dun with this project and will be submitting it to QST, NEWS "North East Weak Signal Group, and I think Servo/Nuts N volts.
With a 5-year degree in robotic and computer repair you'd think I’d know code by now, but I don’t! But i’m learning. I could build this from scratch but why reinvent the wheel when a company has dunned it four you all ready! When it's all dun I'll look up the parts individually and design my own board then put it in a kit form four other hams/robotic teak’s to build and expand on.
I think its all-most time to start ordering parts and fabricating the boards to fit inside my home made 10GHZ transverter that's mounted on my truck.
Soon ill post sum pictures on my web site hear at the house "it's good to cut out the middle man and have a host server that you don’t have to pay four on the internet".
I'm hacking at a vx2e compass right now...if you could post your final code i'll modify it if needed to make the vx2e work and post my results.· Thank you!
Regards,
Christopher
Here's what I ended up with.· For clarification, here's my pinouts..
Pin 1 on V2Xe - P15 on BS2P
Pin 2 on V2Xe - P11 on BS2P
Pin 3 on V2Xe - P12 on BS2P
Pin 4 on V2Xe - P14 on BS2P
Pin 5 on V2Xe - P13 on BS2P
Pin 6 on V2Xe - P10 on BS2P
Pin 7 on V2Xe - ground
Pin 12 on V2Xe· - 3.3v
Pin 14 on V2Xe - ground
-modified code
' {$STAMP BS2p}
'
'seril lcd with v2x.bs2 Bobby Petersen http://69.86.31.158
'This routine shows how to read data from the
'Vector 2x compass.
'compass signals.
select··· CON 14······························ 'select signal.
sdo······ CON 11································· 'serial data out.
sclk····· CON 15································ 'clock signal.
rst······ CON 10································· 'reset signal.
GPS······ VAR Word
index···· VAR Word
PAUSE 10
time····· VAR Word
heading·· VAR Word··························· 'heading variable.
HEADING2· VAR Word
setup
OUTPUT select······························ 'set pin direction.
OUTPUT sclk
OUTPUT rst
INPUT sdo
GOSUB compinit····························· 'initialize the compass.
main
GOSUB getcomp······························ 'get the compass heading.
DEBUG DEC ? heading························ 'Display heading.
DEBUG DEC ? HEADING2
PAUSE 500·································· 'wait a sec.
GOTO main·································· 'loop for ever.
'
'this routine initializes the Vector 2x compass.
'
compinit
HIGH sclk
HIGH select································ 'make sure this is high.
LOW rst···································· 'lower reset pin.
PAUSE 15··································· 'wait 15 ms.
HIGH rst··································· 'raise reset pin.
PAUSE 200·································· 'wait a bit.
RETURN····································· 'done.
'
'This routine gets the heading from the Vector 2x
'compass and places it in the variable heading.
'
getcomp
LOW select································· 'start the Vector 2x.
PAUSE 200·································· 'wait for heading.
heading = 0································ 'clear variable.
SHIFTIN sdo,sclk,MSBPRE,[noparse][[/noparse]heading\16]······· 'get the data from Vector 2x.
HIGH sclk·································· 'reset pins.
HIGH select
'
'this will let you calibrate the V2X
'
' this will tack the heading and asin it leters
pntr VAR Word
idx VAR Word
Headings DATA "NESW"
Show_Heading:
pntr = heading /90
FOR idx = 0 TO 1
READ pntr + idx, heading2
PAUSE 1···································· 'Slow the loop down.
RETURN····································· 'dune.
//
// MiniRover7_100.c
//
// Joe Miller
//
// DATE: 12/15/03
//
// Commands are received through the hardware UART at 9600bps.
// This program Controls 2 R/C Servos using the PCA timer to generate 2 pulse
// widths for standard RC type servos. Six drive motors are controlled in two
// sets via dual H-bridge drivers.
// SYSCLK frequency is 3.0625 MHz using 24.5MHz internal Osc divided by 8
//
// Commands:
// ·'c'·Begin calibrate. 's' stops calibration. use 'r' or 'l' to rotate robot
// ·'s'·Stop. stops robot motors and calibration process. Also sets bearing.
// ·'r'·Rotate right (CW)
//·'l' Rotate left (CCW)
//·'t' stores present heading as a bearing point and move forward while tracking bearing point
//·'f' move forward
//·'h' select heading information as upload data
//·'b' select raw sensor info as upload data
//·'m' select heading and magnitude as upload data
//·'o' upload data off
//·'=' incriment bearing point by 5 degrees
//·'-' decrement bearing point by 5 degrees
//
//
// Target: C8051F330
// Tool chain: KEIL C51 6.03 / KEIL EVAL C51
//
//
//
// Includes
//
#include <c8051f330.h>················ // SFR declarations.
//
// 16-bit SFR Definitions for 'F330
//
sfr16 DP······ = 0x82;················ // data pointer
sfr16 TMR2RL·· = 0xca;················ // Timer2 reload value
sfr16 TMR2···· = 0xcc;················ // Timer2 counter
sfr16 PCA0CP0· = 0xfb;················ // PCA0 Module 0 Capture/Compare
sfr16 PCA0CP1· = 0xe9;················ // PCA0 Module 1 Capture/Compare
sfr16 PCA0···· = 0xf9;················ // PCA0 counter
//
// Global CONSTANTS
//
#define SYSCLK······ 3062500········· ·// SYSCLK frequency in Hz
#define BAUDRATE···· 9600············· ·// Baud rate of UART in bps
#define RTI_RATE· 10·····// RTI rate in milliseconds
#define RTI_BASE· 0xffff - (SYSCLK * RTI_RATE / 1000 )·// Calculate timer2 reload value
#define TIMING_OFFSET 0xffff - 766··// default timing window setting
··········// .25mS/.32653uS· = 766
··········// .25mS is good for Hitech servos
#define GET_MODID·0x00····// Compass Command to get module ID
#define BEG_SAMPLE_COMPASS 0x20···// Compass Command to begin sample
#define GET_XSEN·0x22····// Compass Command to get X sensor raw data
#define GET_YSEN·0x23····// Compass Command to get y sensor raw data
#define GET_HDG··0x2D····// Compass Command to get Heading
#define SVO_L_FWD·0xffff - 0x1680··// Left steering servo position for forward movement
#define SVO_R_FWD·0xffff - 0xE00··// Right steering servo position for forward movement
#define SVO_L_TURN ·0xffff - 0x900··// Left steering servo position for turning
#define SVO_R_TURN ·0xffff - 0x1B80··// Right steering servo position for turning
#define MAX_COMP·0x480····// Maximum steering allowed during tracking mode
#define MOTOR_CW_TURN··0x4a··// Traction motor H-Bridge driver value for clockwise turn
#define MOTOR_CCW_TURN··0x54··// ··...for CCW turn
#define MOTOR_FWD···0x58··//·· ...for forward movement
#define MOTOR_STOP···0xe1··//··· ...to stop the motors
#define TRACKING_RATE·5····// Real time update rate
#define HEADING_RATE·100····// Rate at which heading info is sent to host
#define·HEADING··1·····// Heading mode for upload data
#define RAW···2·····// Raw sensor mode
#define MAG···3·····// magnitude mode
········//postion tracking control loop constants
#define DERIV··100·····// Derivitive gain·
#define PROP··250·····// Proportional gain
#define INTG··10·····// Integral gain
#define SyncChar·0xAA····// V2Xe packet start byte
#define DataReady·0xAB····// V2Xe Data Ready character
#define CommandOK·0xAE····// V2Xe command recieved and understood
#define CMD_startcal·0x0a···// v2Xe start calibration process command
#define CMD_stopcal··0x0b···// v2Xe stop calibration process command
//
// Defdef and Function PROTOTYPES
//
typedef unsigned char Uint8;···// 8 bit unsigned integer
typedef unsigned int Uint16;···// 16 bit unsigned integer
typedef signed int Sint16;····// 16 bit signed integer
void RTI_ISR (void);
void UART_ISR (void);
void proc_cmd(void);
void send_char(Uint8);
void init_peripherals (void);
void get_mag(void);
void get_compass(void);
void V2Xe_send(Uint8);
void V2Xe_read(Uint8);
void init_V2Xe(void);
void update_host(void);
void send16_host(Sint16);
void track(void);
//
// Global VARIABLES
//
sbit·sync·=·0x95;··// Output to compass's Sync input
sbit·drdy·=·0x96;··// Input for compass's Data Ready output
sbit·ssnot·=·0x83;··// Output to compass's Slave Select(Not) input
sbit·nCTS·=·0x90;··// Input from RF Modem's Clear To Send(Not) output
sbit·sclk·= ·0x80;··// Output to compass's SPI serial data input
sbit·mosi·=·0x82;··// Output to compass's· serial data output
sbit·nsvo_pwr·=·0x97;·// Output. When low, power is enabled to the steering servos
Uint16· bearing;··// Current Compass bearing to track
Uint16·heading;··// Current Compass heading
Sint16·error;···// Current heading error
Sint16· error_last;··// Previous heading error
Sint16· error_intg;··// Integral heading error
Sint16· comp;···// heading error compensation
Sint16 ·Xraw, Yraw;··// Raw sensor data
Uint16 work, value;··// General Purpose reuable 16-bit unsigned variables
Uint8·· field_mag;··// Magnetic field magnetude
Uint8·sbuff;···// temporary storage of serial receiver data
Uint8·V2Xe_ret[noparse][[/noparse]24];·// Return data from V2Xe compass is placed in here
Uint8·V2Xe_out[noparse][[/noparse]24]; ·// Data to be sent to V2Xe compess
Uint8·exponent;··// Exponent value from the compass heading floating point number
Uint8·timer;···// reading capture interval timer
Uint8·tracking_update;·// Count down vlaue to next direction tracking update
Uint8·heading_update;·// time left until next heading update
Uint8·data_package;·// use with HEADING, RAW, MAG constants to set data upload format
bit·B_got_sdata;··// have some serial data to put away
bit·B_proc_cmd;···// entire command string received. Ready to process command
bit B_3byte_cmd;··// if set then current command is 3 bytes wide
bit B_OnOff;···// servo pulse train on/off control
bit B_MPcal;···// MPcal is running if this bit is set
bit B_tic_cycle;··// periodic update flag set by RTI tic count underflow
bit B_tracking;···// track heading mode
bit B_calibrating;··// calibration mode
bit B_Txstat;···// UART is busy sending current character
Uint8·i,j,k;···// general purpose 8-bit unsigned variables
//
// MAIN Routine
//
//
//
void main (void)
{
·· ····//Set initial values and conditions
··········// Disable Watchdog timer
·· ·PCA0MD &= ~0x40;·················· ·// WDTE = 0 (clear watchdog timer enable)
··· P0 &= ~1;··························
·· ·init_peripherals ();··············· // initialize peripherals
·ssnot = 1;·······// default pin states
·sync = 0;
··EA = 1;···························· // Enable global interrupts
·· ·B_OnOff = 0;······// servo default is Off
··
·timer=25; while(timer);····// small delay for power supply settling
·
·B_proc_cmd = 1;······// stimulate a stop command to be performed
·sbuff = 's';
·nsvo_pwr ·= 1;·····// Turn off steering servo power
·heading_update = HEADING_RATE;··// init heading update timer accumulator
·init_V2Xe();······// init and send V2Xe default data components
·get_compass();······// initialize heading
//
//·· ······· M A I N·· L O O P
// ··This is an interrupt stimulated process. Interrupts· will generate
//···the flags that the main loop will test and act apon.
//
·while (1) ·{
···// if a command has been recieved
··if (B_proc_cmd)
···proc_cmd();
···// check if it is time to update the direction of the rover
···//··· (to track the previously set bearing)
··if (!tracking_update && B_tracking) {
···track();
···tracking_update = TRACKING_RATE;
···}
···// check if it is time to get another heading from the compass
··if (heading_update == 0· && data_package != 0) {
···if (!B_tracking) get_compass();
···update_host();
···heading_update = HEADING_RATE;
···}
·} //end of while(1)
} // end of main()
//
// Process Command sent from host
//
void proc_cmd (void)
{
·B_proc_cmd = 0;
·switch (sbuff)
·{
·case 'c':··// Start calibration
··V2Xe_out[noparse][[/noparse]0] = CMD_startcal ; V2Xe_out[noparse][[/noparse]1] = 0;
··V2Xe_send(3);
··B_calibrating = 1;···// Calibrating flag
··break;
·case 'r':··// Rotate Right
··nsvo_pwr ·= 0;···// Turn on steering servo power
··PCA0CP0··= SVO_L_TURN;·// Set wheel to turn
··PCA0CP1··= SVO_R_TURN;·// same for left
··PCA0CPM0·=·0xC2;··//
··PCA0CPM1·=·0xC2;··//
··timer = 25; while(timer);···// delay 1/2 second to allow wheel to come around
··timer = 25; while(timer);
··P1 = P1 & ~0x1e | MOTOR_CW_TURN;·// Set motors to CCW rotation pattern
··break;
·case 'l':··// Rotate Left
··nsvo_pwr ·= 0;···// Turn on steering servo power
··PCA0CP0··= SVO_L_TURN;·// Set wheel to turn
··PCA0CP1··= SVO_R_TURN;·// same for left
··PCA0CPM0·=·0xC2;··//
··PCA0CPM1·=·0xC2;··//
··timer = 25; while(timer);···// delay 1/2 second to allow wheel to come around
··timer = 25; while(timer);
··P1 = P1 & ~0x1e | MOTOR_CCW_TURN;·// Set motors to CCW rotation pattern
··break;
·case 't':··// Move Foward and stay on bearing "TRACK"
··nsvo_pwr ·= 0;···// Turn on steering servo power
··PCA0CP0··= SVO_L_FWD;·// Set wheel to middle (striaght)
··PCA0CP1··= SVO_R_FWD;·// same for left
··PCA0CPM0·=·0xC2;··//
··PCA0CPM1·=·0xC2;··//
··timer = 25; while(timer);·// delay 1/2 second to allow wheel to come around
··timer = 25; while(timer);
··P1 = P1 & ~0x1e | MOTOR_FWD;·// Set motors go straight
··get_compass();·····// set bearing
···bearing = heading;
··B_tracking = 1;
··break;
·case 'f':··// Move Foward
··nsvo_pwr ·= 0;···// Turn on steering servo power
··PCA0CP0··= SVO_L_FWD;·// Set wheel to middle (striaght)
··PCA0CP1··= SVO_R_FWD;·// same for left
··PCA0CPM0·=·0xC2;··//
··PCA0CPM1·=·0xC2;··//
··timer = 25; while(timer);·// delay 1/2 second to allow wheel to come around
··timer = 25; while(timer);
··P1 = P1 & ~0x1e | MOTOR_FWD;·// Set motors go straight
··break;
·case 's':··// Stop
··P1 &= MOTOR_STOP;··// Turn motors off
··nsvo_pwr = 1;···// Turn off power to servos to save energy
··if (B_calibrating)··// stop caibration cycle if it was running
···{
···B_calibrating = 0;
···V2Xe_out[noparse][[/noparse]0] = CMD_stopcal ; V2Xe_out[noparse][[/noparse]1] = 0;·// stop cal
···V2Xe_send(3);
···V2Xe_out[noparse][[/noparse]0] = 9 ; V2Xe_out[noparse][[/noparse]1] = 0; ····// save config
···V2Xe_send(3);
···}
//··B_collect = 0;
··B_tracking = 0;···// discontinue tracking mode
··break;
·case 'p':··// Ping·
··send_char('p');
//··while (1);···// set trap for debugging
··break;
·case 'h':··// set heading data package to send to host·
··data_package = HEADING;
··break;
·case 'b':··// set raw sensor data to send to host·
··data_package = RAW;
··break;
·case 'm':··// set heading and magnitude to send to host·
··data_package = MAG;
··break;
·case 'o':··// turn off data package to host·
··data_package = 0;
··break;
·case '-':··// turn off data package to host·
··if (bearing < 5 ) bearing += 360;
··bearing -= 5;
··break;
·case '=':··// turn off data package to host·
··if ((bearing += 5) > 360 ) bearing -= 360;
··break;
·default:
··break;
·} // end of switch case
} // end of proc_cmd()
//
// Get Heading, raw sensor data and magnetude from V2Xe
//
void get_compass(void)
{
·sync = 1;·····// reset V2Xe SPI port and buffers
·timer = 2; while(timer);·// wait at least 10mS
·sync = 0;
·V2Xe_out[noparse][[/noparse]0] = 0x04;···// fill output buffer with 'GetData' command
·V2Xe_out[noparse][[/noparse]1] = 0;
·V2Xe_send(3);····// send request
·V2Xe_read(22);····//·get response
// IEEE 32 bit floating point to scaled integer convertion routine
//·Integer will be scaled * 8 (i.e. 1 = 1/8, 2 = 1/4, 3 = 3/8, 4 = 1/2 ... 8 = 1.0 )
//
//·IEEE32 format:
//
//·33............24...23............16...15.............8...7..............0
//·S E E E· E E E E·· E F F F· F F F F·· F F F F· F F F F·· F F F F· F F F F··
//··· V2Xe_ret[noparse][[/noparse]2]······· V2Xe_ret[noparse][[/noparse]3]······· V2Xe_ret[noparse][[/noparse]4]······· V2Xe_ret[noparse][[/noparse]5]· for compass heading
//··· V2Xe_ret[noparse][[/noparse]7]······· V2Xe_ret[noparse][[/noparse]8]······· V2Xe_ret[noparse][[/noparse]9]······· V2Xe_ret[noparse][[/noparse]10]· for magnetude
//
//
// ·S = Sign
//·E = Exponent
//·F = Mantisa or Binary Fraction
//
//·Number = 1.F * 2^(E-127)
//
//·The code below will assume Sign = 0 (number is positive) for all headings
//· all numbers are truncated to integers (not rounded)
····// if heading is less than 1 degree or zero (the IEEE32 version of zero)
·if ((V2Xe_ret[noparse][[/noparse]2] == 63 & V2Xe_ret[noparse][[/noparse]3] < 144) || V2Xe_ret[noparse][[/noparse]2] == 0 )·
··heading = 0;
·else
··{
··exponent = V2Xe_ret[noparse][[/noparse]2] * 2 + ((V2Xe_ret[noparse][[/noparse]3] > 127)? 1:0);·// move exponent into one byte
··heading = 1;·····// restore hidden 1.0 from normalized mantissa
··// this loop shifts fractional portion into integer heading until exponent reaches 2^(0) exponent
··while (exponent-- > 127)··// 127 is 2^0 (for unit values)
···{
···heading *= 2;····// shift data left into heading
···V2Xe_ret[noparse][[/noparse]3] *= 2;
···heading += ((V2Xe_ret[noparse][[/noparse]3] > 127)? 1: 0);
···V2Xe_ret[noparse][[/noparse]3] += ((V2Xe_ret[noparse][[/noparse]4] > 127)? 1 :0);
···V2Xe_ret[noparse][[/noparse]4] *= 2;
···}
··}
·// convert magnatude from floating point to Uint8 and scale to 1/128th units
·// were magnatude = 128 for a field magnatude of 1.000
·// ·····255 ······ 2.000·· magnetude
·//·····1······· 1/128th
·//·····0·······<1/128th
·// no need to test for absolute zero because it is not possible for magnatude
·// maximum number of bits shifted for this one is only 8 bits because magnatude is Uint8
·exponent = V2Xe_ret[noparse][[/noparse]7] * 2 + ((V2Xe_ret[noparse][[/noparse]8] > 127)? 1:0);·// move exponent into one byte
·field_mag = 1;·····// restore hidden 1.0 from normalized mantissa
·while (exponent-- > 120)··// 120 = (2^-7)binary which are 1/128th unit
··{
··field_mag *= 2;········// shift data left into magnatude variable
··V2Xe_ret[noparse][[/noparse]8] *= 2 ;·······// shift mantisa to read MSB
··field_mag += ((V2Xe_ret[noparse][[/noparse]8] > 127)? 1: 0);·// until exponent reaches 2^(-7) exponent
··}
·// rebuild Raw X and Y sensors data from Sint32 packed in a two-byte array into a single Sint16 value
·if (V2Xe_ret[noparse][[/noparse]14] > 127)
··Xraw = -1*((Sint16)(255 - V2Xe_ret[noparse][[/noparse]14]) * 256 + (255 - V2Xe_ret[noparse][[/noparse]15]) + 1) ;
·else
··Xraw = (Uint16)V2Xe_ret[noparse][[/noparse]14] * 256 + V2Xe_ret[noparse][[/noparse]15];
·if (V2Xe_ret[noparse][[/noparse]19] > 127)
··Yraw = -1*((Sint16)(255 - V2Xe_ret[noparse][[/noparse]19]) * 256 + (255 - V2Xe_ret[noparse][[/noparse]20]) + 1);
·else
··Yraw = (Sint16)V2Xe_ret[noparse][[/noparse]19] * 256 + V2Xe_ret[noparse][[/noparse]20];
}
//
// Send data to V2Xe
//
//·len = # of bytes to send including sync, command and null
//
void V2Xe_send(Uint8 len)
{
·ssnot = 0;
·SPIF = 0;······// reset SPI flag
·SPI0DAT = SyncChar;····// Send header command
·while (SPIF == 0);····// Wait until SPI finishes transfer
·i = 0; len--;
·do········// send string will null terminator
··{
··SPIF = 0x00;····// reset SPI flag
··SPI0DAT = V2Xe_out[noparse][[/noparse]i++];·// Send intended command character
··while (SPIF == 0);···// Wait until SPI finishes transfer
··}
·while (i < len);
·ssnot = 1;
}
//
// Read data from V2Xe
//
//·len = # of return bytes including sync, command and null
//
void V2Xe_read(Uint8 len)
{·········// Wait for return data header start character
·if (len < 3) len = 3;···// minimum len
·ssnot = 0;
·········// which starts with a SyncChar
·do
··{
··SPIF = 0;·····// reset SPI flag
··SPI0DAT = 0x00;····// Send dummy bits
··while (SPIF == 0);···// Wait until SPI finishes transfer
··}
·while (SPI0DAT != SyncChar);·// look for sync character
·i = 0;
·while (--len)·····// get all data
··{
··SPIF = 0;·····// reset SPI flag
··SPI0DAT = 0x00;····// Send dummy bits
··while (SPIF == 0);···// Wait until SPI finishes transfer
··V2Xe_ret[noparse][[/noparse]i++] = SPI0DAT;·// place V2Xe data into receive buffer
··}
·ssnot = 1;
}
//
// This routine checks the Rover's heading and compares this to the previously
//·· set bearing and adjusts the steering to compensate for the difference
//·· a PID filter is utilized to increase response time
//
void track(void)
·{
·B_tic_cycle = 0;·····// reset Real Time clock periodic timer flag
·error_last = error;·····// save previous heading
·get_compass();······// get present compass heading
·error = (Sint16)(heading - bearing); // Calculate· Error
·if (error < -180) error += 360;·// adjust error for wrapping around the 360 degree point
·if (error >· 180) error -= 360;
·if (error_intg < MAX_COMP/INTG && error > 0)··// Calc integral of error
··error_intg += error;····//·· and limit final intgral output to +- 1000 counts
·if (error_intg > -MAX_COMP/INTG && error < 0)
··error_intg += error;
·// compute magnitude of steering correction required(compensation)
·comp = PROP * error + DERIV * (error_last - error) + INTG * error_intg;
·if (comp > MAX_COMP) ·comp = MAX_COMP;··// limit amount of compensation to MAX_COMP
·if (comp < -(MAX_COMP)) comp = -(MAX_COMP);
·PCA0CP0 = SVO_L_FWD + comp;······// Update new servo positions by setting the
·PCA0CP1 = SVO_R_FWD + comp;······//· PCA timer trippoint value
·PCA0CPM0·=·0xC2;·······//···· The PCA is in PWM mode
·PCA0CPM1·=·0xC2;·······//
·}
//
// Initialize V2Xe
//
void init_V2Xe(void)
·{
·ssnot = 0;······// select the V2Xe compass
·sync = 1;······// Reset the compass's SPI command/data stream
·timer = 2; while(timer);··// wait 10-20 ms
·sync = 0;
·timer = 2; while(timer);··// wait 10-20 ms
·V2Xe_out[noparse][[/noparse]0] = 0x03;··// command =· Set data components
·V2Xe_out[noparse][[/noparse]1] = 4;··// number of parameters
·V2Xe_out[noparse][[/noparse]2] = 5;··// request Heading data
·V2Xe_out[noparse][[/noparse]3] = 6;··// magnetude
·V2Xe_out[noparse][[/noparse]4] = 1;··// Raw X
·V2Xe_out[noparse][[/noparse]5] = 2;··// Raw Y
·V2Xe_out[noparse][[/noparse]6] = 0;··// end of string
·V2Xe_send(8);···// send request
·V2Xe_out[noparse][[/noparse]0] = 1;··// command = get module info
·V2Xe_out[noparse][[/noparse]1] = 0;··// end of string
·sync = 1;
·timer = 2; while(timer);··// wait 10-20 ms
·sync = 0;
·timer = 2; while(timer);··// wait 10-20 ms
·V2Xe_send(3);···// send request
·V2Xe_read(9);···// get response
·send_char(V2Xe_ret[noparse][[/noparse]5]);·// send module ID to host as indicator that robot is ready
·send_char(V2Xe_ret[noparse][[/noparse]6]);
·send_char(V2Xe_ret[noparse][[/noparse]7]);
·send_char(V2Xe_ret[noparse][[/noparse]8]);
·send_char(0x0d);···// CR
·send_char(0x0a);···// LF
·ssnot = 1;
·}
//
// Send data to host via RF modem
//
void send_char(Uint8 tx_char)
·{
·while(B_Txstat);····// wait for complete transmission
·while(nCTS);·····// do not send data to radio if its buffer is full
·TI0 = 0;
·SBUF0 = tx_char;
·B_Txstat = 1;·····// set transmission in progress flag
·}
//
// Send data package to host
//
void update_host(void)·{
·if (B_tracking) {··········// If in tracking mode
··send_char('B');
··send16_host((Sint16)bearing);·······// send present bearing value
··send_char(',');
··send_char('C');
··send16_host(comp);··········// and· present compensation value
··}
·if (data_package == HEADING || data_package == MAG) {·// If Heading or Magnutude values are requested
··if (B_tracking)
···send_char(',');
··send_char('H');···········// send Heading information
··send16_host((Sint16)heading);
··}
·if (data_package == MAG) {········// If Magnitude value is requested
··send_char(',');
··send_char('M');
··send16_host((Sint16)field_mag);·······// send magnitude
··}
·if (data_package == RAW) {········// If V2Xe raw magnetometer values are requested
··send16_host(Xraw);
··send_char(',');
··send16_host(Yraw);··········// send raw mag values
··}
·send_char(0x0d); ···// CR
·send_char(0x0a); ···// LF
·}
//
// Convert Sint16 value to ASCII and send to host
//
void send16_host(Sint16 dataV) {
·if (dataV < 0 ) {
··dataV *= -1;
··send_char('-');
··}
·work = dataV;
·if (work > 9999) {
··value = dataV / 10000;
··dataV = dataV % 10000;
··send_char(value + 0x30);
··}
·if (work > 999) {
··value = dataV / 1000;
··if(value) dataV = dataV % 1000;
··send_char(value + 0x30);
··}
·if (work > 99) {
··value = dataV / 100;
··if(value) dataV = dataV % 100;
··send_char(value + 0x30);
··}
·if (work > 9)·{
··value = dataV / 10;
··if(value) dataV = dataV % 10;
··send_char(value + 0x30);
··}
·send_char(dataV + 0x30);
}
//
// Initialization Routine
// This Code was generated using the Cygnal Configuration Wizard
// I added some comments and minor alterartions
//
void init_peripherals (void)
{
//
// Initialize Port and Crossbar settings
//
··· //P0.0· -· SCK· (SPI0), Push-Pull,· Digital··To communicate with V2Xe Compass
··· //P0.1· -· MISO (SPI0), Open-Drain, Digital
··· //P0.2· -· MOSI (SPI0), Push-Pull,· Digital
··· //P0.3· -· Unused,···· Push-Pull,· Digital
··· //P0.4· -· TX0 (UART0), Push-Pull,· Digital
··· //P0.5· -· RX0 (UART0), Open-Drain, Digital
··· //P0.6· -· CEX0 (PCA),· Push-Pull,· Digital··Right Servo pulse output
··· //P0.7· -· CEX1 (PCA),· Push-Pull,· Digital··Left Servo pulse output
··· //P1.0· -· nCTS,· Push-Pull,· Digital···RF Modem control signal
··· //P1.1· -· A1,· Push-Pull,· Digital····H-Bridge Channel A bit 1
··· //P1.2· -· A2,· Push-Pull,· Digital····H-Bridge Channel A bit 2
··· //P1.3· -· B1,· Push-Pull,· Digital····H-Bridge Channel B bit 1
··· //P1.4· -· B2,· Push-Pull,· Digital····H-Bridge Channel B bit 2
··· //P1.5· -· SYNC,· Push-Pull,· Digital···To reset Compass
··· //P1.6· -· DRDY,· Open-Drain, Digital···From Compass Data Ready Pin
··· //P1.7· -· SVO_PWR,· Push-Pull,· Digital··Servo Power Enable
··· P0MDOUT···· = 0xDD;··// 1=push-pull, 0=Open drain
·P0···= ~0xDD;·// Open port bits for inputs and initialize outputs
··· P1MDOUT···· = 0xBF;··// same
·P1···= ~0xBF;
··· P0SKIP····· = 0x08;··// Skip around P0.3 to save for future function
··· XBR0······· = 0x03;··// bring SPI and UART functions routed to pins
··· XBR1······· = 0x42;··// bring CEX0,1 routed and enable Xbar
//
// Initialize Timer settings
//
··· TCON······· = 0x40;··// Timer1 enabled. For Baud Rate generation
··· TMOD······· = 0x20;··// Timer1 Mode2, 8-bit auto reload
··· CKCON······ = 0x18;··// Timer1 and Timer2 to use SYSCLK with no prescaling,
··· TH1········ = 0x61;··// Timer1 reload value for 9600 Baud
··· TMR2CN····· = 0x04;··// Timer2 enabled and configured for 16-bit auto reload
··· TMR2RL····· = RTI_BASE;·// Timer2 reload value for 10mS RTI
·· ·TMR2··· ·= 0x0;··// set timer to beginning to allow time
·······//·for balance of init routines to execute
··
//
// Initialize PCA settings
//
··· PCA0CN····· = 0x40;··// PCA counter enabled
·PCA0MD····· = 0x08;··// PCA to use SYSCLK , WDT disababled
//·PCA0MD····· = 0x09;··// PCA to use SYSCLK , WDT disababled interrupt enabled
·······//· this makes the PCA freq 46Hz (21mS)
·PCA0CP0··= SVO_L_FWD;·// Set servo to middle of stoke (striaght)
·PCA0CP1··= SVO_R_FWD;·// same for left
··· PCA0CPM0··· = 0xC2;··// PCA channel 0 set to 16bit PWM
··· PCA0CPM1··· = 0xC2;··// PCA channel 1 set to 16bit PWM
//
// Initialize UART settings
//
··· SCON0······ = 0x50;··// UART receiver enabled
//
// Initialize SPI port
//
··· SPI0CFG···· = 0x47;··// SPI set to Master Mode, CPHA=0, CPOL=0
··· SPI0CN····· = 0x03;··// SPI enabled
··· SPI0CKR···· = 0xff;··// SPI clock rate set to ~102KHz
//
// Initialize Interrupt settings
//
··· IE········· = 0x30;··// Enable UART and Timer2 (RTI) interrupts
·EIE1······· = 0x10;··// pca interrupt
}
//
//
// Interrupt Service Routines
//
//
//
// RTI_ISR
//
//
// Timer2 is the RTI generator
//
void RTI_ISR (void) interrupt 5 using 1
{
··
·if (timer) --timer;
·if (tracking_update) --tracking_update;
·if (heading_update) --heading_update;
·TF2H = 0;·······// clear interrupt flag
}
//
// UART ISR
//
//
// Here we quickly get the data from the hardware buffer and put it into a storage
// for retrieval by the command processor.
//
void UART_ISR (void) interrupt 4 using 3
{
if (TI0) {
·TI0 = 0;
·B_Txstat = 0;
·}
else {
·sbuff = SBUF0;·······// Transfer data from hardware buffer to software buffer
··B_proc_cmd = 1;
·RI0 = 0;········// Reset Rx interrupt flag
·}
}
····I'll post·my final code Four the V2X. I'm not shure·if it will work on·the V2Xe do to the only two listed in my book are the V2X & V2XG. The V2XG uses mechanical gimbals to minimize the effect of tiltin the compass applications.
After looking over the info four your compass I can see one of your problems
The pins are different this is the link four the V2X book http://www.jameco.com/wcsstore/Jameco/Products/ProdDS/126703.PDF
The setup I use dus not look the same four yours. When I get back from Alabama ill look over it sum moor and try to help the best I can.
·
'
[noparse][[/noparse] I/O Definitions ]
Lcd············ PIN···· 0······················ ' serial comm to LCD
Slct··········· PIN···· 2······················ ' select signal
SDO············ PIN···· 3······················ ' serial data out
SClk··········· PIN···· 4······················ ' clock signal
Rst············ PIN···· 5······················ ' reset signal
'
[noparse][[/noparse] Constants ]
' SIM command bytes
CtrlA·········· CON···· $01···················· ' Clear command
CtrlB·········· CON···· $02···················· ' blank cursor
CtrlD·········· CON···· $04···················· ' configuration prefix
CtrlI·········· CON···· $09···················· ' forward
CtrlJ·········· CON···· $0A···················· ' linefeed
CtrlK·········· CON···· $0B···················· ' vertical tab
CtrlL·········· CON···· $0C···················· ' clear
CtrlM·········· CON···· $0D···················· ' CR
CtrlN·········· CON···· $0E···················· ' blank cursor
CtrlO·········· CON···· $0F···················· ' block cursor
CtrlP·········· CON···· $10···················· ' home
CtrlQ·········· CON···· $11···················· ' underline
CtrlR·········· CON···· $12···················· ' underline blink
CtrlS·········· CON···· $13···················· ' blank (does not clear)
CtrlT·········· CON···· $14···················· ' set descenders
CtrlU·········· CON···· $15···················· ' clear descenders
CtrlX·········· CON···· $18···················· ' set single line
DataEn········· CON···· $FE···················· ' next byte is data
CtrlEn········· CON···· $FF···················· ' next byte is control
#SELECT $STAMP
· #CASE BS2, BS2E, BS2PE
··· T1200······ CON···· 813
··· T2400······ CON···· 396
··· T4800······ CON···· 188
··· T9600······ CON···· 84
··· T19K2······ CON···· 32
··· T38K4······ CON···· 6
· #CASE BS2SX, BS2P
··· T1200······ CON···· 2063
··· T2400······ CON···· 1021
··· T4800······ CON···· 500
··· T9600······ CON···· 240
··· T19K2······ CON···· 110
··· T38K4······ CON···· 45
#ENDSELECT
Inverted······· CON···· $4000
Open··········· CON···· $8000
LcdBaud········ CON···· T9600 + Inverted
'
[noparse][[/noparse] Variables ]
gps············ VAR···· Word
idx············ VAR···· Word
time··········· VAR···· Word
hdng··········· VAR···· Word
hdng2·········· VAR···· Word
pntr··········· VAR···· Nib
char··········· VAR···· Byte
'
[noparse][[/noparse] EEPROM Data ]
HdgStr· DATA··· "N NEE SES SWW NW"
'
[noparse][[/noparse] Initialization ]
Reset:
· PAUSE 100···························· ' let LCD intialize
· SEROUT Lcd, LcdBaud, [noparse][[/noparse]CtrlD, "Q"]···· ' set SIM for 20x4 display
· PAUSE 10····························· ' let LCD clear
Compass_Init:
· HIGH SClk
· HIGH Slct····· 'make sure this is high.
· LOW Rst······· 'lower reset pin.
· PAUSE 15······ 'wait 15 ms.
· HIGH Rst······ 'raise reset pin.
· PAUSE 200····· 'wait a bit.
'
[noparse][[/noparse] Program Code ]
Main:
· DO
··· GOSUB Get_Compass
··· GOSUB Show_Compass
··· GOSUB show_heading2
··· GOSUB Show_Azimuth
··· PAUSE 500
· LOOP
· END
'
[noparse][[/noparse] Subroutines ]
Get_Compass:
· LOW Slct····························· ' start compass
· PAUSE 200···························· ' wait for heading
· hdng = 0····························· ' clear old value
· SHIFTIN SDO, SClk, MSBPRE, [noparse][[/noparse]hdng\16]· ' get new value
· HIGH sclk···························· 'reset pins.
· HIGH Slct
· RETURN
Show_Compass:
· SEROUT Lcd, LcdBaud, [noparse][[/noparse]CtrlP]········· ' home
· PAUSE 5
· SEROUT Lcd, LcdBaud, [noparse][[/noparse]"Heading: ", DEC3 hdng, 223]
· RETURN
Show_Heading2:
· LOOKDOWN hdng, <=[noparse][[/noparse]22,67,112,157,202,247,292,337,360], pntr
· pntr = pntr // 8 * 2
· FOR idx = 0 TO 1
··· READ (HdgStr + pntr + idx), char
··· SEROUT Lcd, LcdBaud, [noparse][[/noparse]char]
· NEXT
· 'RETURN
·
'{$Stamp BS2p}··········· 'Stamp Directive (specifies BS2P)
'*********************************************************************
'·· OEM Example Program for the Basic Stamp 2p uProcessor to
'·· interface with a PNI Corporation V2Xe 2 Axis Compass Module.
'·· This program shows how to get the Heading from the V2Xe.
'·· Copyright 20Dec2003, PNI Corporation
'·· Written by Julian Benton, Applications Engineer
'·· Contact: jbenton@pnicorp.com , 707-566-2260 x356
'*********************************************************************
'Declare Lines
SYNC····· CON 5··········· 'SYNC is pin 5
SSNOT····· CON 7··········· 'SSNOT is pin 7
MOSI····· CON 8··········· 'MOSI is pin 8
MISO····· CON 9··········· 'MISO is pin 9
SCLK····· CON 10········ 'SCLK is pin 10
'Declare Global Constants for Command Datagrams
Flag··········· CON·· $AA····· 'Defines Sync Flag which is the first byte of any command
GetModInfo········ CON·· $1····· 'Defines the frame type that queries the V2Xe for module information
ModInfoResp····· CON·· $2····· 'Defines the response frame type to command GetModInfo
SetDataComponents·· CON·· $3····· 'Defines the frame type for the data components to be sent back
·· XRaw········ CON·· $1····· 'Defines the Component ID for X sensor RAW Data
·· YRaw········ CON·· $2····· 'Defines the Component ID for Y sensor RAW Data
·· Heading····· CON·· $5····· 'Defines the Component ID for Heading
·· Mag········ CON·· $6····· 'Defines the Component ID for Magnetude
·· Dist········ CON·· $8····· 'Defines the Component ID for the distortion flag
·· CalStatus····· CON·· $9····· 'Defines the Component ID for calibration status
SaveConfig········ CON·· $9····· 'Defines the SaveConfig frame type
GetData········ CON·· $4····· 'Defines the frame type which queries the V2Xe for data components
DataResp········ CON·· $5····· 'Defines the response to command GetData frame type
Term··········· CON·· $00····· 'Defines Terminator which is the last byte of any command
'Declare Variables for Response Datagrams
FlagIn········ VAR·· Byte····· 'Defines Sync Flag in the response Datagram
FrameIn········ VAR·· Byte····· 'Defines the Frame Type in the response Datagram
CountIn········ VAR·· Byte····· 'Defines the number of components in the response Datagram
XRawIn········ VAR·· Byte····· 'Defines the XRaw ID in the response Datagram
XRawVal········ VAR·· Word(2)·· 'Defines the XRaw value array in the response Datagram
YRawIn········ VAR·· Byte····· 'Defines the YRaw ID in the response Datagram
YRawVal········ VAR·· Word(2)·· 'Defines the YRaw value array in the response Datagram
HeadingIn········ VAR·· Byte····· 'Defines the Heading ID in the response Datagram
HeadingVAL········ VAR·· Byte(4)·· 'Defines the Heading value array
CompHeading····· VAR·· Word····· 'Defines the Heading
Exponent········ VAR·· Byte····· 'Defines value for Heading calculation
TermIn········ VAR·· Byte····· 'Defines the Terminator of the resonse Datagram
'Interface with the V2Xe
·· HIGH SSNOT
·· LOW·· SSNOT·············· 'Enable SPI Port
'Set the data componets to be read from the V2Xe (Heading and Magnitude)
·· LOW SYNC
·· PULSOUT SYNC,2··········· 'Not required
·· SHIFTOUT MOSI,SCLK,MSBFIRST,[noparse][[/noparse]Flag,SetDataComponents,$1,Heading,Term]····· 'Set Heading as output
·· PAUSE 1
Begin:
·· SHIFTOUT MOSI,SCLK,MSBFIRST,[noparse][[/noparse]Flag,GetData,Term]················· 'Request output (Heading)
·· Loop:
····· SHIFTIN MISO,SCLK,MSBPRE,[noparse][[/noparse]FlagIn]······················· 'Look for Sync Flag
········ IF FlagIn <> $AA THEN Loop
·· SHIFTIN MISO,SCLK,MSBPRE,[noparse][[/noparse]FrameIn,CountIn,HeadingIn,HeadingVal(0),HeadingVal(1),HeadingVal(2),HeadingVal(3),TermIn]····· 'Shift in data (Heading)
'IEEE 32 bit floating point to scaled integer
'Integer will be scaled * 8 (i.e. 1 = 1/8, 2 = 1/4, 3 = 3/8, 4 = 1/2 ... 8 = 1.0 )
'IEEE32 format:
·· '31............24...23............16...15.............8...7..............0
·· 'S E E E· E E E E·· E F F F· F F F F·· F F F F· F F F F·· F F F F· F F F F
···· 'HeadingVal(0)····· HeadingVal(1)····· HeadingVal(2)····· HeadingVal(3)·· //For Compass Heading
···· 'MagVal(0)········· MagVal(1)········· MagVal(2)········· MagVal(3)····· //For Magnitude
·· 'S = Sign
·· 'E = Exponent
·· 'F = Mantisa or Binary Fraction
·· 'Number = 1.F * 2^(E-127)
·· 'The code below will assume Sign = 0 (number is positive) for all headings
·· 'if the heading is less than 1 degree or zero (the IEEE32 version of zero)
·· 'IMPORTANT!!! This code truncates to the nearest decimal instead of rounding.
·· HeadingVal(0) = HeadingVal(0) * 2··········· 'Shift Exponent
·· IF HeadingVal(1) < 128 THEN Step1
····· HeadingVal(0) = HeadingVal(0) + 1
·· Step1:
····· Exponent = HeadingVal(0)
·· Step2:
····· IF Exponent > 126 THEN Step3··········· 'Zero Test
····· CompHeading = 0
·· GOTO Done
·· Step3:
····· CompHeading = 1
·· Loop2:·························· 'Main loop to extract the Heading integer information
····· IF Exponent = 127 THEN Done
····· CompHeading = CompHeading * 2
····· HeadingVal(1) = HeadingVal(1) * 2
····· IF HeadingVal(1) < 128 THEN Step4
····· CompHeading = CompHeading + 1
····· Step4:
········ IF HeadingVal(2) < 128 THEN Step5
········ HeadingVal(1) = HeadingVal(1) + 1
····· Step5:
········ HeadingVal(2) = HeadingVal(2) * 2
········ Exponent = Exponent - 1
·· GOTO Loop2
·· Done:
'Display Heading
····· DEBUG "Heading: ",DEC CompHeading, CR,10
·· Pause 500
·· GOTO Begin·························· 'Repeat
·· High SSNOT·························· 'Disable SPI Port
END
Jon,
Any suggestions on the Float32 function?· Thank you.
Mr. Wolff,
Thank you for your interest in PNI Products. For testing in house I have a used the Basic Stamp 2p to control the V2Xe. Attached you will find a number of examples which should help you out. I do not have much experience with programming the Basic Stamp, and as a result I have not been able to get the Compass example working correctly. I did ask around and received a few hints on getting the Float32 brought into the Basic Stamp in a way that it can be used. I have attached both my non working example along with the "hints". Hopefully, this will be helpful.
Once I have some more time I hope to get all of the examples working and posted to our website for others, such as yourself to use.
Please let me know if you have any questions.
Julian Benton
Applications Engineer
jbenton@pnicorp.com
(707) 566-2260 ext. 356
There was a thread on this topic back on the old Yahoo groups Stamp board last June 8th-10th . (subject: Converting IEEE 32 bit floating point to scaled integer). I think we come up with some nifty compact code there that reduced the conversion from IEEE32 to three lines of PBASIC (for the V2XE ), plus some optional error checking.
groups.yahoo.com/group/basicstamps/message/44991
groups.yahoo.com/group/basicstamps/message/45030
groups.yahoo.com/group/basicstamps/message/45041
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com