Boe-Bot/Tank Tread/CMU-Cam problems

Chefdude548 · 2008-11-12 17:46

I have a Boe-Bot with the tank tread kit and a CMU-Cam that I am trying to get working. I am running the demo code from the CMU-Cam, and the program works, the calibration works and the color tracking works, but the servos refuse to work.

This code is what I am using:

'{$STAMP BS2}
'{$PBASIC 2.5}

' CMUcam1 BoeBot AppMod 1.2 Demo Program
' Copyright 2006, Seattle Robotics
' Basic Stamp 2
' CMUcam1 jumpered for 9600 Baud
' RA Nucci 07/13/2006

' Setup pins for buttons
INPUT 6
INPUT 8
HIGH 6
HIGH 8

RcvData VAR Byte(10) ' Array
SaveColorData VAR Byte(6) ' Array
Result VAR Word
N VAR Byte ' General purpose counter variable
LostTrack VAR Nib
Switch VAR Nib
Debounce VAR Nib
Function VAR Nib

Main:
' Pause for CMUcam1 startup
  FOR N = 2 TO 5 ' Flash all LED's during pause
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]255]
    PAUSE 400
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0]
    PAUSE 400
  NEXT

  FREQOUT 10, 400, 2500 ' Beep to indicate ready

ProgTop:
  ' Select function from push buttons
  GOSUB GetButton
  Debounce=Switch
  GOSUB GetButton
  ' Get button press twice and compare if not the same repeat
  IF Debounce <> Switch THEN ProgTop
  IF Switch = 2 THEN ' If Go/Stop button pushed then beep
    FREQOUT 10, 400, 2500 ' beep
  ENDIF
BRANCH Switch, [noparse][[/noparse]ProgTop,Buttonselect,Execute]

Buttonselect:
  Function = Function + 1
  IF Function > 9 THEN
    Function = 1
    SHIFTOUT 5,11,MSBFIRST,
  ENDIF
  PAUSE 300
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]DCD(Function-1)]
GOTO ProgTop


Execute:
  BRANCH Function, [noparse][[/noparse]ProgTop,Calibrate,SaveColor,TrackColor,Avoid,Adapt,FollowLine,Point,Color]
GOTO ProgTop


GetButton:

  Switch=0

  IF IN6 = 0 THEN
    Switch = 1 ' Select button pressed
    RETURN
  ENDIF

  IF IN8 = 0 THEN
    Switch = 2 ' GO/Stop button pressed
    RETURN
  ENDIF

  IF IN8 = 0 AND IN6 = 0 THEN
    Switch = 3 ' Both buttons pressed at the same time
  ENDIF

RETURN


' Track the color previously saved in EEPROM
TrackColor:
  GOSUB ConfigureCMUcam
  ' Read EEPROM into SaveColorData array
  READ 0, SaveColorData(0)
  READ 1, SaveColorData(1)
  READ 2, SaveColorData(2)
  READ 3, SaveColorData(3)
  READ 4, SaveColorData(4)
  READ 5, SaveColorData(5)
  ' Set Track Color values from EEPROM
  SEROUT 7,84,[noparse][[/noparse]"TC"]
  SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(0)]
  SEROUT 7,84,[noparse][[/noparse]" "]
  SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(1)]
  SEROUT 7,84,[noparse][[/noparse]" "]
  SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(2)]
  SEROUT 7,84,[noparse][[/noparse]" "]
  SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(3)]
  SEROUT 7,84,[noparse][[/noparse]" "]
  SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(4)]
  SEROUT 7,84,[noparse][[/noparse]" "]
  SEROUT 7,84,[noparse][[/noparse]DEC SaveColorData(5),CR]
  PAUSE 200

