xbee not recieving proper values
graffix
Posts: 389
Ive been struggling with some code for awhile.Ive taken the code Ive been working with and ommited alot of it to try and simplify the process and Im still struggling with it.Currently I'd like to get two parallax 2 axis joysticks to control a boe's drive wheels. Via xbee modules. I dont think there is any wiring issues, just code. Basically one joystick controls up-down values and the other left right values. Sent to the boe via xbee modules. Both codes have debug commands. The controller gives good values in its debug window. The recieving boe does not display the same values in its debug window. They also dont refresh at the same rate. Im attaching some code for each and a video. If someone can look it over and suggest a fix that would be great. Thanks ahead of time. If you have any questions please ask. Ps I already have all the demo code and different variations.
http://www.youtube.com/watch?v=ab_NkBmQvtg&feature=plcp
http://www.youtube.com/watch?v=ab_NkBmQvtg&feature=plcp
'recieving boe
'{$STAMP BS2}
'{$PBASIC 2.5}
' -----[ Constants ]-------------------------------------------------------
L_FWD CON 1000
L_REV CON 500
L_STOP CON 750
R_FWD CON 500
R_REV CON 1000
R_STOP CON 750
BAUD_MODE CON 84 '9600 Baud, 8n1 true
'---Variables-------------------------------------------
UD VAR Byte
LR VAR Byte
a VAR Word
b VAR Word
c VAR Word
d VAR Word
e VAR Word
f VAR Word
i VAR Byte
rx_data VAR Byte
'---Pin Descriptions------------------------------------
xbee_rts PIN 9
xbee_Rx PIN 10 ' Dout
xbee_Tx PIN 11 ' Din
Rwheel PIN 13
Lwheel PIN 12
'---initializations------------------------------------
LOW Lwheel
LOW Rwheel
'---main program------------------------------------------
DO
SERIN XBee_Tx, 84, [UD,LR]
DEBUG HOME, "UD =", DEC UD, CLREOL, CR,
"LR = ", DEC LR, CLREOL
SELECT UD
CASE < 270
UD = a
GOSUB forwards
CASE > 400
UD = b
GOSUB backwards
CASE 270 TO 400
UD = c
ENDSELECT
SELECT LR
CASE < 220
LR = d
GOSUB left
CASE > 380
LR = e
GOSUB right
CASE 220 TO 380
LR = f
ENDSELECT
IF (UD = c)AND(LR = f) THEN
GOSUB stall
ENDIF
LOOP
'---Subroutines------------------------------------------
forwards:
PULSOUT Rwheel, L_FWD
PULSOUT Lwheel, R_FWD
PAUSE 20
RETURN
backwards:
PULSOUT Rwheel, R_REV
PULSOUT Lwheel, L_REV
PAUSE 20
RETURN
left:
PULSOUT Lwheel, L_FWD
PULSOUT Rwheel, R_STOP
PAUSE 20
RETURN
right:
PULSOUT Lwheel, L_STOP
PULSOUT Rwheel, R_FWD
PAUSE 20
RETURN
stall:
PULSOUT Rwheel, R_STOP
PULSOUT Lwheel, L_STOP
PAUSE 20
RETURN
'xbee controller
' {$STAMP BS2}
' {$PBASIC 2.5}
'----------------[Initialize]---------------------------
PAUSE 5
SEROUT 0, 84, [22,12]
SEROUT 0, 84,50, [22,17, "Hello", 13, "John"]
PAUSE 1000
'---------------[Variables]-----------------------------
LR VAR Word
UD VAR Word
btn1wrk VAR Byte
btn2wrk VAR Byte
green VAR Bit
red VAR Bit
distance VAR Word
'---------------[Constants]-----------------------------
#SELECT $STAMP
#CASE BS2, BS2E, BS2PE
T9600 CON 84
#ENDSELECT
Lcd PIN 0
Btn1 PIN 7
Btn2 PIN 5
Tx PIN 9 ' XBee DIN
Rx PIN 10 ' XBee DOUT
LRjoy PIN 6
UDjoy PIN 4
Led_A PIN 15
Led_B PIN 14
stickUD CON 25
stickLD CON 25
Baud CON T9600
'-------------------------[Main]---------------------------
Main:
DO
HIGH 6
PAUSE 2
RCTIME 6, 1, LR
HIGH 4
PAUSE 2
RCTIME 4, 1, UD
DEBUG HOME, "UD =", DEC UD, CLREOL, CR,
"LR = ", DEC LR, CLREOL
SEROUT Rx, 84, [UD]
SEROUT Rx, 84, [LR]
LOOP
Comments
Probably the easiest solution is on the controller, to scale the RCTIME value down to fit in a byte.
It worked, Thanks again
After I get the Boe to display the correct values for Btn1 and Btn2. I hope to use one of them to change into a different mode of operation. Say when I press Btn1 the LR joystick then controls the Ping servo(standard servo) while sending the ping value back to be displayed on the controllers LCD until I press Btn1 again. If I can get some some help with this portion that would be great. Thanks ahead of time.
P.S. I would move this to two Prop Boe's if I had the funds to do so.
controller
' {$STAMP BS2} ' {$PBASIC 2.5} ' XBee controller '----------------[Initialize]--------------------------- PAUSE 5 SEROUT 0, 84, [22,12] SEROUT 0, 84,50, [22,17, "Hello", 13, "John"] PAUSE 1000 '---------------[Variables]----------------------------- LR VAR Word UD VAR Word Btn1wrk VAR Byte Btn2wrk VAR Byte green VAR Bit red VAR Bit distance VAR Word '---------------[Constants]----------------------------- #SELECT $STAMP #CASE BS2, BS2E, BS2PE T9600 CON 84 #ENDSELECT Lcd PIN 0 Btn1 PIN 7 Btn2 PIN 5 Tx PIN 9 ' XBee DIN Rx PIN 10 ' XBee DOUT LRjoy PIN 6 UDjoy PIN 4 Led_A PIN 15 Led_B PIN 14 stickUD CON 25 stickLD CON 25 Baud CON T9600 '-------------------------[Main]--------------------------- Main: DO HIGH 6 PAUSE 2 RCTIME 6, 1, LR HIGH 4 PAUSE 2 RCTIME 4, 1, UD UD = UD/3 LR = LR/3 DEBUG HOME, "Controller Com 1" ,CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL SEROUT Rx, 84, [UD,LR] SEROUT Rx, 84, [Btn1,Btn2] LOOPBoe Bot
'{$STAMP BS2} '{$PBASIC 2.5} 'Recieving Boe Bot ' -----[ Constants ]------------------------------------------------------- L_FWD CON 1000 L_REV CON 500 L_STOP CON 750 R_FWD CON 500 R_REV CON 1000 R_STOP CON 750 BAUD_MODE CON 84 '9600 Baud, 8n1 true '---Variables------------------------------------------- UD VAR Word LR VAR Word a VAR Word b VAR Word c VAR Word d VAR Word e VAR Word f VAR Word Btn1 VAR Word Btn2 VAR Word i VAR Byte rx_data VAR Byte '---Pin Descriptions------------------------------------ xbee_rts PIN 9 xbee_Rx PIN 10 ' Dout xbee_Tx PIN 11 ' Din Rwheel PIN 13 Lwheel PIN 12 '---initializations------------------------------------ LOW Lwheel LOW Rwheel '---main program------------------------------------------ DO SERIN XBee_Tx, 84, [UD,LR] SERIN XBee_Tx, 84, [Btn1,Btn2] DEBUG HOME, "Boe Bot Com 13", CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL SELECT UD CASE < 90 UD = a GOSUB forwards CASE > 110 UD = b GOSUB backwards CASE 90 TO 110 UD = c ENDSELECT SELECT LR CASE < 116 LR = d GOSUB right CASE > 140 LR = e GOSUB left CASE 118 TO 140 LR = f ENDSELECT IF (UD = c)AND(LR = f) THEN GOSUB stall ENDIF LOOP '---Subroutines------------------------------------------ forwards: FOR i = 0 TO 2 PULSOUT Rwheel, R_FWD PULSOUT Lwheel, L_FWD PAUSE 20 NEXT RETURN backwards: FOR i = 0 TO 2 PULSOUT Rwheel, R_REV PULSOUT Lwheel, L_REV PAUSE 20 NEXT RETURN left: FOR i = 0 TO 2 PULSOUT Lwheel, L_FWD PULSOUT Rwheel, R_STOP PAUSE 20 NEXT RETURN right: FOR i = 0 TO 2 PULSOUT Lwheel, L_STOP PULSOUT Rwheel, R_FWD PAUSE 20 NEXT RETURN stall: PULSOUT Rwheel, R_STOP PULSOUT Lwheel, L_STOP PAUSE 20 RETURNLCD - pin 0
UDjoystick - pin 4
LRjoystick - pin 6
Btn1 - pin 7
Btn2 - pin 5
XBee Rx - pin 10
XBee Tx - pin 9
XBee RTS- pin 8
LED_A - pin 15
LED_B - pin 14
BoeBot:
XBee RTS - pin 9
XBee Rx - pin 10
XBee Tx - pin 11
servo pal - nInp pin 12
servo pal - alarm pin 13
HM55B Clk pin 0
HM55B En pin 1
HM55B Din/Dout pin 2
Bi_clrLEDred pin 6 ' when high + low 7
Bi_clrLEDgreen pin 7 ' when high + low 6
This is all of hardware I'd like to incorporate and pins used for both controller and Boe Bot.
Im not sure how I'd like to use and code functionality of all of it. Im not sure of how much varible space would be required for each.
Im also not sure how I should establish a two way communication between the xbees so my loops dont freeze up.
I've yet to get the HM55B properly calibrated even in a code by its self. I have three different compasses and I get different readings
from each. I'm sure it's interference, but thats where I'm at regardless.
Here is latest attempt
controller
' {$STAMP BS2} ' {$PBASIC 2.5} ' XBee controller '----------------[Initialize]--------------------------- PAUSE 5 SEROUT 0, 84, [22,12] SEROUT 0, 84,50, [22,17, "Hello", 13, "John"] PAUSE 1000 '---------------[Variables]----------------------------- LR VAR Word UD VAR Word Btn1wrk VAR Byte Btn2wrk VAR Byte green VAR Byte red VAR Byte inDistance VAR Word '---------------[Constants]----------------------------- #SELECT $STAMP #CASE BS2, BS2E, BS2PE T9600 CON 84 #ENDSELECT Lcd PIN 0 Btn1 PIN 7 Btn2 PIN 5 Tx PIN 9 ' XBee DIN Rx PIN 10 ' XBee DOUT RTS PIN 8 LRjoy PIN 6 UDjoy PIN 4 Led_A PIN 15 Led_B PIN 14 stickUD CON 25 stickLD CON 25 Baud CON T9600 '-------------------------[Main]--------------------------- Main: DO HIGH 6 PAUSE 2 RCTIME 6, 1, LR HIGH 4 PAUSE 2 RCTIME 4, 1, UD UD = UD/3 LR = LR/3 DEBUG HOME, "Controller Com 1" ,CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL, CR SEROUT Rx, 84, [UD,LR] SEROUT Rx, 84, [Btn1,Btn2] PAUSE 100 IF Btn2 = 1 THEN GOSUB Get_ping ENDIF LOOP Get_ping: SERIN Tx, 84, [inDistance] DEBUG "inDistance = ", DEC3 inDistance, CLREOL RETURNBoe Bot
'{$STAMP BS2} '{$PBASIC 2.5} 'Recieving Boe Bot ' -----[ Constants ]------------------------------------------------------- L_FWD CON 1000 L_REV CON 500 L_STOP CON 750 R_FWD CON 500 R_REV CON 1000 R_STOP CON 750 CmConstant CON 2260 InConstant CON 890 BAUD_MODE CON 84 '9600 Baud, 8n1 true '---Variables------------------------------------------- UD VAR Byte LR VAR Byte a VAR Byte b VAR Byte c VAR Byte d VAR Byte e VAR Byte f VAR Byte Btn1 VAR Byte Btn2 VAR Byte i VAR Byte rx_data VAR Byte cmDistance VAR Word inDistance VAR Word time VAR Word '---Pin Descriptions------------------------------------ xbee_rts PIN 9 xbee_Rx PIN 10 ' Dout xbee_Tx PIN 11 ' Din nInp PIN 12 'Define the input pin of servo pal Alarm PIN 13 'Define the alarm pin of servo pal HM55B_clk PIN 0 HM55B_en PIN 1 HM55B_Din_Dout PIN 2 Bi_clrLEDred PIN 6 ' when high + low 7 Bi_clrLEDgreen PIN 7 ' when high + low 6 '---initializations------------------------------------ INPUT nInp 'Make sure nInp isn't being driven. DO : LOOP UNTIL nInp 'Wait for ServoPAL to power up. LOW nInp 'Set pin to an output and hold it low PAUSE 100 ' for 100mS to reset ServoPAL. HIGH nInp 'Raise the pin. PAUSE 100 '---main program------------------------------------------ DO SERIN XBee_Tx, 84, [UD,LR] SERIN XBee_Tx, 84, [Btn1,Btn2] PULSOUT 15, 5 PULSIN 15, 1, time cmDistance = cmConstant ** time inDistance = inConstant ** time DEBUG HOME, "Boe Bot Com 13", CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL, CR, "cm = ", DEC3 cmDistance,CLREOL, CR, "in = ", DEC3 inDistance,CLREOL SELECT Btn2 'code locks up when pressed CASE Btn2 = 1 SEROUT XBee_Rx, 84, [indistance] ENDSELECT SELECT UD CASE < 90 UD = a GOSUB forwards CASE > 110 UD = b GOSUB backwards CASE 90 TO 110 UD = c ENDSELECT SELECT LR CASE < 116 LR = d GOSUB right CASE > 140 LR = e GOSUB left CASE 118 TO 140 LR = f ENDSELECT IF (UD = c)AND(LR = f) THEN GOSUB stall ENDIF LOOP '---Subroutines------------------------------------------ forwards: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN backwards: PULSOUT nInp, 500 'Program right servo for full reverse. PULSOUT nInp, 1000 'Program left servo for full reverse. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN left: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN right: PULSOUT nInp, 2000 'Turn left servo off. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN stall: PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 2000 'Turn left servo off. RETURNthough I've added some things to the code I still have same issues as before
Also, the transmitter and receiver can get out of sync and there is no way for the receiver to distinguish UD,LR data from Btn data. So you end up with UC data where you were expecting Btn data. For that reason, it can help to insert a sync character, one that will never be a data value. On the receiving end the command will look something like this:
SERIN XBee_Tx\XBee_rts, 84, [wait(sync),UD,LR,Btn1,Btn2]
where XBee_rts matches the hardware RTS pin, and the sync character may be something like a byte value of 255. With the RTS flow control implemented, it will also be possible to add a timeout to the SERIN, so the BOE can go off and do other things while it is waiting for a command. Or two way communication with flow control on both ends.
' {$STAMP BS2} ' Target device = BASIC Stamp 2 ' {$PBASIC 2.5} ' Language = PBASIC 2.5 ' -----[ I/O Pins ]----------------------------------------------------------- LcdPin PIN 0 ' I/O pin connected to LCD's RX Ping PIN 15 ' -----[ Constants ]---------------------------------------------------------- T9600 CON 84 ' True, 8-bits, no parity, 9600 LcdCls CON 12 ' Form feed -> clear screen LcdCr CON 13 ' Carriage return LcdOff CON 21 ' Turns display off LcdOn CON 22 ' Turns display on Line0 CON 128 ' Line 0, character 0 Line1 CON 148 ' Line 1, character 0 Define CON 248 ' Address defines cust char 0 InConstant CON 890 ' -----[ Variables ]---------------------------------------------------------- custChar VAR Nib ' Custom charcter selector index VAR Nib ' Eeprom index variable dotLine VAR Byte ' 5-pixel dotted line cursor VAR Nib ' Cursor placement value VAR Byte ' Value to be graphed. charCnt VAR Byte ' Character counting variable line VAR Byte ' Line0 or Line1 inDistance VAR Word time VAR Word ' -----[ Initialization ]----------------------------------------------------- PAUSE 100 ' Debounce power supply SEROUT LcdPin, T9600, [LcdOn, LcdCls] ' Initialize LCD PAUSE 5 ' 5 ms delay for clearing display custChar = 3 ' Select Custom Character 3 dotLine = %11111 ' Black all pixels in each line GOSUB Def_Horiz_Bar_Char ' Character define subroutine line = Line0 ' BarGraph on Line 0. ' -----[ Main Routine ]------------------------------------------------------- DO ' Main loop PULSOUT 15, 5 PULSIN 15, 1, time inDistance = inConstant ** time SEROUT 0, 84,[line1, DEC3 inDistance, "inches away"] value = inDistance GOSUB Bar_Graph ' Display as bar graph PAUSE 100 LOOP ' Repeat main loop ' -----[ Subroutine - Bar_Graph ]--------------------------------------------- Bar_Graph: ' Fill from left with black bars value = value MAX 80 ' Limit value - 0 to 80 charCnt = value / 5 ' Number of black bars custChar = 3 ' Choose black custom character IF charCnt > 0 THEN ' If black bars to print then FOR cursor = 0 TO charCnt - 1 ' Print charCnt - 1 black bars GOSUB Disp_Cust_Char ' Print the black bar NEXT ENDIF ' Display Custom Character 2 with a certain number of black columns. cursor = charCnt ' Place cursor custChar = value // 5 ' How many 5ths of a bar? ' Choose bit pattern for custom character definition LOOKUP custChar, [%00000, %10000, %11000, %11100, %11110], dotLine custChar = 2 ' Set custom character to 2 GOSUB Def_Horiz_Bar_Char ' Define the custom character GOSUB Disp_Cust_Char ' Display the custom character ' Print over everything to the right with spaces. IF (charCnt + 1) < 15 THEN ' Partial char left of char 15? FOR cursor = (charCnt + 1) TO 15 ' Fill to right with " " SEROUT LcdPin, T9600, [line + cursor, " "] NEXT ELSEIF value = 80 THEN ' Special case: value = 80 SEROUT LcdPin, T9600, [line + cursor, 3] ELSEIF charCnt = 14 THEN ' Special case: 75 <= value <= 80 SEROUT LcdPin, T9600, [line + 15, " "] ENDIF RETURN ' -----[ Subroutine - Def_Horiz_Bar_Char ]------------------------------------ Def_Horiz_Bar_Char: SEROUT LcdPin, T9600, ' Define custom character [Define + custChar] FOR index = 0 TO 7 ' 7 bytes, define 7 dotted lines SEROUT LcdPin, T9600, [dotLine] ' Send it to the LCD NEXT RETURN ' -----[ Subroutines - Disp_Cust_Char ]--------------------------------------- ' This subroutine displays a custom character. The line variable can ' be set to either Line0 or Line1, and the cursor variable can be set ' to a value between 0 and 15. The custChar variable selects one of the ' LCD's custom characters and should be set to a value between 0 and 7. Disp_Cust_Char: SEROUT LcdPin, T9600, ' Print custom character [line + cursor, custChar] RETURNI've also worked on the controller and boe bot codes last posted here http://forums.parallax.com/showthread.php?143468-push-button-routineI posted that code just before I left for work. I thought it was good to go. Turns out I lost the right turn functionality of it. I tried all kinds of things. Finally I started removing some of the older variables I hadnt used in awhile and assigning the proper space for what was left. It fixed that problem. I also omitted the debug statements (tip I got from Erco) and the code seemed quite fast and responsive. I also added Btn1 centers the ping servo.
controller
' {$STAMP BS2} ' {$PBASIC 2.5} ' XBee controller '---[Initialize]------------------------------------------------- PAUSE 5 SEROUT 0, 84, [22,12] SEROUT 0, 84,50, [22,17, "Hello", 13, "Mr.President!!!"] PAUSE 1000 '---[Variables]--------------------------------------------------- LR VAR Word UD VAR Word Btn1wrk VAR Byte Btn2wrk VAR Byte green VAR Byte red VAR Byte inDistance VAR Word mode VAR Bit '---[Constants]---------------------------------------------------- #SELECT $STAMP #CASE BS2, BS2E, BS2PE T9600 CON 84 #ENDSELECT stickUD CON 25 stickLD CON 25 Baud CON T9600 capture CON 500 '---[Pin descriptions]--------------------------------------------- Lcd PIN 0 Btn1 PIN 7 Btn2 PIN 5 Tx PIN 9 ' XBee DIN Rx PIN 10 ' XBee DOUT RTS PIN 8 ' XBee RTS LRjoy PIN 6 UDjoy PIN 4 Led_A PIN 15 Led_B PIN 14 PIEZO PIN 3 '---[Main]----------------------------------------------------------- Main: DO HIGH 6 PAUSE 2 RCTIME 6, 1, LR HIGH 4 PAUSE 2 RCTIME 4, 1, UD UD = UD/3 LR = LR/3 ' DEBUG HOME, "Controller Com 1" ,CLREOL, CR, ' "UD = ", DEC UD, CLREOL, CR, ' "LR = ", DEC LR, CLREOL, CR, ' "Btn1 = ", DEC Btn1, CLREOL, CR, ' "Btn2 = ", DEC Btn2, CLREOL, CR, ' "inDistance = ", DEC3 inDistance, CLREOL, CR, ' "mode = ", DEC mode, CLREOL SEROUT Rx\RTS, 84, ["!",UD,LR,Btn1,Btn2,mode] IF Btn1 = 1 THEN HIGH 15 ELSE LOW 15 IF Btn2 = 1 THEN mode = mode + 1 'GOSUB Get_ping ENDIF IF mode = 1 THEN HIGH 14 'GOSUB Get_ping IF mode = 0 THEN LOW 14 LOOP '---[ Subroutines ]-------------------------------------------------------- 'Get_ping: 'SERIN Tx\RTS, 84,149, Com_error, '[WAIT("in"),inDistance] 'GOTO main'return Com_Error: ' Handle timeout during tilt controlled navigation DEBUG "A communication error occurred...Reconnecting", CR ' Error message FREQOUT PIEZO, 100, 3000 ' Indicate error PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 1000 GOTO mainBoe Bot
'{$STAMP BS2} '{$PBASIC 2.5} 'Recieving Boe Bot '---[ Constants ]------------------------------------------- L_FWD CON 1000 L_REV CON 500 L_STOP CON 750 R_FWD CON 500 R_REV CON 1000 R_STOP CON 750 CmConstant CON 2260 InConstant CON 890 BAUD_MODE CON 84 '---[Variables]------------------------------------------- UD VAR Byte LR VAR Byte A VAR Bit B VAR Bit Btn1 VAR Bit Btn2 VAR Bit cmDistance VAR Word inDistance VAR Word time VAR Word mode VAR Bit '---[Pin Descriptions]------------------------------------ xbee_rts PIN 9 xbee_Rx PIN 10 ' Dout xbee_Tx PIN 11 ' Din nInp PIN 12 'Define the input pin of servo pal Alarm PIN 13 'Define the alarm pin of servo pal HM55B_clk PIN 0 HM55B_en PIN 1 HM55B_Din_Dout PIN 2 Bi_clrLEDred PIN 6 ' when high + low 7 Bi_clrLEDgreen PIN 7 ' when high + low 6 ping PIN 15 ping_servo PIN 14 '---[Initializations]------------------------------------ INPUT nInp 'Make sure nInp isn't being driven. DO : LOOP UNTIL nInp 'Wait for ServoPAL to power up. LOW nInp 'Set pin to an output and hold it low PAUSE 100 ' for 100mS to reset ServoPAL. HIGH nInp 'Raise the pin. PAUSE 100 '---[Main Program]------------------------------------------ DO SERIN XBee_Tx\xbee_rts, 84, [WAIT("!"),UD,LR,Btn1,Btn2,mode] PULSOUT 15, 5 PULSIN 15, 1, time cmDistance = cmConstant ** time inDistance = inConstant ** time ' DEBUG HOME, "Boe Bot Com 13", CLREOL, CR, ' "UD = ", DEC UD, CLREOL, CR, ' "LR = ", DEC LR, CLREOL, CR, ' "Btn1 = ", DEC Btn1, CLREOL, CR, ' "Btn2 = ", DEC Btn2, CLREOL, CR, ' "cm = ", DEC3 cmDistance,CLREOL, CR, ' "in = ", DEC3 inDistance,CLREOL, CR, ' "mode = ", DEC mode,CLREOL SELECT Btn1 CASE = 1 PULSOUT ping_servo,750 ENDSELECT 'SELECT Btn2 ' CASE = 1 'SEROUT XBee_Rx\xbee_rts, 84, ["in",indistance] 'ENDSELECT SELECT mode CASE = 1 IF LR = > 140 AND mode = 1 THEN PULSOUT ping_servo, 1200 IF LR = < 110 AND mode = 1 THEN PULSOUT ping_servo, 250 PULSOUT 15, 5 PULSIN 15, 1, time cmDistance = cmConstant ** time inDistance = inConstant ** time 'SEROUT XBee_Rx\xbee_rts, 84, ["in",indistance] ENDSELECT SELECT UD CASE < 80 GOSUB forwards CASE > 120 GOSUB backwards CASE 80 TO 120 UD = A ENDSELECT SELECT LR CASE > 140 IF LR = > 140 AND mode = 0 THEN GOSUB left ENDIF CASE < 90 IF LR = < 90 AND mode = 0 THEN GOSUB right ENDIF CASE 110 TO 140 LR = B ENDSELECT IF (UD = A)AND(LR = B) THEN GOSUB stall ENDIF LOOP '---[Subroutines]------------------------------------------ forwards: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN backwards: PULSOUT nInp, 500 'Program right servo for full reverse. PULSOUT nInp, 1000 'Program left servo for full reverse. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN left: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN right: PULSOUT nInp, 2000 'Turn left servo off. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN stall: PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 2000 'Turn left servo off. RETURNThe problems I mentioned in the other thread are still there. Im going to work on combining these three codes into two. I still havent managed to get the ping reading from the boe back to the controller without locking up the program. Any help I can get, would be great. ThanksIm gonna guess that it has to do with the +++ atch atid stuff that isnt present in the above code. Ive tried implementing it and I havent got it to work.
controller code
' {$STAMP BS2} ' {$PBASIC 2.5} ' XBee controller '---[Initialize]------------------------------------------------- PAUSE 5 SEROUT 0, 84, [22,12] SEROUT 0, 84,50, [22,17, "Hello", 13, "Mr.President!!!"] PAUSE 1000 '---[Variables]--------------------------------------------------- LR VAR Word UD VAR Word Btn1wrk VAR Byte Btn2wrk VAR Byte green VAR Byte red VAR Byte inDistance VAR Word mode VAR Bit Btn2new VAR Bit Btn2old VAR Bit '---[Constants]---------------------------------------------------- #SELECT $STAMP #CASE BS2, BS2E, BS2PE T9600 CON 84 #ENDSELECT stickUD CON 25 stickLD CON 25 Baud CON T9600 capture CON 500 '---[Pin descriptions]--------------------------------------------- Lcd PIN 0 Btn1 PIN 7 Btn2 PIN 5 Tx PIN 9 ' XBee DIN Rx PIN 10 ' XBee DOUT RTS PIN 8 ' XBee RTS LRjoy PIN 6 UDjoy PIN 4 Led_A PIN 15 Led_B PIN 14 PIEZO PIN 3 '---[Main]----------------------------------------------------------- Main: DO HIGH 6 PAUSE 2 RCTIME 6, 1, LR HIGH 4 PAUSE 2 RCTIME 4, 1, UD UD = UD/3 LR = LR/3 DEBUG HOME, "Controller Com 1" ,CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL, CR, "mode = ", DEC mode, CLREOL, CR, "in = ", DEC3 inDistance, CLREOL SEROUT Rx\RTS, 84, ["!",UD,LR,Btn1,Btn2,mode] IF Btn1 = 1 THEN HIGH 15 ELSE LOW 15 Btn2new = Btn2 IF Btn2new = 1 AND Btn2old = 0 THEN mode = mode + 1 'GOSUB Get_ping ENDIF Btn2old=Btn2new IF mode = 1 THEN HIGH 14 'GOSUB Get_ping IF mode = 0 THEN LOW 14 LOOP '---[ Subroutines ]-------------------------------------------------------- Get_ping: SERIN Tx\RTS, 84, 149, Com_error,[WAIT("in"),inDistance] 'receive ping reading or time out GOTO main 'return Com_Error: ' Handle timeout during tilt controlled navigation DEBUG " A communication error occurred...Reconnecting", CR ' Error message FREQOUT PIEZO, 100, 3000 ' Indicate error PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 1000 GOTO mainboe code
'{$STAMP BS2} '{$PBASIC 2.5} 'Recieving Boe Bot '---[ Constants ]------------------------------------------- L_FWD CON 1000 L_REV CON 500 L_STOP CON 750 R_FWD CON 500 R_REV CON 1000 R_STOP CON 750 CmConstant CON 2260 InConstant CON 890 BAUD_MODE CON 84 '---[Variables]------------------------------------------- UD VAR Byte LR VAR Byte A VAR Bit B VAR Bit Btn1 VAR Bit Btn2 VAR Bit inDistance VAR Word time VAR Word mode VAR Bit '---[Pin Descriptions]------------------------------------ xbee_rts PIN 9 xbee_Rx PIN 10 ' Dout xbee_Tx PIN 11 ' Din nInp PIN 12 'Define the input pin of servo pal Alarm PIN 13 'Define the alarm pin of servo pal HM55B_clk PIN 0 HM55B_en PIN 1 HM55B_Din_Dout PIN 2 Bi_clrLEDred PIN 6 ' when high + low 7 Bi_clrLEDgreen PIN 7 ' when high + low 6 ping PIN 15 ping_servo PIN 14 piezo PIN 4 '---[Initializations]------------------------------------ INPUT nInp 'Make sure nInp isn't being driven. DO : LOOP UNTIL nInp 'Wait for ServoPAL to power up. LOW nInp 'Set pin to an output and hold it low PAUSE 100 ' for 100mS to reset ServoPAL. HIGH nInp 'Raise the pin. PAUSE 100 '---[Main Program]------------------------------------------ main: DO SERIN XBee_Tx\xbee_rts, 84, [WAIT("!"),UD,LR,Btn1,Btn2,mode] PULSOUT 15, 5 PULSIN 15, 1, time inDistance = inConstant ** time DEBUG HOME, "Boe Bot Com 13", CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL, CR, "mode = ", DEC mode,CLREOL, CR, "in = ", DEC3 inDistance,CLREOL SELECT Btn1 CASE = 1 PULSOUT ping_servo,750 ENDSELECT 'SELECT Btn2 ' CASE = 1 'SEROUT XBee_Rx\xbee_rts, 84, ["in",indistance] 'ENDSELECT SELECT mode CASE = 1 IF LR = > 140 AND mode = 1 THEN PULSOUT ping_servo, 1200 IF LR = < 110 AND mode = 1 THEN PULSOUT ping_servo, 250 PULSOUT 15, 5 PULSIN 15, 1, time inDistance = inConstant ** time 'GOSUB send_ping ENDSELECT SELECT UD CASE < 80 GOSUB forwards CASE > 120 GOSUB backwards CASE 80 TO 120 UD = A ENDSELECT SELECT LR CASE > 140 IF LR = > 140 AND mode = 0 THEN GOSUB left ENDIF CASE < 90 IF LR = < 90 AND mode = 0 THEN GOSUB right ENDIF CASE 110 TO 140 LR = B ENDSELECT IF (UD = A)AND(LR = B) THEN GOSUB stall ENDIF LOOP '---[Subroutines]------------------------------------------ forwards: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN backwards: PULSOUT nInp, 500 'Program right servo for full reverse. PULSOUT nInp, 1000 'Program left servo for full reverse. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN left: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN right: PULSOUT nInp, 2000 'Turn left servo off. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN stall: PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 2000 'Turn left servo off. RETURN send_ping: SEROUT XBee_Rx\xbee_rts, 84, 149, Com_error,["in",indistance] Com_Error: ' Handle timeout during tilt controlled navigation DEBUG " A communication error occurred...Reconnecting", CR ' Error message FREQOUT PIEZO, 100, 3000 ' Indicate error PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 1000 GOTO maintest bar graph program. I eventually would like to have some variant of this working while in mode 1' {$STAMP BS2} ' Target device = BASIC Stamp 2 ' {$PBASIC 2.5} ' Language = PBASIC 2.5 ' -----[ I/O Pins ]----------------------------------------------------------- LcdPin PIN 0 ' I/O pin connected to LCD's RX Ping PIN 15 ' -----[ Constants ]---------------------------------------------------------- T9600 CON 84 ' True, 8-bits, no parity, 9600 LcdCls CON 12 ' Form feed -> clear screen LcdCr CON 13 ' Carriage return LcdOff CON 21 ' Turns display off LcdOn CON 22 ' Turns display on Line0 CON 128 ' Line 0, character 0 Line1 CON 148 ' Line 1, character 0 Define CON 248 ' Address defines cust char 0 InConstant CON 890 ' -----[ Variables ]---------------------------------------------------------- custChar VAR Nib ' Custom charcter selector index VAR Nib ' Eeprom index variable dotLine VAR Byte ' 5-pixel dotted line cursor VAR Nib ' Cursor placement value VAR Byte ' Value to be graphed. charCnt VAR Byte ' Character counting variable line VAR Byte ' Line0 or Line1 inDistance VAR Word time VAR Word ' -----[ Initialization ]----------------------------------------------------- PAUSE 100 ' Debounce power supply SEROUT LcdPin, T9600, [LcdOn, LcdCls] ' Initialize LCD PAUSE 5 ' 5 ms delay for clearing display custChar = 3 ' Select Custom Character 3 dotLine = %11111 ' Black all pixels in each line GOSUB Def_Horiz_Bar_Char ' Character define subroutine line = Line0 ' BarGraph on Line 0. ' -----[ Main Routine ]------------------------------------------------------- DO ' Main loop PULSOUT 15, 5 PULSIN 15, 1, time inDistance = inConstant ** time SEROUT 0, 84,[line1, DEC3 inDistance, "inches away"] value = inDistance GOSUB Bar_Graph ' Display as bar graph PAUSE 100 LOOP ' Repeat main loop ' -----[ Subroutine - Bar_Graph ]--------------------------------------------- Bar_Graph: ' Fill from left with black bars value = value MAX 80 ' Limit value - 0 to 80 charCnt = value / 5 ' Number of black bars custChar = 3 ' Choose black custom character IF charCnt > 0 THEN ' If black bars to print then FOR cursor = 0 TO charCnt - 1 ' Print charCnt - 1 black bars GOSUB Disp_Cust_Char ' Print the black bar NEXT ENDIF ' Display Custom Character 2 with a certain number of black columns. cursor = charCnt ' Place cursor custChar = value // 5 ' How many 5ths of a bar? ' Choose bit pattern for custom character definition LOOKUP custChar, [%00000, %10000, %11000, %11100, %11110], dotLine custChar = 2 ' Set custom character to 2 GOSUB Def_Horiz_Bar_Char ' Define the custom character GOSUB Disp_Cust_Char ' Display the custom character ' Print over everything to the right with spaces. IF (charCnt + 1) < 15 THEN ' Partial char left of char 15? FOR cursor = (charCnt + 1) TO 15 ' Fill to right with " " SEROUT LcdPin, T9600, [line + cursor, " "] NEXT ELSEIF value = 80 THEN ' Special case: value = 80 SEROUT LcdPin, T9600, [line + cursor, 3] ELSEIF charCnt = 14 THEN ' Special case: 75 <= value <= 80 SEROUT LcdPin, T9600, [line + 15, " "] ENDIF RETURN ' -----[ Subroutine - Def_Horiz_Bar_Char ]------------------------------------ Def_Horiz_Bar_Char: SEROUT LcdPin, T9600, ' Define custom character [Define + custChar] FOR index = 0 TO 7 ' 7 bytes, define 7 dotted lines SEROUT LcdPin, T9600, [dotLine] ' Send it to the LCD NEXT RETURN ' -----[ Subroutines - Disp_Cust_Char ]--------------------------------------- ' This subroutine displays a custom character. The line variable can ' be set to either Line0 or Line1, and the cursor variable can be set ' to a value between 0 and 15. The custChar variable selects one of the ' LCD's custom characters and should be set to a value between 0 and 7. Disp_Cust_Char: SEROUT LcdPin, T9600, ' Print custom character [line + cursor, custChar] RETURNBoe code
'{$STAMP BS2} '{$PBASIC 2.5} 'Recieving Boe Bot '---[ Constants ]------------------------------------------- L_FWD CON 1000 L_REV CON 500 L_STOP CON 750 R_FWD CON 500 R_REV CON 1000 R_STOP CON 750 CmConstant CON 2260 InConstant CON 890 BAUD_MODE CON 84 '---[Variables]------------------------------------------- UD VAR Byte LR VAR Byte Btn1 VAR Bit Btn2 VAR Bit inDistance VAR Word time VAR Word mode VAR Bit '---[Pin Descriptions]------------------------------------ xbee_rts PIN 9 xbee_Rx PIN 10 ' Dout xbee_Tx PIN 11 ' Din nInp PIN 12 'Define the input pin of servo pal Alarm PIN 13 'Define the alarm pin of servo pal HM55B_clk PIN 0 HM55B_en PIN 1 HM55B_Din_Dout PIN 2 Bi_clrLEDred PIN 6 ' when high + low 7 Bi_clrLEDgreen PIN 7 ' when high + low 6 ping PIN 15 ping_servo PIN 14 piezo PIN 4 '---[Initializations]------------------------------------ INPUT nInp 'Make sure nInp isn't being driven. DO : LOOP UNTIL nInp 'Wait for ServoPAL to power up. LOW nInp 'Set pin to an output and hold it low PAUSE 100 ' for 100mS to reset ServoPAL. HIGH nInp 'Raise the pin. PAUSE 100 '---[Main Program]------------------------------------------ main: DO SERIN XBee_Tx\xbee_rts, 84, [WAIT("!"),UD,LR,Btn1,Btn2,mode] PULSOUT 15, 5 PULSIN 15, 1, time inDistance = inConstant ** time DEBUG HOME, "Boe Bot Com 13", CLREOL, CR, "UD = ", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL, CR, "Btn1 = ", DEC Btn1, CLREOL, CR, "Btn2 = ", DEC Btn2, CLREOL, CR, "mode = ", DEC mode,CLREOL, CR, "in = ", DEC3 inDistance,CLREOL SELECT Btn1 CASE = 1 PULSOUT ping_servo,750 ENDSELECT 'SELECT Btn2 ' CASE = 1 'SEROUT XBee_Rx\xbee_rts, 84, ["in",indistance] 'ENDSELECT SELECT mode CASE = 1 IF LR = > 140 AND mode = 1 THEN PULSOUT ping_servo, 1200 IF LR = < 110 AND mode = 1 THEN PULSOUT ping_servo, 250 PULSOUT 15, 5 PULSIN 15, 1, time inDistance = inConstant ** time 'GOSUB send_ping ENDSELECT SELECT UD CASE < 80 GOSUB forwards CASE > 120 GOSUB backwards CASE 80 TO 120 GOSUB stall ENDSELECT SELECT LR CASE > 140 IF LR = > 140 AND mode = 0 THEN GOSUB left ENDIF CASE < 90 IF LR = < 90 AND mode = 0 THEN GOSUB right ENDIF CASE 110 TO 140 GOSUB stall ENDSELECT LOOP '---[Subroutines]------------------------------------------ forwards: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN backwards: PULSOUT nInp, 500 'Program right servo for full reverse. PULSOUT nInp, 1000 'Program left servo for full reverse. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN left: PULSOUT nInp, 1000 'Program right servo for full forward. PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN right: PULSOUT nInp, 2000 'Turn left servo off. PULSOUT nInp, 500 'Program left servo for full forward. PULSOUT nInp, 10 'Program alarm for 1 seconds. DO : LOOP UNTIL Alarm 'Wait for Alarm. RETURN stall: PULSOUT nInp, 2000 'Turn right servo off. PULSOUT nInp, 2000 'Turn left servo off. RETURN send_ping: SEROUT XBee_Rx\xbee_rts, 84, 149, Com_error,["in",indistance] Com_Error: ' Handle timeout during tilt controlled navigation DEBUG " A communication error occurred...Reconnecting", CR ' Error message FREQOUT PIEZO, 100, 3000 ' Indicate error PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 100 FREQOUT PIEZO, 100, 3000 PAUSE 1000 GOTO mainwireless Remote
' Xbee Servo Base test #1 ' {$STAMP BS2} ' {$PBASIC 2.5} '----------------[Initialize]--------------------------- PAUSE 5 SEROUT 2, 84, [22,12] SEROUT 2, 84,50, [22,17, "Hello", 13, "Geno"] PAUSE 1000 SEROUT 2, 84, [22,12] PAUSE 5 '---------------[Variables]----------------------------- LR VAR Word UD VAR Word btn1wrk VAR Byte btn2wrk VAR Byte Led VAR Bit Led1 VAR Bit '---------------[Constants]----------------------------- RTS PIN 11 Tx PIN 14 ' XBee DIN Rx PIN 15 ' XBee DOUT stickUD CON 25 stickLD CON 25 '-------------------------[Main]--------------------------- Main: DO HIGH 13 PAUSE 2 RCTIME 13, 1, UD HIGH 12 PAUSE 2 RCTIME 12, 1, LR DEBUG HOME, "UD =", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL SEROUT Tx, 84, [UD] SEROUT Tx, 84, [LR] LOOPBoe-Bot
'Xbee Servo Remote test #2 ' {$STAMP BS2} ' {$PBASIC 2.5} #SELECT $STAMP #CASE BS2, BS2E, BS2PE T9600 CON 84 #CASE BS2SX, BS2P T9600 CON 240 #CASE BS2PX T9600 CON 396 #ENDSELECT '---------------[Variables]----------------------------- LR VAR Word UD VAR Word '---------------[Constants]----------------------------- RTS PIN 11 Tx PIN 6 ' XBee DIN Rx PIN 7 ' XBee DOUT stickUD CON 25 stickLD CON 25 Baud CON T9600 'Set Baud Rate ' ************** XBee Configuration ********************* PAUSE 500 DEBUG "Configuring XBee...",CR PAUSE 2000 ' Guard Time SEROUT Tx,Baud,["+++"] ' Command Mode sequence PAUSE 2000 ' Guard Time SEROUT Tx,Baud,["ATD6 1,CN",CR] ' RTS enable (D6 1) ' Exit Command Mode (CN) ' ***************** Main Loop Main: DO SERIN Rx, 84, [UD, LR] DEBUG HOME, "UD =", DEC UD, CLREOL, CR, "LR = ", DEC LR, CLREOL IF UD > stickUD +2 THEN GOSUB Forward IF UD < stickUD -2 THEN GOSUB Backwards IF UD = stickUD +2 OR -2 THEN GOSUB Stay IF LR > stickLD +2 THEN GOSUB Right IF LR < stickLD -2 THEN GOSUB Left IF LR = stickLD +2 OR -2 THEN GOSUB Stay LOOP '-------------------------[Sub Routine]----------------------- Forward: PULSOUT 13, 850 PULSOUT 12, 650 PAUSE 20 RETURN Backwards: PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 20 RETURN Right: PULSOUT 13, 848 PULSOUT 12, 850 PAUSE 20 RETURN Left: PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 RETURN Stay: PULSOUT 13, 750 PULSOUT 12, 755 PAUSE 20 RETURN