Apendix A

Appendix A
Programming Code
BASICX PROCESSOR CODE

Attribute VB_Name = "good prog"

'-------------------------------------------------------------------------------

Option Explicit

' This module is used to transfer data to and from the serial port.

'dim Main1Stack(1 to 70) as byte

dim NavigationStack(1 to 50) as byte

dim ReadSensorsStack(1 to 75) as byte

Dim GetVectorStack(1 to 75) as byte

DIM Vector As byte 'global Variables

DIM VectorFlag As Boolean

Private Const InputBufferSize As Integer = 13 ' 4-byte buffer.

Private Const OutputBufferSize As Integer = 10 ' 1-byte buffer.

Private InputBuffer(1 To InputBufferSize) As Byte

Private OutputBuffer(1 To OutputBufferSize) As Byte

Private Const ASCII_LF As Byte = 10

Private Const ASCII_CR As Byte = 13

Private Const ASCIIplus As Byte = 43

Private Const ASCIIminus As Byte = 45

Private Const ASCIIdecimal As Byte = 46

Private Const ASCIIzero As Byte = 48

'-------------------------------------------------------------------------------

Public Sub OpenSerialPort( _

ByVal PortNumber As Byte, _

ByVal BaudRate As Long)

' Opens a serial port at the specified baud rate.

' Com1 requires that the network be disabled. On the BasicX-01

' Developer Board, it may be necessary to raise pin 14, which can be

' done here or in the chip I/O initialization.

'>>If (PortNumber = 1) Then

'>> Call PutPin(14, bxOutputHigh)

'>>End If

Call OpenQueue(InputBuffer, InputBufferSize)

Call OpenQueue(OutputBuffer, OutputBufferSize)

Call OpenCom(PortNumber, BaudRate, InputBuffer, OutputBuffer)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutByte( _

ByVal Value As Byte)

' Sends one byte of binary data to the serial port. The byte is sent

' directly without translating it to a string.

Call PutQueue(OutputBuffer, Value, 1)

End Sub

'-------------------------------------------------------------------------------

Public Sub GetByte( _

ByRef Value As Byte, _

ByRef Success As Boolean)

' Inputs a byte from the serial port, if available. Returns regardless. The

' Success flag is set depending on whether a byte is available.

' The byte is in direct binary format -- it is not in string format.

' Find out if anything is in the queue.

Success = StatusQueue(InputBuffer)

' If data is in the queue, extract it.

If (Success) Then

Call GetQueue(InputBuffer, Value, 1)

Else

Value = 0

End If

End Sub

'-------------------------------------------------------------------------------

Public Sub NewLine()

' Outputs a <CR> <LF> to the serial port.

Call PutByte(ASCII_CR)

Call PutByte(ASCII_LF)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutLine( _

ByRef Tx As String)

' Outputs a String type, followed by <CR> <LF>. Output is to the serial

' port.

Call PutStr(Tx)

Call NewLine

End Sub

'-------------------------------------------------------------------------------

Public Sub PutStr( _

ByRef Tx As String)

' Outputs a String type to the serial port.

Dim Length As Integer, Ch As String * 1, bCh As Byte

Dim I As Integer

Length = Len(Tx)

For I = 1 To Length

Ch = Mid(Tx, I, 1)

bCh = Asc(Ch)

Call PutByte(bCh)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutB( _

ByVal Value As Byte)

' Outputs a Byte type to the serial port.

Dim Digit(1 To 3) As Byte

Dim i As Integer, NDigits As Integer

Const Base As Byte = 10

NDigits = 0

NDigits = NDigits + 1

Digit(NDigits) = Value Mod Base

Value = Value \ Base

Loop Until (Value = 0)

For i = NDigits To 1 Step -1

Call PutByte(Digit(i) + ASCIIzero)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutHexB( _

ByVal Value As Byte)

' Outputs a Byte type to the serial port. Hexadecimal format is used.

Dim Digit(1 To 2) As Byte, D As Byte

Dim i As Integer, NDigits As Integer

Const Base As Byte = 16

Const ASCIIhexBias As Byte = 55

NDigits = 0

NDigits = NDigits + 1

D = Value Mod Base

If (D < 10) Then

D = D + ASCIIzero

Else

D = D + ASCIIhexBias

End If

Digit(NDigits) = D

Value = Value \ Base

Loop Until (Value = 0)

For i = NDigits To 1 Step -1

Call PutByte(Digit(i))

End Sub

'-------------------------------------------------------------------------------

Public Sub PutI( _

ByVal Value As Integer)

' Outputs an Integer type to the serial port.

Call PutL(CLng(Value))

End Sub

'-------------------------------------------------------------------------------

Public Sub PutUI( _

ByVal Value As UnsignedInteger)

' Outputs an UnsignedInteger type to the serial port.

Dim L As Long, Tmp As New UnsignedInteger

Tmp = Value

L = 0

' Copy Value into the lower two bytes of L.

Call BlockMove(2, MemAddress(Tmp), MemAddress(L))

Call PutL(L)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutUL( _

ByVal Value As UnsignedLong)

