A Tail-Dragger...

****Update 1.0*******

I have discovered that with the sensors all the way to the front, the robot had a hard time figuring out where it's "corners" were... I have moved them back with great results. Also I ammended the code to add a "slide" feature. Instead of one wheel forward and one reverse, one motor runs forward while the other is off. I am still working on an encoder (with the help of cowgod and Jimmy --thanks) to tell the robot how far it has turned or how far it has gone. I have updated the video and included the audio this time. --Also at the bottom of this post I have included the code I wrote. -Please tell me what is wrong with my code so I can improve it!

******Original Post******** 

So I wanted to make a 3 Wheel... and I also wanted to write my own code... So I did!

After trying, with little success, to hack into Fritz's standard code I have written my own. I am still very confused with all the sub-routines and return paths, I guess I will figure them out eventually. For now, this robot is controlled with a very simple system, check all three sensors and drive forward if they are cool. If one of the IR sensors gets tripped turn away. If for some reason the IR's miss something and the SFR05 gets too close to something the robot will stop, back up a little then do a L-C-R look around. From there it will simply turn in the direction that is farthest away. From there it will continue with its basic drive-around rules. The code works quite well, actually.

In the near future, I plan to add some white and black stripes on the drive gears and also install two line-follow sensors. This should allow me to actually count revolutions and therefore know exactly how far the robot has gone and how much it has turned.

Oh yeah, one more...

If you check out this blog post:

https://www.robotshop.com/letsmakerobots/node/591

I now know EXACTLY what I am going to do with it... Ha Ha! --It's gunna be awesome!

The Code: 

symbol trig = 5
symbol echo = 1
symbol range = w3
symbol spintime = 20
symbol backtime = 20
symbol sonardanger = 70
symbol chillandlook = 100
symbol danger = 50

main:
readadc 1,w4
readadc 2,w5
pulsout trig,1
pulsin echo,1,range
pause 10
if range =< sonardanger then goto back
if w4 > danger then goto Leftturn
if w5 > danger then goto Rightturn
if w4 < danger and w5 < danger then goto driveahead
goto main

Back:
low 2 : low 3
pause 250
for b4 = 1 to backtime step 1
servo 2,200
pause 10
servo 3,75
pause 10
next b4
low 2 : low 3
servo 4,75
pause 500
pulsout trig,1
pulsin echo,1,w6 ' look right
pause 10
servo 4,175
pause 500
pulsout trig,1
pulsin echo,1,w7 ' look left
pause 10
servo 4,120
pause 150
low 4
if w6 > w7 then goto goright
if w7 > w6 then goto goleft


Driveahead:
servo 2,75
pause 3
servo 3,180
pause 3
goto main



Leftturn:
readadc 1,w4
select case w4
case < danger
goto Driveahead
case 50 to 60
goto slideleft
case 61 to 120
goto Sharp_Lsub
case > 120
goto Back
endselect


Rightturn:
readadc 2,w5
select case w5
case < danger
goto Driveahead
case 50 to 60
goto slideright
case 61 to 120
goto Sharp_Rsub
case > 120
goto Back
endselect

Sharp_Lsub:
servo 2,75
pause 3
servo 3,75
pause 3 ' Spin left
goto main

Sharp_Rsub:
servo 2,200
pause 3
servo 3,200
pause 3 ' Spin right.
goto main

slideright:
low 2
servo 3,200
pause 3
goto main

slideleft:
low 3
servo 2,75
pause 3
goto main


goright:
for b4 = 1 to spintime step 1
servo 2,200
pause 3
servo 3,200
pause 3 ' Spin right.
next b4
goto main


goleft:
for b5 = 1 to spintime step 1
servo 2,75
pause 3
servo 3,75
pause 3 ' Spin left
next b5
goto main