View Full Version : Code help

11-21-2009, 11:54 AM
Hello, I am currently trying to interface two ping sonar sensors with two futaba servo motors. The end product should basically be a vehicle that can navigate obstacles. anyways the problem is my robot seems to want to do the jig and the wheels move for maybe a quarter second and then stop and then move and so the robot basically does nothing but spaz and waddle along. I have tested both sonars independently from the motors and the readings from them are fine. I have also run the motors in a simple do loop and they run fine. So there must be something wrong with my code. I would greatly appreciate if someone could look over my code and tell me if there appears to be a problem. THANKS!!

' {$STAMP BS2}
' {$PBASIC 2.5}
' -----[ I/O Definitions ]-------------------------------------------------
ping1 PIN 1
ping2 PIN 2
clock PIN 3
ToServo1 PIN 14
ToServo2 PIN 15
' -----[ Constants ]-------------------------------------------------------
Trigger CON 5 ' trigger pulse = 10 uS
Scale CON $2 ' raw x 2.00 = uS

RawToCm CON 2257 ' 1 / 29.034 (with **)

IsHigh CON 1 ' for PULSOUT
IsLow CON 0

Halt2 CON 763
Halt1 CON 760
Full2 CON 723
Full1 CON 805
Back2 CON 803
Back1 CON 715
' -----[ Variables ]-------------------------------------------------------
rawDist VAR Word ' raw measurement
rawDist2 VAR Word
cm VAR Word
cm2 VAR Word
Left1 VAR Word
Right2 VAR Word

' -----[ Main ]-----------------------------------------------------------
GOSUB DetermineNextMove
'-----[ Subroutines ]---------------------------------------------------
ping1 = IsLow
PULSOUT ping1, Trigger ' activate sensor
PULSIN ping1, IsHigh, rawDist ' measure echo pulse
rawDist = rawDist * Scale ' convert to uS
rawDist = rawDist / 2 ' remove return trip
cm = rawDist ** RawToCm ' convert to centimeters

ping2 = IsLow ' make trigger 0-1-0
PULSOUT ping2, Trigger ' activate sensor
PULSIN ping2, IsHigh, rawDist2 ' measure echo pulse
rawDist2 = rawDist2 * Scale ' convert to uS
rawDist2 = rawDist2 / 2 ' remove return trip
cm2 = rawDist2 ** RawToCm ' convert to centimeters

IF (cm > 100 AND cm2 > 100) THEN
GOSUB Both_full
ELSEIF cm2 < 15 THEN
ELSEIF cm > cm2 THEN
GOSUB Go_left
ELSEIF cm < cm2 THEN
GOSUB Go_Right

PULSOUT ToServo1, Full1 'sends fullspeed pulse
PULSOUT ToServo2, Full2 'sends fullspeed pulse

Left1 = cm - cm2
Left1 = Left1 / 2
Left1 = Full1 - Left1
PULSOUT ToServo1, Left1 'sends left turn pulse
PULSOUT ToServo2, Full2 'sends fullspeed pulse

Right2 = cm2 - cm
Right2 = Right2 / 2
Right2 = Full2 + Right2
PULSOUT ToServo1, Full1 'sends fullspeed pulse
PULSOUT ToServo2, Right2 'sends right turn pulse

PULSOUT ToServo1, Full1
PULSOUT ToServo2, Back2

PULSOUT ToServo1, Back1
PULSOUT ToServo2, Full2

Mike Green
11-21-2009, 12:13 PM
I think you're running out of time for the servo control pulses. Servo motors require a control pulse about 50 times a second. That's once every 20ms. If they don't get a control pulse in that time, they shut themselves off until the next pulse comes in and the result is a kind of stuttering movement. In addition, your "GETOUTOFTHECORNER" routines have a delay of 300ms which is way way longer than the servos will tolerate.

Your reading of each PING will take up to about 20ms, that's 40ms for the two of them. On top of that, you have a pause of 20ms in most of the movement subroutines which adds up to at least 60ms total per cycle. Add some milliseconds for the statements themselves to execute and you've got probably 70-75ms in most movement cycles.


1) Read only one PING per cycle through the main loop. Alternate them. Their readings won't change particularly quickly, so it won't matter.

2) Use the raw PING data to set the PAUSE delay. On a BS2, the width of the pulse is measured in 2us units. Divide it by 500 to get milliseconds and subtract from 20 to get the PAUSE delay like: PAUSE 20 - raw/500.