' Outputs an UnsignedLong type to the serial port.

Dim UL As New UnsignedLong, L As Long, Digit As New UnsignedLong

Dim I As Integer, Temp As New UnsignedLong

' If the top bit is clear, the number is ready to go.

If ((Value And &H80000000) = 0) Then

Call PutL(CLng(Value))

Exit Sub

End If

' Divide by 10 is done by a right shift followed by a divide by 5.

' First clear top bit so we can do a signed divide.

UL = Value

UL = UL And &H7FFFFFFF

' Shift to the right 1 bit.

L = CLng(UL)

L = L \ 2

' Put the top bit back, except shifted to the right 1 bit.

UL = CuLng(L)

UL = UL Or &H40000000

' The number now fits in a signed long.

L = CLng(UL)

L = L \ 5

Call PutL(L)

' Multiply by 10. Since multiply is not implemented for UnsignedLong, we

' do the equivalent addition.

Temp = CuLng(L)

UL = 0

For I = 1 To 10

UL = UL + Temp

' Find the rightmost digit.

Digit = Value - UL

Call PutL(CLng(Digit))

End Sub

'-------------------------------------------------------------------------------

Public Sub PutL( _

ByVal Value As Long)

' Outputs a Long type to the serial port.

' Reserve space for "2147483648".

Dim Digit(1 To 10) As Byte

Dim NDigits As Integer

Dim i As Integer

Const Base As Long = 10

' The working number must be zero or negative. Otherwise the negative

' limit will cause overflow if we take its absolute value.

If (Value < 0) Then

Call PutByte(ASCIIminus)

Else

Value = -Value

End If

NDigits = 0

NDigits = NDigits + 1

Digit(NDigits) = CByte( Abs(Value Mod Base) )

Value = Value \ Base

Loop Until (Value = 0)

' Digits are stored in reverse order of display.

For i = NDigits To 1 Step -1

Call PutByte(Digit(i) + ASCIIzero)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutSci( _

ByVal Value As Single)

' Outputs floating point number in scientific notation format. The format

' is such that 13 characters are always generated. Sign characters are

' included for both mantissa and exponent. Exponents have 2 digits,

' including a leading zero if necessary.

' Example Formats: "+1.234567E+00"

' "-7.654321E-20"

' "+3.141593E+05"

' "+0.000000E+00"

Dim Mantissa As Single, Exponent As Integer, LMant As Long

Call SplitFloat(Value, Mantissa, Exponent)

' Sign.

If (Mantissa < 0.0) Then

Call PutByte(ASCIIminus)

Else

Call PutByte(ASCIIplus)

End If

' Convert mantissa to a 7-digit integer.

LMant = FixL((Abs(Mantissa) * 1000000.0) + 0.5)

' Correct for roundoff error. Mantissa can't be > 9.999999

If (LMant > 9999999) Then

LMant = 9999999

End If

' First digit of mantissa.

Call PutByte( CByte(LMant \ 1000000) + ASCIIzero)

' Decimal point.

Call PutByte(ASCIIdecimal)

' Remaining digits of mantissa.

LMant = LMant Mod 1000000

Call InsertZeros(LMant)

Call PutL(LMant)

' Exponent.

Call PutByte(69) ' E

If (Exponent < 0) Then

Call PutByte(ASCIIminus)

Else

Call PutByte(ASCIIplus)

End If

' A 2-digit exponent has a leading zero.

If (Abs(Exponent) < 10) Then

Call PutByte(ASCIIzero)

End If

Call PutI(Abs(Exponent))

End Sub

'-------------------------------------------------------------------------------

Private Sub InsertZeros( _

ByVal X As Long)

Dim NumZeros As Byte, I As Byte

If (X >= 100000) Then

Exit Sub ' 100 000 <= X

ElseIf (X >= 10000) Then

NumZeros = 1 ' 10 000 <= X <= 99 999

ElseIf (X >= 1000) Then

NumZeros = 2 ' 1 000 <= X <= 9 999

ElseIf (X >= 100) Then

NumZeros = 3 ' 100 <= X <= 999

ElseIf (X >= 10) Then

NumZeros = 4 ' 10 <= X <= 99

Else

NumZeros = 5 ' 0 <= X <= 9

End If

For I = 1 To NumZeros

Call PutByte(ASCIIzero)

End Sub

'-------------------------------------------------------------------------------

Public Sub PutS( _

ByVal Value As Single)

' Outputs a floating point number to the serial port. If the number can be

' displayed without using scientific notation, it is. Otherwise scientific

' notation is used.

Dim X As Single, DecimalPlace As Integer, Mantissa As Single

Dim Exponent As Integer, DigitPosition As Integer, Factor As Long

Dim LMant As Long, DecimalHasDisplayed As Boolean

' Special case for zero.

If (Value = 0.0) Then

Call PutByte(ASCIIzero)

Call PutByte(ASCIIdecimal)

Call PutByte(ASCIIzero)

Exit Sub

End If

X = Abs(Value)

' Use scientific notation for values too big or too small.

If (X < 0.1) Or (X > 999999.9) Then