TopTrack:
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
  GOSUB GetButton  ' Check for stop button press
  IF Switch = 2 THEN' Switch 2 is stop button
  ' Two beeps for Stop
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]DCD (Function-1)]
    PAUSE 200
    FREQOUT 10, 400, 2500
    PAUSE 200
    FREQOUT 10, 400, 2500
    ' Reset the green tracking LED
    SEROUT 7, 84, [noparse][[/noparse]"L1 0",CR] ' Green LED off
    PAUSE 100
    SEROUT 7, 84, [noparse][[/noparse]"L1 2",CR] ' Green LED auto mode
    PAUSE 100
    GOTO ProgTop
  ENDIF

  ' Send command - Track color (with no arguments) -
  ' will track last color grabbed by TW command
  SEROUT 7, 84, [noparse][[/noparse]"TC",CR]
  SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
  PAUSE 100

  ' 45 is aprox H center, 70 is aprox V center

' Track and follow logic
IF Function = 3 THEN
  IF RCVData(2) > 65 AND RcvData(9) > 25 THEN RotateLeft
  IF RCVData(2) < 25 AND RcvData(9) > 25 THEN RotateRight
  IF RCVData(7) > 120 AND RcvData(9) > 25 THEN Bwd
  IF RCVData(7) < 60  AND RcvData(9) > 25 THEN Fwd
ENDIF

' Move and avoid logic
IF Function = 4 THEN
  IF RCVData(3) > 50 AND RcvData(9) > 20 THEN Fwd
  IF RCVData(3) < 50 THEN BackUp
  IF RcvData(9) <= 20 THEN BackUp
ENDIF

' Adaptive tracking logic
IF Function = 5 THEN
  IF RCVData(2) > 65 AND RcvData(9) > 25 THEN RotateLeft
  IF RCVData(2) < 25 AND RcvData(9) > 25 THEN RotateRight
  IF RCVData(7) < 60 AND RcvData(9) > 25 THEN Fwd
  IF LostTrack > 6 THEN TW
  LostTrack = LostTrack + 1
  IF RcvData(9) > 25 THEN LostTrack = 0
ENDIF

' Line Following logic
IF Function = 6 THEN
  IF RCVData(2) > 50 AND RcvData(9) > 5 THEN TurnLeft ' 45
  IF RCVData(2) < 40 AND RcvData(9) > 5 THEN TurnRight ' 45
  IF RCVData(2) >= 40 AND RCVData(2) =< 50 AND RcvData(9) > 5 THEN Fwd
ENDIF

' Finger point logic
IF Function = 7 THEN
  IF RCVData(2) > 60 AND RcvData(9) > 25 THEN RotateRight ' Centroid X
  IF RCVData(2) < 30 AND RcvData(9) > 25 THEN RotateLeft ' Centroid X
  IF RCVData(3) > 30 AND RcvData(9) > 15 THEN Bwd ' Centroid Y
ENDIF

' Color logic
IF Function = 8 THEN

  SEROUT 7, 84, [noparse][[/noparse]"TC 30 240 16 30 16 30",CR] ' Red
  SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
  IF RcvData(9) > 20 THEN
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]195] ' All Red LED's on
    PAUSE 500
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
  ENDIF

  SEROUT 7, 84, [noparse][[/noparse]"TC 16 40 30 240 90 170",CR] ' Green
  SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
  IF RcvData(9) > 20 THEN
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]24] ' All Green LED's on
    PAUSE 500
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
  ENDIF

  SEROUT 7, 84, [noparse][[/noparse]"TC 200 240 200 240 16 50",CR] ' Yellow
  SERIN 9, 84, [noparse][[/noparse]STR RcvData\10]
  IF RcvData(9) > 20 THEN
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]36] ' All Yellow LED's on
    PAUSE 500
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
  ENDIF

ENDIF

  ' Flash LED's to show bottom of tracking loop
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]129] ' 100000001
GOTO TopTrack


