  INFO - FAQ - CC2-Forum - CCPro-Forum |
|
'----------------------------------------------------------------------------------------------------' '***********************************************************************' '* WINDT SYSTEMS *' '* COMPASS CMPS03 STEP BEARING v1.1 for CCIUM2.0 with CCIAB2.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 MID.ANGLE (MIDDLE 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' 'adding 1/2 of STEPS and then dividing this number by STEPS.' 'Example 1:' '16 BIT BEARING = 900 (represents 90,0º) STEPS = 18 (represents 1,8º)' 'STEP BEARING = ((16 BIT BEARING + (STEPS / 2)) / STEPS' 'STEP BEARING = (900 + 9) / 18' 'STEP BEARING = 50' 'MID.ANGLE = STEP BEARING * STEPS' 'MID.ANGLE = 50 * 18' 'MID.ANGLE = 900' 'Adding 1/2 of STEPS is needed otherwise STEP BEARING is shifted too far clockwise.' 'Example 2: without adding 1/2 of STEPS and STEPS at 18, STEP_BEARING = 0 is between 0º and 1,7º.' 'Example 3: adding 1/2 of STEPS and STEPS at 18, STEP BEARING = 0 is between 359,1º and 0,8º.' 'You can change the STEPS variable, look under *CONSTANTS* and change the number.' 'With STEPS at 18 then the STEP_BEARING represents/ 0 = N/ 50 = E/ 100 = S/ 150 = W.' '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] define step_bearing word[3] '*******************************************' '**************** CONSTANTS ****************' define steps 18 '*******************************************' '****************** 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:"; print step_bearing; print" "; print"#L201#"; print"MID.ANGLE:"; print (step_bearing * steps) / 10; 'PRINT MID.ANGLE IN FRONT OF THE COMMA' put &h2e '.' print (step_bearing * steps) - ((step_bearing * steps) / 10 * 10); 'PRINT MID.ANGLE BEHIND THE COMMA' 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 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 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' step_bearing = (iic_word + (steps / 2)) / steps 'CALCULATING STEP BEARING' if step_bearing = 3600 / steps then step_bearing = 0 'ROLL-OVER' 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 |
Re: COMPASS CMPS03 STEP BEARING v1.1 for CCIUM2.0 with CCIAB2.0 (von ReinhardB - 25.11.2004 8:51)