Call PutSci(Value)

Exit Sub

End If

' What follows is non-exponent displays for 0.1000000 < Value < 999999.9

' Sign.

If (Value < 0.0) Then

Call PutByte(ASCIIminus)

End If

If (X < 1.0) Then

Call PutByte(ASCIIzero) ' Leading zero.

Call PutByte(ASCIIdecimal)

DecimalHasDisplayed = True

DecimalPlace = 0

' Convert number to a 7-digit integer.

LMant = FixL((X * 10000000.0) + 0.5)

Else

Call SplitFloat(X, Mantissa, Exponent)

DecimalPlace = Exponent + 2

' Convert mantissa to a 7-digit integer.

LMant = FixL((Abs(Mantissa) * 1000000.0) + 0.5)

' Correct for roundoff error. Mantissa can't be > 9.999999

If (LMant > 9999999) Then

LMant = 9999999

End If

DecimalHasDisplayed = False

End If

Factor = 1000000

For DigitPosition = 1 To 7

If (DigitPosition = DecimalPlace) Then

Call PutByte(ASCIIdecimal)

DecimalHasDisplayed = True

End If

Call PutByte( CByte(LMant \ Factor) + ASCIIzero )

LMant = LMant Mod Factor

' Stop trailing zeros, except for one immediately following the

' decimal place.

If (LMant = 0) Then

If (DecimalHasDisplayed) Then

Exit Sub

End If

Factor = Factor \ 10

End Sub

'-------------------------------------------------------------------------------

Private Sub SplitFloat( _

ByVal Value As Single, _

ByRef Mantissa As Single, _

ByRef Exponent As Integer)

' Splits a floating point number into mantissa and exponent. The mantissa

' range is such that 1.0 <= Abs(Mantissa) < 10.0 for nonzero numbers, and

' zero otherwise.

Dim X As Single, Factor As Single

' Zero is a special case.

If (Value = 0.0) Then

Mantissa = 0.0

Exponent = 0

Exit Sub

End If

X = Abs(Value)

Exponent = 0

Factor = 1.0

' Multiply or divide by ten to transform number to value between 1 and 10.

If (X >= 10.0) Then

X = X / 10.0

Factor = Factor * 10.0

Exponent = Exponent + 1

ElseIf (X < 1.0) Then

X = X * 10.0

Factor = Factor * 10.0

Exponent = Exponent - 1

Else

' When we reach this point, then 1.0 <= mantissa < 10.0.

Exit Do

End If

Loop

' Determine mantissa.

If (Exponent = 0) Then

Mantissa = Value

ElseIf (Exponent > 0) Then

Mantissa = Value / Factor

Else

Mantissa = Value * Factor

End If

End Sub

'-------------------------------------------------------------------------------

'++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

'=========================>>>>>>>Real Program Code Not Lib Starts Here<<<<<<<<<<<

'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

'++++++++++++++++++++++++++++++++++++++

'+++++++++++++++++++++++++++++

'+++++++++++++++++++++++

'+++++++++++++++

'++++++++++

'++++++

'+++

Sub Main()

VectorFlag=False

Vector=90

Call DefineCom3(14,13,&H88) 'ninverted logic 8data no parity nmea 1.0 1.1 1.2

Call OpenSerialPort(3,4800)

' 'start serial RX

CallTask "ReadSensors",ReadSensorsStack

CallTask "Navigation", NavigationStack

'CallTask "Main1", Main1Stack

'CallTask "GetVector",GetVectorStack

DIM DataReady As boolean

DataReady=False

DIM ReturnVector As Byte

DIM DTMFRX(1 To 2) AS BYTE

DIM CC AS INTEGER

DIM C AS BYTE

DIM Output As String *15 'long=8lat=7

DIM nOutput As String *15

C=0

Call PutPin(5,bxOutputLow) '

CC=1 'compatible

call Sleep(3.0)

'Debug.Print CStr(DataReady)

DataReady = StatusQueue(InputBuffer)

' If data is in the queue, extract it.

If (DataReady) Then

Call GetQueue(InputBuffer, ReturnVector, 1)

if (ReturnVector<>34) and (ReturnVector<>13) then

If Semaphore(VectorFlag) then

Vector=ReturnVector

Debug.Print CStr(Vector)

VectorFlag=False 'Look to see if computer has sent vector yet

end if

End If

'Debug.Print CStr(Vector)

''Call Sleep(0.0) 'change to 15 sec

