Ping Ultrasonic Sensor
I am building a miniature elevator and want to have the sensor picking up the distance from the cab to the top of the elevator shaft at all times while the other parts of the program ( Asking where you want to go and calculating whether to go up or down) are running. Is there a way to script this in P basic or is the basic stamp 2 not capable of handling two processes at once? I have tried, but the program does not do what I want.
Here is my program code.
' {$STAMP BS2}
' {$PBASIC 2.5}
CurrentFloor VAR Byte
DestinationFloor VAR Byte
distance VAR Word
time VAR Word
' ultrasonic rangefinder detecting
Sensor:
PULSOUT 14, 5
PULSIN 14, 1, time
DEBUG HOME, "time = ", DEC5 time
distance = time ** 2251
DEBUG CR, "Distance = ", DEC4 distance, " cm"
DEBUG CR, "Distance in decimal = ", SDEC distance
PAUSE 100
' finding current floor.
IF distance = 93 THEN 'must change 3 to height from ceiling of elevator.
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 1
ENDIF
IF distance = 83 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 2
ENDIF
IF distance = 73 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 3
ENDIF
IF distance = 63 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 4
ENDIF
IF distance = 53 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 5
ENDIF
IF distance = 43 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 6
ENDIF
IF distance = 33 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 7
ENDIF
IF distance = 23 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 8
ENDIF
IF distance = 13 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 9
ENDIF
IF distance = 3 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 10
ENDIF
' asking where you are going.
Main:
DEBUG CLS,"The ", SDEC ? CurrentFloor
DEBUG CR, "Enter a destination floor: " ' prompt user
DEBUGIN SNUM DestinationFloor ' retrieve number in any format
DEBUG CRSRXY, 0, 2, ' display number in decimal
SDEC ? DestinationFloor
PAUSE 1000
' finding which way to go. Up or Down.
IF (CurrentFloor < DestinationFloor) THEN
DEBUG "Going Up", CR
LOW 9
HIGH 10
ENDIF
IF (CurrentFloor > DestinationFloor) THEN
DEBUG "Going Down", CR
HIGH 9
LOW 10
ENDIF
IF (CurrentFloor = DestinationFloor) THEN
DEBUG "Floor Reached", CR
HIGH 9
HIGH 10
GOTO OpenDoor
ENDIF
OpenDoor:
counter VAR Word
FREQOUT 11, 1000, 3500
DEBUG "Counterclockwise approx. 40 degrees", CR
FOR counter = 1 TO 50
PULSOUT 12, 720
PAUSE 20
NEXT
PAUSE 5000
DEBUG "Clockwise approx. 40 degrees", CR
FOR counter = 1 TO 50
PULSOUT 12, 200
PAUSE 20
NEXT
DEBUG "All done."
RETURN
What am I missing?
Here is my program code.
' {$STAMP BS2}
' {$PBASIC 2.5}
CurrentFloor VAR Byte
DestinationFloor VAR Byte
distance VAR Word
time VAR Word
' ultrasonic rangefinder detecting
Sensor:
PULSOUT 14, 5
PULSIN 14, 1, time
DEBUG HOME, "time = ", DEC5 time
distance = time ** 2251
DEBUG CR, "Distance = ", DEC4 distance, " cm"
DEBUG CR, "Distance in decimal = ", SDEC distance
PAUSE 100
' finding current floor.
IF distance = 93 THEN 'must change 3 to height from ceiling of elevator.
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 1
ENDIF
IF distance = 83 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 2
ENDIF
IF distance = 73 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 3
ENDIF
IF distance = 63 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 4
ENDIF
IF distance = 53 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 5
ENDIF
IF distance = 43 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 6
ENDIF
IF distance = 33 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 7
ENDIF
IF distance = 23 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 8
ENDIF
IF distance = 13 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 9
ENDIF
IF distance = 3 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 10
ENDIF
' asking where you are going.
Main:
DEBUG CLS,"The ", SDEC ? CurrentFloor
DEBUG CR, "Enter a destination floor: " ' prompt user
DEBUGIN SNUM DestinationFloor ' retrieve number in any format
DEBUG CRSRXY, 0, 2, ' display number in decimal
SDEC ? DestinationFloor
PAUSE 1000
' finding which way to go. Up or Down.
IF (CurrentFloor < DestinationFloor) THEN
DEBUG "Going Up", CR
LOW 9
HIGH 10
ENDIF
IF (CurrentFloor > DestinationFloor) THEN
DEBUG "Going Down", CR
HIGH 9
LOW 10
ENDIF
IF (CurrentFloor = DestinationFloor) THEN
DEBUG "Floor Reached", CR
HIGH 9
HIGH 10
GOTO OpenDoor
ENDIF
OpenDoor:
counter VAR Word
FREQOUT 11, 1000, 3500
DEBUG "Counterclockwise approx. 40 degrees", CR
FOR counter = 1 TO 50
PULSOUT 12, 720
PAUSE 20
NEXT
PAUSE 5000
DEBUG "Clockwise approx. 40 degrees", CR
FOR counter = 1 TO 50
PULSOUT 12, 200
PAUSE 20
NEXT
DEBUG "All done."
RETURN
What am I missing?
Comments
The Stamp is single threaded. It can only do one thing at a time, but it can interleave two different tasks.
In particular, servos require a control pulse every 20ms or so and the Stamp normally does a PAUSE statement to provide the 20ms timing.
It's easy to do something else during that 18ms or so idle period from one 1-2ms control pulse to another. Typically, your program calls a
subroutine instead of the PAUSE and that subroutine does something else useful for about 18ms, then returns.
Some things can't be done this way. In that case, you need to use an external controller to handle the timing dependent stuff, like a ServoPAL
to make the servo control pulses every 20ms or so. The Stamp just tells the ServoPAL how long a pulse is needed and the ServoPAL continues
producing that pulse until it's commanded to do something else. Similarly, you can buy a serial input buffer to read serial characters and store
them until the Stamp can get around to looking at them. If the Stamp is busy doing something else when serial data comes in, it normally
ignores it.
There are "library" routines available from the Propeller Object Exchange (ObEx - obex.parallax.com/) or included with the Propeller Tool that can handle multiple servos just by specifying the desired position of the servo (see Servo32 in ObEx - obex.parallax.com/objects/51/). There's also the BS2 Compatibility Library (in the ObEx - obex.parallax.com/objects/30/) that has routines to provide the same functionality of most of the BS2 I/O statements (like FREQOUT and DEBUG).
There is a BS2 form factor version of the Propeller (the Spin Stamp) that plugs into existing Stamp sockets. The only thing you have to be careful of is that the Propeller's I/O pins are 3.3V outputs and accept at most 3.3V logic inputs. If you're using the PING))), you'll need a 1K resistor in series with the data line as shown in the sample code in the ObEx (obex.parallax.com/objects/114/). It's usually a good idea to put 1K resistors in series with the servo control lines and you may need to adjust any input resistors in your h-bridge circuit.
The Spin Stamp has a serial I/O interface that works like the programming / DEBUG connection on the Stamps. Parallax doesn't guarantee that it will work with any PC serial port because the output voltage is 3.3V rather than the 5V of the Stamps. I've not had any problems using it with several different USB to Serial Adapters.
Post Edited (Mike Green) : 12/21/2008 6:14:36 PM GMT
' {$STAMP BS2}
' {$PBASIC 2.5}
CurrentFloor VAR Byte
DestinationFloor VAR Byte
distance VAR Word
time VAR Word
' ultrasonic rangefinder detecting
Sensor:
PULSOUT 14, 5
PULSIN 14, 1, time
DEBUG HOME, "time = ", DEC5 time
distance = time ** 2251
DEBUG CR, "Distance = ", DEC4 distance, " cm"
DEBUG CR, "Distance in decimal = ", SDEC distance
PAUSE 100
' finding current floor.
IF distance = 93 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 1
ENDIF
IF distance = 83 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 2
ENDIF
IF distance = 73 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 3
ENDIF
IF distance = 63 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 4
ENDIF
IF distance = 53 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 5
ENDIF
IF distance = 43 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 6
ENDIF
IF distance = 33 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 7
ENDIF
IF distance = 23 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 8
ENDIF
IF distance = 13 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 9
ENDIF
IF distance = 3 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 10
ENDIF
' asking where you are going.
Main:
DEBUG CLS,"The ", SDEC ? CurrentFloor
DEBUG CR, "Enter a destination floor: " ' prompt user
DEBUGIN SNUM DestinationFloor ' retrieve number in any format
DEBUG CRSRXY, 0, 2, ' display number in decimal
SDEC ? DestinationFloor
' finding which way to go. Up or Down.
IF (CurrentFloor < DestinationFloor) THEN
DEBUG "Going Up", CR
LOW 9
HIGH 10
GOTO Sensor2
elseIF (CurrentFloor > DestinationFloor) THEN
DEBUG "Going Down", CR
HIGH 9
LOW 10
GOTO Sensor2
elseIF (CurrentFloor = DestinationFloor) THEN
DEBUG "Floor Reached", CR
HIGH 9
HIGH 10
ENDIF
return
Sensor2:
do
PULSOUT 14, 5
PULSIN 14, 1, time
DEBUG HOME, "time = ", DEC5 time
distance = time ** 2251
DEBUG CR, "Distance = ", DEC4 distance, " cm"
DEBUG CR, "Distance in decimal = ", SDEC distance
PAUSE 100
' finding current floor.
IF distance = 93 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 1
ENDIF
IF distance = 83 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 2
ENDIF
IF distance = 73 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 3
ENDIF
IF distance = 63 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 4
ENDIF
IF distance = 53 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 5
ENDIF
IF distance = 43 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 6
ENDIF
IF distance = 33 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 7
ENDIF
IF distance = 23 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 8
ENDIF
IF distance = 13 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 9
ENDIF
IF distance = 3 THEN
FREQOUT 11, 200, 3000
FREQOUT 11, 50, 2200
CurrentFloor = 10
ENDIF
LOOP UNTIL (CurrentFloor = DestinationFloor)
GOTO OpenDoor
OpenDoor:
counter VAR Word
FREQOUT 11, 1000, 3500
DEBUG "Counterclockwise approx. 40 degrees", CR
FOR counter = 1 TO 50
PULSOUT 12, 720
PAUSE 20
NEXT
PAUSE 5000
DEBUG "Clockwise approx. 40 degrees", CR
FOR counter = 1 TO 50
PULSOUT 12, 200
PAUSE 20
NEXT
DEBUG "All done."
RETURN
Main:
open door
get user input and display it
close door
read PING (update current floor variable)
determine motor direction (by comparing desired floor with current floor)
enable motor
DO
· read PING subroutine (update CurrentFloor variable)
· announce each floor subroutine (sound tones when a floor is reached)
LOOP UNTIL CurrentFloor=DestinationFloor
disable motor
GOTO Main
Post Edited (Mikerocontroller) : 12/22/2008 6:04:08 AM GMT