INFO - FAQ - CC2-Forum - CCPro-Forum |
'----------------------------------------------------------------------------------------------------' '***********************************************************************' '* WINDT SYSTEMS *' '* COMPASS CMPS03 STEP BEARING v1.0 for CC1M2.01 with CC1AB2.0 *' '* H.J. WINDT *' '* 2004 *' '***********************************************************************' '----------------------------------------------------------------------------------------------------' 'This program reads the 16 bit bearing data from the MILFORD Magnetic Compass Module (CMPS03) #5-124 ' 'via the I2C bus and displays the data on the lcd as STEP BEARING and REP.ANGLE (REPRESENTS ANGLE).' 'STEP BEARING range is from 0 to 199, this represents steps of 1,8º.' 'STEP BEARING is of course not as accurate as the 16 BIT BEARING but simplifies using the CMPS03 and' 'gets rid of the jittery action of the 16 bit bearing.' 'HINT: USE STEP BEARING FOR ROBOT NAVIGATION!' 'Remember... all variables are integer, it is what makes this software work!' 'Calculating the STEP BEARING is done by first getting the 16 BIT BEARING from the CMPS03 module and' 'then dividing this number by 18.' 'Example:' '16 BIT BEARING = 900 (represents 90,0º) divide it by 18 (represents 1,8º)' 'STEP BEARING = 16 BIT BEARING / 18' 'STEP BEARING = 900 / 18' 'STEP BEARING = 50' 'REP.ANGLE = STEP BEARING * 18' 'REP.ANGLE = 50 * 18' 'REP.ANGLE = 900' 'Feel free to use and share this software!' '----------------------------------------------------------------------------------------------------' '*************** INS and OUTS ***************' define lcd_light_off port[16] '*******************************************' '**************** VARIABLES ****************' define config byte[1] define iic_error bit[7] define register byte[2] define iic_high_byte byte[3] define iic_low_byte byte[4] define iic_word word[2] '*******************************************' '**************** CONSTANTS ****************' '*******************************************' '****************** SETUP ******************' print"#ON_LCD#"; print"#INIT#"; print"#CLR#"; print"#OFF#"; lcd_light_off = 0 '*******************************************' '***************** PROGRAM *****************' #start gosub compass_read print"#ON_LCD#"; print"#L101#"; print"STEP BEARING:"; iic_word = iic_word / 18 'calculating STEP BEARING' print iic_word; print" "; print"#L201#"; print"REP.ANGLE:"; iic_word = iic_word * 18 'calculating REP.ANGLE' print iic_word / 10; put &h2e '.' print iic_word - (iic_word / 10 * 10); put &hdf 'º' print" "; print"#OFF#"; goto start '*******************************************' '*************** SUBROUTINES ***************' #compass_read print"#ON_IIC#"; print"#START#"; put &b11000000 'CMPS03 ADDRESS AND WRITE' put &b00000010 'CMPS03 START REGISTER FOR 16 BIT COMPASS DATA' print"#STOP#"; print"#START#"; put &b11000001 'CMPS03 ADDRESS AND READ' get iic_high_byte 'READ HIGH BYTE' get iic_low_byte 'READ LOW BYTE' print"#STOP#"; print"#OFF#"; gosub check_for_iic_error 'CHECK FOR I2C ERROR' if iic_error = 1 then goto compass_read if iic_word < 0 or iic_word > 3599 then goto compass_read 'CHECK FOR CMPS03 DATA ERROR' return #check_for_iic_error print"#ON_CONFIG#"; get config if iic_error = 0 then goto pass_iic_error_clear iic_error = 0 put config iic_error = 1 #pass_iic_error_clear print"#OFF#"; return '*******************************************' '****************** DATA *******************' '*******************************************' |
Antwort schreiben |