'''loop moved to bottom of main1

''call sleep(0.0)

''End Sub

'start up and go straight

''Sub Main1()

call Sleep(0.0)

'Do '''''''removed for mte

'Debug.Print CStr(DTMFRX(CC))

'Call WaitForInterrupt(28) ''''''removed for mte 'wait for interrupt

if (GetPin(11)=0) then

call Sleep(0.0)

'Debug.Print "int"

DTMFRX(CC)=0

DTMFRX(CC)= DTMFRX(CC) + (GetPin(10)*1)

DTMFRX(CC)= DTMFRX(CC) + (GetPin(9)*2)

DTMFRX(CC)= DTMFRX(CC) + (GetPin(8)*4)

DTMFRX(CC)= DTMFRX(CC) + (GetPin(7)*8)

if DTMFRX(CC)=10 then

DTMFRX(CC)=0

end if

Debug.Print CStr(DTMFRX(CC))

call sleep(0.1)

if (DTMFRX(CC)<16) AND (DTMFRX(1)<>DTMFRX(2)) AND (C<>16) Then

'iofo valid dtmf and is not a paralelle nstrobe and longlat is not fully recieved

If DTMFRX(CC)>9 then

'do nothing

else

nOutput=Output

Output = CStr(DTMFRX(CC)) & nOutput 'Join The Words

Debug.Print OutPut

Debug.Print CStr(C)

C=C+1

end if

If CC=1 Then

CC=CC+1

Else

CC=1

End if

ElseIf (C = 16) then

Call PutPin(5,bxOutputHigh)

call sleep(0.3) 'relay time

Debug.Print Output

Call PutStr(Output) 'output string to serial port

Call NewLine() 'tell laptop data all there

C=0 'set byte counter to zero for next cord.

end if

Call PutPin(5,bxOutputLow) 'switch reed relay 1=dtmf 0=gps

end if

Loop

'''''''Loop

End Sub

'///////////////////////////////////////////////////////////////////////////////

Private Sub GetVector()

''''''''''Call GetByte(ReturnVector,DataReady)'''-to reduce stack

'if (DataReady) then

' If Semaphore(VectorFlag) then

' Vector=ReturnVector

' VectorFlag=False 'Look to see if computer has sent vector yet

' end if

'end if

End Sub

'\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\

Private Sub ReadSensors()

Const echo as byte =17

Const init as byte =15 ' note 36 bytes of ram (min 34)

Const blnk as byte =16

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

dim RunOnce as boolean 'indicates if transition

RunOnce=False

dim TempArray(1 to 4) as integer '1=far+2=counter position 3=short

dim tof as single

dim distance(1 to 12) as integer

dim c as integer

DIM SendBackSonarCounter as byte

SendBackSonarCounter=0

c=0

TempArray(3)=0

TempArray(1)=0

call PutPin(echo,bxInputTristate)

call PutPin(init, bxOutputLow)

call PutPin(blnk,bxOutputLow)

call Sleep(0.0)

''''''Debug.Print Cstr(GetPin(18));"ok";Cstr(RunOnce)

if (GetPin(18) = 1) and (RunOnce = False) then

c=c + 1

call PutPin(19, bxOutputHigh)

Call PutPin(init, bxOutputHigh)

call PutPin(echo,bxInputTristate)

call RCtime(echo,0,tof)

distance(c)=Cint((tof / 2.0) * 34400.00)

Call PutPin(init, bxOutputLow)

'Debug.Print CStr(CInt(distance(c)));" <-";CStr(c);" ";

if (c <> 12 ) then

RunOnce =True

end if

elseif (GetPin(18)=0) and (RunOnce = true) then

RunOnce=false 'reset for next index

end if

if (c=13) and (RunOnce = false) then 'compensated for mtr

for c = 1 to 12 step 1

'Debug.Print CStr(CInt(distance(c)));" <-";CStr(c);" ";

if (distance(c) > TempArray(1)) then

TempArray(1)=distance(c) 'set as the distance

TempArray(2)=c 'set position

elseif distance(c) < TempArray(3) then

TempArray(3)=distance(c)

TempArray(4)=c

end if

' Debug.Print "past";Cstr(TempArray(3))'''''''';Cstr(TempArray(1))

call PutPin(19, bxOutputHigh)

if (TempArray(3)<300) and (TempArray(4) >4) and (TempArray(4) < 9) then

'do replacement of semaphore vector until next gps update

If Semaphore(VectorFlag) then

if TempArray(2) > 7 then

Vector=180

elseif TempArray(2)<5 then

Vector=0

end if

VectorFlag=False 'Look to see if computer has sent vector yet

end if

call PutPin(19, bxOutputLow) 'send sonar back to start

if (GetPin(18)=0) then

SendBackSonarCounter=SendBackSonarCounter + 1

end if

loop until SendBackSonarCounter=12

SendBackSonarCounter=0

call PutPin(19, bxOutputHigh) 'start new scan

call Sleep(0.1) 'kill momentum

c=0

end if

loop

End Sub

'///////////////////////////////////////////////////////////////////////////

Sub Navigation()

call Sleep(0.0)

Const PinOC1A As Byte = 26 ' Also green LED

Const PinOC1B As Byte = 27

DIM OldVector(1 to 2) As Byte 'pwm value 0-255dc

