Shop OBEX P1 Docs P2 Docs Learn Events
Need help programing i need a compass and sonar to work together — Parallax Forums

Need help programing i need a compass and sonar to work together

Indy_007Indy_007 Posts: 7
edited 2010-02-21 01:52 in BASIC Stamp
I have the basic stamp and i have a compass and sonar hooked up they will run seperatly but not together. I am trying to run them both with rules but i am having trouble writing them, the best i can figure is it needs to run off sonar for a little while then when the wall ends it will use the compass heading, any help and suggestions???
thanks

Comments

  • JDJD Posts: 570
    edited 2010-02-19 19:00
    Indy_007,

    For programming help, can you please attach what code you have started so far and what is or is not working with it? You can use the attachment manager to upload your sample code so we can review it. It helps to have all or most the information before offering suggestions.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Respectfully,


    Joshua Donelson
    www.parallax.com
  • Mike GreenMike Green Posts: 23,101
    edited 2010-02-19 19:04
    It sounds like it's a bit early in your design process to deal with programming. Start with describing in more detail just what you want to accomplish. What do you want to use the sonar for? What do you want to use the compass for? What's the environment where the Stamp is operating?
  • Indy_007Indy_007 Posts: 7
    edited 2010-02-20 00:12
    Okay lets start off with what I have. I have a working compass program, I sit it down reset it, and let it go. It will stay going that direction even if it is knocked off of course. I also have a working sonar program that will follow a wall. Both of these codes work great seperately. I am wanting to combine both the compass and the sonar. In such a way that it will stay going striaght when the wall ends _____{··· }________then if possible to start back with the sonar when the wall comes back into the sonar, it will stay with the sonar.

    I am thinking that I need to set the rules to use the sonar for a couple of loops then when it is going straight get the compass heading, then if the distance gets extremely large it will go and use the compass heading. The reason i want to do this is when just using the compass if it is not straight it will vear off course.

    here are the sonar rules

    ·· Rule1: '· Go a bit left
    ··· IF (sonarDistance > (wallOffset + someTolerance)) THEN
    ··········· PULSOUT RightServo, RightZeroSpeed - speed
    ··········· PULSOUT LeftServo,· LeftZeroSpeed· + turnspeed
    ··········· ruleFired = True
    ········ ENDIF

    ·· Rule2: '· Go a bit right
    ··· IF (sonarDistance < (wallOffset - someTolerance)) THEN
    ··········· PULSOUT RightServo, RightZeroSpeed - turnspeed
    ··········· PULSOUT LeftServo,· LeftZeroSpeed· + speed
    ··········· ruleFired = True
    ········ ENDIF
    ·· Rule3: '· Go forward
    ··· IF (sonarDistance = ((wallOffset - someTolerance) OR (wallOffset + someTolerance))· THEN
    ··········· PULSOUT RightServo, RightZeroSpeed· - speed
    ··········· PULSOUT LeftServo,· LeftZeroSpeed·· + speed
    ··········· ruleFired = True
    ········ ENDIF
    bsp
    bsp
    Wallcomp.bsp
    9K
    rtf
    rtf
    wall comp.rtf
    11K
  • legoman132legoman132 Posts: 87
    edited 2010-02-20 13:22
    Make the program first find the wall. Then follow it a little bit and get the heading. Next, give the US sensor priority as it follows the wall. If the US distance goes above a certain range, the robot will assume the wall has ended. If so, give the compass priority and keep the robot going straight, occasionally checking to see if the US sensor reads below the "wall not found" threshold. When it does, give the US sensor priority and follow the wall again.
  • Indy_007Indy_007 Posts: 7
    edited 2010-02-20 13:24
    Now can you put that into a code for me?
  • FranklinFranklin Posts: 4,747
    edited 2010-02-20 17:00
    That's the fun of using Parallax products, we leave the hobby up to you. If you have specific questions perhaps we can help you with those.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    - Stephen
  • Indy_007Indy_007 Posts: 7
    edited 2010-02-20 20:54
    i think this is gonna work look it over and let me know what you think
    ' {$STAMP BS2}
    ' {$PBASIC 2.5}

    '******************************************************************************
    'Joshua Endicott
    'Mechanical Engineering Technology Student
    'WVU Institute of Technology
    '******************************************************************************
    ' A HMC6352 compass module and sonar PING are used to steer a straight path
    ' and follow the a Wall
    '
    '.............................................................................
    ' Circuit Diagram
    '..............................................................................


    '
    ' BS2 | +5v
    ' | |
    ' |
    ' | | |
    ' pin 0|
    | I/O |
    ' | | Sonar |
    ' | | |
    ' |
    ' | |
    ' | ---
    ' | -
    ' |
    ' | +5v
    ' | |
    ' |
    ' | | |
    ' pin 9|
    | Data |
    ' | | HMC6352 |
    ' | | |
    ' pin 8|
    | Clock |
    ' | | |
    ' |
    ' | |
    ' | ---
    ' | -
    ' |
    ' pin 13|
    ' | |
    ' pin 12|
    ____|____
    ' | | +5vdc | |
    ' | ____|____ | | Right |
    ' | +5vdc | | ---| Servo |
    ' | | | Left | | |
    ' | ---| Servo | |_________|
    ' | | | |
    ' | |_________| ---
    ' | | -
    ' | ---
    ' | -
    '______________|
    '
    '..............................................................................

    '******************************************************************************
    ' System Constants
    '******************************************************************************
    wallOffset CON 2000
    someTolerance CON 100

    ' System Constants
    '******************************************************************************
    ' Used by the Compass module
    HMC6352_READ CON $43
    HMC6352_WRITE CON $42
    GET_HEADING CON $41
    sck CON 1
    sio CON 0

    '...Program Constants
    True CON 1
    False CON 0

    '
    ' Number Ranges
    '.....................................
    ' range 0 --> 32766 positive
    ' +0 --> + 32766
    '.....................................
    ' range 65535 --> 32767 negative
    ' -1 --> -32767
    '.....................................
    MaxNum CON 65535
    halfMaxNum CON 32767

    '...Motor Constants
    '
    RightZeroSpeed CON 750
    LeftZeroSpeed CON 750

    speed CON 200
    turnspeed CON 50

    '...Allowable drift degrees *10
    delta CON 50



    '******************************************************************************
    ' I/O Pin Assignments
    '******************************************************************************
    PING PIN 10 ' Sonar

    SDA PIN 9 ' Compass Data pin
    SCL PIN 8 ' COmpas clock pin

    RightServo PIN 13
    LeftServo PIN 12 '


    '******************************************************************************
    ' Variables
    '******************************************************************************
    Theta VAR Word ' Commanded heading
    Heading VAR Word ' Actual Heading
    h1 VAR Word ' Past Headings for
    h2 VAR Word ' data smoothing
    h3 VAR Word
    h4 VAR Word

    diff VAR Word ' Heading - Theta
    absDiff VAR Word ' ABS(Heading - Theta)
    dir VAR Bit ' Left/Right direction flag

    ruleFired VAR Word ' Rule Flag

    I2C_DATA VAR Byte ' Compass
    I2C_LSB VAR Bit ' Compass

    sonarDistance VAR Word
    oldSonar VAR Word



    '******************************************************************************
    ' Main Control Loop
    '******************************************************************************
    DO
    '
    ' get Sensors
    '

    GOSUB getSonar

    '
    ' Check Rules Only one will fire per loop
    '
    ruleFired = False

    Rule1: ' Go a bit left
    IF ((sonarDistance > (wallOffset + someTolerance) AND (sonarDistance < 3000))) THEN
    PULSOUT RightServo, RightZeroSpeed - speed
    PULSOUT LeftServo, LeftZeroSpeed + turnspeed
    ruleFired = True
    ENDIF

    Rule2: ' Go a bit right
    IF (sonarDistance < (wallOffset - someTolerance)) THEN
    PULSOUT RightServo, RightZeroSpeed - turnspeed
    PULSOUT LeftServo, LeftZeroSpeed + speed
    ruleFired = True
    ENDIF
    Rule3: ' Go forward
    IF (sonarDistance = ((wallOffset - someTolerance) OR (wallOffset + someTolerance)) AND (ruleFired = False)) THEN
    PULSOUT RightServo, RightZeroSpeed - speed
    PULSOUT LeftServo, LeftZeroSpeed + speed
    GOSUB Compass_Get_Heading 'return Heading
    h1 = Heading
    Heading = (h1 + h2 +h3 + h4) /4 ' Average
    h4 = h3 : h3 = h2 : h2 = h1 ' Shift
    diff = Heading - Theta
    absDiff = ABS(diff)
    IF absDiff > 1800 THEN absDiff = 3600 - absdiff
    IF (absDiff + Theta)//3600 = Heading THEN :dir = 0 :ELSE :dir =1 :ENDIF
    DEBUG DEC heading, LF
    ruleFired = True
    ENDIF

    Rule4:
    IF ((sonardistance > 3000)) THEN
    Rule0: ' Go a bit left based on Compass
    IF (absDiff > delta) AND (dir =0) AND (ruleFired = False) THEN
    PULSOUT RightServo, RightZeroSpeed - speed
    PULSOUT LeftServo, LeftZeroSpeed + turnspeed
    ruleFired = True
    ENDIF

    Rule5: ' Go forward based on Compass
    IF (absDiff = delta ) AND (ruleFired = False) THEN
    PULSOUT RightServo, RightZeroSpeed - speed
    PULSOUT LeftServo, LeftZeroSpeed + speed
    ruleFired =True
    ENDIF

    Rule6: ' Go a bit right based on Compass
    IF (absDiff < delta) AND (dir =0) AND (ruleFired = False) THEN
    PULSOUT RightServo, RightZeroSpeed - turnspeed
    PULSOUT LeftServo, LeftZeroSpeed + speed
    ruleFired =True
    ENDIF
    ENDIF





    LOOP
    '******************************************************************************
    Compass_Get_Heading: ' Compass module subroutine return Heading
    '
    GOSUB I2C_Start
    I2C_DATA = HMC6352_WRITE
    GOSUB I2C_Write
    I2C_DATA = GET_HEADING
    GOSUB I2C_Write
    GOSUB I2C_Stop

    GOSUB I2C_Start
    I2C_DATA = HMC6352_READ
    GOSUB I2C_Write
    PAUSE 6
    GOSUB I2C_Read
    Heading.HIGHBYTE = I2C_DATA
    GOSUB I2C_ACK
    GOSUB I2C_Read
    Heading.LOWBYTE = I2C_DATA
    GOSUB I2C_NACK
    GOSUB I2C_Stop
    RETURN
    '..............................................................................

    '
    I2C_Start: ' Compass
    '
    LOW SDA
    LOW SCL
    RETURN
    '..............................................................................

    '
    I2C_Stop: ' Compass
    '
    LOW SDA
    INPUT SCL
    INPUT SDA
    RETURN
    '..............................................................................

    '
    I2C_ACK: ' Compass
    '
    LOW SDA
    INPUT SCL
    LOW SCL
    INPUT SDA
    RETURN
    '..............................................................................

    '
    I2C_NACK: ' Compass
    '
    INPUT SDA
    INPUT SCL
    LOW SCL
    RETURN
    '..............................................................................

    '
    I2C_Read: ' Compass
    '
    SHIFTIN SDA, SCL, MSBPRE, [noparse][[/noparse]I2C_DATA]
    RETURN
    '..............................................................................

    '
    I2C_Write: ' Compass
    '
    I2C_LSB = I2C_DATA.BIT0
    I2C_DATA = I2C_DATA / 2
    SHIFTOUT SDA, SCL, MSBFIRST, [noparse][[/noparse]I2C_DATA\7]
    IF I2C_LSB THEN INPUT SDA ELSE LOW SDA
    INPUT SCL
    LOW SCL
    INPUT SDA
    INPUT SCL
    LOW SCL
    RETURN
    '..............................................................................


    '******************************************************************************
    ' getSonar return sonarDistnce
    '******************************************************************************
    getSonar:
    PING = 10 ' make trigger 0-1-0
    PULSOUT PING, 13 ' activate sensor
    PULSIN PING, 1, sonarDistance ' measure echo pulse

    '... Make sure a non zero reading is obtained
    IF sonarDistance = 0 THEN sonarDistance = oldSonar
    oldSonar = sonarDistance
    DEBUG DEC sonarDistance, LF
    RETURN
  • legoman132legoman132 Posts: 87
    edited 2010-02-21 01:52
    Wouldn't the robot try to navigate based on the ping value even though the distance was outside the set range?
    Try putting the code for rule 4 above rule 1 in the code.
    Take out the "AND (sonarDistance < 3000)" in rule 1 and add "AND (ruleFired = False)" in the IF THEN for rules 1 and 2

    Otherwise, it looks like it might work, but add something to make sure that if the ping is interrupted before four program loops so that the averaging wont return a bad heading target (i.e. program looped three times, actual heading is 180 degrees, average returns a target heading of (0+180+180+180)/4= 135 degrees, robot turns 45 degrees off course).
Sign In or Register to comment.