Shop OBEX P1 Docs P2 Docs Learn Events
Compass code problem — Parallax Forums

Compass code problem

AndreiAndrei Posts: 15
edited 2009-06-14 21:46 in BASIC Stamp
If I write any code to uses the compass in navigation, my robot starts rotating ccw. I have a HITACHI HM55B
What should I do?

Thank you

Comments

  • FranklinFranklin Posts: 4,747
    edited 2009-06-13 13:22
    Attach the code that you have and a diagram of how you have it connected to the robot.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    - Stephen
  • AndreiAndrei Posts: 15
    edited 2009-06-14 10:33
    I want to combine the compass with the line follower, so at those two points on the picture my robot turn right/left with 90 degrees, go straight, turn back and continue to follow the line. I did this without the compass but now I want to use the compass.

    This is my code without the compass:

    ' {$STAMP BS2}
    ' {$PBASIC 2.5}
    qtis VAR Nib
    counter VAR Word
    OUTB = %1111
    DO
    GOSUB check_qtis
    SELECT qtis
    CASE %1000
    PULSOUT 13, 670
    PULSOUT 12, 670

    CASE %1100
    PULSOUT 13, 750
    PULSOUT 12, 670

    CASE %0100
    PULSOUT 13, 790
    PULSOUT 12, 670

    CASE %0110
    PULSOUT 13, 800
    PULSOUT 12, 700

    CASE %0010
    PULSOUT 13, 830
    PULSOUT 12, 710

    CASE %0011
    PULSOUT 13, 830
    PULSOUT 12, 750

    CASE %0001
    PULSOUT 13, 830
    PULSOUT 12, 830

    CASE %0111
    FOR counter = 1 TO 33
    PULSOUT 13, 780
    PULSOUT 12, 780
    PAUSE 20
    NEXT

    FOR counter = 1 TO 58
    PULSOUT 13, 750 + counter '820
    PULSOUT 12, 750 - counter '680
    PAUSE 20
    NEXT

    FOR counter = 58 TO 1
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 - counter
    PAUSE 20
    NEXT

    FREQOUT 3, 2000, 3000

    FOR counter = 1 TO 60
    PULSOUT 13, 750 - counter '680
    PULSOUT 12, 750 + counter '820
    PAUSE 20
    NEXT

    FOR counter = 60 TO 1
    PULSOUT 13, 750 - counter
    PULSOUT 12, 750 + counter
    PAUSE 20
    NEXT
    FOR counter = 1 TO 30
    PULSOUT 13, 720
    PULSOUT 12, 720
    PAUSE 20
    NEXT

    FOR counter = 1 TO 35
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 - counter
    PAUSE 20
    NEXT

    CASE %1110
    FOR counter = 1 TO 30
    PULSOUT 13, 720
    PULSOUT 12, 720
    PAUSE 20
    NEXT

    FOR counter = 1 TO 58
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 - counter
    PAUSE 20
    NEXT


    FOR counter = 58 TO 1
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 - counter
    PAUSE 20
    NEXT

    FREQOUT 3, 2000, 3000

    FOR counter = 1 TO 60
    PULSOUT 13, 750 - counter
    PULSOUT 12, 750 + counter
    PAUSE 20
    NEXT

    FOR counter = 60 TO 1
    PULSOUT 13, 750 - counter
    PULSOUT 12, 750 + counter
    PAUSE 20
    NEXT

    FOR counter = 1 TO 32
    PULSOUT 13, 780
    PULSOUT 12, 780
    PAUSE 20
    NEXT
    FOR counter = 1 TO 35
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 - counter
    PAUSE 20
    NEXT

    CASE ELSE
    PAUSE 3

    ENDSELECT


    LOOP

    check_qtis:
    DIRB = %1111
    PAUSE 0 ' 0 -- suprafata alba
    DIRB = %0000 ' 1 -- suprafata neagra
    PAUSE 0
    qtis = INB

    RETURN

    I want to use the averaging code for the compass, wich is this:

    '
    [noparse][[/noparse] Title ]
    ' Smart Sensors and Applications - TestCompassAveraged.bs2
    ' Test to make sure Hitachi HM55B Compass Module is working.
    ' {$STAMP BS2}
    ' {$PBASIC 2.5}
    '
    [noparse][[/noparse] I/O Definitions ]
    DinDout PIN 13 ' P2 transceives to/from Din/Dout
    Clk PIN 12 ' P0 sends pulses to HM55B's Clk
    En PIN 11 ' P2 controls HM55B's /EN(ABLE)
    '
    [noparse][[/noparse] Constants ]
    Reset CON %0000 ' Reset command for HM55B
    Measure CON %1000 ' Start measurement command
    Report CON %1100 ' Get status/axis values command
    Ready CON %1100 ' 11 -> Done, 00 -> no errors
    NegMask CON %1111100000000000 ' For 11-bit negative to 16-bits
    Negative CON 1 ' Word.bit15 = 1 -> negative
    Positive CON 0 ' Word.bit15 = 0 -> positive
    '
    [noparse][[/noparse] Variables ]
    x VAR Word ' x-axis data
    y VAR Word ' y-axis data
    status VAR Nib ' Status flags
    angle VAR Word ' Store angle measurement
    mCount VAR Nib ' Measurement count
    xSum VAR Word ' x-axis measurement accumulator
    ySum VAR Word ' y-axis measurement accumulator
    sign VAR Bit ' Sign bit
    '
    [noparse][[/noparse] Main Routine ]
    DO ' Main loop
    GOSUB Compass_Get_Axes ' Get x, and y values
    angle = x ATN -y ' Convert x and y to brads
    angle = angle */ 361 ' Convert brads to degrees
    DEBUG HOME, "x-axis N(-S) = ",SDEC x, ' Display axes and degrees
    CLREOL, CR, "y-axis W(-E) = ",
    SDEC y, CLREOL, CR, CR, "angle = ",
    DEC angle, " degrees", CLREOL
    PAUSE 150 ' Debug delay for slower PCs
    LOOP ' Repeat main loop
    '
    [noparse][[/noparse] Subroutine - Compass_Get_Axes ]
    Compass_Get_Axes: ' Compass module subroutine
    xSum = 0 ' Accumulators to zero
    ySum = 0
    FOR mCount = 1 TO 10 ' Take ten measurements
    HIGH En: LOW En ' Send reset command to HM55B
    SHIFTOUT DinDout,clk,MSBFIRST,[noparse][[/noparse]Reset\4]
    HIGH En: LOW En ' HM55B start measurement cmd
    SHIFTOUT DinDout,clk,MSBFIRST,[noparse][[/noparse]Measure\4]
    status = 0 ' Clear previous status flags
    DO ' Status flag checking loop
    HIGH En: LOW En ' Measurement status command
    SHIFTOUT DinDout,clk,MSBFIRST,[noparse][[/noparse]Report\4]
    SHIFTIN DinDout,clk,MSBPOST,[noparse][[/noparse]Status\4] ' Get Status
    LOOP UNTIL status = Ready ' Status ready? Exit loop
    SHIFTIN DinDout,clk,MSBPOST,[noparse][[/noparse]x\11,y\11] ' Get x & y axis values
    HIGH En ' Disable module
    IF (y.BIT10 = 1) THEN y = y | NegMask ' Store 11-bits as signed word
    IF (x.BIT10 = 1) THEN x = x | NegMask ' Repeat for other axis
    xSum = xSum + x ' Keep a running sum of x
    ySum = ySum + y ' Keep a running sum of y
    NEXT
    sign = xSum.BIT15 ' Store sign of xSum
    xSum = ABS(xSum) ' Take absolute value
    x = xSum / 10 ' x = the average measurement
    IF xSum // 10 >=5 THEN x = x + 1 ' Fraction > .5? Round up
    IF sign = Negative THEN x = - x ' if xSum negative, negate x
    sign = ySum.BIT15 ' Store sign of ySum
    ySum = ABS(ySum) ' Take absolute value
    y = ySum / 10 ' y = the average measurement
    IF ySum // 10 >=5 THEN y = y + 1 ' Fraction > .5? Round up
    IF sign = Negative THEN y = - y ' if ySum negative, negate y
    RETURN


    If I run this second code, my robot start turning ccw and it dosen't stop.

    Please help to finish my license project.

    Thank you very much.
    PICT0058.JPG
    833 x 768 - 112K
    PICT0059.JPG
    1024 x 728 - 389K
  • phil kennyphil kenny Posts: 233
    edited 2009-06-14 14:05
    If you post the combined code, perhaps someone can help.

    Without seeing how you combined the two programs, it isn't
    possible to find the problem.

    Including the actual Stamp program as an attachment is always
    a good idea.

    phil
  • AndreiAndrei Posts: 15
    edited 2009-06-14 16:41
    But I'm not sure how should this code look sience with the average code my robot starts rotating. Why is that?
  • W9GFOW9GFO Posts: 4,010
    edited 2009-06-14 17:21
    You might want to use different PINS for the compass....

    PULSOUT 13, 670