'''''DIM alreadystopped as byte

''''''alreadystopped=0

OldVector(1)=128

OldVector(2)=128

Dim c as byte

dim VectorFloat as single

Const PWMmode8bit As Byte = bx0000_0001

Const PWMmode9bit As Byte = bx0000_0010

Const PWMmode10bit As Byte = bx0000_0011

Const PWMmodeOff As Byte = bx0000_0000

Const MaskOC1A As Byte = bx1000_0000

Const MaskOC1B As Byte = bx0010_0000

' Turn off Timer1.

' Set Timer1 to 8-bit PWM mode.

' Initialize pin directions.

Call PutPin(PinOC1A, bxOutputLow)

Call PutPin(PinOC1B, bxOutputLow)

' Clear duty cycles on both OCR1A and OCR1B pins.

' Start Timer1 according to the specified rate setting.

' Enable PWM for both pins.

OldVector(1)=Register.OCR1AL

OldVector(2)=Register.OCR1BL

if (Vector>80) and (Vector<100) then

c=0

'Debug.Print "90"

'do

' call Sleep(0.05)

'PLEASE FIX ME

' if ((5+Register.OCR1AL) < 128) then '5 will set damping

' Register.OCR1AH = 0 'a=left=pin26;;;;b=right side=27

' Register.OCR1AL = Register.OCR1AL + 5

' end if

' if ((5+Register.OCR1BL) < 128) then

' Register.OCR1BH = 0

' Register.OCR1BL = Register.OCR1BL + 5

' end if

' loop until ((5 + Register.OCR1AL) > 128) and ((5 + Register.OCR1BL) > 128)

''alreadystopped=0

elseif Vector <80 then 'based on 2m/sec

' Debug.Print "<80"

Call Sleep(2.0) 'also needs fine tuning/calibration

Vector=90 'goes straight unless told other wise

''alreadystopped=0

elseif (Vector>99) and (Vector<181) then

'Debug.Print ">100"

Call Sleep(2.0) 'also needs fine tuning/calibration

Vector=90 'goes straight unless told other wise

''alreadystopped=0

else 'slow down and stop 3 sec.

'if alreadystopped<>1 then

''for c=16 To 0 step -1

call Sleep(0.15)

''' alreadystopped=1

''next

''end if

end if

'save old vectors for ramping

loop

End Sub

'\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\

GPS NAVIGATION PROGRAM

CLOSE #1 'CLOSE PORT IF LEFT OPEN

OPEN "COM1:4800,N,8,1" FOR RANDOM AS #1

CONST PI = 3.141592654#

DIM LATMINTEGER AS INTEGER

DIM LONMINTEGER AS INTEGER

DIM PROJECTEDVECTOR AS DOUBLE

DIM VECTORSOLOUTION AS DOUBLE

DIM LATMCOPY AS DOUBLE

DIM LONMCOPY AS DOUBLE

DIM RXLATMCOPY AS DOUBLE

DIM RXLONMCOPY AS DOUBLE

DIM RXLATMINTEGER AS INTEGER

DIM RXLONMINTEGER AS INTEGER

DIM RXLATD AS INTEGER

DIM RXLATM AS DOUBLE

DIM RXLOND AS INTEGER

DIM RXLONM AS DOUBLE

DIM LOND(1 TO 7) AS INTEGER

DIM LONM(1 TO 7) AS DOUBLE

DIM NORTHSOUTH(1 TO 7) AS STRING

DIM LATD(1 TO 7) AS INTEGER

DIM LATM(1 TO 7) AS DOUBLE

DIM WESTEAST(1 TO 7) AS STRING

DIM HOURS(1 TO 7) AS INTEGER

DIM MINUTES(1 TO 7) AS INTEGER

DIM SECONDS(1 TO 7) AS DOUBLE

DIM C AS INTEGER

DIM LOADER AS INTEGER

DIM HEADING(1 TO 7) AS SINGLE

DIM SPEED(1 TO 7) AS SINGLE

DIM VALID(1 TO 7) AS STRING

DIM VALIDCOUNTER(1 TO 7) AS INTEGER

DIM VELOCITY AS SINGLE

DIM BEARING AS DOUBLE

C = 0

'GOTO RXCODEPROCESS '!!!!TEMPORARY FOR TESTING TBR WHEN REST IS DONE!!!!

STARTR: 'STARTING POINT

ON ERROR GOTO STARTR 'IGNORE TIME OUT FLAGS

'LINE INPUT #2, RX$

'LPRINT SOMETHING DIRECTION CODE

STARTCOMRX:

