Shop OBEX P1 Docs P2 Docs Learn Events
BOE_BOT RoboRealm with PAN/TILT — Parallax Forums

BOE_BOT RoboRealm with PAN/TILT

jbodjbod Posts: 46
edited 2008-03-03 01:36 in Robotics
A while back a forum member posted some code (VB Script and .BS2) to get Roborealm working with the Boe-Bot ... the setup had 4 servos (2 wheels, one pan, one title) ... I am trying to get a copy or understand the setup (code) to control the pan/tilt servos ... I have the following code and·have been able to control my BB with it (just the wheel servos)·... I don't see any where in the code statements for the pan/tilt servos (VB or BS2)?

I know RR has added a new module for BB ... however their module is based on having an eb500 connection and I am using a XBEE connection (serial) so I am using the RR serial module for that portion ... If some one has a copy of the code or can explain (point me in the right direction) how to use the code I even have to get this working (pan/tilt) I would greatly appreciate it !!!! hop.gif

Thanks in advanced!!!!
·
===========================================================================
VBSCRIPT:
' initialize our start motor values to neutral
left_base = 750
right_base = 750
·
·
' get current image size
width = GetVariable("IMAGE_WIDTH")
center = width / 2
·
' get the size (width or height) of the current
' bounding box
size = GetVariable("COG_BOX_SIZE")
·
' if it is equal to "" then no object was detected
if size <> "" then
·
' get the horizontal center of gravity found by the COG
' module
cogX = GetVariable("COG_X")
' if it is less than 75 the blob is on the left side
' of the screen so move that way
if cogX < center-5 then
left_base = 750-(center-cogX)/2
right_base = 750
' otherwise move to the right if above 85 pixels (and no
' movement if 75 < cogX < 85 )
elseif cogX > center+5 then
left_base = 750
right_base = 750+(cogX-center)/2
end if
' if the ball is too close then we have to move backwards
if (size>90) then
left_motor = left_base+((40-size)*2)
right_motor = right_base-((40-size)*2)
' otherwise move forward
else
left_motor = left_base-((size-40)*2)
right_motor = right_base+((size-40)*2)
end if
' set the final motor values to be picked up by
' the SSC module
end if
if (size<20) then
left_motor=750
right_motor=750
end if
SetVariable "LEFT_MOTOR", CInt(left_motor)
SetVariable "RIGHT_MOTOR", CInt(right_motor)
·
============================================================================================================
PBASIC CODE
' {$STAMP BS2}
' {$PBASIC 2.5}
x VAR Word
y VAR Word
main:
SERIN 16,16780,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y] ' Change SERIN 0,84,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y] for eb500
PULSOUT· 14, x
PULSOUT· 15, y
PAUSE 5
GOTO main

