Problems with a combined stripe following/whisker navigation program. Help?

accidentprone8

Okay, Im working on a school project which tasks us with creating a program that can follow a line and navigate around obstacles. To do this, my partner and I combined the stripe following with ir detectors·and roaming with whiskers program from the book robotics with the boe bot by andy lindsay. Heres the kicker. When we ran the program, the bot would move straight when not detecting,·turn left to follow the line and navigate obstacles, but it would not turn right to follow the line, which threw us off. We discovered that one of variables, pulseright, was a byte instead of a word. We fixed it. The result was devistating. The boe bot still navigates with the whiskers, and tries to follow the line by turning left, but for the most part turns in a·small right loop. I've been trying to fix it for weeks. Unless it hits something however, most variations just have it turning in that right loop. We've tried using different peoples boe bots, replacing every ir component, nothing works. I'm especially in a bind as my partner wont be here for the rest of the school year. The program follows below. Any advice or suggestions would be very much appreciated.



' Robotics with the Boe-Bot - StripeFollowingBoeBot.bs2
' Boe-Bot adjusts its position to move toward objects that are closer than
' zone 3 and away from objects further than zone 3.· Useful for following a
' 2.25 inch wide vinyl electrical tape stripe.
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
'

---

[noparse][[/noparse] Constants ]

---

Kpl··········· CON··· 35··············· ' Change from -35 to 35
Kpr··········· CON··· -35·············· ' Change from 35 to -35
SetPoint······ CON··· 2················ ' Change from 2 to 3.
CenterPulse··· CON··· 750
'

---

[noparse][[/noparse] Variables ]

---

freqSelect···· VAR·· Nib
irFrequency··· VAR·· Word
irDetectLeft·· VAR·· Bit
irDetectRight· VAR·· Bit
distanceLeft·· VAR·· Nib
distanceRight· VAR·· Nib
pulseLeft····· VAR·· Word
pulseRight···· VAR·· Word
pulseCount VAR Byte······································· 'FOR…NEXT loop counter.
'

---

[noparse][[/noparse] Initialization ]

---

FREQOUT 4, 2000, 3000·················· 'Signal program start/reset.
'

---

[noparse][[/noparse] Main Routine ]

---

DO
· GOSUB Get_Ir_Distances
· ' Calculate proportional output.
· pulseLeft =· SetPoint - distanceLeft· * Kpl + CenterPulse
· pulseRight = SetPoint - distanceRight * Kpr + CenterPulse
· DEBUG HOME, ? pulseLeft
· DEBUG ? pulseRight
· GOSUB Send_Pulse
IF (IN5 = 0) AND (IN7 = 0) THEN···························· 'Both whiskers detect obstacle
·GOSUB Back_Up··············································· 'Back up and U-turn (left twice)
·GOSUB Turn_Left
·GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN········································· 'Left whisker contacts
·GOSUB Back_Up················································ 'Back up and turn right
·GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN········································· 'Right whisker contacts
·GOSUB Back_Up················································ 'Back up and turn left
·GOSUB Turn_Left
ELSE·························································· 'Both whiskers 1, no contacts
·GOSUB Forward_Pulse·········································· 'Apply a forward pulse
ENDIF························································· 'and check again
LOOP
'

---

[noparse][[/noparse] Subroutine - Get IR Distances ]

---

Get_Ir_Distances:
· distanceLeft = 0
· distanceRight = 0
· FOR freqSelect = 0 TO 4
··· LOOKUP freqSelect, [noparse][[/noparse]37500,38250,39500,40500,41500], irFrequency
··· FREQOUT 8,1, irFrequency
··· irDetectLeft = IN9
··· distanceLeft = distanceLeft + irDetectLeft
··· FREQOUT 2,1, irFrequency
··· irDetectRight = IN0
··· distanceRight = distanceRight + irDetectRight
· NEXT
· RETURN
'

---

[noparse][[/noparse] Subroutine - Get Pulse]

---

Send_Pulse:
· PULSOUT 12,pulseLeft
· PULSOUT 13,pulseRight
· PAUSE 5
· RETURN
· '

---

[noparse][[/noparse] Subroutines ]

---

Forward_Pulse:······························ ' Send a single forward pulse.
· PULSOUT 13, 850
· PULSOUT 12, 650
· PAUSE 20
· RETURN
Turn_Left:
· FOR pulseCount = 0 TO 20·················· ' Left turn, about 90 degrees
··· PULSOUT 13, 650
··· PULSOUT 12, 650
··· PAUSE 20
· NEXT
· RETURN
Turn_Right:
· FOR pulseCount = 0 TO 20·················· ' Right turn, about 90 degrees
··· PULSOUT 13, 850
··· PULSOUT 12, 850
··· PAUSE 20
· NEXT
· RETURN
Back_Up:····································· ' Back up.
· FOR pulseCount = 0 TO 40
··· PULSOUT 13, 650
··· PULSOUT 12, 850
··· PAUSE 20
· NEXT
· RETURN

accidentprone8

Also, when we hold the boe bot in the air, it seems to move straight, but as soon as the ir hits a surface it turns right. It probably a tuning problem, but seeing as we have replaced the sensors, it doesnt make much sense, and nothing in the constants I've tried to do seems to do the trick.