Microcontroller Code - WordPress.com

russianharmoniousElectronics - Devices

Nov 2, 2013 (3 years and 11 months ago)

94 views

Microcontroller 
Code
 
 
Get_IR.bs2
 
This program receives basic IR pulses. The program pulses
 out a certain frequency from 
the infrared LED, and the receiver returns a 0 if
 an infrared pulse is detected (bouncing 
off 
an obstacle in front of the robot). 
 
' {$STAMP BS2}

' {$PBASIC 2.5}


irDetectFront
VAR Bit

frequency

VAR Word



DO

frequency=36500
‘emits a 36500 Hz infrared wave

GOSUB getIR



IF (irDetectFront=0) THEN


DEBUG “
object seen


ELSE

DEBUG “no object”

ENDIF

LOOP


getIR:

FREQOUT 8,1,frequency

irDetectFront=IN9

RETURN

 
 
 
Compass_FindNorth.bs2
 
This program uses the compass to figure out the microcontroller’
s orientation relative to 
north
, and move the microcontroller so it faces north.
 
 
' {$STAMP BS2}

' {$PBASIC 2.5}


irDetectLeft
VAR Bit

' detects the left ir sens
or

irDetectRight VAR Bit

' detects the right ir sensor

pulseCount VAR Byte
' counter for turn loops


D
inDout PIN 6


' P6 transmit
to/from
Din/Dout

Clk PIN
5

' P5 sends pulses to
HM55B Clk

En
PIN 4
'
P4 controls HM55B's /EN(ABLE)


Reset CON %0000
' Reset command for HM55B

Measure CON %1000 ' S
tart measurement command

Report CON %1100
' Get status/axis
command

Ready CON %1100
' 11
-
>
Done, 00
-
> no errors

NegMask
CON %1111100000000000

' For 11
-
bit negative to 16
-
bits


i VAR Bit

x
VAR Word
' x
-
axis data

y VAR Wor
d ' y
-
axis data

status VAR Nib
' Sta
tus flags

a
ngle VAR Word
' Store angle measurement



FREQOUT 4, 2000, 3000

' Signal program start/reset.


DO


FREQOUT 8, 1, 37000



irDetectLeft = IN9




FREQOUT 2, 1, 37000


irDetectRight = IN0



DO WHILE (irDetectRi
ght
= 1) OR (irDetectLeft = 1)



PULSOUT 13, 650



PULSOUT 12, 850
‘go forward



FREQOUT 8, 1, 37000





irDetectLeft = IN9



FREQOUT 2, 1, 37000



irDetectRight = IN0
‘check for obstacles


LOOP



IF irDetectRight
= 0 THEN



FOR pulseCount = 0 TO 100



PULSOUT 13, 650




PULSOUT 12, 850



NEXT


FOR pulseCount = 0 TO 140



PULSOUT 13, 850



PULSOUT 12, 850

NEXT


GOSUB Compass_Get_Axes

' Get x, and y values

angle = x
ATN
-
y
' Convert x and y to bin radian


angle = angle */ 360



IF (angle < 100) OR (angle > 80) THEN


drift=0


ENDIF

LOOP


' FOR pulseCount = 0 TO 122


get_IR:



FREQOUT 8, 1, 37000 ' Store IR detection values in



irDe
tectLeft
= IN9 ' bit variables.


FREQOUT 2, 1, 37000



irDetectRight = IN0

RETURN


Compass_Get_Axes:

' Compass module subroutine


HIGH En: LOW En ' Send reset command to HM55B



SHIFTOUT DinDout,clk,MSBFIRST,[Reset
\
4]


HIGH En: LOW
En

'
HM55B start command



SHIFTOUT DinDout,clk,MSBFIRST,[Measure
\
4]



status = 0

' Clear previous status flags




DO





HIGH En: LOW En





SHIFTOUT DinDout,clk,MSBFIRST,[Report
\
4]





SHI
FTIN DinDout,clk,MSBPOST,[Status
\
4] ' Get Status



LOOP UNTIL status = Ready




SHIFTIN Di
nDout,clk,MSBPOST,[x
\
11,y
\
11]





HIGH En



IF (y.BIT10 = 1) THEN y = y | NegMask



IF (x.BIT1
0 = 1) THEN x = x | NegMask


RETURN

 
Get_IR_Distance.bs2
 
This  program  finds  out  how  far  away  an  obstacle  is  using  information  from  only  the 
infrared  receiver. 
The  program  essentially  starts  with  an  extremely  short