' Calibrate CMUcam1 for lighting conditions
Calibrate:
  ' Reset CMUcam1
  SEROUT 7, 84, [noparse][[/noparse]"RS",CR]
  PAUSE 200
  ' Turn on auto adjust
  SEROUT 7, 84, [noparse][[/noparse]"CR 18 44",CR]
  PAUSE 100
  ' Delay loop for CMUcam1 to auto adjust to lighting conditions
  FOR N = 2 TO 255
    PAUSE 100
    SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]DCD (N)]
    GOSUB GetButton ' Check for stop button pressed
    IF Switch = 2 THEN EXIT ' Switch 2 is stop button
  NEXT
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' 000000000
  ' Turn off auto adjust
  SEROUT 7, 84, [noparse][[/noparse]"CR 18 44 19 32",CR]
  PAUSE 100
  GOSUB ConfigureCMUcam
  SHIFTOUT 5,11,MSBFIRST, ' 100000000
  FREQOUT 10, 400, 2500
  PAUSE 300
  FREQOUT 10, 400, 2500
GOTO ProgTop


SaveColor:
' sample and save color
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0]
  SEROUT 7,84,[noparse][[/noparse]"PM 1",CR]
  PAUSE 100
  ' Set a small window in center
  SEROUT 7,84,[noparse][[/noparse]"SW 30 54 50 90",CR]
  PAUSE 100
  ' Set raw mode 3
  SEROUT 7,84,[noparse][[/noparse]"RM 3",CR]
  PAUSE 100
  'set minimun and maximun values
  SaveColorData(0)=16
  SaveColorData(1)=240
  SaveColorData(2)=16
  SaveColorData(3)=240
  SaveColorData(4)=16
  SaveColorData(5)=240
  ' Get Mean
  SEROUT 7,84,[noparse][[/noparse]"GM",CR]
  SERIN 9,84, [noparse][[/noparse]STR rcvdata\8]
  PAUSE 200
  IF rcvdata(2)<=46 THEN gm1
    SaveColorData(0)=(rcvdata(2)-30)
  gm1:
  IF rcvdata(3)<=46 THEN gm2
    SaveColorData(2)=(rcvdata(3)-30)
  gm2:
  IF rcvdata(4)<=46 THEN gm3
    saveColorData(4)=(rcvdata(4)-30)
  gm3:
  IF rcvdata(2)>210 THEN gm4
    saveColorData(1)=(rcvdata(2)+30)
  gm4:
  IF rcvdata(3)>240 THEN gm5
    saveColorData(3)=(rcvdata(3)+30)
  gm5:
  IF rcvdata(4)>240 THEN gm6
    saveColorData(5)=(rcvdata(4)+30)
  gm6:
  ' Set raw mode to 0
  SEROUT 7,84,[noparse][[/noparse]"RM 0",CR]
  PAUSE 100
  ' Return window to full size
  SEROUT 7,84,[noparse][[/noparse]"SW",CR]
  ' Write color tracking range values to end of EEPROM
  WRITE 0,saveColorData(0)
  WRITE 1,saveColorData(1)
  WRITE 2,saveColorData(2)
  WRITE 3,saveColorData(3)
  WRITE 4,saveColorData(4)
  WRITE 5,saveColorData(5)
  GOSUB ConfigureCMUcam
  SHIFTOUT 5,11,MSBFIRST,
  ' Two beeps
  FREQOUT 10, 400, 2500
  PAUSE 400
  FREQOUT 10, 400, 2500
  PAUSE 400
GOTO ProgTop


Avoid:
  GOSUB ConfigureCMUcam
  SEROUT 7,84,[noparse][[/noparse]"TW",CR]
  PAUSE 1000 ' Wait 1 second for TW
GOTO TopTrack


Adapt:
  GOSUB ConfigureCMUcam
TW:
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]255] ' All LED's on
  FREQOUT 10, 150, 2500 ' Beep
  PAUSE 100
  FREQOUT 10, 150, 2500 ' Beep
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]0] ' All LED's off
  SEROUT 7,84,[noparse][[/noparse]"TW",CR] ' Send Track Window command
  PAUSE 1000 ' Wait 1 second for TW to complete
  LostTrack = 0 ' Reset LostTrack counter
GOTO TopTrack


