Robotic Leg Blue Tooth Issue
NWUpgrades
Posts: 292
OK. Using a BS2 chip· and the following code along with the BoeBot Remote Control program, I am able to connect and control my servos on a BOE board. However, When I connect my BS2sx chip, I am able to connect, but none of the arrows work to control the servos. I modified the program to Con 240, multiplied the buffer by 2.5 and also the Servo Speed by 2.5. What I am missing that is not letting my BS2sx chip work like the BS2 chip does?
·File...... Easy Bluetooth.bs2
'·· Purpose... Control a Boe-Bot with a GUI interface
'·· Author.... Technical Support & RoboTech SRL
'·· E-mail.... support@parallax.com
'·· Started... April 1st 2009
'·· Updated... N/A
'
'·· {$STAMP BS2sx}
'·· {$PBASIC 2.5}
'
' =========================================================================
'
[noparse][[/noparse] Program Description ]
' This program is what is loaded into the BASIC Stamp prior to using the GUI interface
' from RoboTech SLR. It allows a Boe-Bot to be controlled via a GUI interface.
'
' Commands are as followed:
'
'···· Up Arrow··· = Forward
'···· Down Arrow· = Backwards
'···· Left Arrow· = Left Turn
'···· Right Arrow = Right Turn
'···· Space Bar·· = Stop
'
' You can press and hold and release an arrow key for 2 seconds to access a double speed
' of each action. For example, Press Up Arrow, release to make the Boe-Bot move faster
' forward.
'
[noparse][[/noparse] I/O Definitions ]
BT_RX········· PIN···· 2···················· ' RX of the Easy Bluetooth
BT_TX········· PIN···· 0···················· ' TX of the Easy Bluetooth
LED··········· PIN···· 5···················· ' Indicator LED for Bluetooth Connection
'
[noparse][[/noparse] Constants ]
BT_SPEED······ CON···· 240·················· ' baud 9600 true UART
'
[noparse][[/noparse] Variables ]
tLeft········· VAR···· Word················· ' Left Servo control pulse durations
tRight········ VAR···· Word················· ' Right Servo control pulse durations
temp·········· VAR···· Word················· ' Temp variable
' Buffer array not declared as buffer VAR Word(5) for SERIN functionality.
' It can still be accessed as buffer(0), buffer(1), etc.· However,
' buffer0, buffer1, etc. should be used in SERIN commands with variations
' of WAIT.
· buffer0······· VAR···· Byte················· ' Buffer - Start char = $ff
· buffer1······· VAR···· Byte················· ' Message Index value
· buffer2······· VAR···· Byte················· ' Command
· buffer3······· VAR···· Byte················· ' Argument 1 (return data 1)
· buffer4······· VAR···· Byte················· ' Argument 2 (return data 2)
· buffer········ VAR···· buffer0·············· ' For standard array indexing
· msgIndex······ VAR···· Byte················· ' message index
· rxc··········· VAR···· Byte················· ' Receive Clear
'
[noparse][[/noparse] Initialization ]
Program_Start:
· LOW LED
· DEBUG CLS
· PAUSE 1000······················································ ' Wait for the RBT-001 radio to be ready.
· msgIndex = 0
· buffer0 = $FF··················································· ' Connection packet
· buffer1 = msgIndex
· buffer2 = $CC
· buffer3 = 250
· buffer4 = 250
· GOSUB Set_Servo_Speed
· DEBUG CR,"Waiting connection..."································ ' wait for connection request
· SERIN BT_RX, BT_SPEED, [noparse][[/noparse]WAITSTR buffer \ 3, buffer3, buffer4]··· '
'
[noparse][[/noparse] Program Code ]
DO
· SELECT buffer2
· CASE $CC························································ ' Connect
··· HIGH LED
··· msgIndex = 0
··· DEBUG CR,"Connected"
··· GOSUB Reply
· CASE $DD························································ ' Disconnect
··· LOW LED
··· DEBUG CR,"Disconnected"
··· GOSUB Reply
··· GOTO Program_Start
· CASE $11························································ ' Servo
··· DEBUG CR,"Servo"
··· GOSUB Set_Servo_Speed
··· GOSUB Reply
· CASE ELSE······················································· ' Error
··· buffer2 = $EE
··· GOSUB Reply
· ENDSELECT
·Resume:·························································· ' If Message not rcvd, try again
·PULSOUT 13, tLeft················································ ' Servo control pulses
·PULSOUT 12, tRight
·SERIN BT_RX, BT_SPEED, 10, Resume,······························· ' Get next command
····· [noparse][[/noparse]WAITSTR buffer \ 2, buffer2,
····· buffer3, buffer4]
·PULSOUT 13, tLeft················································ ' Servo control pulses again
·PULSOUT 12, tRight
LOOP
Reply:
· msgIndex = msgIndex + 1········································· ' Increment message index for reply.· Next message from PC has to use
· buffer1 = msgIndex·············································· ' reply's buf[noparse][[/noparse]1].
· SEROUT BT_TX, BT_SPEED, [noparse][[/noparse]STR buffer \5]
'
[noparse][[/noparse] Subroutines ]
Set_Servo_Speed:·················································· ' Range of 0 to 200 with 100 = stopped maps to 650 to 850 with 750 stopped.
· tLeft = buffer3 + 1625··········································· ' Decode servo speed.
· tRight = buffer4 + 1625
RETURN
'
[noparse][[/noparse] Data ]
ResetOnOff········· DATA 0········································ ' On/off toggle w/ Reset button
RequestConnect····· DATA $FF, 0, 1, 0, 0
ConnectionGranted·· DATA $FF, 0, 2, 0, 0
RequestCommand····· DATA $FF, 0, 3, 0, 0
ServoSpeeds········ DATA $FF, 0, 4, 0, 0
Post Edited (NWUpgrades) : 10/2/2009 8:41:03 PM GMT
·File...... Easy Bluetooth.bs2
'·· Purpose... Control a Boe-Bot with a GUI interface
'·· Author.... Technical Support & RoboTech SRL
'·· E-mail.... support@parallax.com
'·· Started... April 1st 2009
'·· Updated... N/A
'
'·· {$STAMP BS2sx}
'·· {$PBASIC 2.5}
'
' =========================================================================
'
[noparse][[/noparse] Program Description ]
' This program is what is loaded into the BASIC Stamp prior to using the GUI interface
' from RoboTech SLR. It allows a Boe-Bot to be controlled via a GUI interface.
'
' Commands are as followed:
'
'···· Up Arrow··· = Forward
'···· Down Arrow· = Backwards
'···· Left Arrow· = Left Turn
'···· Right Arrow = Right Turn
'···· Space Bar·· = Stop
'
' You can press and hold and release an arrow key for 2 seconds to access a double speed
' of each action. For example, Press Up Arrow, release to make the Boe-Bot move faster
' forward.
'
[noparse][[/noparse] I/O Definitions ]
BT_RX········· PIN···· 2···················· ' RX of the Easy Bluetooth
BT_TX········· PIN···· 0···················· ' TX of the Easy Bluetooth
LED··········· PIN···· 5···················· ' Indicator LED for Bluetooth Connection
'
[noparse][[/noparse] Constants ]
BT_SPEED······ CON···· 240·················· ' baud 9600 true UART
'
[noparse][[/noparse] Variables ]
tLeft········· VAR···· Word················· ' Left Servo control pulse durations
tRight········ VAR···· Word················· ' Right Servo control pulse durations
temp·········· VAR···· Word················· ' Temp variable
' Buffer array not declared as buffer VAR Word(5) for SERIN functionality.
' It can still be accessed as buffer(0), buffer(1), etc.· However,
' buffer0, buffer1, etc. should be used in SERIN commands with variations
' of WAIT.
· buffer0······· VAR···· Byte················· ' Buffer - Start char = $ff
· buffer1······· VAR···· Byte················· ' Message Index value
· buffer2······· VAR···· Byte················· ' Command
· buffer3······· VAR···· Byte················· ' Argument 1 (return data 1)
· buffer4······· VAR···· Byte················· ' Argument 2 (return data 2)
· buffer········ VAR···· buffer0·············· ' For standard array indexing
· msgIndex······ VAR···· Byte················· ' message index
· rxc··········· VAR···· Byte················· ' Receive Clear
'
[noparse][[/noparse] Initialization ]
Program_Start:
· LOW LED
· DEBUG CLS
· PAUSE 1000······················································ ' Wait for the RBT-001 radio to be ready.
· msgIndex = 0
· buffer0 = $FF··················································· ' Connection packet
· buffer1 = msgIndex
· buffer2 = $CC
· buffer3 = 250
· buffer4 = 250
· GOSUB Set_Servo_Speed
· DEBUG CR,"Waiting connection..."································ ' wait for connection request
· SERIN BT_RX, BT_SPEED, [noparse][[/noparse]WAITSTR buffer \ 3, buffer3, buffer4]··· '
'
[noparse][[/noparse] Program Code ]
DO
· SELECT buffer2
· CASE $CC························································ ' Connect
··· HIGH LED
··· msgIndex = 0
··· DEBUG CR,"Connected"
··· GOSUB Reply
· CASE $DD························································ ' Disconnect
··· LOW LED
··· DEBUG CR,"Disconnected"
··· GOSUB Reply
··· GOTO Program_Start
· CASE $11························································ ' Servo
··· DEBUG CR,"Servo"
··· GOSUB Set_Servo_Speed
··· GOSUB Reply
· CASE ELSE······················································· ' Error
··· buffer2 = $EE
··· GOSUB Reply
· ENDSELECT
·Resume:·························································· ' If Message not rcvd, try again
·PULSOUT 13, tLeft················································ ' Servo control pulses
·PULSOUT 12, tRight
·SERIN BT_RX, BT_SPEED, 10, Resume,······························· ' Get next command
····· [noparse][[/noparse]WAITSTR buffer \ 2, buffer2,
····· buffer3, buffer4]
·PULSOUT 13, tLeft················································ ' Servo control pulses again
·PULSOUT 12, tRight
LOOP
Reply:
· msgIndex = msgIndex + 1········································· ' Increment message index for reply.· Next message from PC has to use
· buffer1 = msgIndex·············································· ' reply's buf[noparse][[/noparse]1].
· SEROUT BT_TX, BT_SPEED, [noparse][[/noparse]STR buffer \5]
'
[noparse][[/noparse] Subroutines ]
Set_Servo_Speed:·················································· ' Range of 0 to 200 with 100 = stopped maps to 650 to 850 with 750 stopped.
· tLeft = buffer3 + 1625··········································· ' Decode servo speed.
· tRight = buffer4 + 1625
RETURN
'
[noparse][[/noparse] Data ]
ResetOnOff········· DATA 0········································ ' On/off toggle w/ Reset button
RequestConnect····· DATA $FF, 0, 1, 0, 0
ConnectionGranted·· DATA $FF, 0, 2, 0, 0
RequestCommand····· DATA $FF, 0, 3, 0, 0
ServoSpeeds········ DATA $FF, 0, 4, 0, 0
Post Edited (NWUpgrades) : 10/2/2009 8:41:03 PM GMT
Comments
"BT_SPEED CON 240 ' baud 9600 true UART"
from 240 to 990 (396 x 2.5), but I'm not sure. I was having a similar problem with getting my new BS2sx to work with the the new propeller servo controllers. I changed the baud Con from 396 to 990 and it works now. Below is the modified code. I hope it works for you too.
' {$STAMP BS2sx}
' {$PBASIC 2.5}
'hipAngles.bs2 .Simple code to try out differant servo postions
'hip VAR Byte
'knee VAR Byte
'foot VAR Byte
leg VAR Byte
pw VAR Word
ra VAR Byte
sdat CON 15
baud CON 990 'mutltiplied 396 by 2.5 for the BS2sx which is 2.5X faster than BS2
LFhip CON 5 'Left Front hip
LFknee CON 6
LFfoot CON 7
LRhip CON 8 'Left Rear hip
LRknee CON 9
LRfoot CON 10
RFhip CON 0 'Fight Front hip
RFknee CON 1
RFfoot CON 2
RRhip CON 13 'Right Rear hip
RRknee CON 14
RRfoot CON 15
LPawHip CON 24 'Left Paw hip
LPawKnee CON 25
LPawFoot CON 26
RpawHip CON 29 'Right Paw hip
RPawKnee CON 30
RPawFoot CON 31
DO
GOSUB Front_Left
LOOP
'---Subroutine
Front_Left:
ra = 9
DO
'Rotate hip tword each other
DEBUG CR, "hips moving!"
pw = 900
SEROUT Sdat, Baud+$8000, [noparse][[/noparse]"!SC", RpawHip, ra, pw.LOWBYTE, pw.HIGHBYTE, CR]
'PAUSE 1000
pw = 1200
SEROUT sdat, baud+$8000,[noparse][[/noparse]"!SC", Rfhip, ra, pw.LOWBYTE, pw.HIGHBYTE, CR]
pw = 600
SEROUT sdat, baud+$8000,[noparse][[/noparse]"!SC", Rrhip, ra, pw.LOWBYTE, pw.HIGHBYTE, CR]
pw = 600
SEROUT sdat, baud+$8000,[noparse][[/noparse]"!SC", LpawHip, ra, pw.LOWBYTE, pw.HIGHBYTE, CR]
pw = 1200
SEROUT sdat, baud+$8000,[noparse][[/noparse]"!SC", Lfhip, ra, pw.LOWBYTE, pw.HIGHBYTE, CR]
pw = 900
SEROUT sdat, baud+$8000,[noparse][[/noparse]"!SC", Lrhip, ra, pw.LOWBYTE, pw.HIGHBYTE, CR]
PAUSE 1000
LOOP
RETURN