'{$STAMP BS2} ' {$PBASIC 2.5} '********************************************************************************************************* '* Program: ant.BS2 Author:James Ross, Kristin Eicher '* Date: 4/5/08 Revision: 4.0 * 'gen ant code. '********************************************************************************************************* 'overall desc: '1) set up and init all vars '2) begin main loop. 'decide IF robot has found target(targetfound= 1) ' (to decide if found food, count the consecutive times the robot see a bright light) ' (if the robot see a bright light more then 5 times without seeing any low light ) ' (THEN target is found) ' if yes ' if scout -> send memory contens() else stop. ' ' else '3 read all sensors '4 evaluate ground ' to evaluate ground look at the prev. ground color and the current ground color ' if current ground is white and prev ground was white then continue ' if current ground is black and prev ground was black then continue ' if current ground is black and prev ground was white count # times robot sees black ' IF the ground is consecutive black 15 times THEN set Bit TO ground color = black ' reset total steps to 0 (robot will now search by default moving forward untill wall) ) ' if current ground is white and prev ground was black count consecutiv time sees white ' IF the ground is consecutive white 15 times THEN set Bit TO ground color = white ' reset total steps to 0 (robot will now search by default moving forward until wall' ' '5 if total steps are < 128 search by moving forward until hit a wall '6 if total steps are >128 and < 255 do left wall hug search '7 if total steps are > 255 do right wall hug search ' --------------------------------------------------------------------------------- ' when searching and robot gets stuck in a coner count number of times robot ' makes a left -> right -> left -> right... left -> right w/o moving forward. ' once this COUNT is > 5 consecutive r/l movements rotate 150. deg to find ' the least obstructed position and continue searching. choose to rotate ' right 150 deg. If robot first made a right turn when it started To GEt ' stuck else rotate left 150 deg. ' (memory contents will be) ' (the search method used to find the next ground color) ' (F= forward search found the change in floor color ' (L= left wall hug found the change in floor color ' (R= right wall hug search found the change in floor color) ' (memory contents examp) ' ' B = the food is on a black floor ' F ->first search by forward seach ' ,F -> 2nd seach by forward seach ' ,L -> 3rd seach by left wall hug ' ,F -> 4th seach by forward seach ' ,R -> 5th seach by right wall hug seach ' ,L -> 6th seach by Left wall hug seach ' F -> 7th seach by forward seach ' ' instead of using a map, it uses a seach technique to find the next color grnd ' 'white| black | white |black '-------------------------------- '| |.......| |......| '| |__.....| |---|......| '| .......... |End|......| '| .......... / |......| '| |........../____ |......| '| |........./ | .......| ' |________/ | .......| ' ----------- ' start 'to solve(best case).. there are other ways also '1) right wall hug to black '2) forward seach to white '3) right wall hug to black '4) left wall hug TO white '5) forward seach to target(taget on white ground) 'declares '******************************************************************************* 'program vars '=============================================================================== '=============================================================================== 'ping sensors wallDistRight VAR Word 'left sonic wallDistLeft VAR Word 'right sonic 'memory Value VAR Byte(6) 'the memory 'movement rotateTrys VAR Nib 'rotation attemps forwardSteps VAR Nib 'F steps attempts rotateRight VAR Nib 'R movement attempts rotateLeft VAR Nib 'L movement attempts dir VAR Nib 'current direction for fixing if stuck in corner ndir VAR Nib 'new direction for fixing if stuck in corner isScout VAR Bit 'is this robot a scout 'light sensor lightSensor VAR Word 'to detect if light(food) is found whiteGroundCounter VAR Nib 'detect if ground color is white blackGroundCounter VAR Nib 'detect if ground color is black foodCounter VAR Nib 'count consecutive time see food 'change in light sensors foundTarget VAR Bit 'found the food bit groundChange VAR Bit 'Ground color changed 1= white 0 = black preGroundChange VAR Bit 'prev ground color 'gen vars genVarX VAR Nib 'general variable genVarY VAR Nib 'general variable counter VAR Nib 'gen counter index VAR Nib 'gen index var 'send receve vars Checksum VAR Byte 'Calculated Checksum PacketCount VAR Byte 'Packet Number and Data Value Count globalCounter VAR Word 'step counter generalCounter VAR Byte Packet VAR Nib 'Packet Number DCount VAR Nib 'Data count memPtr VAR Nib 'map memory pointer '---------------------------------------------------------------------------------- '---------------------assignments--------------------------------------------------- 'rf const Rx CON 1 'Receive I/O pin number Tx CON 0 'Transmit I/O pin number N9600 CON $4054 'Baud mode value for 9600 baud, 8,N,1 TxFlow CON 2 rotateTrys = 0 globalCounter = 0 generalCounter = 0 isScout = 0 '1 = do scout no help: 0 = worker ant with data from scout Value(0) = "f" 'the Map Value(1) = "f" Value(2) = "f" Value(3) = "f" Value(4) = "f" Value(5) = "f" memPtr = 1 'is this a follower ant(if yes listen for instructions) IF(isScout= 0) THEN GOTO waitForInstructions ENDIF '------------------------------------------------------------------------------------ 'Controller '******************************************************************************* 'basic control operation of robot '=============================================================================== '=============================================================================== Main: 'start by reading the sensors GOSUB search IF isScout = 1 THEN 'this is a follower ant; it will try using the 'map' GOSUB travelFollow ELSE 'this is a scout ant try to find the targe with no 'map' GOSUB travelSearch ENDIF GOTO main END 'logic model: '****************************************************************************** 'moveing, getting unstuck R/L, determining food source, 'determin progress To target, search methods, '=============================================================================== '=============================================================================== 'read all censors and evaluate Search: 'get the distance from the walls GOSUB readSonarRight: GOSUB readSonarLeft: 'check ground color GOSUB readGroundSensor GOSUB evaluateGround 'check if the front sensor see food(bright light source) GOSUB lookForFood: RETURN '=============================================================================== '=============================================================================== 'determin next travel method '(forward search default, left wall hug if progress not being made then right wall hug) travelSearch: 'move back if too close to wall(2 inches minimum) 'IF (wallDistRight < 2 OR wallDistLeft < 2) THEN 'GOSUB Move_B 'ENDIF 'move left or right if the left or right side is to close to a wall 'IF (wallDistLeft < 3 AND wallDistRight > 3) THEN GOSUB Move_R 'IF (wallDistLeft > 3 AND wallDistRight < 3) THEN GOSUB Move_L 'count each iteration throught the travelSearch function (reset after 383 iterations [128 * 3]) 'globalCounter = globalCounter + 1 IF(globalCounter > 383) THEN globalCounter = 0 ENDIF 'the search IF isScout = 1 THEN 'if this is a scout ant 'choose forwardSearch,leftWallHug OR rightWallHug AND 'update 'map'(1 time everytime gnd color changes) 'forward search IF globalCounter < 128 THEN GOSUB forwardSearch Value(memPtr) = "f" ENDIF 'left wall hug travel IF globalCounter >= 128 AND globalCounter < 255 THEN GOSUB hugRightWallSearch: Value(memPtr) = "l" ENDIF 'right wall hug travel IF globalCounter >= 255 THEN GOSUB hugLeftWallSearch Value(memPtr) = "r" ENDIF GOSUB evaluateLastMove: ENDIF RETURN '=============================================================================== '=============================================================================== 'determin next travel method '(forward search default, left wall hug if progress not being made then right wall hug) travelFollow: 'move back if too close to wall(2 inches minimum) IF (wallDistRight < 3 OR wallDistLeft < 3) THEN GOSUB Move_B IF (wallDistRight < 3 AND wallDistLeft < 3) THEN GOSUB Move_B GOSUB Move_B GOSUB Move_B GOSUB Move_B GOSUB Move_B ENDIF 'move left or right if the left or right side is to close to a wall IF (wallDistLeft < 3 AND wallDistRight > 3) THEN GOSUB Move_R IF (wallDistLeft > 3 AND wallDistRight < 3) THEN GOSUB Move_L 'count each iteration throught this function (reset after 128 iterations ) 'try this search method for 128 steps before giving up IF globalCounter > 128 THEN globalCounter = 0 memPtr = memPtr + 1 ENDIF 'select the search method SELECT Value(memPtr) CASE "f" GOSUB forwardSearch CASE "l" GOSUB hugRightWallSearch: CASE "r" GOSUB hugLeftWallSearch CASE ELSE 'junk data don't know what to do look at the next instruction memPtr = memPtr + 1 ENDSELECT 'look at last movment GOSUB evaluateLastMove: 'if did not find the target after 6 instructions give up (follower-to->scouts) IF memPtr > 6 THEN isScout= 1 ENDIF RETURN '=============================================================================== '=============================================================================== 'search by always moving forward until can't move forwardSearch: 'always move back IF about TO hit a wall IF (wallDistRight <= 2 OR wallDistLeft <= 2) THEN IF(wallDistRight < wallDistLeft) THEN DO WHILE wallDistLeft < 5 AND wallDistRight <5 GOSUB readSonarleft GOSUB readSonarRight GOSUB Move_R LOOP ENDIF IF(wallDistRight > wallDistLeft) THEN DO WHILE wallDistRight < 5 AND wallDistLeft <5 GOSUB readSonarRight GOSUB readSonarleft GOSUB Move_L LOOP ENDIF IF(wallDistRight = wallDistLeft) THEN DO WHILE wallDistLeft < 5 AND wallDistRight <5 GOSUB readSonarleft GOSUB readSonarRight GOSUB Move_R LOOP ENDIF ENDIF 'too close to the right wall move away and count times I had to move away from right wall IF(wallDistRight < 3 AND wallDistleft > 3) THEN GOSUB Move_R rotateLeft = rotateLeft + 1 rotateTrys= rotateTrys+1 forwardSteps = 0 ENDIF 'too close to the left wall move away and count times I had to move away from left wall IF(wallDistLeft < 3 AND wallDistRight >3 ) THEN GOSUB Move_L rotateRight = rotateRight + 1 rotateTrys= rotateTrys+1 forwardSteps = 0 ENDIF 'not close to left or right wall. move forward and reset rotation counter IF(wallDistRight >=3 AND wallDistLeft >=3 ) THEN rotateTrys = 0 forwardSteps = forwardSteps +1 GOSUB move_F ENDIF RETURN '=============================================================================== '=============================================================================== 'search by following the right wall hugRightWallSearch: IF(wallDistLeft <=3 ) THEN GOSUB move_l ENDIF IF(wallDistLeft >=7 ) THEN GOSUB move_R ENDIF IF(wallDistLeft => 2 AND wallDistRight => 2 ) THEN GOSUB move_f ENDIF IF(wallDistLeft <=2 OR wallDistRight <=2 ) THEN DO WHILE wallDistRight < 5 AND wallDistLeft <5 GOSUB readSonarRight GOSUB readSonarleft GOSUB Move_L LOOP ENDIF GOSUB move_f RETURN '=============================================================================== '=============================================================================== 'search by following the left wall hugLeftWallSearch: IF(wallDistRight <=3 ) THEN GOSUB move_R ENDIF IF(wallDistRight >=7 ) THEN GOSUB move_L ENDIF IF(wallDistRight => 2 AND wallDistLeft => 2 ) THEN GOSUB move_F ENDIF IF(wallDistRight <=2 OR wallDistLeft <=2 ) THEN DO WHILE wallDistLeft < 5 AND wallDistRight <5 GOSUB readSonarRight GOSUB readSonarleft GOSUB Move_R LOOP ENDIF GOSUB move_F RETURN '=============================================================================== '=============================================================================== 'look For food lookForFood: GOSUB readTopFoodSensor 'did the robot see any food(detect the light) IF(lightSensor <= 30 ) THEN foodCounter = 0 folowLight: GOSUB readSonarRight GOSUB readSonarLeft GOSUB readTopFoodSensor 'params genVarX = 1 'times TO sweep R AND L genVarY = 1 'passed or failed to find the light 'food source found let the other robots know how I got here IF (lightSensor <=1 ) THEN GOSUB SendMessage END ENDIF IF lightSensor > 30 THEN 'lost the light try to find it again GOSUB findLight: ELSE 'follow the light GOSUB motionFindLightFix ENDIF 'keep tack of # times failed to find the light IF(genVarY = 0) THEN foodCounter = foodCounter +1 ELSE foodCounter = 0 ENDIF 'failed to find the light 4 times go back to other search methods IF foodCounter > 4 THEN RETURN ENDIF GOTO folowLight ENDIF RETURN '=============================================================================== '=============================================================================== 'determin if last move was successful(is robot stuck?) evaluateLastMove: 'did i rotate more then 8 times without going forward? IF (rotateTrys > 8) THEN rotateTrys = 0 GOSUB readSonarRight: GOSUB readSonarLeft: 'stuck and saw right wall last IF(rotateRight > rotateLeft) THEN IF(wallDistRight < 5) THEN GOSUB findNewHeadingRight ENDIF 'stuck and saw left wall last ELSE IF(wallDistLeft < 5) THEN GOSUB findNewHeadingLeft ENDIF ENDIF ENDIF RETURN '=============================================================================== '=============================================================================== 'stuck on left side rotate and try to find new direction on left findNewHeadingLeft: dir = 0 'move right and look for the direction where the distance 'of a wall is the furthest from the right ping sensor FOR counter = 1 TO 8 GOSUB Move_R GOSUB readSonarRight IF dir < wallDistRight THEN dir = wallDistRight ndir = index ENDIF NEXT 'the area has been scouted the new direction is to rotate (8 - ndir) times from current position FOR counter = 2 TO (8 - ndir) GOSUB Move_L GOSUB readSonarRight IF(dir < wallDistRight-10) OR (dir > wallDistRight+10) THEN RETURN NEXT RETURN '=============================================================================== '=============================================================================== 'stuck on right side rotate and try to find new direction on right findNewHeadingRight: dir = 0 'move left and look for the direction where the distance 'of a wall is the furthest from the left ping sensor FOR counter = 0 TO 8 GOSUB Move_L GOSUB readSonarLeft IF dir < wallDistLeft THEN dir = wallDistLeft ndir = index ENDIF NEXT 'the area has been scouted the new direction is to rotate (8 - ndir) times from current position FOR counter = 2 TO (8 - ndir) GOSUB Move_R GOSUB readSonarLeft IF(dir < wallDistLeft-10) OR (dir > wallDistLeft+10) THEN RETURN NEXT RETURN '=============================================================================== '=============================================================================== 'used to determin if progress is being made in the travel evaluateGround: 'keep track of the ground color on last evaluation preGroundChange = groundChange 'prevent false reading on ground color '(COUNT times I see a single color before deciding the reading is true) 'does bottom light senesor see a dark color? IF(lightSensor > 320 ) THEN 'count times I see the dark ground and reset the whiteGroundCounter blackGroundCounter = blackGroundCounter + 1 whiteGroundCounter = 0 ENDIF 'does bottom light senesor see a light color? IF(lightSensor <= 320 ) THEN 'count times I see the light ground and reset the blackGroundCounter whiteGroundCounter = whiteGroundCounter + 1 blackGroundCounter = 0 ENDIF 'I have read the ground color is black 15 times in a row[the ground color is now black] IF(blackGroundCounter > 15) THEN groundChange = 1 ENDIF 'I have read the ground color is white 15 times in a row[the ground color is now white] IF(whiteGroundCounter > 15) THEN groundChange = 0 ENDIF 'has the ground color changed from the last iteration? IF preGroundChange <> groundChange THEN 'reset the globalCounter and incrament the map memory pointer globalCounter = 0 memPtr = memPtr +1 ENDIF RETURN '=============================================================================== '=============================================================================== 'rotates left and right to try to find the light again findLight: '1=(true)-found the light genVarY = 1 'try right side for light(move right genVarX times) FOR counter = 0 TO genVarX 'check TO see IF I see a BRIGHT light after rotating right) GOSUB Move_R GOSUB readTopFoodSensor IF lightSensor < 30 THEN 'yes see the light now(returns with genVarY = 1) RETURN ENDIF NEXT 'did not find on last try now try left side FOR light FOR counter = 0 TO 3*genVarX 'check TO see IF I see a BRIGHT light after rotating left GOSUB Move_L GOSUB readTopFoodSensor IF lightSensor < 30 THEN 'yes see the light now(returns with genVarY = 1) RETURN ENDIF NEXT 'failed to find the light (returns with genVarY = 0) 'rotate robot to previous dierection genVarY = 0 FOR counter = 0 TO genVarX GOSUB Move_R NEXT RETURN '=============================================================================== '=============================================================================== 'keep from running into walls when following light motionFindLightFix: GOSUB forwardSearch RETURN '=============================================================================== '=============================================================================== 'low level operations '******************************************************************************* ' basic operations: send/receve, move f/l/r/b, 'pingSensor R/L, grnd Censor T/B, memory read/write' '=============================================================================== '=============================================================================== readSonarRight: PULSOUT 5, 5 ' activate sensor PULSIN 5, 1, wallDistRight wallDistRight = wallDistRight /74 'DEBUG "distL ", DEC5 wallDistRight, CR RETURN '=============================================================================== '=============================================================================== readSonarLeft: PULSOUT 6, 5 ' activate sensor PULSIN 6, 1, wallDistLeft wallDistLeft = wallDistLeft /74 ' DEBUG "---distR ", DEC5 wallDistLeft, CR RETURN '=============================================================================== '=============================================================================== readTopFoodSensor: HIGH 7 PAUSE 2 RCTIME 7,1, lightSensor 'DEBUG "t7= ", DEC5 lightSensor, CR RETURN '=============================================================================== '=============================================================================== readGroundSensor: HIGH 4 PAUSE 2 RCTIME 4,1, lightSensor 'DEBUG "tl= ", DEC5 lightSensor, CR RETURN '=============================================================================== '=============================================================================== Move_F: FOR index = 1 TO 7 PULSOUT 12, 700 PULSOUT 13, 800 NEXT RETURN '=============================================================================== '=============================================================================== Move_B: FOR index = 1 TO 14 PULSOUT 12, 800 PULSOUT 13, 700 NEXT RETURN '=============================================================================== '=============================================================================== Move_R: FOR index = 1 TO 7 PULSOUT 12, 765 PULSOUT 13, 765 PAUSE 20 NEXT RETURN '=============================================================================== '=============================================================================== Move_L: FOR index = 1 TO 7 PULSOUT 12, 725 PULSOUT 13, 725 PAUSE 20 NEXT RETURN '=============================================================================== '=============================================================================== SendMessage: 'This function reads the path data from memory, calculates the checksum for each value in the data path ' AND constructs the packets AND calls the transmit routine DCount = -1 'Initialize Data Count Packet = Packet + 1 'Increment packet number (1-15) Packet = Packet MIN 1 Checksum = Packet 'initialize checksum to packet number GetNext: DCount = DCount + 1 Checksum = Checksum ^ Value(DCount) 'Calculate current checksum IF DCount => 5 THEN MessageDone 'If max data size, send message now GOTO GetNext 'Get next data dirArray MessageDone: 'send out the map SEROUT Tx\TxFlow,N9600,[STR Value\DCount+1] GOTO MessageDone DEBUG "----sent this->", STR Value\DCount+1 ,CR RETURN '=============================================================================== '=============================================================================== 'the receved msg waitForInstructions: DEBUG "list___" 'WAIT 4 seconds before trying again SERIN Rx, N9600, 4000, main2,[STR Value\6] DEBUG "----rec this->", STR Value 'receved the map now follow it isScout= 1 GOSUB move_b GOTO Main main2: 'got nothing try again FOR msg GOTO waitForInstructions