'Freebasic Rangefinder Reciever Code

#Include "FBGFX.bi"
Using fb

ScreenRes 800,600,32

Const TRUE = 1
Const FALSE = 0

Const maxdistance = 100          'Maximum distance we want to read
Const factor = 250 / maxdistance     'scale factor for drawing bars
Const minang = 10            'Starting angle, as a cheap servo was used moving to 0 causes problems
Const maxang = 150            'Set to keep view in front of robot
Const totalang = maxang-minang
Const angfactor = 1

Const pi = 3.14159265359

Dim As UByte rec_Data
Dim As UByte bounce
Dim As UByte datastart

Dim As uByte fin, gotang, gotmessage

Dim As ubyte serialData
Dim As String message, marker

Dim As Integer ang, i
Dim As Double distance

Declare Function radang(ang As Double) As Double
Declare Sub draw_line(ang As Integer, distance As Double)

fin = FALSE
gotang = FALSE
gotmessage = FALSE
datastart = TRUE

Open com "COM14:57600,N,8,1,BIN,CS0,DS0" As 1        'Opens communication with serial port (in this case bluetooth)
Sleep 100

ang=0

Cls
Circle (400,300),255,RGB(196,0,0),,,,F
Line (0,301)-(800,600),RGB(0,0,0),bf
Do    

   If MultiKey(SC_ESCAPE) Then fin = TRUE
   
   If LOC(1) > 0 Then
      If datastart = TRUE Then 
         Cls
         Circle (400,300),255,RGB(196,0,0),,,,F
         Line (0,301)-(800,600),RGB(0,0,0),bf
         datastart = FALSE 
      End If
      Get #1, ,serialData
   EndIf

   If serialdata <> 10 Then message = message & Chr(serialdata)     'start to decode the information coming in
   If serialdata = 10 Then 
      'Print message
      If Left(message,5) = "BEGIN" Then                 'Start of data stream
         datastart = TRUE
         Print "TADA"
      EndIf
      If Left(message,1) = "S" Then                     'Angle Data
         ang = Val(Right(message,Len(message)-1)) - 10
         gotang = TRUE
      EndIf
      If Left(message,1) = "V" Then                     'Distance Data
         distance = Val(Right(message,Len(message)-1))
         gotmessage = TRUE
      EndIf
      message = ""
   EndIf
   
   If gotang = TRUE And gotmessage = TRUE Then                 'draw line on display if both angle and data recieved
      gotang = FALSE
      gotmessage = FALSE
      If ang = 0 Then 
      EndIf
      draw_line(ang,distance)
   EndIf
   
   For i = 0 To 250 Step 50
      Circle (400,300),i,RGB(255,255,255),radang(20),radang(160)
      marker = Str(Int(i/factor))
      Draw String (400-(4*Len(marker)),290-i), marker, RGB(255,255,255)
   Next i
   Line (399-totalang,350)-(401+totalang,400),RGB(0,0,255),b

Loop Until fin = TRUE
Close #1

End 0

Sub draw_line(ang As Integer, distance As Double)
   
   Dim As Double x1,y1,x2,y2, fdist, anginc, i, inc, ang2
   Dim As Integer x3, c, d
   
   If distance = 0 Then distance = maxdistance
   Static lastdist As Double
   
   ang2 = 360 - ang - ((180-(totalang))/2)

   fdist = distance * factor
   inc = (lastdist - distance) /500
   x1 = 400
   y1 = 300
   x3 = ((totalang-ang)*2)+ (400-(totalang))
   c = (255-((distance/maxdistance) * 255))
   If c < 0 Then c = 0
   If c > 255 Then c = 255
   
   For i = -1 To 0 Step 0.002
      d = c + (inc*i)
      If d > 255 Then d = 255
      
      x2 = ((fdist + (inc*i)) * Cos(radang(ang2 + i))) + x1
      y2 = ((fdist + (inc*i)) * Sin(radang(ang2 + i))) + y1
      
      Line (x1,y1)-(x2,y2),RGB(0,0,0)
      Line (x3 + i,350) - (x3 + i,400),RGB(d,d,d)
   Next i
   
   lastdist = distance
End Sub

Function radang(ang As Double) As Double
   
   Dim rad As Double
   
   rad = ang * (pi/180)
   
   Return rad
   
End Function