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. 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.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

I also posted this in the basic stamp forum, but here seemed more appropiate.

Desy2820

I·don't have any specific advice for your program, just some general ideas and tips.

1.· Double check your wiring and connections.· For example, your servos should connect to·P12(left) and P13 (right).· Your whiskers should connect to·P5 and P7.· The IR LEDs should connect to·P8 and P2.· The IR recievers·should connect to·P0 and P9.· You may want to use·PIN to assign meaninful·names to the pins, for example·"IRINLEFT·PIN 9".· In the program, simply use "IRINLEFT", instead·of a pin assignment.· See the PBASIC help for more.

2.· Double check your batteries.· Four alkaline cells should measure around six volts at VIN to VSS.· Four NiCAD or NiMH will measure lower, about 4.8 volts.· If nothing else,·try a fresh set.

3.· Make sure that you have the whiskers wired correctly, with a resistor to VDD so the stamp "sees" a solid logic high when the whiskers aren't detecting.· Not having a "pull-up" resistor will cause the pins to "float" giving you odd and rapidly changing results.· Use the example sketch under the "Button" command in the PBASIC help.

4.· Add some more "DEBUG" commands to your program so you can see what is going on.· Use switch position one or unplug the servos and leave the serial cable connected after programming.· By adding·DEBUGs to show you inputs and calculation results·at different places in your program, you·can find errors. Try adding DEBUGs to your subroutines too, just to see what the robot is trying to do, for example "DEBUG "TURNING LEFT."· Remove or comment out all your debug commands once the program works correctly, as they can really slow it down.

5.· Your progam already has some DEBUG commands in it.· What result are you getting when you move the black and white sections by hand under the detectors?· Are they what you expect or think you should see?

6.· Double check the polarity on your LEDS.· If you get them backwards, they won't light up, so your detectors will never "see" any thing!

I hope this helps and gets your project moving forward!