'''''''''DO WHILE (LEN(D$) < 15) 'CHANGE THIS IN FINAL STEP

PRINT "HERE IT GOES"

LINE INPUT #1, D$ 'PUT RECEIVED IN STRING IN D

COPYD$ = D$ 'MAKE A SECOND COPY

PRINT "RX = "; D$

''''''LOOP

WHILE ASC(LEFT$(D$, 1)) < 33 'TRIM OFF CR AND LF

D$ = RIGHT$(D$, LEN(D$) - 1)

WEND

'GLL CODE START?

IF LEFT$(D$, 6) = "$GPRMC" GOTO RMCPROCESS 'CHANGE GLL-RMC

IF LEFT$(D$, 6) = "$GPGLL" GOTO STARTR 'WAS GLLPROCESS

IF LEFT$(D$, 6) = "$GPRMB" GOTO STARTR 'IGNORE DGPS SENSOR DATA

GOTO RXCODEPROCESS

GOTO STARTR

GLLPROCESS:

'IF C = 30 THEN

'C = 0

'END IF

'LOND = VAL(MID$(D$, 8, 2))'LAT DEGREES

'LONM = VAL(MID$(D$, 10, 6))'LAT MIN

'NORTHSOUTH$ = MID$(D$, 17, 1)'NORTH OR SOUTH

'LATD = VAL(MID$(D$, 19, 3))'LONG DEGREES NOTE MID(X,X,X)=$ OFFSET LENGTH

'LATM = VAL(MID$(D$, 22, 6))'LONG MIN

'WESTEAST$ = MID$(D$, 29, 1)'EAST OR WEST

'HOURS = VAL(MID$(D$, 31, 2))'HOURS

'MINUTES = VAL(MID$(D$, 33, 2))'MINUTES

'SECONDS = VAL(MID$(D$, 35, 6))'SECONDS

'PRINT "LAT"; LATD; "."; LATM; " "; WESTEAST$

'PRINT "LON"; LOND; "."; LONM; " "; NORTHSOUTH$

'PRINT HOURS; ":"; MINUTES; "."; SECONDS

GOTO STARTR:

RMCPROCESS:

IF C = 7 THEN

C = 0

END IF

C = C + 1 'ARRAY DECLARED NOT FOR ZERO 1-25

IF LOADER < 4 THEN

LOADER = LOADER + 1 'USED TO INITIALIZE TO REMOVE ARRAY ERRORS

END IF

VALID$(C) = MID$(D$, 15, 1)'DATA VALID FLAG A=OK V=INVALID

LOND(C) = VAL(MID$(D$, 17, 2))'LAT DEGREES

LONM(C) = VAL(MID$(D$, 19, 6))'LAT MIN

NORTHSOUTH$(C) = MID$(D$, 26, 1)'NORTH OR SOUTH

LATD(C) = VAL(MID$(D$, 28, 3))'LONG DEGREES NOTE MID(X,X,X)=$ OFFSET LENGTH

LATM(C) = VAL(MID$(D$, 31, 6))'LONG MIN

WESTEAST$(C) = MID$(D$, 38, 1)'EAST OR WEST

HOURS(C) = VAL(MID$(D$, 8, 2))'HOURS

MINUTES(C) = VAL(MID$(D$, 10, 2))'MINUTES

LONMINTEGER = VAL(MID$(D$, 19, 2)) 'NEEDED FOR ARC TAN CALC.

LATMINTEGER = VAL(MID$(D$, 31, 2)) 'SAME

SECONDS(C) = VAL(MID$(D$, 12, 2))'SECONDS

SPEED(C) = VAL(MID$(D$, 40, 3))

HEADING(C) = VAL(MID$(D$, 44, 5))

'removed from debug for overflow error handling -leave lower case

'LINE INPUT #1, D$ 'PUT RECEIVED IN STRING IN D 'PREVENTS OVERFLOW

PRINT "LAT"; LATD(C); "."; LATM(C); " "; WESTEAST$(C); "LONG"; LOND(C); "."; LONM(C); " "; NORTHSOUTH$(C)

PRINT HOURS(C); ":"; MINUTES(C); "."; SECONDS(C)

PRINT "SPEED "; SPEED(C); " HEADING "; HEADING(C); " VALID "; VALID$(C)

PRINT RXLATM; "RXLATM"

PRINT RXLONM; "RXLONM"

PRINT "RXLATMCOPY"; RXLATMCOPY; "RXLONMCOPY"; RXLONMCOPY; "RXLATINTEGER"; RXLATINTEGER

PRINT C

PRINT "BEARING"; BEARING; "VELOCITY"; VELOCITY

'F C > 2 THEN 'UPDATE POINT FOR POINT NOW

GOTO CONTROL 'UPDATE NAVIGATION VECTOR/SOLUTION EVERY SEC

'ND IF

LAST = SECONDS(C)

GOTO STARTR

RXCODEPROCESS:

PRINT "RXCODE IN PROCESS"

RXLATM = VAL(MID$(D$, 12, 3)) 'check offset before use

PRINT RXLATM

RXLONM = VAL(MID$(D$, 6, 3))

PRINT RXLONM

RXLONMINTEGER = VAL(MID$(D$, 4, 2)) 'DOES NOT SUPPORT IMPLICT DECLARATIONS

RXLATMINTEGER = VAL(MID$(D$, 11, 2)) 'GET # OF DECIMAL MIN.

RXLOND = VAL(MID$(D$, 1, 3)) 'check number of deg?

RXLATD = VAL(MID$(D$, 9, 2))

RXLATMCOPY = RXLATM

RXLONMCOPY = RXLONM

'RXLATMCOPY = RXLATMCOPY - RXLATMINTEGER

'RXLONMCOPY = RXLONMCOPY - RXLONMINTEGER not needed

'PRINT "RX CODE COPYD$"; COPYD$

'RECIEVES ALL CORD

'BUT ONLY USES MIN

'MAX DISTANCE ~9 MILES

GOTO STARTR:

CONTROL:

IF SECONDS(C) = LAST OR VALID$(C) = "V" THEN

REALVALID = 0

GOTO STARTR 'DON'T PROCESS AVG CORD.

ELSE

REALVALID = 1 'SET AS VALID FLAG

VALIDCOUNTER(C) = 1 'STORE VALID FLAG COLLECTION FOR NAVIAGTION

'SOLUTION

PRINT CHR$(16 + 2 + 64) 'MOVE AHEAD SLOWLY TO ALLOW BEARING CALC

END IF

'IF C > 2 AND LOADER > 2 THEN 'SECTION THAT AVG ERRATIC VALUES

VELOCITY = (SPEED(C)) '+ SPEED(C - 1) + SPEED(C - 2)) / 3)

'ELSEIF C = 1 THEN

'VELOCITY = ((SPEED(1) + SPEED(30) + SPEED(29)) / 3)

'ELSEIF C = 2 THEN

'VELOCITY = ((SPEED(2) + SPEED(1) + SPEED(30)) / 3)

'ELSE 'DIR RIGHT LEFT

'TO BE MORE ACCURATE ERRATIC IF STOPPED

'END IF

IF REALVALID = 1 THEN 'AND VALIDCOUNTER(C - 1) = 1 THEN 'LONG IF

'IF DATA IS VALID NAVIGATE

PRINT "REAL VALID"

LATMCOPY = (LATM(C))' + LATM(C - 1)) / 2

'AVERAGE THE LAST 4 SECONDS OF TRAVEL

LONMCOPY = (LONM(C))' + LONM(C - 1)) / 2

'GET CURRENT # OF DECIMAL MINUTES

LATMCOPY = LATMCOPY - LATMINTEGER

LONMCOPY = LONMCOPY - LONMINTEGER

PRINT "LATMCOPY"; LATMCOPY; "LONMCOPY"; LONMCOPY

PRINT "LATMINTEGER"; LATMINTEGER; "LONMINTEGER"; LONMINTEGER

'PRINT ATN(TAN(PI / 4!)), PI / 4! 'Output is: .7853981635 .7853981635

'FOR REFERANCE FROM HELP FILE

PRINT "RATIO BEFORE ARC TAN"; ((RXLATMCOPY - LATMCOPY) / (RXLONMCOPY - LONMCOPY))

PROJECTEDVECTOR = ATN((RXLATMCOPY - LATMCOPY) / (RXLONMCOPY - LONMCOPY))

'ARC TAN OF THE RUN/RUN COMPARED TO RX CODE

PRINT "PROJECTEDVECTOR AFTER DIVISION "; PROJECTEDVECTOR

PROJECTEDVECTOR = (PROJECTEDVECTOR) * (180 / PI)'CONVERT RAD => DEG

PRINT PROJECTEDVECTOR; "<==BEFORE QUAD COMPENSATION"

IF PROJECTEDVECTOR < 0 THEN 'SECTION DETERMINES QUAD AND

'REAL VECTOR

IF (RXLONMCOPY - LONMCOPY) < 0 AND (RXLATMCOPY - LATMCOPY) > 0 THEN

PROJECTEDVECTOR = (PROJECTEDVECTOR + 90) + 90 'MAKE+ANGLE AND ADD (QUAD*90)

ELSE

PROJECTEDVECTOR = (PROJECTEDVECTOR + 90) + 270 'CODE OK

END IF

ELSE

IF (RXLONMCOPY - LONMCOPY) < 0 AND (RXLATMCOPY - LATMCOPY) < 0 THEN

PROJECTEDVECTOR = PROJECTEDVECTOR + 180

END IF

PRINT PROJECTEDVECTOR; "<==AFTER QUAD "

'IF + SUBTRACT FOR VECTORSOLUTION

IF C < 5 THEN

IF (LONM(C) - LONM(C + 2)) = 0 THEN

PRINT "DIVIDE BY ZERO"

GOTO SKIP 'HANDLES DIVIDE ERRORS

END IF

BEARING = ATN((LATM(C) - LATM(C + 2)) / (LONM(C) - LONM(C + 2)))

PRINT "NEW<5 BEARING BEFORE COMP"; BEARING

BEARING = (BEARING) * (180 / PI)'CONVERT RAD => DEG

IF BEARING < 0 THEN 'SECTION DETERMINES QUAD AND

'REAL BEARING

IF (LONM(C) - LONM(C + 2)) < 0 AND (LATM(C) - LATM(C + 2)) > 0 THEN

BEARING = (BEARING + 90) + 90'MAKE+ANGLE AND ADD (QUAD*90)

ELSE

BEARING = (BEARING + 90) + 270 'CODE OK

END IF

ELSE

IF (LONM(C) - LONM(C + 2)) < 0 AND (LATM(C) - LATM(C + 2)) < 0 THEN

BEARING = BEARING + 180

END IF

PRINT "NEW<5 BEARING AFTER COMP"; BEARING

ELSE

IF (LONM(C) - LONM(C - 4)) = 0 THEN

PRINT "DIVIDE BY ZERO"

GOTO SKIP 'HANDLES DIVIDE ERRORS

END IF

BEARING = ATN((LATM(C) - LATM(C - 4)) / (LONM(C) - LONM(C - 4)))

PRINT "NEW>5 BEARING BEFORE COMP"; BEARING

BEARING = (BEARING) * (180 / PI)'CONVERT RAD => DEG

IF BEARING < 0 THEN 'SECTION DETERMINES QUAD AND

'REAL BEARING

IF (LONM(C) - LONM(C - 4)) < 0 AND (LATM(C) - LATM(C - 4)) > 0 THEN

BEARING = (BEARING + 90) + 90'MAKE+ANGLE AND ADD (QUAD*90)

ELSE

BEARING = (BEARING + 90) + 270 'CODE OK

END IF

ELSE

IF (LONM(C) - LONM(C - 4)) < 0 AND (LATM(C) - LATM(C - 4)) < 0 THEN

BEARING = BEARING + 180

END IF

PRINT "NEW>5 BEARING AFTER COMP"; BEARING

END IF

'DIVIDE BY ZERO ERROR VECTOR

VECTORSOLUTION = BEARING - PROJECTEDVECTOR 'check

PRINT "VECTORSOLUTBEFOREOVERSH"; VECTORSOLUTION; "BEARING"; BEARING; "PRO VECTOR"; PROJECTEDVECTOR

IF VECTORSOLUTION < -200 THEN

VECTORSOLUTION = VECTORSOLUTION + 360

ELSEIF VECTORSOLUTION > 200 THEN 'SECTION CONTROLS OVERSHOOT

VECTORSOLUTION = VECTORSOLUTION - 360 'ACTIVTE AFTER DEBUG

END IF

PRINT "NEW FINAL VECTORSOLUTION"; VECTORSOLUTION

IF ((RXLONMCOPY - LONMCOPY) ^ 2 + (RXLATMCOPY - LATMCOPY) ^ 2) ^ .5 < .007 THEN

WRITE #1, CHR$(8 + 2 + 64)'stop and put arms forward

PRINT "STOP ==>DESTINATION REACHED"

ELSEIF VECTORSOLUTION < -16 THEN

'MAX RESOLUTION =~35 DEGREES 'LEFT

'25 DEGREEN TURN

IF VECTORSOLUTION < -16 AND VECTORSOLUTION > -25 THEN

WRITE #1, CHR$(16 + 3 + 160) 'DIR RIGHT LEFT

PRINT "LEFT 25 DEGREES"

'TURN 25 DEGREES LEFT

ELSEIF VECTORSOLUTION < -35 THEN

'TURN 50-45 DEGREES

WRITE #1, CHR$(24 + 3 + 192) 'INSERT NUMBER FOR TURN OF 50 DEGREES

PRINT "LEFT 50 DEGREES"

END IF

ELSEIF VECTORSOLUTION > 16 THEN 'RIGHT

IF VECTORSOLUTION > 16 AND VECTORSOLUTION < 25 THEN

WRITE #1, CHR$(16 + 5 + 96)'DIR RIGHT LEFT

PRINT "RIGHT 25 DEGREES"

'TURN 25 DEGREES RIGHT

ELSEIF VECTORSOLUTION > 35 THEN

'TURN 50-45 DEGREES

WRITE #1, CHR$(24 + 6 + 64) 'INSERT NUMBER FOR TURN OF 50 DEGREES

PRINT "RIGHT 50 DEGREES"

END IF

ELSE

'GO STRAIGHT OR STOP

'PUT ARMS FORWARD OR BACK

SKIP:

IF ((RXLONMCOPY - LONMCOPY) ^ 2 + (RXLATMCOPY - LATMCOPY) ^ 2) ^ .5 < .007 THEN

WRITE #1, CHR$(8 + 2 + 64)'stop and put arms forward

PRINT "STOP ==>DESTINATION REACHED"

ELSE

WRITE #1, CHR$(24 + 2 + 64) 'FULL WARP AHEAD

PRINT "GO STRAIGHT)"

END IF

END IF 'END OF LONG IF

GOTO STARTR 'START OVER

SUB REED

STARTSUB:

END SUB

HOST SIDE PROGRAM

CLOSE #1 'CLOSE PORT IF LEFT OPEN

OPEN "COM1:4800,N,8,1" FOR RANDOM AS #1

'OPEN "TEMP.TXT" FOR RANDOM AS #1

DIM SHARED LONGA AS STRING

DIM SHARED LAT AS STRING

DIM SEND AS STRING

SEND$ = "UNMOD"

START:

LINE INPUT ; "ENTER THE LONG. XXXXXXXX"; LONGA$

LINE INPUT ; "ENTER THE LAT. XXXXXXX"; LAT$

'PRINT SEND$

'SEND$ = LONGA$ LAT$

TIMER ON

ON TIMER(15) GOSUB SEND

PRINT "DATA SENT IN 15 SECONDS"

LOOP

SEND:

WRITE #1, LONGA$

WRITE #1, LAT$

PRINT "THANK YOU DATA SENT IS EFFECTIVE IN 10 SEC"

GOTO START

Hosted by www.Geocities.ws