BS2 Code To Test Roomba SCI
skynugget
Posts: 172
Hey all,
I was in need to test all of the sensors on my Roomba Discovery with a BS2. Here is what I came up with, hopefully it can save some of you hackers some time! I am neither a Stamp or Roomba Guru, so its not pritty, and it just about uses every bit of eeprom and ram available on a BS2 to get it done. Hope it helps someone out, if you make it better post it here. :>
I was in need to test all of the sensors on my Roomba Discovery with a BS2. Here is what I came up with, hopefully it can save some of you hackers some time! I am neither a Stamp or Roomba Guru, so its not pritty, and it just about uses every bit of eeprom and ram available on a BS2 to get it done. Hope it helps someone out, if you make it better post it here. :>
' {$STAMP BS2}
' {$PBASIC 2.5}
'roomba sensor test - used to see all of the values from the roomba sci via basic stamp
'puts roomba into safe mode, then debugs
RooDD PIN 12
RooRXD PIN 11
RooTXD PIN 10
LED PIN 7
fbaud CON 32 '19200
sbaud CON 396 '2400
' Roomba commands
RooStart CON 128
RooBaud CON 129
RooControl CON 130
RooFull CON 132
RooPower CON 133
RooSpot CON 134
RooClean CON 135
RooDrive CON 137
RooMotors CON 138
RooSong CON 140
RooPlay CON 141
RooSensors CON 142
RooDock CON 143
'roomba sensor vars
'packet 1
RooBumpwheel VAR Byte
RooWall VAR Bit
RooCliffL VAR Bit
RooCliffFL VAR Bit
RooCliffFR VAR Bit
RooCliffR VAR Bit
RooVWall VAR Bit
RooMOTO VAR Byte
RooDirtL VAR Byte
RooDirtR VAR Byte
'Packet 2
RooRemOp VAR Byte
RooButtons VAR Byte
RooDistance VAR Word
RooAngle VAR Word
'packet 3
RooVolt VAR Word
RooCState VAR Nib
RooAmp VAR Word
RooTemp VAR Byte
RooCharge VAR Word
RooCapacity VAR Word
i VAR Nib 'counter
INIT:
'debug screen
DEBUG CLS
DEBUG " RoomBa Readings ", CR
DEBUG "-------------------------------------", CR
DEBUG "WheelDrops L C R Bump L R ", CR
DEBUG "-------------------------------------", CR
DEBUG "Cliffs L FL FR R Wall VW ", CR
DEBUG "-------------------------------------", CR
DEBUG "MotO DL DR MB V SB ", CR
DEBUG "-------------------------------------", CR
DEBUG "Dirt Detect L R ", CR
DEBUG "-------------------------------------", CR
DEBUG "OpCode Btns Pr St Cn Mx ", CR
DEBUG "-------------------------------------", CR
DEBUG "Distance Angle ", CR
DEBUG "-------------------------------------", CR
DEBUG "mVolts mAmps ", CR
DEBUG "-------------------------------------", CR
DEBUG "Temp ChargeState ", CR
DEBUG "-------------------------------------", CR
DEBUG "Charge mAmpH ", CR
' wake up Roomba
LOW RooDD
PAUSE 100
HIGH RooDD
PAUSE 2000
' pulse dd 3 times to go to 19.2kbps
FOR i = 0 TO 2
LOW RooDD : LOW LED : PAUSE 250
HIGH RooDD : HIGH LED : PAUSE 250
NEXT
' start up SCI, then downshift speed again to 2400 bps for basic stamp
SEROUT RooRXD, fbaud, [noparse][[/noparse]RooStart]
PAUSE 100
SEROUT RooRXD, fbaud, [noparse][[/noparse]RooBaud, 3] ' 2400
PAUSE 100
SEROUT RooRXD, sbaud, [noparse][[/noparse]RooStart]
PAUSE 100
SEROUT RooRXD, sbaud, [noparse][[/noparse]RooControl]
PAUSE 100
'SEROUT RooRXD, sbaud, [noparse][[/noparse]RooFull]
Main:
DO
SEROUT RooRXD, sbaud, [noparse][[/noparse]RooSensors, 1]
SERIN RooTXD, sbaud, 2000, NoGo, [noparse][[/noparse]RooBumpwheel,RooWall,RooCliffL,RooCliffFL,RooCliffFR,
RooCliffR,RooVWall,RooMOTO,RooDirtL,RooDirtR]
SEROUT RooRXD, sbaud, [noparse][[/noparse]RooSensors, 2]
SERIN RooTXD, sbaud, 2000, NoGo, [noparse][[/noparse]RooRemOp,RooButtons,RooDistance.HIGHBYTE,RooDistance.LOWBYTE,
RooAngle.HIGHBYTE,RooAngle.LOWBYTE]
SEROUT RooRXD, sbaud, [noparse][[/noparse]RooSensors, 3]
SERIN RooTXD, sbaud, 2000, NoGo, [noparse][[/noparse]RooCState,RooVolt.HIGHBYTE,RooVolt.LOWBYTE,RooAmp.HIGHBYTE,
RooAmp.LOWBYTE,RooTemp,RooCharge.HIGHBYTE,RooCharge.LOWBYTE,
RooCapacity.HIGHBYTE,RooCapacity.LOWBYTE]
GOSUB DebugOOt
LOOP
DebugOOt:
'clear error
DEBUG CRSRXY,0,1 ,"--------------------", CR
'BumpWheel Set
DEBUG CRSRXY,13,2, DEC roobumpwheel.BIT3
DEBUG CRSRXY,18,2, DEC roobumpwheel.BIT4
DEBUG CRSRXY,23,2, DEC roobumpwheel.BIT2
DEBUG CRSRXY,32,2, DEC roobumpwheel.BIT1
DEBUG CRSRXY,36,2, DEC roobumpwheel.BIT0
'cliffs
DEBUG CRSRXY,9,4, DEC RooCliffL
DEBUG CRSRXY,14,4, DEC RooCliffFL
DEBUG CRSRXY,19,4, DEC RooCliffFR
DEBUG CRSRXY,23,4, DEC RooCliffR
'walls
DEBUG CRSRXY,30,4, DEC RooWall
DEBUG CRSRXY,36,4, DEC RooVwall
'motor over current
DEBUG CRSRXY,12,6, DEC Roomoto.BIT4
DEBUG CRSRXY,17,6, DEC Roomoto.BIT3
DEBUG CRSRXY,22,6, DEC Roomoto.BIT2
DEBUG CRSRXY,26,6, DEC Roomoto.BIT1
DEBUG CRSRXY,31,6, DEC Roomoto.BIT0
'Dirt Detect
DEBUG CRSRXY, 20,8, DEC RooDirtL
DEBUG CRSRXY, 30,8, DEC RooDirtR
'remote codes and buttons
DEBUG CRSRXY,7,10, DEC RooRemOp
DEBUG CRSRXY,20,10, DEC RooButtons.BIT3
DEBUG CRSRXY,25,10, DEC RooButtons.BIT2
DEBUG CRSRXY,30,10, DEC RooButtons.BIT1
DEBUG CRSRXY,35,10, DEC RooButtons.BIT0
'distance and angle
DEBUG CRSRXY,9,12, SDEC RooDistance
DEBUG CRSRXY,26,12, SDEC RooAngle, 11
'volts amps
DEBUG CRSRXY,7,14, DEC RooVolt
DEBUG CRSRXY,26,14, SDEC RooAmp, 11
'Temp Chargstate
DEBUG CRSRXY,5,16, SDEC RooTemp
'DEBUG CRSRXY,26,16, " "
SELECT RooCState
CASE 0
DEBUG CRSRXY,26,16, "Not Charging", 11
CASE 1
DEBUG CRSRXY,26,16, "Recover",11
CASE 2
DEBUG CRSRXY,26,16, "Charging",11
CASE 3
DEBUG CRSRXY,26,16, "Trickle" ,11
CASE 4
DEBUG CRSRXY,26,16, "Waiting" ,11
CASE 5
DEBUG CRSRXY,26,16, "Error" ,11
ENDSELECT
'charge AND cap
DEBUG CRSRXY,7,18, DEC RooCharge
DEBUG CRSRXY,26,18, DEC RooCapacity, 11
RETURN
NoGO:
DEBUG CRSRXY,0,1, "***COMM ERROR!",CR
GOTO main