FollowLine:
  GOSUB ConfigureCMUcam
  SEROUT 7,84,[noparse][[/noparse]"TC 16 50 16 50 16 50",CR]' Black line color
  PAUSE 100
GOTO Toptrack


Point:
  GOSUB ConfigureCMUcam
  SEROUT 7,84,[noparse][[/noparse]"TC 200 240 180 240 50 100",CR] ' Skin color
  PAUSE 100
GOTO Toptrack


Color:
  GOSUB ConfigureCMUcam
GOTO Toptrack


ConfigureCMUcam:
  SEROUT 7,84,[noparse][[/noparse]"PM 1",CR]' Poll Mode
  PAUSE 100
  SEROUT 7,84,[noparse][[/noparse]"RM 3",CR]' Raw Mode
  PAUSE 100
  SEROUT 7,84,[noparse][[/noparse]"CR 32 8",CR]' White Balance smart mode
  PAUSE 100
RETURN


BackUp:
  ' backup
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]195]
  FOR n = 1 TO 15
    PULSOUT 14,1000 ' **
    PULSOUT 15,500 ' **
    PAUSE 15
  NEXT
  PAUSE 200
  ' rotate about 90 degrees
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]32]
  FOR n = 1 TO 100
    PULSOUT 14,1000 ' **
    PULSOUT 15,1000 ' **
  NEXT
GOTO TopTrack

RotateLeft:
  SHIFTOUT 5,11,MSBFIRST,
  FOR n = 1 TO 10
    PULSOUT 14,500 ' **
    PULSOUT 15,500 ' **
  NEXT
GOTO TopTrack
RotateRight:
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]32]
  FOR n = 1 TO 10
    PULSOUT 14,1000 ' **
    PULSOUT 15,1000 ' **
  NEXT
GOTO TopTrack
TurnLeft:
  SHIFTOUT 5,11,MSBFIRST,
  FOR n = 1 TO 40
    PULSOUT 14,500 ' **
    PULSOUT 15,750 ' **
  NEXT
GOTO TopTrack
TurnRight:
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]32]
  FOR n = 1 TO 40
    PULSOUT 14,750 ' **
    PULSOUT 15,1000 ' **
  NEXT
GOTO TopTrack
Bwd:
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]195]
  FOR n = 1 TO 10
    PULSOUT 14,1000 ' **
    PULSOUT 15,500 ' **
    PAUSE 15
  NEXT
GOTO TopTrack
Fwd:
  SHIFTOUT 5,11,MSBFIRST,[noparse][[/noparse]24]
  FOR n = 1 TO 10
    PULSOUT 14,500 ' **
    PULSOUT 15,1000 ' **
    PAUSE 15
  NEXT
GOTO TopTrack

On a topic of lesser importance, I am also trying to get code working for the gripper tool, if anyone has any tips.

Thank you in advance

allanlane5 · 2008-11-12 19:18

The "control signal" for a Servo is a PULSOUT of a certain size, 1 mSec to 2 mSec depending on position, REPEATED every 20 to 50 mSec. The "center" position (1.5 mSec, or PULSOUT ServoPin, 750) is "stop".

If you don't do the repeat, the servo is not going to move. It has a very simple comparator on-board, and each input pulse it compares with its current position, and moves the 'output shaft' a little to compensate. Thus you need MULTIPLE 20 mSec "refreshes" of the Servo signal to get anything to happen.

Secondly, for this to work the BS2 and the Servo MUST share a 'common ground', otherwise the BS2 servo control signal is "floating" compared to the Servo and the Servo can't read it.

Chefdude548 · 2008-11-13 14:49

Thank you. I must not have thought of that, as this is the code that comes with the CMU-Cam.

I am sorry to say that that did not help. I can't see where the problem is. This is the demo code that comes with the camera.

Post Edited (Chefdude548) : 11/14/2008 5:42:18 PM GMT

Boe-Bot/Tank Tread/CMU-Cam problems

Comments