First Sight

Back on Thursday I managed to wire up both cameras to serial connections 2 and 3 on the Arduino, and began the process of getting the images over to the PC for display / processing. Over the past few days I've been gradually progressing, but getting the code to work reliably has been a hard task. Initially I was just gonna try and send them over usb serial and then handle blue tooth later, but as it turned out, using blue tooth for communication and the normal usb serial for debugging was much more useful. This is the pair of cameras wired up:

The cameras connected to the prototyping board inside MmBot

So, over the past few blogs I've ended up with:

• A system for remote communications over blue tooth
• 2 link sprite cameras, mounted on a nice 'head'
• A nice API for using the cameras

First step was to work out how to allow the PC to send a command to request a camera image, get it's information and eventually retrieve the data. Remember this whole thing has to be totally none-blocking. In other words, it needs to be a piece of code that runs once per frame, checks the state of things, and takes action if there's something to do. I settled upon writing a state machine to acheive this, which is shown below:

State machine for reading data from cameras

You can see how much more complex things get when you want them to be none blocking!

Anyhoo, the key bit is really the blue section. Here we're repeatedly grabbing some data from the camera, waiting for it to get to the pc, and then bailing out if we've reached the end of the photo. 

With a bit of debugging this process actually worked fairly well, but I wasn't entirely satisfied. The issue here is that the system has to wait for the next chunk of the image to get to the pc, before being allowed to read more from the camera. Surely it'd be better if we could be busy reading the next chunk while simultaneously sending the current chunk to the PC!

Doing the sending/receiving simultaneously isn't actually that tricky - it's basically a case of streaming. At the moment I just have 1 buffer, so after reading it from the camera, I have to wait until it's been requested by pc and transferred into the serial transmit buffer before refilling. However if I make that buffer big enough to contain 2,3 or even 32 chunks of image then I can be reading from one and writing to another! This code can easily get tricky to write, but I've written it wrong enough times over the years to finally know how to get it right first time :) Here's the state machine code I ended up with:

    void UpdateCamera()
    {
      switch(CameraStage)
      {
        case 0:
          //idle - i.e. nothing requested
          break;
        case 1:
          //remote has requested a picture be taken, so tell camera to begin taking picture
          Serial.println("Requesting camera take picture");
          Cam->StartTakingPicture();
          CameraStage++;
          //fall through
        case 2:
          //wait for camera response
          if(!Cam->Update())
            break;
          Serial.println("Camera picture taken");
          CameraStage++;
          //fall through
        case 3:
          //request the file size
          Serial.println("Requesting camera file size");
          Cam->GetFileSize(&PictureSize);
          CameraStage++;
          //fall through
        case 4:
          //wait for camera response
          if(!Cam->Update())
            break;
          Serial.print("Camera file size received: ");
          Serial.println(PictureSize);
          CameraStage++;
          //fall through
        case 5:
          //this is the main content bit
          //first, we bail out if we haven't yet sent the remote the data that is in the picture buffer
          if(PictureStreamUsed >= PICTURE_STREAM_SIZE)
            break;
          //next, we check if we have reached the end of the photo
          if(PictureReadAddress >= PictureSize)
          {
            //reached the end, so jump to stage 7 (where we stop taking the picture)
            CameraStage=7;
            break;
          }  
          //need more data, so begin getting content
          Cam->GetContent(PictureBuffer+PictureStreamWrite,&PictureAmountRead,32,PictureReadAddress);
          CameraStage++;
           //fall through
        case 6:
          //wait for camera response
          if(!Cam->Update())
            break;
          //got response, so increment the read address and loop back to stage 5
          PictureStreamWrite = (PictureStreamWrite + PictureAmountRead) % PICTURE_STREAM_SIZE;
          PictureStreamUsed += PictureAmountRead;
          PictureReadAddress += PictureAmountRead;
          CameraStage=5;
          break;
        case 7:
          //all done, so tell camera to stop taking picture
          Cam->StopTakingPicture();
          CameraStage++;
          //fall through
        case 8:
          //wait for camera
          if(!Cam->Update())
            break;    
          CameraStage++;
          //fall through
        case 9:
          //finally, wait for remote to drain the camera stream
          if(PictureStreamUsed > 0)
            break;
          //reset camera state
          CameraStage=0;
          PictureSize = 0;
          PictureAmountRead = 0;
          PictureReadAddress = 0;
          PictureSendAddress = 0;
          PictureStreamWrite = 0;
          PictureStreamRead = 0;
          PictureStreamUsed = 0;
          //fall through
        default:
          //anything that hits here just goes back to stage 0
          CameraStage = 0;
          break;  
      }
    }  

It's quite a common model in c++. You use a switch statement that falls through from case to case, only breaking out when some required condition isn't met yet (such as the camera hasn't responded yet). The code above contains the stream filling part (see cases 5 and 6), however the stream reading is in the command processing here:

    case COMMAND_PHOTO_DATA_LEFT:
      {
        //check if there's any data available
        if(CS1.PictureStreamUsed > 0)
        {
          //got data, so work out how much to send, up to a maximum of (currently) 32 bytes
          int bytes_to_send = min(CS1.PictureStreamUsed,32);
          
          //clamp the amount to avoid overrunning the end of the stream buffer
          if( (CS1.PictureStreamRead+bytes_to_send) > PICTURE_STREAM_SIZE )
          {
            bytes_to_send = PICTURE_STREAM_SIZE-CS1.PictureStreamRead;
          }
           
          //write out number of bytes to send, followed by the actual data
          WriteWord(bytes_to_send);
          Comms->write(CS1.PictureBuffer+CS1.PictureStreamRead,bytes_to_send);
          //update stream position
          CS1.PictureStreamRead = (CS1.PictureStreamRead+bytes_to_send)%PICTURE_STREAM_SIZE;
          CS1.PictureStreamUsed -= bytes_to_send;
          CS1.PictureSendAddress += bytes_to_send;
        }
        else
        {
          //no data, so write '0' (to indicate 0 bytes)
          WriteWord(0);
        }
        GCurrentCommand = TOTAL_COMMANDS;
      }
      break;

I've got one set of those commands for the left camera, and one for the right. The main command (get content) is sent by the pc to request the next chunk of the photo. Provided data is available, we pull it out of the picture buffer and send it out the serial port. Note that it's all wrapped up in a simple class. This allows me to have 2 cameras and 2 streams - 1 for each eye!

With a few tweaks to the PC side code I was able to show both images on screen. This is a photo of MmBot looking at me:

MmBot looking at me

And this is what MmBot could see (for some reason I have grey hair here - I really don't btw):

What MmBot can see when looking at me!

Awesome!

I won't bother showing the new PC code - it's basically the same stuff that I've already posted a few times. A thread sends commands to MmBot over blue tooth, waits for data to come back, and once a whole photo is received it tells the main thread to show it in an image box. The only difference is that it now reads 2 pictures instead of one.

It's now Saturday and I've come a fair way with it, but am still occasionally losing data, and eventually entering some form of deadlock, where the PC is waiting for data that isn't coming. Hopefully a few hours debugging today will solve the problem. My gut right now is that I'm overflowing the buffer on the blue tooth modem, and need to figure out / wire up the 'ready to send' pin.

Unfortunately once this is working I doubt I'm gonna be getting more than 1 frame every 5 to 10 seconds - the fact is the Arduino is cool but doesn't have the raw power needed for image processing.. Even so, it's a good experiment and once I get the raspberry pie I'll be able to do the work on-board in something closer to real-time.