Let's Make Robots!

start here bot with servos

Trying to create code to run a start here robot with 2 servos for driving wheels and a ping sensor with a picaxe 08m2, it should be know by all I cannot even program a vcr, I can blink an led tho so there may be some hope for me yet. this program makes for some crazy behavior that is out of control. Not sure if I am not naming stuff correctly or what.I know ounce I get going I will need to tweek values. Been at this for some time like 3 3 hours sesions just to get this far.

'Mini-Skybot
'ossipee

symbol motorR = 1 ; call pin for right motor
symbol motorL = 2 ; call pin for left motor
symbol motorRC = 150 ; stop motor
symbol motorLC = 150 ; stop motor
symbol trigger = 0 ; call pin that send pulse
symbol echo = 4 ; call pin that gets pulse back
symbol distance = w1 ; 16 bit variable for range
symbol dangerL = 30 ; time to look each way choose

servo motorR,motorRC ; set pins as servo outputs
servo motorL,motorLC

pause 3000 ; wait 3 seconds to start

main:
pulsout trigger,2 ; sends 20us pulse out to ping
pulsin echo,3,distance ; measures range in 10us steps
pause 10 ; ping needs recharge
readadc 4,w1 ; measure for safe distance
if w1 < dangerL then ; safe go forward
gosub nodanger ; go forward
else
gosub whichway ; look how to go
endif
goto main

nodanger:
servopos motorR,75
servopos motorL,225
return


whichway:
gosub totalhalt ; stop everything for crash
gosub turnleft ; turn left
pause 1000 ; stop for a bit
readadc 1,w1 ; take a peek is it safe
return ; go back to start
gosub turnright ; turn right
pause 1000 ; stop for a bit
readadc 1,w2 ; take a peak
gosub totalhalt
if w1<w2 then
gosub turnright
else
gosub turnleft
endif
return

turnleft:
servopos motorR, 75
servopos motorL, 75
return

turnright:
servopos motorR, 75
servopos motorL, 225
return

totalhalt:
servopos motorR, 150
servopos motorL, 150
pause 1000
return

any help or links would be appreaciated

 

AttachmentSize
2013-03-21_01.20.54.jpg1.9 MB

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.
Maxhirez's picture

This code works on Maus in the configuration I mentioned.  It's pretty much what you'd expect-goes forward until it senses an obstacle, then turns left and right and gets distance readings, compares them and goes forward in the direction that has the most distant obstacle then starts all over again.  

 

'Mini-Skybot 

'ossipee

 

symbol motorR = 1 ; call pin for right motor

symbol motorL = 2 ; call pin for left motor

symbol motorRC = 145 ; stop motor

symbol motorLC = 147 ; stop motor

symbol trigger = 0 ; call pin that send pulse

symbol echo = 4 ; call pin that gets pulse back

symbol tm = w1 ;variable for microseconds read off of HCSR04

symbol inches = w2; variable for converting microseconds to distance, about 15 us/in

symbol dangerL = 7 ;arbitrary safe distance to obstacles-make it lower than you'd think

symbol leftObstacle = w3; variable to store distance  read when looking left

symbol rightObstacle = w4; variable to store distance  read when looking right

 

servo motorR,motorRC ; set pins as servo outputs

servo motorL,motorLC

 

pause 3000 ; wait 3 seconds to start

 

main:

gosub myHCSR04

if inches > dangerL then ; safe go forward

gosub nodanger ; go forward

else

gosub whichway ; look how to go

endif

goto main

 

myHCSR04:

pulsout C.0,1; sends 20us pulse out to ping

pulsin C.4,1,tm ; measures range in 10us steps

inches = tm/15

pause 100

return

 

nodanger:

servopos motorR,225

servopos motorL,75

return 

 

 

whichway:

gosub totalhalt ; stop everything for crash

gosub turnleft ; turn left

pause 780; for 3/4 second or 90 degrees left depending on your servos

gosub totalhalt; stop

gosub myHCSR04;  to take reading

leftObstacle = inches;

gosub turnright ; turn right

pause 1500 ; for 1.5 seconds or 180 degrees right depending on your servos

gosub totalhalt; stop

gosub myHCSR04; to take reading

rightObstacle = inches

if leftObstacle > rightObstacle then; if the left obstacle is more distant than the right

gosub turnleft; go back around to the left

pause 1560; for 1.5 seconds or 180 degrees

else 

return; no need to turn if the right obstacle is more distant

endif

return

 

turnleft:

servopos motorR, 75

servopos motorL, 75

return

 

turnright:

servopos motorR, 225

servopos motorL, 225

return

 

totalhalt:

servopos motorR, 145

servopos motorL, 147

return

 

Maxhirez's picture
This really just echoes what Andy said in a way, but the Ping is a digital sensor-I'm guessing that the readadc calls in main and whichway are left over from the infra red sensor version of the SHR. In your main if statement the comparison should probably be between the variables distance and dangerL rather than w1 and dangerL as a result-who knows what the ADC will read the values coming off the echo pin as (5v? 0v? Some almost PWM value in between?). DangerL in this case should be set to a value in microseconds that corresponds to the minimum reflectance distance you want the bot to get to any object before calling totalhalt and whichway. Then in whichway, you need to get the ultrasonic readings again, which as Andy pointed out will be much easier with a subroutine. I have Maus rigged with an 08M2 and an SR05 at the moment just because I want to try this! It uses practically the same set up. If I get it going once my tract starts cooperating again I'll let you know!
AndyGadget's picture

Rather than writing the whole program on one go, test each part individually first and then build the code up into the behaviour you want. For example :-

(Variable & pin definitions at the top)

do

pulsout trigger,2 ; sends 20us pulse out to ping
pulsin echo,3,distance ; measures range in 10us steps

debug ' Show values on a window in PE.

wait 1

loop

You can test each motor in the same way, then build them up into the 'turn' routines. (Using values only slightly higher or lower than the centre value will give you some speed control.)  Once you're happy with a section you can easily ignore it with the #REM / #ENDREM directives to commant out a block of code.

AndyGadget's picture

Are your servos actually stationary at 150?  If you've modified them yourself you may need to tweak those values a bit.

If you're using a Ping sensor, you don't need the ADC statement to get the range.  The pulse-width returned in your variable 'distance' is proportional to the range.  Use that to check your distance.

I suggest you make a subroutine like this :

GetRange:

pulsout trigger,2 ; sends 20us pulse out to ping
pulsin echo,3,distance ; measures range in 10us steps

return

and use 'gosub GetRange' each time you need to do a distance measurement.

Also check out the 'debug' command which will allow you to see the value of variables as the program runs

Also. shouldn't your 'turnleft' routine be 'servopos motorR, 225' not 75?

Andy.

also, you need to put in a couple more 'if - then' statements in your 'whichway' routine as it will never get past the 'return ; go back to start' line.

got to go now but I'll take a closer look later.