roboforum.ru

' Scribbler Robot PHASE 1
' Eric Ostendorff 3/10/2008
' modified Parallax readir routine
' drive robot with IR remote/Sony TV codes
' MUTE button starts track mode, STOP ends track mode

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

' define variables
irPulse        VAR     Word                  ' Stores pulse widths
remoteCode     VAR     Byte                  ' Stores remote code
trackflag      VAR     Bit                   ' 1 for track mode

main:                                        ' start main program
trackflag=0                                  ' 0 = driving mode
LOW 8:LOW 9:LOW 10                           ' LEDs off = driving mode
GOSUB readir                                 ' read IR remote code
IF remoteCode=101 THEN GOSUB stopp           'center button=stop
IF remoteCode=116 THEN GOSUB fwd             'up arrow=forward
IF remoteCode=117 THEN GOSUB bwd             'down arrow=backward
IF remoteCode=52  THEN GOSUB left            'left arrow=left
IF remoteCode=51  THEN GOSUB right           'right arrow=right
IF remoteCode=20  THEN GOSUB track           'MUTE = track routine
GOTO main                                  'loop back, ignore other keys

fwd:PULSOUT 12,3000     'right motor forward calibration 3000=max fwd
PULSOUT 13,3000         'left motor forward calibration
RETURN

bwd:PULSOUT 12,1050    'right motor backward calibration 1000=max reverse
PULSOUT 13,1000        'left motor backward calibration
RETURN

stopp:PULSOUT 12,2000  '2000 = stop right motor
PULSOUT 13,2000        '2000 = stop left motor
RETURN

left:PULSOUT 12,3000   'left motor full forward
PULSOUT 13,2000'       'stop left motor
RETURN

right:PULSOUT 12,2000  'stop right motor
PULSOUT 13,3000        'right motor full forward
RETURN

leftslow:PULSOUT 12,2900   'left motor slow
PULSOUT 13,2000'           'stop left motor
RETURN


track:
trackflag=1                           ' set track mode
HIGH 8:HIGH 9:HIGH 10                 ' turn on 3 LEDs for track mode
GOSUB readir                          ' read ir remote
IF remoteCode=20 THEN GOSUB fwd       ' forward if MUTE received
IF remotecode=0 THEN GOSUB leftslow   ' circle left if MUTE not received
IF remotecode=101 THEN GOTO main      ' exit on STOP command
GOTO track


readir:                              'Parallax read ir remote subroutine
  remoteCode = 0

  RCTIME 6, 1, irPulse
  IF IN7=1 THEN GOSUB stopp:GOTO main   '  if stalled, stop!
  IF irPulse < 1000 THEN endir                 'no start signal, skip ahead
  getdata:PULSIN 6, 0, irPulse                 ' Get data pulses.
  IF irPulse > 500 THEN remoteCode.BIT0 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT1 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT2 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT3 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT4 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT5 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT6 = 1
  IF (remoteCode < 10) THEN remoteCode = remoteCode + 1
  IF (remoteCode = 10) THEN remoteCode = 0
  endir:IF trackflag=1 THEN endread   'tracking, skip ahead
  IF remotecode=0 THEN endread        'zero or no signal, skip ahead
  DEBUG ? remoteCode
  FREQOUT 11,60,730    'beep speaker
  endread:
RETURN

' Scribbler Robot PHASE 2
' Eric Ostendorff 3/10/2008
' modified Parallax readir routine
' drive robot with IR remote/Sony TV codes
' POWER button triggers find charger/leave charger routines


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

' define variables

irPulse        VAR     Word                  ' Stores pulse widths
remoteCode     VAR     Byte                  ' Stores remote code
beacon         VAR     Byte                  ' beacon count ID
counter        VAR     Byte                  ' local loop counter
nosee          VAR     Byte                  ' beacon lost counter


main:
GOSUB readir
IF remoteCode=101 THEN GOSUB stopp
IF remoteCode=116 THEN GOSUB fwd
IF remoteCode=117 THEN GOSUB bwd
IF remoteCode=52  THEN GOSUB left
IF remoteCode=51  THEN GOSUB right
IF remoteCode=21  THEN trackcharger        ' power key
GOTO main                                  'loop back, ignore other keys

trackcharger:HIGH 9'  LED on!
nosee=0'                                   'reset lost beacon counter
GOSUB beep
seek:IF IN7=1 THEN GOTO stall              'if stalled, abort
COUNT 6,15,beacon'  id beacon to drive toward it