3) Use www.emesystems.com (http://www.emesystems.com) and the "app-notes" link at the bottom of the page. You'll get a nice discussion of a variety of information including information on PBasic statement execution time. You can use this to adjust the PAUSE value downwards to compensate for the statement execution time in your program.

11-21-2009, 12:59 PM
Do you mean switching them like this? That should work.


11-21-2009, 01:04 PM
I am using a basic stamp 2 parallax micro controller just so you know. I did the reversal. When the switch is on one the ping sensors act lights flash bright green but when the switch is on two the act lights are emitting a barely noticeable little twinkle. The motors are still jerky. I lowered all the pauses to 10 for now and the two motors are now spinning jerkily in opposite directions no matter where the sensors are pointed. I have no idea whats going.

11-21-2009, 02:09 PM
If you change something, please repost your full code as an attachment.

11-21-2009, 02:24 PM
should be attached

Mike Green
11-21-2009, 09:19 PM
1) If you're running off batteries, they may be exhausted. Try using a plug-in power source or at least fresh batteries.

2) The motors will continue to be jerky until you reduce the PAUSE times as I suggested. They may still be jerky when the PINGs don't see an obstacle. (When they're at maximum range, the time is close to 20ms.)

11-23-2009, 09:31 AM
Ok so now when I try and run it the ping sensor 1 flashes once then motor one moves forward for an instant and motor two goes backward for an instant than it does nothing. if i hold my hand in front of ping sensor1 it then it does the same as if I don't but it then gets stuck in the get out of the corner1 routine and jerkily spins. My power has been directly from the outlet all along

Mike Green
11-23-2009, 10:03 AM
You're modifying "rawDist" before using it for the Pauses. The whole idea of using that is to compensate in the PAUSE for the time used for the PULSIN. The way you have things now is that you're using only one PING per servo cycle, so Pausebreak has to reflect that. You've also applied Scale and a factor of 2, then you divide rawDist + rawDist2 by 100. That doesn't get you a value in milliseconds. Anyway, clean it up and get the units correct and the jerkiness may go away.

11-23-2009, 10:31 AM
Alright I think i Have done the Pausebreak part correctly. But it is still flashing once and then spinning in a circle how can you possibly explain that?

11-23-2009, 10:33 AM
And still jerky

Mike Green
11-23-2009, 11:06 AM
To initialize cm2 properly, you need to call ReadPING2 once before calling DetermineNextMove like this:

' -----[ Main ]-----------------------------------------------------------

I think it's time to use DEBUG to make sure various values (like cm and cm2) are what you expect them to be. It may be that there's just too much code executed each time through the loop and it's taking too long. Use the EmeSystems website information to estimate how long your various statements take. I would try the various motion subroutines to see if they actually move the BoeBot the way you expect, particularly Go_right and Go_left. I would rethink DetermineNextMove to make sure it's doing what you want.

When I have a complex program that doesn't do what I expect, I do what I've outlined ... Use some kind of DEBUG statement to make sure what's going on is what I think is supposed to happen. I also test small parts of the actual program, calling small subroutines from a modified main loop to make sure they're doing what they're supposed to be doing.

11-23-2009, 11:39 AM
Okay I did that. I tried this...

' -----[ Main ]-----------------------------------------------------------
GOSUB ReadPing2
GOSUB Readping1
GOSUB Go_right
Pausebreak= rawDist/500
Pausebreak= rawDist2/500

still choppy with Go_left Both_full or Go_right.

it was working before with the same code except with only the options of both full go left and go right in the determine next move routine and then for some reason it just stopped all of a sudden and started moving choppy and acting unpredictably. is it possible that my computer may be corrupting the micro controller in some way?? It was not doing anything like this before all we did was add two more possible subroutines to determine next move in order to try and make the movement smoother. I have tried simply removing those newer routines and it doesn't make any difference. It worked before as I said when the code was downloaded from my project partners computer. so what was working before does not work now and i have checked my connections I wonder if my computer has somehow damaged the micro controller..:( that would very unfortunate. Although I have tried using a test for my sonars and it works and when I use a separate program consisting of just motor commands in a loop the motors run fine too.

Mike Green
11-23-2009, 12:30 PM
Your computer can't damage the Stamp. It can't corrupt the Stamp. You can download a program that doesn't work, but the next download from the PC will overwrite what's in the EEPROM with the new program which hopefully works better.

I still think your program is running out of available time to produce servo control pulses. As I said, you have only about 20ms available between pulses to the servos and you're trying to do a lot in that time. That's one of the common causes for jerky servo movement.