Code help

micromang · 2009-11-21 04:54

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}
' -----[noparse][[/noparse] I/O Definitions ]-------------------------------------------------
ping1            PIN     1
ping2            PIN     2
clock            PIN     3
ToServo1         PIN     14
ToServo2         PIN     15
' -----[noparse][[/noparse] 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
' -----[noparse][[/noparse] Variables ]-------------------------------------------------------
rawDist         VAR     Word                    ' raw measurement
rawDist2        VAR     Word
cm              VAR     Word
cm2             VAR     Word
Left1           VAR     Word
Right2          VAR     Word

' -----[noparse][[/noparse] Main ]-----------------------------------------------------------
DO
  GOSUB ReadPING1
  GOSUB ReadPING2
  GOSUB DetermineNextMove
LOOP
'-----[noparse][[/noparse] Subroutines ]---------------------------------------------------
ReadPING1:
  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
  RETURN

ReadPING2:
  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
  RETURN

DetermineNextMove:
  IF (cm > 100 AND cm2 > 100) THEN
  GOSUB Both_full
  ELSEIF cm < 15 THEN
  GOSUB GETOUTOFTHECORNER1
  ELSEIF cm2 < 15 THEN
  GOSUB GETOUTOFTHECORNER2
  ELSEIF cm > cm2 THEN
  GOSUB Go_left
  ELSEIF cm < cm2 THEN
  GOSUB Go_Right
  ENDIF
  RETURN

Both_full:
  PULSOUT ToServo1, Full1                           'sends fullspeed pulse
  PULSOUT ToServo2, Full2                           'sends fullspeed pulse
  PAUSE 10
  RETURN

Go_left:
  Left1 = cm - cm2
  Left1 = Left1 / 2
  Left1 = Full1 - Left1
  PULSOUT ToServo1, Left1                           'sends left turn pulse
  PULSOUT ToServo2, Full2                           'sends fullspeed pulse
  PAUSE 20
  RETURN

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

GETOUTOFTHECORNER1:
  PULSOUT ToServo1, Full1
  PULSOUT ToServo2, Back2
  PAUSE 300
  RETURN

GETOUTOFTHECORNER2:
  PULSOUT ToServo1, Back1
  PULSOUT ToServo2, Full2
  PAUSE 300
  RETURN

Mike Green · 2009-11-21 05:13

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.

Solutions:

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

micromang · 2009-11-21 05:59

Do you mean switching them like this? That should work.

do
readping1
determinenextstep
readping2
determinenextstep
loop

micromang · 2009-11-21 06:04

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.

SRLM · 2009-11-21 07:09

If you change something, please repost your full code as an attachment.

micromang · 2009-11-21 07:24

should be attached

Mike Green · 2009-11-21 14:19

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

micromang · 2009-11-23 02:31

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 · 2009-11-23 03:03

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.

micromang · 2009-11-23 03:31

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?

micromang · 2009-11-23 03:33

And still jerky

Mike Green · 2009-11-23 04:06

To initialize cm2 properly, you need to call ReadPING2 once before calling DetermineNextMove like this:

'

[noparse][[/noparse] Main ]

GOSUB ReadPING2
DO

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.

micromang · 2009-11-23 04:39

Okay I did that. I tried this...

'

[noparse][[/noparse] Main ]

GOSUB ReadPing2
DO
GOSUB Readping1
GOSUB Go_right
Pausebreak= rawDist/500
GOSUB ReadPING2
Pausebreak= rawDist2/500
LOOP

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..[noparse]:([/noparse] 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 · 2009-11-23 05:30

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.

Code help

Comments