range 
frequency,  then  increases  the  range  of  the  IR  frequency  until  it  sees  an
  object. 
It  then 
relays  this  information  to  the  microcontroller.  This  method  of  finding  distance  is 
expanded in 
Triangulator.bs2
.
 
' {$STAMP BS2}

' {$PBASIC 2.5}


irDetectFront VAR Bit

frequency VAR Word


DO

frequency=43000

GOSUB getIR

DO WHILE (irDetectFront=1) AND (frequency>37000)

frequency=frequency
-
100

GOSUB getIR


LOOP

IF (frequency>=42800) THEN

DEBUG "shortest"


ELSEIF (frequency>=42000) THEN

DEBUG "shorter"

ELSEIF (frequency>=39000) THEN

DEBUG "medium"

ELSEIF (frequency>=37500) THEN

DEBUG "long"


ELSEIF (frequency>=37000) THEN

DEBUG "longer"


ELSE




DEBUG "longest"

ENDIF


DEBUG CR

DEBUG ? frequency

LOOP


getIR:


FREQOUT 8, 1, frequency


irDetectFront = IN9

RETURN

We used this program to chart the range of distances that we could find using infrared, 
and compiled those distances into the graph below.
 
 
According to the graph, the infrared sensors can only accurately find the distance of 
an 
object located between 20 to 38 c
entimeters away from the robot, making infrared only 
useful for medium

range distances
.
 
 
 
 
 
 
 
36000 
37000 
38000 
39000 
40000 
41000 
42000 
43000 
44000 

10 
20 
30 
40 
50 
60 
Frequency required to detect object (Hz) 
Object distance from infrared source (cm) 
Triangulator.bs2
 
This code did not function as we wanted it to, but below we have included the basic 
math principles that we a
pplied to developing the code.
 
 
 
 
 
 
 
 
 
             User
 
The basic principle behind triangulation is to
 
create a triangle of which all sides are of a 
 
known length. 
Then, the law of cosines can
 
be applied.
 
 
 
 
 
 
 
 
     a
 
 
 
b
 
 
 
 
 
   
   
d
 
 
Using this 
formula, all the angle
 
measures of the triangle can be
 
calculated, and a vector can be
 
synthesized from the information.
 
     Source 1
 
 
 
    c
 
 
    Source 2
 
 
TransmitterCode.bs2
 
The transmitter code works to send its compass value to the receiver, which is
 the Boe

Bot. The code sends it continuously. 
 
'{$STAMP BS2}

'{$PBASIC 2.5}


'Variables

DinDout PIN 6 ' P6
transceives to/from Din/Dout

Clk PIN 5 ' P5 sends
pulses to HM55B's Clk

En
PIN 4 ' P4 controls
HM55B's /EN(ABLE)


Reset CON %0000 ' Reset
command for HM55B

Measure CON %1000 ' Start
measurement command

Report CON %1100
' Get
status/axis values command

Ready CON %1100 ' 11
-
> Done,
00
-
> no errors

NegMask CON %1111100000000000 ' For 11
-
bit
negative to 16
-
bits

i VAR Bit

x VAR Word
' x
-
axis data

y VAR Word ' y
-
axis data

status VAR Nib ' Status flags

angle VAR Word

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''


DO

GOSUB Compass_Get_Axes
' Get x, and y
values


angle = x ATN
-
y ' Convert x
and y to brads

angle = angle */ 360


PULSOUT 1, 1200 'Sync pulse for the receiver

SEROUT 1, 16468, [ "!", 0, 0, angle.HIGHBYTE,
angle.LOWBYTE ]

DEBUG CR

' PAUSE 100


LOOP


Compass_
Get_Axes:
' Compass module subroutine


HIGH En: LOW En



SHIFTOUT DinDout,clk,MSBFIRST,[Reset
\
4]




HIGH En: LOW En ' HM55B start
measurement command



SHIFTOUT DinDout,clk,MSBFIRST,[Measure
\
4]



status = 0



DO
' Status flag checking loop



HIGH En: LOW En




SHIFTOUT DinDout,clk,MSBFIRST,[Report
\
4]



SHIFTIN DinDout,clk,MSBPOST,[Status
\
4] ' Get Status

LOOP UNTIL status = Ready ' Exit loop
when status is ready




SHIFTIN DinDout,clk,MSBPOST,[x
\
11,y
\
11] ' Get x & y
axis values



HIGH En



IF (y.BIT10 = 1) THEN y = y | NegMask



IF (x.BIT10
= 1) THEN x = x | NegMask


RETURN

 
 
 
 
 
 
 
 
ReceiverCode.bs2
 
This program simply prints out the compass readings sent by the transmitter.
 
'{$STAMP BS2}

'{$PBASIC 2.5}


x VAR Byte

y VAR Word

z VAR Byte


DO


SERIN 1, 16468, [WAIT("!"), z, x, y.HIGHBYTE, y.LOWBYTE]


DEBUG ? y

LOOP

 
 
Avoider.bs2
 
This is the final avoider program we made. It runs forward until it sees an obstacle in its 
path and then decide whther to move around the abstacle from the right or left. It 
knows when it reaches the end of the obstacle when one of its side 
IR sensors sto
ps 
detecting the obstacle. 
 

' {$STAMP BS2} ' Stamp directive.

' {$PBASIC 2.5} ' PBASIC directive.

DEBUG "Program Running!"


'
-----
[ Variables ]
--------------------------------------

irDetectLeft VAR Bit

irDetectRight VAR Bit

pulseCount VAR Byte


'
-----
[
Initialization ]
---------------------------------




FREQOUT 4, 2000, 3000 ' Signal program start/reset.


'
-----
[ Main Routine ]
------------------------
-----------

DO


FREQOUT 8, 1, 37000
' Store IR detection values in


irDe
tectLeft
= IN9 ' bit variables.


FREQOUT 2, 1, 37000


irDetectR
ight = IN0


IF (irDetectLeft =
0) AND (irDetectRight = 0) THEN

GOSUB Back_Up
' Both IR pairs detect obstacle

GOSUB Turn_Left
' Back up & U
-
turn (left twice)

GOSUB Turn_Left


ELSEIF (irDetectLeft
=
0) THEN ' Left IR pair detects

GOSUB
Back_Up ' Back up & turn right

GOSUB Turn_Right


ELSEIF (irDetectRight =
0) THEN ' Right IR pair detects

GOSU
B Back_Up ' Back up & turn left

GOSUB Turn_Left


ELS
E ' Both IR pairs 1, no detects

GOSUB Forward_
Pulse ' Apply a forward pulse


ENDIF ' and check again

LOOP

'
-----
[ Subroutine
]
-------------------------
------------

Forward_Pulse:
' Send a single forward pulse.

PULSOUT 13,650

PULSOUT 12,850

PAUSE 20

RETURN


Turn_Left
: ' Left turn, about 90
-
degrees.


FOR pulseCount = 0 TO 20

PULSOUT 13, 650

PULSOUT 12, 650

PAUSE 20

NEXT

RETURN


Turn_Right:

FOR pulseCount = 0 TO 20 ' Right turn, about 90
-
degrees.


PULSOUT 13, 850



PULSOUT 12, 850



PAUSE 20

NEXT

RETURN


Back_Up: ' Back up.

FOR pulseCount = 0 TO 40



PULSOUT 13, 850



PULSOUT 12, 650



PAUSE 20

NEXT

RETURN
 
 
 
 
 
 
 
 
 
ADC_Code.bs2
 
This program was designed to help us understand the relationship between the output 
votage
  from  the  receiver  pin  called  the  RSSI.  The  voltage  is  directly  related  to  the 
distance  between  the  transmitter  and  receiver. 
The  RSSI  voltage  was  read  through  an 
ADC (analog to digital converter). 
 
 
' {$STAMP BS2}

' {$PBASIC 2.5}

x VAR Byte

y VAR Word

z V
AR Byte

adcBits VAR Byte

v VAR Byte

r VAR Byte

v2 VAR Byte

v3 VAR Word

CS PIN 9

CLK PIN 8

DataOutput PIN 7

DEBUG CLS 'Start display.


DO

SERIN 1, 16468, [WAIT("!"), z, x, y.HIGHBYTE, y.LOWBYTE]


GOSUB ADC_Data

GOSUB Calc_Volts


v3 = adcBits
* 100 / 12

LOOP


ADC_Data:

LOW CLK

LOW CS

PULSOUT CLK, 210

SHIFTIN DataOutput,CLK,MSBPOST,[adcBits
\
8]

HIGH CS

RETURN


Calc_Volts:

v = adcBits * 5 / 255

r = (adcBits * 5) // 255

v2 = (r * 100) / 255

RETURN


Display:

DEBUG HOME

DEBUG "8
-
bit binary value: ", DEC2 adcBits

DEBUG CR, CR, "DVM Reading: " '. new line

DEBUG DEC1 v, ".", DEC2 v2, " Volts" '. new line

RETURN