Ultrasonic Radar & Robot

'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

Ok, so the first couple of images i posted showed no more than a sensor on a servo.  Since then i've put together a wheelbase (not the most efficient setup, but it serves a purpose for the moment), added a bluetooth connection, and an IR avoidance sensor.  

The wheelbase is made from a couple of motors and gearboxes salvaged from an old robot toy.  Sadly these are massively power hungry, and so the drive batteries don't last very long at all.  

I've used an uber cheap HC-06 bluetooth module from ebay for wireless communication, and although it was fussy at the start, it seems to stream the data nicely.  Sadly however i've not figured out a way of putting it to sleep when it's not in use, causing more of a power drain on the system.  Coupled with the cheap servo the arduino is also drawing it's fair share of current (although from a different set of batteries from the motors).   

 Next step is definitely to sort out the power issues.  Hopefully starting with the control side, and shutting down both the servo, bluetooth, and possibly even the ultrasonic sensor when they are not needed.