3D Scanner with 2 RC Servos and a distance Sensor


i just finished building a very simple 3d scanner: :smiley:

very bacis infos on my website: Arduino 3D Scanner

Im working on a clean code what i can post, the current one is a nightmare... :-/

Its so simple to build stuff like this with the Arduino, perfect thing :slight_smile:


Great idea and realization. I would like to see some result pictures. :slight_smile: Scanned room for example. :slight_smile: How is the accuracy...

the accuracy is ok, in close range (~30cm to 80cm) its very good.
But down to 300cm its getting bad, very sensitive on the type of surface.

im working on a way to combine multiple scans. So i can scan a complete room or an object from all sides.

But it takes forever, i need to wait about 100ms at every position to get a stable distance reading.
The sample in the picture above took already 5min!

There are so many things i want to improve and implement... ::slight_smile:

First thing is to make it nice, and get some error handling that its not stoping at 90% loosing all data :-[

That accuracy is good enough to drive a mapping system for something like a more advanced Roomba (robotic vacuum cleaner). If you can get any more speed out of it and be able to track your position within the space, you could make a robot that can efficiently cover an entire oddly shaped space with very little redundant coverage.

Nice work!

that thing looks incredibly interestering, especially for my kind of job... With such a thing, driven to the maximum, one could perfectly map caves...
that makes me think of a need for yet another arduino ^^

Post more details please :slight_smile:

im already reworking the complete thing...

the accuracy is not good enough for me, i would like to get something like +-1cm over 0-3m.

now im putting a webcam + Laserpointer on the Servos :sunglasses:
driven with Processing this gives me an optical distance Sensor!

Based on this: http://www.pages.drexel.edu/~twd25/webcam_laser_ranger.html

It should speed up a Scan and gives extrem accuracy :smiley:

damn I love this ! ^^

verry nice project! ive been thinking: what if you put one of these and a compass on a headmounted display and did it somehow really fast in real time, then you would have some kind of night vision bat googles ::slight_smile:

now featured on Hacked Gadgets :smiley:

i will post the sourcecodes later today :slight_smile:

the Arduino code:

//test by STMAR 24.02.2009

//Start to see how to control the Servos...

#include <AFSoftSerial.h>

#include <Servo.h> 
Servo myServoPan;  // create servo object to control a servo 
Servo myServoTilt;  // create servo object to control a servo 

AFSoftSerial mySerial =  AFSoftSerial(3, 2);

int distpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin 
char CurrentServo;
int value;

int valueA;
int valueB;

int fresh = 0;

void setup()  
  myServoPan.attach(9);  // attaches the servo on pin 9 to the servo object 
  myServoTilt.attach(10);  // attaches the servo on pin 9 to the servo object 

  pinMode(13, OUTPUT);
  Serial.println("Scanturm V0.1");

void loop()                     // run over and over again

void get_keyboard_commands() {
  char input[80];
  int incount = 0;
  if ( Serial.available()) {
    // read in a string byte by byte
    while (Serial.available()) {
       input[incount++] = Serial.read();      
    input[incount] = '\0';  // puts an end on the string
    switch(input[0]) {
      case 'P':        
        fresh = 1;
      case 'T':        
        fresh = 1;
      case 'A': 
        val = analogRead(distpin);            // reads the value of the Distance Sensor (value between 0 and 1023)  
        fresh = 2;
       // value=(ch-'0')*20;  // comes in as ascii value, subtract off '0' to convert to numbers     
        value = atoi(input);
      if (CurrentServo=='P') {
        valueA = value;
      else if (CurrentServo=='T') {
        valueB = value;
      //Serial.print(" ");
      fresh = 0;  
      Serial.print("A: ");
      fresh = 0;

The VB.net Code driving the Servos:

    Public Function DriveServo(ByVal Pan As Integer, ByVal Tilt As Integer) As String

        'Valid Ranges are from 0 to 155

        If MSCOMM1.PortOpen = True And ComReady = True Then

            MSCOMM1.Output = Chr(200).ToString          'For Sync

            MSCOMM1.Output = Chr(Pan).ToString          'Drives the Pan

            MSCOMM1.Output = Chr(Tilt).ToString         'Drives the Tilt

            'MSCOMM1.Output = Chr(123).ToString          'Triggers the ADW

            Dim AnalogResult As String
            AnalogResult = FetchAnalogReturn()

            'Debug.Print("AnaIn: " & AnalogResult)

            Return AnalogResult

            Return ""
        End If

    End Function

And the FetchAnalogReturn Function:

    Private Function FetchAnalogReturn() As String
        'This needs a timeout!
        'More then 200ms and a defoult should be returned.

        Dim InputStr As String

        InputStr = Nothing

        Dim Time1 As Int32

        Time1 = My.Computer.Clock.TickCount + 200


            InputStr = InputStr & MSCOMM1.Input

            If My.Computer.Clock.TickCount > Time1 Then
                GoTo Fehler
            End If

        Loop Until InStr(InputStr, "$", CompareMethod.Text)

        InputStr = Replace(InputStr, ": ", "")
        InputStr = Replace(InputStr, "$", "")
        InputStr = Replace(InputStr, vbCrLf, "")

        GoTo Normal
        InputStr = "0"
        Debug.Print("Timout during analog Receive happend!")


        Return InputStr

    End Function

The Scanning Function:

   Private Sub AreaScan(Optional ByRef Delay As Integer = 0)

        Dim P, X As Integer
        Dim ADWRaw, Dist As String
        Dim Dir As String

        Dim Pmin, Pmax, Tmin, Tmax, Pstep, Tstep, Points As Integer

        Dim TimeStart, TimeDuration As Int32

        'The Parameters:

        Delay = 100

        Pmin = 40
        Pmax = 100
        Pstep = 1

        Tmin = 60
        Tmax = 100
        Tstep = 1


        Points = ((Pmax - Pmin) / Pstep) * ((Tmax - Tmin) / Tstep)

        RichTextBox1.Text = Nothing
        RichTextBox2.Text = Nothing

        ProgressBar1.Maximum = Pmax
        ProgressBar1.Minimum = Pmin
        ProgressBar1.Value = Pmin

        DriveServo(Pmin, Tmin)

        TimeStart = My.Computer.Clock.TickCount

        Dir = "Down"

        For P = Pmin To Pmax Step Pstep

            If Dir = "Down" Then

                For X = Tmin To Tmax Step Tstep

                    If Delay > 0 Then
                        DriveServo(P, X)
                    End If

                    ADWRaw = DriveServo(P, X)

                    Dist = ADWtoDist(ADWRaw)
                    PolarWorld(P, X, 1) = Dist

                    PrintCartesian(P, X, Dist)


                Dir = "Up"


                For X = Tmax To Tmin Step Tstep * -1

                    If Delay > 0 Then
                        DriveServo(P, X)
                    End If

                    ADWRaw = DriveServo(P, X)

                    Dist = ADWtoDist(ADWRaw)
                    PolarWorld(P, X, 1) = Dist

                    PrintCartesian(P, X, Dist)


                Dir = "Down"

            End If

            ProgressBar1.Value = P



        TimeDuration = My.Computer.Clock.TickCount - TimeStart

        Debug.Print("It took: " & TimeDuration & "ms for " & Points & " Points")

        Debug.Print("Average: " & TimeDuration / Points & "ms")
        Debug.Print("What is: " & (TimeDuration / Points) - Delay & "ms more than the Delay")

    End Sub

its messy, buts works nicely! :slight_smile:

totally awesome.
This week i was in two caves and i was thinking about your project.. thats a thing i could work on...

i wonder if there is some Arduino-only solution available for this.. if so that'd be the greatest thing i could envision for me persooanlly...
Yet i know the Arduino is not capable of doing that kind of picture or rather video editing needed to calculate the distance there should be other ways for achieving this as well.. i mean those simple distance calcuaters dont work with a webcam as well i think?

Instead of writing the data to serial, log it to an SD card. Then do all the crunching and rendering offline. That would allow you to mobilize the device.

you can log it onto an SD card, there should be enough room for some scans :wink:

if for example you always move the scanner exactly 2m forward you can take that into account as an offset during the polar/cartesian conversion.

also use a more accurate servo lib, im having only 155 steps for the 180¬į.
if you can get something like 0,5¬į resolution its getting much bether at some distance!

Hey designer.

I wonder if you could help me, I'm thinking of extending your work with the scanner. I've been reading up on polar coordinates and I just wanted to understand how produce a 3d image.

From my understanding the scanner is at point 0, everything else is relative to the servos and distance.

When you convert to cartesian coordinates, again, they are relative. so you may have a -10, -10 (for example) where 0,0 is your scanner.

I dont know [anything] much about Mesh (or any other 3d rendering software), but from the tech specs, you just feed it a collection of cartesian readings in the right format and it'll render the result - leaving you to "clean up" the image.

Any guidance would be highly appreciated!

if for example you always move the scanner exactly 2m forward you can take that into account as an offset during the polar/cartesian conversion.

You could use a wheel with a 1m (or any known) circumference and a rotation sensor so that the code calculates the distance for you. In fact, you could even make the scanner head rotate 360 degrees one rotation and then back and have it log a zig-zag helical scan of sorts. As long as you know the traveled distance in relation to the scan head angular movement, you can calculate the 3D space.

This would give you a straight-but-bumpy map of the cave. You would need to measure ahead and behind some distance so you could detect and calculate turns and other directional changes.

Thats my way of getting a image:

lets just look it up for 1 point (Code is VB.net)

1.) Origin.

the rotationpoint from the pan-servo is 0,0,0 (X,Y,Z) or 0,0(R,Angle)

2.) Where¬īs your scanned point.

for example the pan servo is at 50¬į and the tilt servo at 30¬į. your reading a analog value of lets say 100.

so first convert the analog value to distance:

    Public Function ADWtoDist(ByVal ADW As Integer) As String

        'If the Value is smaller as 38, the result would be biger as 300cm, what makes no sense.
        If ADW < 38 Then ADW = 38

        ADWtoDist = 10565 * ADW ^ -0.982

        'Debug.Print("Dist: " & ADWtoDist & " cm")
    End Function

3.) then we convert the polar coordinates into cartesian:

Basics: Maths - Polar coordinates- Martin Baker

in Code it looks like this:

 Public Function PolarToCartesian(ByVal TiltAngle As Double, ByVal PanAngle As Double, ByVal R As Double) As Array

        'This will be 1=X,2=Y,3=Z
        Dim Result(3) As Double

        'Basic: http://www.euclideanspace.com/maths/geometry/space/coordinates/polar/index.htm
        ' x = r sin(a1) cos(a2)
        ' y = r sin(a1) sin(a2)
        ' z = r cos(a1)
        'Where a1 = TiltAngle, a2 = PanAngle, r = R

        'All Angles must be Radians, and they must be 0-180, not 0-155

        TiltAngle = TiltAngle * 180 / 155
        PanAngle = PanAngle * 180 / 155

        TiltAngle = Math.PI * TiltAngle / 180.0
        PanAngle = Math.PI * PanAngle / 180.0

        'Now they can be calculatet:

        Result(1) = (R * Sin(TiltAngle) * Cos(PanAngle)) + TextBox1.Text
        Result(2) = (R * Sin(TiltAngle) * Sin(PanAngle)) + TextBox2.Text
        Result(3) = R * Cos(TiltAngle)

        Return Result

    End Function

the 0-155 to 1-180 conversion comes from my servos, if you move them from 0-155 they make already 0-180 degrees... ::slight_smile:

4.) And know this must be saved into a file!

Easiest format is obj: OBJ - Wikipedia

example file from my scan:

v 31.9121705541201 33.5715843563032 17.1546264824741
v 33.984405250283 35.7515740185642 17.1399861761586
v 36.1267443214351 38.0053134414284 17.037524539716
v 32.3026021291672 33.9823181399808 14.1901570731361
v 32.4941081567727 34.1837823666023 13.2370150128776
v 32.8076369704424 34.5136144912581 12.3293085742347
v 32.8370021129878 34.544506603671 11.3148117078009
v 32.4527234486948 34.1402456784305 10.1785760817876

to do that in Code:

            Dim Cartesian(3) As Double

            Cartesian = PolarToCartesian(Tilt, Pan, Dist)

            Dim OutStr, OutStr2, X, Y, Z, As String

            OutStr = Nothing

            X = Replace(Cartesian(1).ToString, ",", ".")
            Y = Replace(Cartesian(2).ToString, ",", ".")
            Z = Replace(Cartesian(3).ToString, ",", ".")

            'This is the obj String:

            OutStr2 = "v " & X & " " & Y & " " & Z & vbCrLf


5.) and at the end of a scan save it to a .obj file:

RichTextBox2.SaveFile("N:\Programme\Meshlab\ArduinoScan.obj", RichTextBoxStreamType.PlainText)

now just open this .obj file in Meshlab, and your ready to run :slight_smile:

6.) Meshlab

look here: NXT 3D scanner

under "3D reconstruction" theres a perfect description on how to use Meshlab!


Ah thats fantastic, I'll give that a go as soon as I recieve my measurer.

Thanks a lot, I'll let you know my results :slight_smile:

somebody has taken this to the extreme: Pruned: Mapping Abysses

very cool stuff :sunglasses: