Sensor Sweeps via Remote

So I've built a load of fancy circuitry and am just updating the PC code to be able to send commands to make use of all my new work (see the Going Remote post). This means my PC interface now has the following commands:


  COMMAND_KEEPALIVE,
  COMMAND_PING,
  COMMAND_MOTION,
  COMMAND_COMPASS,
  COMMAND_SAY,
  COMMAND_SET_SENSOR_POSITION,
  COMMAND_SET_MOTOR_SPEEDS,
  COMMAND_HORIZONTAL_SENSOR_SWEEP,


The final 3 are the new ones, the first 2 of which are fairly clear - they set the sensor platform position (x and y), or set the motor speeds (left and right). The sensor sweep however tells the arduino to move the sensor platform across an entire row, taking PING readings at fixed intervals. This returns you a list of up to 181 (0 to 180 degrees) individual readings.

This simple bit of c# code shows me sending commands to the Arduino to say hello, set some motor speeds and then move the sensor platforms to random locations.

        public void DoPortThread()
        {
            //open serial port
            SerialPort port = new SerialPort("COM7", 9600);
            port.Open();


            //clear any bytes waiting in it
            while (port.BytesToRead > 0)
                port.ReadByte();


            //say hello, then set motor speeds to 0
            SendSay(port, SPEECHMSG_HELLO);
            SendMotorSpeeds(port, 0,0);

            //start moving sensor to random locations

            Random r = new Random();

            while (true)

            {

                SendSensorPositions(port, r.Next(10,170), r.Next(60,100));

                Thread.Sleep(300);

            }

        }

And here's a video of the result:

Next up is the sensor sweep command which is a bit more interesting:


            for(int y = 45; y <= 100; y+=1)
            {
                ushort[] res = SendHorizontalSensorSweep(port,1);
                for (int i = 0; i < 1; i++ )
                   Dispatcher.Invoke(new Action(delegate { DoneSensorSweep(y+i, res); }), new object[] { });


                SendSensorPositions(port, 90, y);
                Thread.Sleep(300);
            }

This takes a sensor sweep on each row from orientation 45 to 100, and then tells the dispatcher to call 'DoneSensorSweep' on the main thread. This is basically a fancy way of just calling a function without stalling the UI in a Microsoft WPF c# application. 

I'll show the whole 'DoneSensorSweep' function, as it's quite interesting. It takes as parameters a row and the data that was recorded, then writes it into an image.

       public void DoneSensorSweep(int row, ushort[] data)

        {

            System.Diagnostics.Debug.Write("Scan " + row.ToString() + ": ");

            foreach (ushort d in data)

                System.Diagnostics.Debug.Write(d.ToString() + " ");

            System.Diagnostics.Debug.WriteLine("");

            // Reserve the back buffer for updates.

            WriteableBitmap.Lock();

            int pixels_per_data = 180 / (data.Length - 1);

            unsafe

            {

                // Get a pointer to the back buffer.

                int pBackBuffer = (int)WriteableBitmap.BackBuffer;

                pBackBuffer += row * WriteableBitmap.BackBufferStride;

 

                // Find the address of the pixel to draw.

                int col = 0;

                foreach(ushort val in data)

                {

                    for (int i = 0; i < pixels_per_data; i++)

                    {

                        int pix = pBackBuffer + col * 4;

                        int greyscale = 255 - (val/2 <= 255 ? val/2 : 255);

                        int color_data = greyscale << 16; // R

                        color_data |= greyscale << 8;   // G

                        color_data |= greyscale << 0;   // B

                        *((int*)pix) = color_data;

                        col++;

                    }

                }

            }

            // Specify the area of the bitmap that changed.

            WriteableBitmap.AddDirtyRect(new Int32Rect(0, row, 181, 1));

            // Release the back buffer and make it available for display.

            WriteableBitmap.Unlock();

        }

The final result is a screen shot like this:

Believe it or not, that's an ultra sound scan of a chair on a stool. Don't beleive me? Check this out:

OK it's not exactly computer vision, and I'll definitely be adding colour stereo cameras soon, but even in this raw unprocessed form it is.. well... cool :)

Next up, I've got my blu tooth modem + dongle now, so will try doing some stuff wirelessly!