IF beacon<17 THEN checkbase'   limit pulse count reading low
IF beacon>20 THEN checkbase'   limit pulse count reading high

PAUSE 125'                     fudge factor, rotate to center beacon
GOSUB fwd:GOTO trackcharger

checkbase:GOSUB basedetect'                           look for base reflection
GOSUB left'                                 no base seen, scan for beacon
nosee=nosee+1'                              increment lost beacon count
IF nosee=255 THEN stall'                    timeout
GOTO seek

basedetect:
FOR B0=1 TO 10'     check 10X if both sensors trigger
FREQOUT 14,1,38500
IF IN6=1 THEN RETURN
FREQOUT 15,1,38500
IF IN6=1 THEN RETURN

NEXT
GOSUB fwdslow
HIGH 8'                      green LED 1 on
HIGH 3'                      IR line follow LEDs on
lookwhite:FOR B0=1 TO 40
IF IN4=1 THEN lookwhite'     go back if line sensor dark
IF IN5=1 THEN lookwhite'     go back if line sensor dark
IF IN7=1 THEN GOTO stall
NEXT
HIGH 9'                      green LED 2 on
lookblack:FOR B0=1 TO 20
IF IN4=0 THEN lookblack'     go back if line sensor white
IF IN5=0 THEN lookblack'     go back if line sensor white
IF IN7=1 THEN GOTO stall
NEXT
GOSUB stopp
HIGH 10'                     green LED 3 on

dock:
PAUSE 300
GOSUB spinright
hold2:IF IN4=0 AND IN5=0 THEN hold2' wait if all white seen
linefollow:'                              follow line to charger
IF IN4=0 AND IN5=0 THEN GOSUB bwdslow
IF IN4=1 AND IN5=0 THEN GOSUB right
IF IN4=0 AND IN5=1 THEN GOSUB left
IF IN4=1 AND IN5=1 THEN GOSUB fwd
IF IN7=0 THEN linefollow'                 if no stall, loop back
GOSUB stopp'                              charging now
LOW 3'                                    ir leds off
LOW 8'                                    3 green LEDs off
LOW 9
LOW 10

FOR counter=1 TO 3'                       beep 3X  CELEBRATE!
GOSUB beep
PAUSE 50
NEXT

charge:GOSUB readir'          wait in charger for POWER command only
IF remoteCode<>21 THEN FREQOUT 11,600,400:GOTO charge' no can do beep
GOSUB bwd'                    maneuver out of charger
PAUSE 600
GOSUB spinright
PAUSE 750
GOSUB fwd
PAUSE 1500
GOSUB stopp
GOTO main

stall:GOSUB stopp
LOW 3:LOW 8:LOW 9:LOW 10'       all LEDs off
GOTO main

fwd:
PULSOUT 12,2950'   right motor was 2800
PULSOUT 13,3000'   left motor
RETURN

fwdslow:
PULSOUT 12,2600'   right motor was 2800
PULSOUT 13,2550'   left motor
RETURN

bwd:PULSOUT 12,1000'   right
PULSOUT 13,1150'   left
RETURN

bwdslow:PULSOUT 12,1400
PULSOUT 13,1400
RETURN

stopp:PULSOUT 12,2000
PULSOUT 13,2000
RETURN

left:PULSOUT 12,3000
PULSOUT 13,2000'    off
RETURN

right:PULSOUT 12,2000' OFF
PULSOUT 13,3000
RETURN

spinleft:
PULSOUT 12,3000
PULSOUT 13,1000
RETURN

spinright:
PULSOUT 12,1500
PULSOUT 13,2500
RETURN


beep:FREQOUT 11,60,730
RETURN


readir:                              'Parallax read ir remote subroutine

  remoteCode = 0

  DO                                    ' Wait for end of resting state
  RCTIME 6, 1, irPulse
  IF IN7=1 THEN stopp                        '  if stalled, stop!
  LOOP UNTIL irPulse > 1000

  PULSIN 6, 0, irPulse                       ' Get data pulses.
  IF irPulse > 500 THEN remoteCode.BIT0 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT1 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT2 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT3 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT4 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT5 = 1
  RCTIME 6, 0, irPulse
  IF irPulse > 300 THEN remoteCode.BIT6 = 1

  IF (remoteCode < 10) THEN remoteCode = remoteCode + 1
  IF (remoteCode = 10) THEN remoteCode = 0
  GOSUB beep

RETURN