Comments

  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-02-28 18:24
    Hi jbod, you already have the basis of what will eventually be the final program what you need to do now is add the pan and tilt to the existing.

    To make things easier I would start by adding the pan and getting that right before moving on to tilt. Also before including the pan value in the serial control and the Basic Stamp code I would first test the values from within the RR program. To get up and running with this you will need to add two more variables to the VB script, one variable to calculate the position of the "blob" and one variable for serial output. In keeping with the code that Tom wrote you might call the first variable pan_base and the second variable PAN_MOTOR, the second variable would be included at the end of the script taking it's value from pan_base

    SetVariable "LEFT_MOTOR", CInt(left_motor)
    SetVariable "RIGHT_MOTOR", CInt(right_motor)
    SetVariable "PAN_MOTOR", CInt(pan_base)

    For test purposes you could get the value for pan_base from left_base and right_base

    left_base = 750-(center-cogX)/2····· 'existing line

    pan_base=left_base

    right_base = 750+(cogX-center)/2·· 'existing line

    pan_base=right_base

    Using the Watch_Variables control (under Statements) you should be able to see the value of PAN_MOTOR change as you move your colored object left and right, the values may not be right to begin with but you should have an indication of how close or how far the calculation is.

    Jeff T.
  • Steve JoblinSteve Joblin Posts: 784
    edited 2008-02-28 18:59
    The "Color Tracking Tutorial" on RoboRealm (http://www.roborealm.com/tutorial/color_object_tracking_2/slide040.php) provides the code for providing "tilt".
  • jbodjbod Posts: 46
    edited 2008-02-28 20:15
    Thanks guys!!!· I will be doing some testing tonight [noparse]:)[/noparse]
  • jbodjbod Posts: 46
    edited 2008-02-29 01:00
    I am now getting RR to respond with var changes ... if I have the following code for the stamp:

    x VAR Word
    y VAR Word
    main:
    SERIN 16,16780,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y,DEC4 z] ' Change SERIN 0,84,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y] for eb500
    PULSOUT· 14, x
    PULSOUT· 15, y
    PAUSE 5
    GOTO main



    What would be the best way to add the VAR for PAN_MOTOR ...

    x VAR Word
    y VAR Word·
    z VAR Word
    main:
    SERIN 16,16780,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y,DEC4 z] ' Change SERIN 0,84,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y] for eb500
    PULSOUT· 14, x
    PULSOUT· 15, y
    PULSOUT 13, z
    PAUSE 5
    GOTO main



    Would this be correct?
  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-02-29 06:21
    Hi jbod, the second of you examples would be correct, you will need to modify the RR serial string to include the PAN_MOTOR variable

    ! [noparse][[/noparse]LEFT_MOTOR] <lf> [noparse][[/noparse]RIGHT_MOTOR] <lf> [noparse][[/noparse]PAN_MOTOR] <lf>.

    If your running with the test example I gave that is probably not going to work well. The link to the RR tilt routine Steve Joblin posted is your best bet for both axis, for pan just substitute the y with x.

    That shouldn't be too hard now you have a handle on the VB script, if you need a little help with it post back.

    Jeff T.
  • WhitWhit Posts: 4,191
    edited 2008-02-29 14:02
    Is this the post you were speaking of?

    http://forums.parallax.com/showthread.php?p=646454

    Sounds like you've about got it anyway!

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Whit+


    "We keep moving forward, opening new doors, and doing new things, because we're curious and curiosity keeps leading us down new paths." - Walt Disney
  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-02-29 15:38
    Yes Whit thats the original post where Tom did a little pioneering. I and a couple of others helped iron out the communications side of things and Tom completed the project without ever posting the pan and tilt code as far as I know. That is not too great an obstacle, the code that we have has the screen size and the COG (center of gravity) x and y coordinates. So all we have to do is work out an increment of movement dependant on x and y and keep it within the bounds of screen size. The COG tends to move slightly and rapidly·so the wheel movement had a tolerence of + or - 5 from center before taking any action which kept things a little smoother, I imagine the same will apply to the pan and tilt.

    Jeff T.
  • jbodjbod Posts: 46
    edited 2008-03-01 03:12
    Not having a whole lot of luck [noparse]:([/noparse]· ... I can get it tweeked to moved fwd and bkw ... but it is VERY jerky and the pan is very slow ... here is what I am using for code:



    STAMP:


    ' {$STAMP BS2}
    ' {$PBASIC 2.5}
    x VAR Word
    y VAR Word
    z VAR Word
    main:
    SERIN 16,16780,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y,DEC4 z] ' Change SERIN 0,84,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y] for eb500
    PULSOUT· 15, x
    PULSOUT· 14, y
    PULSOUT· 12, z
    PAUSE 5
    GOTO main



    VB:

    ' initialize our start motor values to neutral
    left_base = 750
    right_base = 750

    pan_base = 750


    ' get current image size
    width = GetVariable("IMAGE_WIDTH") - I am using 160x120 -
    center = width / 2

    ' get the size (width or height) of the current
    ' bounding box
    size = GetVariable("COG_BOX_SIZE")

    ' if it is equal to "" then no object was detected
    if size <> "" then

    ' get the horizontal center of gravity found by the COG
    ' module
    cogX = GetVariable("COG_X")
    ' if it is less than 75 the blob is on the left side
    ' of the screen so move that way
    if cogX < center-5 then
    left_base = 750-(center-cogX)/2
    right_base = 750
    pan_base = left_base
    ' otherwise move to the right if above 85 pixels (and no
    ' movement if 75 < cogX < 85 )
    elseif cogX > center+5 then
    left_base = 750
    right_base = 750+(cogX-center)/2
    pan_base = right_base
    end if
    ' if the ball is too close then we have to move backwards
    if (size>90) then
    left_motor = left_base+((40-size)*2)
    right_motor = right_base-((40-size)*2)
    ' otherwise move forward
    else
    left_motor = left_base-((size-40)*2)
    right_motor = right_base+((size-40)*2)
    end if
    ' set the final motor values to be picked up by
    ' the SSC module
    end if
    if (size<20) then
    left_motor=750
    right_motor=750

    pan_motor=750
    end if
    SetVariable "LEFT_MOTOR", CInt(left_motor)
    SetVariable "RIGHT_MOTOR", CInt(right_motor)
    SetVariable "PAN_MOTOR", CInt(pan_base)
  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-03-01 17:20
    Hi jbod, the Stamp code should be fine.

    The VB script is executed once every frame so we do not want to center the pan_base every frame we want to achieve a smooth left to right right to left movement. We also want to prevent the motor being driven beyond it's limits of between 500 and 1000. Eventually we will also have to account for the movement of the BOT, we want to try and achieve a position where the object is center screen and pan_base = 750. So if pan_base is at it's leftmost position (500) we want the BOT to turn left until pan_base is at 750. I have attached a modification to the VB script that initializes pan_position at 750 during the first 50 frames, after that a value is added or subtracted depending on the x location. You can try different values and see which works best, I started with a value of 2. At the end of the script there are two If...Then's to make sure the servo is not overdriven. There is a lot of room for improvement but this should give an idea if we are headed in the right direction.

    Jeff T.
    vbs
    vbs
    pan_test.vbs
    2K
  • jbodjbod Posts: 46
    edited 2008-03-01 19:15
    There Jerky movment is in the FWD and BCK movement? When the wheels turn it is very jumpy/jerky ... with either code?
  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-03-01 19:43
    Hi jbod, try the pan without the wheel motors, comment the following two lines out of the Stamp code

    PULSOUT· 15, x
    PULSOUT· 14, y

    just to see if the pan smooths out.

    Jeff T.
  • jbodjbod Posts: 46
    edited 2008-03-01 21:06
    The wheel movement is the issue ... not the pan ... The wheels move FWD and BCK real jerky ...
  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-03-01 22:30
    Hi jbod, I don't have a camera or servos set up here, the only way I can tell whats happening is by asking you, that way I can figure out some of whats going on. You've already told me the wheels are jerky but you havn't told me if the pan is smooth without the wheels. The jerkiness is coming from the fact we have the BOT and the pan trying to home in on the object at the same time. If we can get them both to operate independantly in a way we like then we can work out how to combine them. The pan code is not finished yet, as it is now it should track left or right of center, we have yet to tell it when and how to track back to center. I don't have a ready made answer so unless we get one we have got to move slowly and patiently.

    Jeff T.
  • jbodjbod Posts: 46
    edited 2008-03-01 23:08
    Here is what I have going so far ... I have been trying to get the Bot to move FWD and BCK clean ... It moves FWD, BCK, L and R ... However it is all jerky (all movement)·... This is even with out the PAN_MOTOR code applied ... I am trying to iron aout the basics first before even getting the pan code up [noparse]:([/noparse]



    Code if be the stamp code?



    Here is what I have so far:

    VB

    ' initialize our start motor values to neutral
    count=GetVariable("IMAGE_COUNT")

    if count <50 then
    · pan_base=720
    · left_base = 750
    · right_base = 750
    end if

    SetVariable "LEFT_MOTOR", CInt(left_motor)
    SetVariable "RIGHT_MOTOR", CInt(right_motor)
    SetVariable "PAN_MOTOR", CInt(pan_base)

    ' get current image size
    width = GetVariable("IMAGE_WIDTH")
    center = width / 2

    ' get the size (width or height) of the current
    ' bounding box
    size = GetVariable("COG_BOX_SIZE")

    ' if it is equal to "" then no object was detected
    if size <> "" then

    ' get the horizontal center of gravity found by the COG
    ' module
    cogX = GetVariable("COG_X")
    ···
    ' if it is less than 75 the blob is on the left side
    ' of the screen so move that way
    · if cogX < center-5 then
    ··· left_base = 750-(center-cogX)
    ··· right_base = 750

    ' otherwise move to the right if above 85 pixels (and no
    ' movement if 75 < cogX < 85 )
    · elseif cogX > center+5 then
    ··· left_base = 750
    ··· right_base = 750+(cogX-center)
    · end if

    ' if the ball is too close then we have to move backwards
    · if (size>45) then
    ··· left_motor = left_base+((50-size)*5)
    ··· right_motor = right_base-((50-size)*5)

    ' otherwise move forward
    · else
    ··· left_motor = left_base-((size-50)*5)
    ··· right_motor = right_base+((size-50)*5)
    · end if

    ' set the final motor values to be picked up by
    if (size>90) then
    left_motor=750
    right_motor=750
    end if

    if pan_base<500 then
    ·· pan_base=500
    end if

    if pan_base>1000 then
    ·· pan_base=1000
    end if

    end if






    STAMP:


    ' {$STAMP BS2}
    ' {$PBASIC 2.5}
    x VAR Word
    y VAR Word
    z VAR Word

    HIGH 15
    FREQOUT 4, 500, 3000
    LOW 15

    main:
    SERIN 16,16780,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y,DEC4 z] ' Change SERIN 0,84,[noparse][[/noparse]WAIT ("!"),DEC4 x,DEC4 y] for eb500
    PULSOUT· 15, x
    PULSOUT· 14, y
    PULSOUT· 12, z
    PAUSE 5
    GOTO main
  • UnsoundcodeUnsoundcode Posts: 1,532
    edited 2008-03-02 02:40
    Ok jbod, lets rewind a little and try to get the wheel motion smooth. One important piece of the setup is making sure your camera is adjusted correctly. When the camera is setup right the COG displayed on the screen should be practically motionless, if it's bouncing around the motion is going to be transferred to the BOT.

    Left_base=750 and right_base=750 should not be included inside the If ...Then.

    I'm attaching the last VB script that Tom posted and that he said was working well, use this in its unaltered form and let me know how the wheels perform.

    Jeff T.
    vbs
    vbs
    Last.vbs
    1K
  • jbodjbod Posts: 46
    edited 2008-03-03 01:36
    Trying another approach at the same time ... hope this does not muddy the water to much [noparse]:)[/noparse]

    I also have a CMU-CAM and would like to use it to drive the PAN/TILT servos (12 Pan, 13 Tilt) ... this code works great that came with the cam ... I would think it would be pretty straight forward to add to more servos for the PAN/TILT to the code ... ANy thoughts on where to start with this [noparse]:)[/noparse]

    '{$STAMP BS2}
    '{$PBASIC 2.5}
    ' CMUcam1 BoeBot AppMod 1.2 Demo Program
    ' Copyright 2006, Seattle Robotics
    ' Basic Stamp 2
    ' CMUcam1 jumpered for 9600 Baud
    ' RA Nucci 07/13/2006
    ' Setup pins for buttons
    INPUT 6
    INPUT 8
    HIGH 6
    HIGH 8
    RcvData VAR Byte(10) ' Array
    SaveColorData VAR Byte(6) ' Array
    Result VAR Word
    N VAR Byte ' General purpose counter variable
    LostTrack VAR Nib
    Switch VAR Nib
    Debounce VAR Nib
    Function VAR Nib
    Main:
    ' Pause for CMUcam1 startup
    · FOR N = 2 TO 5 ' Flash all LED's during pause
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]255]
    ··· PAUSE 400
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0]
    ··· PAUSE 400
    · NEXT
    · FREQOUT 10, 400, 2500 ' Beep to indicate ready
    ProgTop:
    · ' Select function from push buttons
    · GOSUB GetButton
    · Debounce=Switch
    · GOSUB GetButton
    · ' Get button press twice and compare if not the same repeat
    · IF Debounce <> Switch THEN ProgTop
    · IF Switch = 2 THEN ' If Go/Stop button pushed then beep
    ··· FREQOUT 10, 400, 2500 ' beep
    · ENDIF
    BRANCH Switch, [noparse][[/noparse]ProgTop,Buttonselect,Execute]
    Buttonselect:
    · Function = Function + 1
    · IF Function > 9 THEN
    ··· Function = 1
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]1]
    · ENDIF
    · PAUSE 300
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]DCD(Function-1)]
    GOTO ProgTop

    Execute:
    · BRANCH Function, [noparse][[/noparse]ProgTop,Calibrate,SaveColor,TrackColor,Avoid,Adapt,FollowLine,Point,Color]
    GOTO ProgTop

    GetButton:
    · Switch=0
    · IF IN6 = 0 THEN
    ··· Switch = 1 ' Select button pressed
    ··· RETURN
    · ENDIF
    · IF IN8 = 0 THEN
    ··· Switch = 2 ' GO/Stop button pressed
    ··· RETURN
    · ENDIF
    · IF IN8 = 0 AND IN6 = 0 THEN
    ··· Switch = 3 ' Both buttons pressed at the same time
    · ENDIF
    RETURN

    ' Track the color previously saved in EEPROM
    TrackColor:
    · GOSUB ConfigureCMUcam
    · ' Read EEPROM into SaveColorData array
    · READ 0, SaveColorData(0)
    · READ 1, SaveColorData(1)
    · READ 2, SaveColorData(2)
    · READ 3, SaveColorData(3)
    · READ 4, SaveColorData(4)
    · READ 5, SaveColorData(5)
    · ' Set Track Color values from EEPROM
    · SEROUT 7,84,[noparse][[/noparse]"TC"]
    · SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(0)]
    · SEROUT 7,84,[noparse][[/noparse]" "]
    · SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(1)]
    · SEROUT 7,84,[noparse][[/noparse]" "]
    · SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(2)]
    · SEROUT 7,84,[noparse][[/noparse]" "]
    · SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(3)]
    · SEROUT 7,84,[noparse][[/noparse]" "]
    · SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(4)]
    · SEROUT 7,84,[noparse][[/noparse]" "]
    · SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(5),CR]
    · PAUSE 200
    TopTrack:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
    · GOSUB GetButton· ' Check for stop button press
    · IF Switch = 2 THEN' Switch 2 is stop button
    · ' Two beeps for Stop
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]DCD (Function-1)]
    ··· PAUSE 200
    ··· FREQOUT 10, 400, 2500
    ··· PAUSE 200
    ··· FREQOUT 10, 400, 2500
    ··· ' Reset the green tracking LED
    ··· SEROUT 7, 84, [noparse][[/noparse]"L1 0",CR] ' Green LED off
    ··· PAUSE 100
    ··· SEROUT 7, 84, [noparse][[/noparse]"L1 2",CR] ' Green LED auto mode
    ··· PAUSE 100
    ··· GOTO ProgTop
    · ENDIF
    · ' Send command - Track color (with no arguments) -
    · ' will track last color grabbed by TW command
    · SEROUT 7, 84, [noparse][[/noparse]"TC",CR]
    · SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
    · PAUSE 100
    · ' 45 is aprox H center, 70 is aprox V center
    ' Track and follow logic
    IF Function = 3 THEN
    · IF RCVData(2) > 65 AND RcvData(9) > 25 THEN RotateLeft
    · IF RCVData(2) < 25 AND RcvData(9) > 25 THEN RotateRight
    · IF RCVData(7) > 120 AND RcvData(9) > 25 THEN Bwd
    · IF RCVData(7) < 60· AND RcvData(9) > 25 THEN Fwd
    ENDIF
    ' Move and avoid logic
    IF Function = 4 THEN
    · IF RCVData(3) > 50 AND RcvData(9) > 20 THEN Fwd
    · IF RCVData(3) < 50 THEN BackUp
    · IF RcvData(9) <= 20 THEN BackUp
    ENDIF
    ' Adaptive tracking logic
    IF Function = 5 THEN
    · IF RCVData(2) > 65 AND RcvData(9) > 25 THEN RotateLeft
    · IF RCVData(2) < 25 AND RcvData(9) > 25 THEN RotateRight
    · IF RCVData(7) < 60 AND RcvData(9) > 25 THEN Fwd
    · IF LostTrack > 6 THEN TW
    · LostTrack = LostTrack + 1
    · IF RcvData(9) > 25 THEN LostTrack = 0
    ENDIF
    ' Line Following logic
    IF Function = 6 THEN
    · IF RCVData(2) > 50 AND RcvData(9) > 5 THEN TurnLeft ' 45
    · IF RCVData(2) < 40 AND RcvData(9) > 5 THEN TurnRight ' 45
    · IF RCVData(2) >= 40 AND RCVData(2) =< 50 AND RcvData(9) > 5 THEN Fwd
    ENDIF
    ' Finger point logic
    IF Function = 7 THEN
    · IF RCVData(2) > 60 AND RcvData(9) > 25 THEN RotateRight ' Centroid X
    · IF RCVData(2) < 30 AND RcvData(9) > 25 THEN RotateLeft ' Centroid X
    · IF RCVData(3) > 30 AND RcvData(9) > 15 THEN Bwd ' Centroid Y
    ENDIF
    ' Color logic
    IF Function = 8 THEN
    · SEROUT 7, 84, [noparse][[/noparse]"TC 30 240 16 30 16 30",CR] ' Red
    · SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
    · IF RcvData(9) > 20 THEN
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]195] ' All Red LED's on
    ··· PAUSE 500
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
    · ENDIF
    · SEROUT 7, 84, [noparse][[/noparse]"TC 16 40 30 240 90 170",CR] ' Green
    · SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
    · IF RcvData(9) > 20 THEN
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]24] ' All Green LED's on
    ··· PAUSE 500
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
    · ENDIF
    · SEROUT 7, 84, [noparse][[/noparse]"TC 200 240 200 240 16 50",CR] ' Yellow
    · SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
    · IF RcvData(9) > 20 THEN
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]36] ' All Yellow LED's on
    ··· PAUSE 500
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
    · ENDIF
    ENDIF
    · ' Flash LED's to show bottom of tracking loop
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]129] ' 100000001
    GOTO TopTrack

    ' Calibrate CMUcam1 for lighting conditions
    Calibrate:
    · ' Reset CMUcam1
    · SEROUT 7, 84, [noparse][[/noparse]"RS",CR]
    · PAUSE 200
    · ' Turn on auto adjust
    · SEROUT 7, 84, [noparse][[/noparse]"CR 18 44",CR]
    · PAUSE 100
    · ' Delay loop for CMUcam1 to auto adjust to lighting conditions
    · FOR N = 2 TO 255
    ··· PAUSE 100
    ··· SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]DCD (N)]
    ··· GOSUB GetButton ' Check for stop button pressed
    ··· IF Switch = 2 THEN EXIT ' Switch 2 is stop button
    · NEXT
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' 000000000
    · ' Turn off auto adjust
    · SEROUT 7, 84, [noparse][[/noparse]"CR 18 44 19 32",CR]
    · PAUSE 100
    · GOSUB ConfigureCMUcam
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]1] ' 100000000
    · FREQOUT 10, 400, 2500
    · PAUSE 300
    · FREQOUT 10, 400, 2500
    GOTO ProgTop

    SaveColor:
    ' sample and save color
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0]
    · SEROUT 7,84,[noparse][[/noparse]"PM 1",CR]
    · PAUSE 100
    · ' Set a small window in center
    · SEROUT 7,84,[noparse][[/noparse]"SW 30 54 50 90",CR]
    · PAUSE 100
    · ' Set raw mode 3
    · SEROUT 7,84,[noparse][[/noparse]"RM 3",CR]
    · PAUSE 100
    · 'set minimun and maximun values
    · SaveColorData(0)=16
    · SaveColorData(1)=240
    · SaveColorData(2)=16
    · SaveColorData(3)=240
    · SaveColorData(4)=16
    · SaveColorData(5)=240
    · ' Get Mean
    · SEROUT 7,84,[noparse][[/noparse]"GM",CR]
    · SERIN 9,84, [noparse][[/noparse]STR rcvdata\8]
    · PAUSE 200
    · IF rcvdata(2)<=46 THEN gm1
    ··· SaveColorData(0)=(rcvdata(2)-30)
    · gm1:
    · IF rcvdata(3)<=46 THEN gm2
    ··· SaveColorData(2)=(rcvdata(3)-30)
    · gm2:
    · IF rcvdata(4)<=46 THEN gm3
    ··· saveColorData(4)=(rcvdata(4)-30)
    · gm3:
    · IF rcvdata(2)>210 THEN gm4
    ··· saveColorData(1)=(rcvdata(2)+30)
    · gm4:
    · IF rcvdata(3)>240 THEN gm5
    ··· saveColorData(3)=(rcvdata(3)+30)
    · gm5:
    · IF rcvdata(4)>240 THEN gm6
    ··· saveColorData(5)=(rcvdata(4)+30)
    · gm6:
    · ' Set raw mode to 0
    · SEROUT 7,84,[noparse][[/noparse]"RM 0",CR]
    · PAUSE 100
    · ' Return window to full size
    · SEROUT 7,84,[noparse][[/noparse]"SW",CR]
    · ' Write color tracking range values to end of EEPROM
    · WRITE 0,saveColorData(0)
    · WRITE 1,saveColorData(1)
    · WRITE 2,saveColorData(2)
    · WRITE 3,saveColorData(3)
    · WRITE 4,saveColorData(4)
    · WRITE 5,saveColorData(5)
    · GOSUB ConfigureCMUcam
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]2]
    · ' Two beeps
    · FREQOUT 10, 400, 2500
    · PAUSE 400
    · FREQOUT 10, 400, 2500
    · PAUSE 400
    GOTO ProgTop

    Avoid:
    · GOSUB ConfigureCMUcam
    · SEROUT 7,84,[noparse][[/noparse]"TW",CR]
    · PAUSE 1000 ' Wait 1 second for TW
    GOTO TopTrack

    Adapt:
    · GOSUB ConfigureCMUcam
    TW:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]255] ' All LED's on
    · FREQOUT 10, 150, 2500 ' Beep
    · PAUSE 100
    · FREQOUT 10, 150, 2500 ' Beep
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
    · SEROUT 7,84,[noparse][[/noparse]"TW",CR] ' Send Track Window command
    · PAUSE 1000 ' Wait 1 second for TW to complete
    · LostTrack = 0 ' Reset LostTrack counter
    GOTO TopTrack

    FollowLine:
    · GOSUB ConfigureCMUcam
    · SEROUT 7,84,[noparse][[/noparse]"TC 16 50 16 50 16 50",CR]' Black line color
    · PAUSE 100
    GOTO Toptrack

    Point:
    · GOSUB ConfigureCMUcam
    · SEROUT 7,84,[noparse][[/noparse]"TC 200 240 180 240 50 100",CR] ' Skin color
    · PAUSE 100
    GOTO Toptrack

    Color:
    · GOSUB ConfigureCMUcam
    GOTO Toptrack

    ConfigureCMUcam:
    · SEROUT 7,84,[noparse][[/noparse]"PM 1",CR]' Poll Mode
    · PAUSE 100
    · SEROUT 7,84,[noparse][[/noparse]"RM 3",CR]' Raw Mode
    · PAUSE 100
    · SEROUT 7,84,[noparse][[/noparse]"CR 32 8",CR]' White Balance smart mode
    · PAUSE 100
    RETURN

    BackUp:
    · ' backup
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]195]
    · FOR n = 1 TO 15
    ··· PULSOUT 14,1000 ' **
    ··· PULSOUT 15,500 ' **
    ··· PAUSE 15
    · NEXT
    · PAUSE 200
    · ' rotate about 90 degrees
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]32]
    · FOR n = 1 TO 100
    ··· PULSOUT 14,1000 ' **
    ··· PULSOUT 15,1000 ' **
    · NEXT
    GOTO TopTrack
    RotateLeft:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]4]
    · FOR n = 1 TO 10
    ··· PULSOUT 14,500 ' **
    ··· PULSOUT 15,500 ' **
    · NEXT
    GOTO TopTrack
    RotateRight:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]32]
    · FOR n = 1 TO 10
    ··· PULSOUT 14,1000 ' **
    ··· PULSOUT 15,1000 ' **
    · NEXT
    GOTO TopTrack
    TurnLeft:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]4]
    · FOR n = 1 TO 40
    ··· PULSOUT 14,500 ' **
    ··· PULSOUT 15,750 ' **
    · NEXT
    GOTO TopTrack
    TurnRight:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]32]
    · FOR n = 1 TO 40
    ··· PULSOUT 14,750 ' **
    ··· PULSOUT 15,1000 ' **
    · NEXT
    GOTO TopTrack
    Bwd:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]195]
    · FOR n = 1 TO 10
    ··· PULSOUT 14,1000 ' **
    ··· PULSOUT 15,500 ' **
    ··· PAUSE 15
    · NEXT
    GOTO TopTrack
    Fwd:
    · SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]24]
    · FOR n = 1 TO 10
    ··· PULSOUT 14,500 ' **
    ··· PULSOUT 15,1000 ' **
    ··· PAUSE 15
    · NEXT
    GOTO TopTrack
Sign In or Register to comment.