PULSOUT 12, 670




    DinDout PIN 13 ' P2 transceives to/from Din/Dout
Clk PIN 12 ' P0 sends pulses to HM55B's Clk



    Rich H
  • phil kennyphil kenny Posts: 233
    edited 2009-06-14 17:55
    Rich is right. I completely overlooked the fact that you left the pin
    assignments the same for the robot and the compass. His change will fix it.

    phil
  • AndreiAndrei Posts: 15
    edited 2009-06-14 19:08
    thank you very much, I didn't even think at that problem. Now it's good. I suppose I will come back with other problems regarding my code.

    Thank you again.
  • AndreiAndrei Posts: 15
    edited 2009-06-14 19:29
    How should I make my robot to turn 90 degrees cv or ccv?

    DO ' Main loop
    GOSUB Compass_Get_Axes ' Get x, and y values
    angle = x ATN -y ' Convert x and y to brads
    angle = angle */ 361 ' Convert brads to degrees
    DEBUG HOME, "x-axis N(-S) = ",SDEC x, ' Display axes and degrees
    CLREOL, CR, "y-axis W(-E) = ",
    SDEC y, CLREOL, CR, CR, "angle = ",
    DEC angle, " degrees", CLREOL
    PAUSE 150
    FOR counter = 1 to 33
    PULSOUT 13, 780
    PULSOUT 12, 780
    PAUSE 20
    NEXT
    LOPP

    How should I intorduce my angles in the last part of code?
    How should I proceed if I want my robot to turn with 90 degrees in one direction or another?
    Please help me, this would be my last problem

    Thank you
  • W9GFOW9GFO Posts: 4,010
    edited 2009-06-14 21:46
    Andrei said...
    How should I make my robot to turn 90 degrees cv or ccv?

    I haven't done this but I would start by checking the angle right before turning. Then inside your FOR.. Next loop insert some code that checks the angle and if it is angle + 90 or angle - 90 then exit the loop. That would be a simple way to do it and maybe a good beginning.

    To make a better code I would work the difference between the angle you want and the angle you are at into the the length of the pulse that you are sending to the servos.

    Something like 
    PULSOUT 12, 750 + (angleIwant - angleIamAt)
PULSOUT 13, 750 - (angleIwant - angleIamAt)



    Not presented as a solution, just a place to start.

    Also, posting the same question in multiple threads is generally discouraged.

    Rich H

    Post Edited (W9GFO) : 6/14/2009 9:54:42 PM GMT
Sign In or Register to comment.