Navigation




Autonomously Guided Robot

04-20-2009, Past Projects, Muhhamad M. Raza

As part of a two member team, the following code was written in PBASIC (for the Basic Stamp 2 from parallax) to read in distance measurements from several Sharp GP2D120 Infrared (IR) sensors. The sensor output was nonlinear, therefore, a linearization equation was to be found. The analog output of the sensor was converted to digital (A/D conversion) using the ADC0831CCN analog to digital converter. A total of four sensors were used situated around the robot. Two parallax BOE boards were used. One was used to read in the sensor signals and the second was used to drive the motors. Since the codes are long and difficult to read online, please contact me if you (i.e. a prospective employer) is interested in viewing these codes, and I would be happy to provide you with the source files.

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

' Define constants & variables

CS1 CON 4 'adc 1 (sensor1)
CLK1 CON 5'adc 1 (sensor1)
Dout1 CON 6'adc 1 (sensor1)

CS2 CON 7 'adc 1 (sensor2)
CLK2 CON 8'adc 1 (sensor2)
Dout2 CON 9'adc 1 (sensor2)

CS3 CON 10 'adc 1 (sensor3)
CLK3 CON 11'adc 1 (sensor3)
Dout3 CON 12'adc 1 (sensor3)

CS4 CON 1 'adc 1 (sensor4)
CLK4 CON 2'adc 1 (sensor4)
Dout4 CON 3'adc 1 (sensor4)

'SERIAL INTERFACE:
FC PIN 14 ' flow control pin
SO PIN 15 ' serial output
T1200 CON 813
T2400 CON 396
T9600 CON 84
T19K2 CON 32
T38K4 CON 6
Inverted CON $4000
Open CON $8000
Baud CON T38K4 + Inverted
'-------------------------

condition VAR Byte
Datain1 VAR Byte
Datain2 VAR Byte
Datain3 VAR Byte
Datain4 VAR Byte
btnWrk VAR Byte
index VAR Byte
side VAR Byte
Distance1 VAR Byte
Distance2 VAR Byte
Distance3 VAR Byte
Distance4 VAR Byte
Timer VAR Word
Wkspace1 VAR Byte
i VAR Byte

Wkspace1 = 0
index = 0
side =0
Timer = 0
LOW 0

wallside:
BUTTON 13, 0, 255, 1, btnWrk, 0, No_Press
side = 1
GOTO LED
END

Main:
GOSUB Getdata
GOSUB Dstcalc
SEROUT SO\FC, Baud, [side,distance1,distance2,distance3,distance4]
GOTO Main

Dstcalc:
Distance1=14030/(Datain1+65)-2
Distance2=14030/(Datain2+65)-2
Distance3=14030/(Datain3+65)-2
Distance4=14030/(Datain4+65)-2
RETURN

Getdata: 'Acquire conversion from 0831
LOW CS1 'Select the chip
LOW CS2
LOW CS3
LOW CS4

LOW CLK1 'Ready the clock line.
LOW CLK2
LOW CLK3
LOW CLK4

PULSOUT CLK1,10 'Send a 10 uS clock pulse to the 0831
SHIFTIN Dout1, CLK1, MSBPOST,[Datain1\8] 'Shift in data
PULSOUT CLK2,10
SHIFTIN Dout2, CLK2, MSBPOST,[Datain2\8]
PULSOUT CLK3,10
SHIFTIN Dout3, CLK3, MSBPOST,[Datain3\8]
PULSOUT CLK4,10
SHIFTIN Dout4, CLK4, MSBPOST,[Datain4\8]

HIGH CS1 'Stop conversion
HIGH CS2
HIGH CS3
HIGH CS4
RETURN

No_Press:
IF index<=5 THEN
index=index+1
GOTO wallside
ELSE
GOTO LED
ENDIF

LED:
IF side =0 THEN
HIGH 0
PAUSE Timer*5
LOW 0
ELSEIF side=1 THEN
FOR i =1 TO 5
PAUSE Timer/2
TOGGLE 0
PAUSE Timer/2
TOGGLE 0
NEXT
ENDIF
GOTO Main

'Status:
'Interpreting the sensor information
'SELECT side 'determinining which wall to follow

'CASE =0
'IF (43<=distance1 AND distance1<=48 AND distance4<55) OR distance4<55 THEN
'condition =0 'turn left
'ELSEIF distance1>70 AND distance3>70 THEN
'condition = 4 'right turn
'ELSEIF 43<=distance1 AND distance1<=48 AND distance4<55 AND distance2<60 THEN
'condition =10 'deadend
'ELSEIF 43<=distance1 AND distance1<=48 AND 45<=distance3 AND distance3<=50 THEN
'condition=2 'parallel
'ELSEIF distance1>50 AND 45<=distance3 AND distance3<=50 THEN
'condition=12 'skewing away
'ELSEIF distance1<44 AND 45<=distance3 AND distance3<=50 THEN
'condition=14 'skewing in
'ELSE
'condition =2 'default parallel
'ENDIF

'CASE =1
'condition=1
'ENDSELECT

'RETURN

Continue