INFO - FAQ - CC2-Forum - CCPro-Forum |
aha > > '----------------------------------------------------------------------------------------------------' > '***********************************************************************' > '* 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 |