RobotBuzz

'****************************************************************
'* Name : robot_motorc.BAS *
'* Author : robotGroup
'* Notice : Copyright (c) 2004 *
'* : All Rights Reserved *
'* Date : 4/16/2004 *
'* Version : 1.0 *
'* Notes : *
'* : *
'****************************************************************

DEFINE OSC 4
' ----------------------------------------------------- PORT SET-UP
'set port b to output:

TRISB = %00000000
TRISD = %00000000

'serial
TRISC = %10000000
tx var portc.6 'out
rx var portc.7 ' in

'analog
' Define ADCIN parameters:
DEFINE ADC_BITS 10 ' Set number of bits in result
DEFINE ADC_CLOCK 3 ' Set clock source (3=rc)
DEFINE ADC_SAMPLEUS 50 ' Set sampling time in uS

TRISA = %11111111 ' Set PORTA to all input
ADCON1 = %10000010 ' Set PORTA analog and right justify

' ----------------------------------------------------- CONSTANTS
'serial
inv9600 con 16468 ' baudmode (9600 8-N-1 inverted)
basicSpeed con 100 ' used in IR detection

lightThreshold con 100 'phototransistor threshold
searchTimeOut con 30000 'how log does it look
' ----------------------------------------------------- VARIABLES

'-- ir and ultrasonic vars

getIR var WORD 'getIR is the IR reading
readMotion var WORD 'normalized IR reading
readMotion2 var WORD 'normalized IR reading

getSonic var WORD 'getSonic is the Ultrasonic reading
rightTransistor VAR BYTE 'getLight is Transistor the on the right
leftTransistor VAR BYTE 'getleftlight is the tranistor on the left
centerTransistor VAR BYTE 'getcenterlight is the center transistor

lastRight var BYTE
lastLeft var BYTE
lastCenter var BYTE

'-- state flags
leftLightVar var bit
rightLightVar var bit
centerLightVar var bit

lastRightFlag var bit
lastLeftFlag var bit
lastCenterFlag var bit

foundCenter var bit
iFoundIt var bit
iLostIt var bit

'-- counters
turnCounter var word
turnCounter = 0

' ----------------------------------------------------- FAKEDATA

'getSonic = 375

' ----------------------------------------------------- START MAIN

'startup sequence

main:
HIGH PORTD.2
Pause 100
LOW PORTD.2

'get values

ADCIN 0, getIR ' get the ADC value of Ultrasonic reading from pin A1
ADCIN 2, lefttransistor 'GET THE TRANSISTOR READING on the right
ADCIN 3, centerTRANSISTOR 'GET THE TRANSISTOR READING ON THE LEFT
ADCIN 4, rightTRANSISTOR 'GET THE READING ON THE CENTER TRANSISTOR

'IR goes from 5 ish (spikes up to 12ish)
' 3 inches white 480-520
' 6 inches white 360-380
' 12 inches white 190's
' 2 feet white 100's
' 3 feet white 70's

'debug sensor infomation
'for somereason this seems to lock up for a while
'SEROUT2 tx, inv9600, ["ir is ", dec readMotion, 13,10, "RIGHTLIGHT IS ", dec RIGHTTRANSISTOR, 13,10, "LEFTLIGHT IS ", dec LEFTTRANSISTOR, 13,10, "CENTERLIGHT IS ", dec CENTERTRANSISTOR, 13,10]

If (getIR > 400) Then fullBackward

IF (centerTransistor > lightThreshold) THEN
'the value to set this should be such that going forward is the right thing to do?
centerLightVar = 1
else
centerLightVar = 0
endif

IF (lefttransistor > lightThreshold) THEN
leftLightVar = 1
else
leftLightVar = 0
endIF

IF (righttransistor > lightThreshold) THEN
rightLightVar = 1
else
rightLightVar = 0
endIF

IF (centerLightVar == 1) THEN centerT
if (leftLightVar == 1) and (rightLightVar == 1) then compareSides
if (leftLightVar == 1) then leftT
if (rightLightVar == 1) then rightT

IF (getIR >= 300) AND (getIR <= 400 )Then leftBackward
IF (getIR >= 250) AND (getIR <= 300 )Then rightBackward
IF (getIR >= 220) AND (getIR <= 250 )Then standStillLeft
IF (getIR >= 200) AND (getIR <= 220 )Then standStillRight
IF (getIR < 200) then fullForward

'check out HPWM and todd's PWM code
'http://stage.itp.nyu.edu/~tvh204/code/simplepwm.html

goto main

compareSides:
if lefttransistor > righttransistor then leftT
if lefttransistor < righttransistor then rightT
if lefttransistor = righttransistor then leftT
goto main

centerT:
'rc4
HIGH PORTC.4
'go forward
PORTB = %01010000
pause 500
LOW PORTC.4
lastCenter = centerTransistor
lastCenterFlag = 1
'set all wiggle timers to 0?
goto main

rightT:
'rc4
HIGH PORTD.3
gosub searchingRight
LOW PORTD.3
lastRight = centerTransistor
lastRightFlag = 1
goto main

leftT:
'rd3
HIGH PORTC.5
gosub searchingLeft
LOW PORTC.5
lastLeft = centerTransistor
lastLeftFlag = 1
goto main

searchingRight:
while (centerTransistor < lightThreshold) and (turnCounter < searchTimeOut)
ADCIN 3, centerTransistor
'left wheel forward to turn right
PORTB = %01000000
turnCounter = turnCounter + 1
wend
turnCounter = 0
return

searchingLeft:
while (centerTransistor < lightThreshold) and (turnCounter < searchTimeOut)
ADCIN 3, centerTransistor
'right wheel forward to turn left
PORTB = %00010000
turnCounter = turnCounter + 1
wend
turnCounter = 0
return

fullForward:
PORTB = %01010000
goto main

fullBackward:
PORTB = %10100000
goto main

fullStop:
PORTB = %00000000
goto main

leftForward:
PORTB = %01000000
goto main

leftBackward:
PORTB = %10000000
goto main

rightForward:
PORTB = %00010000
goto main

rightBackward:
PORTB = %00100000
goto main

standStillLeft:
PORTB = %01100000
pause 700
goto main

standStillRight:
PORTB = %10010000
pause 500
goto main