[Robotgroup] Robot Head: BS2 Stamp programming Help?!
LHudson
lhudson73 at yahoo.com
Tue Aug 5 10:21:04 PDT 2008
Since I am currently lacking the skills to post something to the WIKI in a timely fashion, I'll just C&P here:
' {$STAMP BS2}
' {$PBASIC 2.5}
' ##### Declare the Command Constants #####
' ~ Speech Commands
'
CMD_Mouth_Pin2_ON CON 21
CMD_Mouth_Pin2_OFF CON 20
CMD_Mouth_Pin1_ON CON 11
CMD_Mouth_Pin1_OFF CON 10
CMD_Mouth_Pin4_ON CON 41
CMD_Mouth_Pin4_OFF CON 40
CMD_Mouth_Pin5_ON CON 51
CMD_Mouth_Pin5_OFF CON 50
' ~ Movement Commands
'
CMD_Move_FORWARD CON 111
CMD_Move_BACKWARD CON 100
CMD_Move_Rotate_LEFT CON 101
CMD_Move_Rotate_RIGHT CON 110
CMD_Move_STOP CON 199
' ~ Pan and Tilt Commands
'
' Command Arguments >= 1000 are reserved for Pan and Tilt Commands
' The Constant value (0 to 130) times 10 is equal to the PULSOUT rate
' which determines the servo position.
'
' The First Digit determines which servo - 1: Pan, 2: Tilt, e.g.:
' 1065 - Centers the Pan servo
' 2065 - Centers the Tilt servo
' 1130 - Rotates the head all the way to the Right
' 2130 - Tilts the head all the way Back
' 1000 - Rotates the head all the way to the Left
' 2000 - Tilts the head all the way to the Front
' ##### Initialize Runtime #####
BAUD CON 84
COMDATA VAR Word
p VAR Word
' ##### Initialize Speech #####
FOR p = 1 TO 3
HIGH 2: PAUSE 100
HIGH 1: PAUSE 100
HIGH 4: PAUSE 100
HIGH 5: PAUSE 400
LOW 2
LOW 1
LOW 4
LOW 5
NEXT
' ##### Initialize Pan and Tilt #####
Component_Arg VAR Bit
Center_Arg CON 650
PIN_PanTilt_Pan PIN 14
POS_PanTilt_Pan VAR Word
CMD_PanTilt_Pan VAR Word
PIN_PanTilt_Tilt PIN 15
POS_PanTilt_Tilt VAR Word
CMD_PanTilt_Tilt VAR Word
' Center the Servos
POS_PanTilt_Pan = Center_Arg
POS_PanTilt_Tilt = Center_Arg
FOR p = 0 TO 255
PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
NEXT
' ##### Loop to get the Commands #####
DO
'~ SERIN 16\0, BAUD, [DEC COMDATA]
DEBUG CR, "Enter Command: "
DEBUGIN DEC COMDATA
DEBUG CR, "Command = ", COMDATA
IF COMDATA < 100 THEN
' !-- Execute a Speech Command
DEBUG "Speech Command: ", COMDATA
SELECT COMDATA
CASE CMD_Mouth_Pin2_ON: HIGH 2
CASE CMD_Mouth_Pin2_OFF: LOW 2
CASE CMD_Mouth_Pin1_ON: HIGH 1
CASE CMD_Mouth_Pin1_OFF: LOW 1
CASE CMD_Mouth_Pin4_ON: HIGH 4
CASE CMD_Mouth_Pin4_OFF: LOW 4
CASE CMD_Mouth_Pin5_ON: HIGH 5
CASE CMD_Mouth_Pin5_OFF: LOW 5
ENDSELECT
ELSEIF COMDATA < 1000 THEN
' !-- Execute a Movement Command
DEBUG "Movement Command: ", COMDATA
ELSE
' !-- Execute a Pan or Tilt Command
IF COMDATA < 2000 THEN
CMD_PanTilt_Pan = 10 * (COMDATA - 1000)
DEBUG "Pan Command: ", COMDATA
' Adjust the Pan to the Command argument
IF POS_PanTilt_Pan < CMD_PanTilt_Pan THEN
DO UNTIL POS_PanTilt_Pan = CMD_PanTilt_Pan
PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
POS_PanTilt_Pan = POS_PanTilt_Pan + 5
LOOP
ENDIF
IF POS_PanTilt_Pan > CMD_PanTilt_Pan THEN
DO UNTIL POS_PanTilt_Pan = CMD_PanTilt_Pan
PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
POS_PanTilt_Pan = POS_PanTilt_Pan - 5
LOOP
ENDIF
ELSE
CMD_PanTilt_Tilt = 10 * (COMDATA - 2000)
DEBUG "Tilt Command: ", COMDATA
' Adjust the Tilt to the Command argument
IF POS_PanTilt_Tilt < CMD_PanTilt_Tilt THEN
DO UNTIL POS_PanTilt_Tilt = CMD_PanTilt_Tilt
PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
POS_PanTilt_Tilt = POS_PanTilt_Tilt + 5
LOOP
ENDIF
IF POS_PanTilt_Tilt > CMD_PanTilt_Tilt THEN
DO UNTIL POS_PanTilt_Tilt = CMD_PanTilt_Tilt
PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
POS_PanTilt_Tilt = POS_PanTilt_Tilt - 5
LOOP
ENDIF
ENDIF
ENDIF
'PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
'PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
LOOP
L. Scott Hudson
"There are known knowns. These are things we know that we know. There are known unknowns. That is to say, there are things we know we don't know. But, there are also unknown unknowns. These are things we don't know we don't know."
-- Donald Rumsfeld, describing the IBM "Rational Software" Platform
--- On Tue, 8/5/08, LHudson <lhudson73 at yahoo.com> wrote:
From: LHudson <lhudson73 at yahoo.com>
Subject: [Robotgroup] Robot Head: BS2 Stamp programming Help?!
To: "The Robot Group Mailing List" <robotgroup at puremagic.com>
Date: Tuesday, August 5, 2008, 11:48 AM
I've included my code module as an attachment.
I have four pins connected to a "Mouth Module" with LEDs that light
up. The Serial Port is tied to a DOT NET speech programming API event. The
BS2 "listens" for the Serial data inside of a DO...LOOP:
DO
'~ SERIN 16\0, BAUD, [DEC COMDATA]
DEBUG CR, "Enter Command: "
DEBUGIN DEC COMDATA
SELECT COMDATA
CASE CMD_Mouth_Pin2_ON: HIGH 2
CASE CMD_Mouth_Pin2_OFF: LOW 2
CASE CMD_Mouth_Pin1_ON: HIGH 1
CASE CMD_Mouth_Pin1_OFF: LOW 1
CASE CMD_Mouth_Pin4_ON: HIGH 4
CASE CMD_Mouth_Pin4_OFF: LOW 4
CASE CMD_Mouth_Pin5_ON: HIGH 5
CASE CMD_Mouth_Pin5_OFF: LOW 5
ENDSELECT
LOOP
I am happy to report that the mouth movement is quite realistic and
entertaining, when tied to the Speech API Events! I will bring this to the
Show & Tell on Thursday. Also, this is weird, but it doesn't matter if
you use the SERIN or the DEBUGIN Code, it still works the same whether you are
using the DOT NET Serial Interface or the Stamp Editor?!
But okay here is my actual question: I am having problems with the Pan &
Tilt servos that are attached to the head. If you inspect the code attachment,
you can see the input processing to determine the servo position. With some
experimentation, I was able to engineer it so that by inputting a command, e.g.
"1010" or "2130" I was able to position the head... not with
100% consistency or accuracy, but it's a good start.
The problem is that if there is no command to execute the Pan & Tilt, then
the servos receive no PULSOUT. With no charge to the motors, the slightest
bump can cause a severe case of whiplash. My tentative solution was to put
additional PULSOUT codes before the ...LOOP:
DO
'~ SERIN 16\0, BAUD, [DEC COMDATA]
DEBUG CR, "Enter Command: "
DEBUGIN DEC COMDATA
IF COMDATA < 100
' {Do the Speech Code}
ELSEIF COMDATA < 1000
' {Do the Movement Code}
ELSE
' {Do the Pan or Tilt Code}
ENDIF
PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
LOOP
However this causes much weirdness to happen. In the attachment, there is an
initialization sequence; it lights up the LEDs so I can verify they are
connected to the correct pins, and it also centers the servos, before the
DO...LOOP that receives the input and executes the command. When I include
the two PULSOUT codes before the ...LOOP, the result is that this initiation
sequence, which is supposed to occur before the DO...LOOP, executes over and
over and over again!
Here is another thing: sometimes, when I am debugging in the Stamp Editor, it
will go through the initiation sequence 10-11x before finally displaying the
"Enter Command:" prompt in the debug window. Is this somehow
related?
Thanks!
L. Scott Hudson
"There are known knowns. These are things we know that we know. There are
known unknowns. That is to say, there are things we know we don't know. But,
there are also unknown unknowns. These are things we don't know we don't
know."
-- Donald Rumsfeld, describing the IBM "Rational Software" Platform
_______________________________________________
Robotgroup mailing list
Robotgroup at puremagic.com
http://lists.puremagic.com/cgi-bin/mailman/listinfo/robotgroup
More information about the Robotgroup
mailing list