Of Finite State Machines and Robotics...

My friend Jon Hylands had asked a simple question the other day as to whether I used Finite State Machines in my code.

 I quipped back "Of course! Is there any other way?" 

But that got me to thinking... Was this always the case?  And where did trial and error coding end and planned FSM begin?  In discussions with a few other Hobby Roboticists, it became evident that the concept of a "Finite State Machine" was not fully clear.

The Academics in the group (who probably wouldn't read my tripe anyway) would tell you that a Finite State machine is " a mathematical model of computation used to design both computer programs and sequential logic circuits. It is conce......"   blah blah blah......

 Flow chart and a set of tables!    A finite state machine is simply a flow chart that follows a table of possible states that something can be in, provided the appropriate inputs.

I know many of us, myself included start off small projects just stringing pieces of code together to "get a result".  But very soon into the project you realize that you need to define the parameters of operation. 

For instance, in a very simple "Object Avoidance Robot"  that just wanders around and makes sure it doesn't hit things, there is still a State Machine to follow. 

Following the above routine, you should be able to wander until your battery dies...  Not very smart, not very useful, but predictable.

Even if you have a "Fully Manual Control" Robot, ie: you have some means of providing motion commands and expect it to react in kind,  you likely still have some kind of logic flow (State Machine) running to manage this motion.

For  instance something as "Simple" as a "Move Forward to position X,Y" command might *really* look like this:

When you start drawing out the various "states" of your Robot:

• Stopped
• moving forward - forward path clear - left side clear - right side clear
• moving forward - obstacle in distance - left side clear - right side clear
• moving forward - obstacle in near - left side clear - right side clear
• moving forward - forward path clear - left side blocked - right side clear
• moving forward - obstacle in distance - left side blocked - right side clear
• moving forward - obstacle in near - left side blocked - right side clear
• etc...

You will realize just how convoluted this can get... This is where a "State Table" comes in handy.

Turn each question into a Yes or No, True or False.  Don't ask "how far is it to the obstacle?"  but rather "Is there an obstacle within 12cm?" 

In this table, we are simply looking forward, not side to side.  Is our path clear, or is there an object present, distant or near, and is our compass heading on target or drifting Clockwise or Counter Clockwise.

The answers to these Boolean states will drive the left and right wheel Direction and Speed.

Creating State Tables for your various routines will help you better understand the expected outcomes based on inputs.  You may suddenly understand why your Robot keeps getting trapped in that corner!

References:

http://playground.arduino.cc/Code/FiniteStateMachine

http://en.wikipedia.org/wiki/Finite-state_machine

Raspberry PI - Processing Serial data from Arduino in python

I typically send a bunch of information back and forth between Arduino and Raspberry PI. 

Data such as Odometry, Range Findings, Environmental information, and health of the Rover. 

Today, I'm doing this via SPI, but the question was asked about how this is done via Serial, and specifically how to read and process the Arduino Serial Data in python, on the Raspberry PI.

So here is how *I've* tackled this (your mileage may vary, this is just an example)

On the Arduino Side:
(I send a CRLF terminated line starting with a data type descriptor: in this case "SNSR" for sensor data)

void Send_Sensors() {
   Serial.print("SNSR,");
   Serial.print( now);   Serial.print(","); 
   Serial.print(motion);   Serial.print(",");
 
   Serial.print(AccScaled.XAxis);   Serial.print(","); 
   Serial.print(AccScaled.YAxis);   Serial.print(","); 
   Serial.print(AccScaled.ZAxis);   Serial.print(",");
   Serial.print(CurrentHeading);    Serial.print(",");
 
 
    Serial.print(DirectAhead); Serial.print(",");      Serial.print(DirectBehind);  Serial.print(",");
   Serial.print(DirectRight);  Serial.print(",");  Serial.print(DirectLeft); Serial.print(",");
   Serial.print(FrontRight);  Serial.print(",");  Serial.print(FrontLeft);   Serial.print(",");
   Serial.print(Temperature);  Serial.print(",");  Serial.print(Pressure);   Serial.print(",");
   Serial.print(Altitude);  Serial.print(",");  Serial.println(batvolt);
 

} 

On the Raspberry Pi Python Side:
(Caveat: this is just a snippet from my code... it will not function fully as is:)

• I import Serial
• I declare a function to read and parse a line from Serial
• I set up the serial port and open it.
• I then wait for incoming data in my main loop.  If the first field matches what I'm looking for, I update my variables.

#! /usr/bin/python
#

import serial

########################################################################
# Global Variables   -  Yes I know these are bad...

accel_x = 0.0
accel_y = 0.0
accel_z = 0.0
heading = 0.0
directahead = 0.0
directbehind = 0.0
directleft = 0.0
directright = 0.0
voltage = 0

ready=0                        # ready to send command to arduino
timeout = 0                    # To avoid infinite wait state, we will count 100 loops and assume failure
line = None                     # Holds current response from Arduino Serial
line_len = 0                    # number of items in "line"


##########################################################################
# Declare functions

def get_arduino_response():
   global line
   global line_len
  
   if (arduino.inWaiting() > 0):              # Number of characters in arduino buffer  
       line = arduino.readline()
       line = line.split(",")
       line_len = len(line)
       print 'Receiving: line[0]= ' , line, 'Num Items= ', line_len, 'timeout= ', timeout


##########################################################################
# Set up serial port to Arduino
arduino = serial.Serial('/dev/ttyACM0', 115200, timeout = 10)
arduino.open()


time.sleep(10)    # Time for Arduino to reset and settle
arduino.flushInput()  


##########################################################################
# Main Loop

try:
    running = True

    while 1:                    # Continuous loop
       # os.system('clear')
       stamp = time.time()

       get_next_command()            # find the next available command to process

       get_arduino_response()                   # If data available from arduino, retrieve into "line"
      

       if line[0] == "SNSR" and line_len == 13:    # Ensure entire line came through
          ardtime = line[1]
              motion = line[2]
          accel_x = float(line[3])
          accel_y = float(line[4])
          accel_z = float(line[5])
          heading = float(line[6])
          directahead = float(line[7])
          directbehind = float(line[8])
          directleft = float(line[9])
          directright = float(line[10])
          voltage = line[11]
          UID = line[12]
          UID = UID.strip()


          print 'line ' , ardtime, accel_x, accel_y, accel_z, heading ,'done'


 
   

except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
    print "\nKilling Thread..."
    db.close()
print "Done.\nExiting."

 

My new OSLRF Open Source Laser Range Finder

So I received this awesome package in the mail yesterday from LightWare Optoelectronics in South Africa.

I had posted a link over on LetsMakeRobots last week to highlight this new product that had been developed for the Hobbyist / Open Source Community.     Open source laser rangefinder sensor. up to 9m $100USD

Open source laser rangefinder sensor. Including optics, laser, detector, amplifiers and sequential-equivalent-time-sampling (SETS) circuits, it is a 'bare-metal' front end component for a laser rangefinder system. Now available.

• Range :: 0.5 ... 9 m
• Resolution :: Adjustable
• Update rate :: Adjustable: 3 ... 50 readings per second
• Accuracy :: Adjustable
• Power supply voltage :: 12 V (10 … 16 V)
• Power supply current :: 50 mA
• Outputs & interfaces :: Timing & laser signal outputs
• Dimensions :: 27 x 56 x 65 mm
• Weight :: 57 g
• Mounting :: 4 x M3 (3.2 mm diameter)
• Connections :: 0.1 in. pitch header
• Optical aperture :: 53 mm
• Laser power :: 14 W (peak), 6 mW (average), Class 1M
• Operating temperature :: - 20°C ... + 60°C

The next morning after posting it, I was greeted by an email from the manufacturer, thanking me for the posting and offering me a trial.  I quickly accepted, on the promise that I would completely document my process/progress, and would share back anything I developed for it. 
I was quite pleasantly surprised to see a package from DHL within just a few business days (Coming from South Africa to Canada!) and here begins my journey into un-packaging, marvelling, reading... more reading,  whiteboarding, and developing a useful and cheap interface between this Laser Range Finder module, and our hobbyist Robots.

As the documentation describes, this is a bare bones Range finder, requiring a microcontroller to set up and process the ranging data.  Nothing we are not all familiar with on our various IR and UltraSound Range Finders.  There's just a little bit more to this one.
LightWare Optoelectronics  employs Sequential Equivalent Time Sampling (a process developed by Tektronix to allow their oscilloscopes to work at higher frequencies) to represent a more manageable time scale for the Laser time of flight measurement. This allows us hobby roboticists to use our existing processors (Arduino or Atmel, PIC, Propeller, etc...) to interface and take measurements from it.

Directly from the manual:

The OSLRF-01 is a time-of-flight, “bare-metal” sensor that forms the front end of a laser rangefinder system. It runs autonomously
when power is applied and produces electrical signals that can be analysed to determine the time it takes for a laser pulse to travel
from the unit, to a surface and back again.


The OSLRF-01 solves the most critical engineering problems that designers face when making a time-of-flight laser rangefinder. These
are:

1. The laser needs to be “fired” using a very short current pulse of tens of amps. The high speed driver components must be shielded to prevent optical and electronic leakage which would otherwise interfere with the detector and mask the return signal.
2. The detector needs to pick up the very weak return signal and amplify it to a level well above any background noise. This amplification is done using high speed amplifiers that are expensive and consume a lot of power.
3. The time between the outgoing laser pulse and the return signal needs to be measured with very high precision in order to calculate the distance. Clocking speeds of 15GHz would be needed in a timer capable of 1cm resolution and this is impractical.
4. Collimating optics for the outgoing laser beam and collection optics for the return signal are needed to make the system work over a reasonable range. These can be expensive components.

The OSLRF-01 consists of a laser, photodiode, optics, amplifiers and sequential-equivalent-timebase-sampling (SETS) circuits. These
components work together to create signals that are easy to analyse, having been amplified and slowed down to a manageable
speed. The output signals from the OSLRF-01 include the outgoing laser pulse, the return signal and various timing references.

I am currently laying out a daughter card based on my exisiting I2C Atmel ATtiny84 Ultrasound Scanner.  This daughter card will mount over the existing screw holes on the OSLRF01, will manage a pan servo, and provide 180 degrees of ranging data to your Robot or Autonomous Rover via I2C.

Future updates to this blog will document that process....

(Here is where you ask for features!)

References:
http://lightware.co.za/shop/en/laser-sensors/24-oslrf-01.html
http://www.google.ca/patents/US7746058
http://www.rp-photonics.com/time_of_flight_measurements.html
http://en.wikipedia.org/wiki/Time_of_flight
http://en.wikipedia.org/wiki/Laser_rangefinder

Got some video up of Bottwo in action!

First, here is Bottwo upon initialization command.

In this video, it starts with two full circles to calibrate the compass.  This does not have to be  done frequently, but for the purpose of completeness, I've included it here.  After compass calibration, Bottwo attempts to find a couple distinct landmarks that it can use to presume it's location. Yeah... it pretty much guesses based on shape/angle/orientation of a couple local features.

Localization assumes that you have some prior knowledge of the surroundings.  In this case, I have provided the rover with a compass oriented bitmap of my floorplan as a base.  Any object detected, but NOT on the floorplan will be added to a second map of transient objects, which is superimposed upon the floorplan.

This next video is Bottwo sitting idle, waiting for a command. On the pan servo, is a pair of MaxSonar EZ0  Ultrasound range finders 180 degrees apart. It is simply doing a Left, Right, Center Sonar scan front and rear.  The rover also has front, rear, left and right Sharp Infrared range finders for collision avoidance.

Below, this video shows Bottwo instructed through it's web interface, to go to a location that is obstructed by a wall....

Using Sonar scanning left to right, Bottwo attempts to go directly to the destination. It get's obstructed by a wall, and then backtracks to find an exit.  It then repositions itself to complete the task.

You will notice a couple little corrections of path when it is heading down the hallway.  I am still (*STILL*) having problems keeping both motors running at the same speed.  The PID routine monitors left and right encoders, attempts to correct for left count verso right count, but ultimately get's overridden by Compass bearing if it is too far out.

(I refuse to upload the dozen or so videos where it crashed into office chairs, clipped the door frame, etc... LOL)

References:

http://en.wikipedia.org/wiki/Robotic_mapping

http://rossum.sourceforge.net/papers/Localization/PosPosterv4.pdf  <-----  READ THIS!!!

Minimizing timing impact of Diagnostic use of Serial.print function in Arduino

While developing routines for my rover, especially diagnostic routines to identify sensor errors or conflicts, I have been using the Serial.print() function.   

I've found it very useful over the past several months. 
However, like all high level functions, it comes at a cost... timing...  

I decided to see how much of a cost, and if there was anything I could do to reduce this cost.  

Here are my results....

This is a typical diagnostic report for my distance ranging module.  There are four Sharp Infrared Distance Sensors (one on each of front/rear/left/right)  and there are two MaxBotix Ultrasound modules on a servo 180 degrees from one another.  In this example, these give me Frontleft, Front, FrontRight, RearRight, Rear, RearLeft.

      Serial.print("StartTime with separate Serial.print statements = "); 
      Serial.println(test=millis()); 

         Serial.print("Front: ");  Serial.print(rangefront[midpoint]); 
         Serial.print("  Rear: ");  Serial.print(rangerear[midpoint]);  
         Serial.print("  Left: ");  Serial.print(rangeleft[midpoint]); 
         Serial.print("  Right: ");  Serial.print(rangeright[midpoint]);

         Serial.print("    Front Sonar: ");  Serial.print(FrontSonar[midpoint]/58); 
         Serial.print("  Rear Sonar: ");  Serial.print(RearSonar[midpoint]/58);  

         Serial.print("FrontLeft: ");  Serial.print(FrontLeft); 
         Serial.print("  FrontRight: ");  Serial.print(FrontRight);  
         Serial.print("  Front: ");  Serial.print(Front); 
         Serial.print("  RearLeft: ");  Serial.print(RearLeft); 
         Serial.print("  RearRight: ");  Serial.print(RearRight);  
         Serial.print("  Rear: ");  Serial.println(Rear); 

      Serial.print("EndTime with separate Serial.print statements = "); 
      Serial.println(millis()-test); 

On an 8Mhz Arduino Pro Mini, this snippit takes 17ms to generate and send this report.

But what if I used string concatenation, and only sent ONE string our Serial like this?

      Serial.print("StartTime with one Serial.print statement and concat strings = "); 
      Serial.println(test=millis()); 
        
         // Assemble the string first, then print it once.
         
         response = "";     // Flush previous message.
         response += "  Front: ";      response += rangefront[midpoint];  
         response += "  Rear: ";     response += rangerear[midpoint]; 
         response += "  Left: ";     response += rangeleft[midpoint];   
         response += "  Right: ";    response += rangeright[midpoint]; 

         response += "  Front Sonar: ";  response += FrontSonar[midpoint]/58; 
         response += "  Rear Sonar: ";  response += RearSonar[midpoint]/58;  

         response += "  FrontLeft: ";  response += FrontLeft; 
         response += "  FrontRight: ";  response += FrontRight;  
         response += "  Front: ";    response += Front; 
         response += "  RearLeft: "; response += RearLeft; 
         response += "  RearRight: ";  response += RearRight;  
         response += "  Rear: ";     response += Rear; 

         Serial.println(response);

      Serial.print("EndTime with one Serial.print statement and concat strings = "); 
      Serial.println(millis()-test); 

 On an 8Mhz Arduino Pro Mini, this snippit only takes 6ms to generate and send this report.
And before you ask why I don't just serially concatenate in one statement... the Arduino IDE doesn't seem to support that, at least not for mixed data types.

Can I do any better?  What if I reduces the TEXT portion to just comma separators, and only sent ONE string our Serial like this?

      Serial.print("StartTime with no filler and concat strings = "); 
      Serial.println(test=millis()); 
        
         // Assemble the string first, then print it once.
         
         response = "";     // Flush previous message.
         response += rangefront[midpoint];  response += ", ";    
         response += rangerear[midpoint];  response += ", "; 
         response += rangeleft[midpoint];   response += ", ";    
         response += rangeright[midpoint]; response += ", "; 

         response += FrontSonar[midpoint]/58; response += ", ";  
         response += RearSonar[midpoint]/58;   response += ", "; 

         response += FrontLeft; response += ", ";  
         response += FrontRight; response += ", ";    
         response += Front;  response += ", ";  
         response += RearLeft; response += ", ";  
         response += RearRight;  response += ", ";   
         response += Rear; 

         Serial.println(response);

      Serial.print("EndTime with no filler and concat strings = "); 
      Serial.println(millis()-test); 

 On an 8Mhz Arduino Pro Mini, this snippit now only takes between 3-4ms to generate and send this report.

This is a significant improvement, and reduces the overall impact diagnostics has on my loop timing....

Hope you find this helpful...

HMC6352 Compass Module in Continuous Read Mode for Arduino

I have a need (desire?) to use compass heading as part of my PID loop to keep my rover heading true.  Using my well established trial and error coding technique, I was unable to get a sufficient sample rate from the HMC6352 Compass module using code I pilfered.. er... borrowed (I plan to give it back!) from the Interwebs.

Most people seem to use the HMC6352 Compass module in "Standby" Mode, or at least every snippit of code I've seen through Google searches does...

The problem with Standby or Query mode, is that it requires a 6ms settling time for each reading, and if you are continuously polling it anyway, you really save little energy.  By setting the HMC6352 into "Continuous Read" mode, you can get the most recent heading immediately. At least up to 20hz.

From the Datasheet:

Operational Modes

The HMC6352 has three operational modes plus the ability to enter/exit the non-operational (sleep) mode by

command. Sleep mode sends the internal microprocessor into clock shutdown to save power, and can be brought back by the “W” command (wake). The “S” command returns the processor to sleep mode. The three operational modes are defined by two bits in the internal HMC6352 Operation Mode register. If the master device sends the “L” command, the current operational mode control byte in the RAM register is loaded into the internal EEPROM register and becomes the default operational mode on the next power-up. The application environment of the HMC6352 will dictate the most suitable operational mode.

Standby Mode:

(Operational Mode=0) This is the factory default mode. The HMC6352 waits for master device commands or change in operational mode. Receiving an “A” command (get data) will make the HMC6352 perform a

measurement of sensors (magnetometers), compute the compensated magnetometer and heading data, and wait for the next read or command. No new measurements are done until another “A” command is sent. This mode is useful to get data on demand or at random intervals as long as the application can withstand the time delay in getting the data.

Query Mode:

(Operational Mode=1) In this mode the internal processor waits for “A” commands (get data), makes the

measurements and computations, and waits for the next read command to output the data. After each read command, the HMC6352 automatically performs another get data routine and updates the data registers. This mode is designed to get data on demand without repeating “A” commands, and with the master device controlling the timing and data throughput. The tradeoff in this mode is the previous query latency for the advantage of an immediate read of data. The above two modes are the most power conserving readout modes.

Continuous Mode:

(Operational Mode=2) The HMC6352 performs continuous sensor measurements and data computations at selectable rates of 1Hz, 5Hz, 10Hz, or 20Hz, and updates the output data bytes. Subsequent “A” commands are un-necessary unless re-synchronization to the command is desired. Data reads automatically get the most

recent updates. This mode is useful for data demanding applications. The continuous mode measurement rate is selected by two bits in the operational mode selection byte, along with the mode selection and the periodic Set/Reset bit. The periodic Set/Reset function performs a re-alignment of the sensors magnetic domains in case of sensor perming (magnetic upset event), operating temperature shifts, and normal thermal agitation of the domains. Exposure of the HMC6352 to magnetic fields above 20 gauss (disturbing field threshold) leads to possible measurement inaccuracy or “stuck” sensor readings until the set/reset function is performed. With the periodic Set/Reset bit set, the set/reset function occurs every few minutes.

So, what all this means is: It is possible to get data from the HMC6352 at up to a 20hz rate without incurring a 6ms delay for stabilizing... And you get to remove a few lines of setup and read for each loop as well!

----------------------------------------------------------------------------------------------------------------------------------

Here is a HMC6352 heading read morph of most searches I've found for the Arduino:

float Get_Compass()
{
    Wire.beginTransmission(HMC6352SlaveAddress);
    Wire.write('A');                           // The "Get Data" command
    Wire.endTransmission();

            //delays required by HMC6352 upon receipt of the command
           //Get Data. Compensate and Calculate New Heading : 6ms
   delay(6);

  Wire.requestFrom(HMC6352SlaveAddress, 2);  //get the two data bytes, MSB and LSB

           //"The heading output data will be the value in tenths of degrees
            //from zero to 3599 and provided in binary over the two bytes."
  byte MSB = Wire.read();
  byte LSB = Wire.read();

  float HeadingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
  float HeadingInt = HeadingSum / 10;
  return HeadingInt;
}

----------------------------------------------------------------------------------------------------------------------------------

Here is the new and improved HMC6352 Continuous heading read  for the Arduino:

   There is a little bit of setup, but this is more then compensated for by the lines removed from the actual READ process.

void setup()

{

  Wire.beginTransmission(HMC6352SlaveAddress);
   Wire.write('G');                               // Command - Write to RAM
   Wire.write(0x74);                            // Operational mode control byte register address 74(hex).
   Wire.write(0x72);                           // 20 Hz - Continuous mode.
   Wire.endTransmission();
}

float Get_Compass()
{
  Wire.requestFrom(HMC6352SlaveAddress, 2);  //get the two data bytes, MSB and LSB

          //"The heading output data will be the value in tenths of degrees
          //from zero to 3599 and provided in binary over the two bytes."
  byte MSB = Wire.read();
  byte LSB = Wire.read();

  float HeadingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
  float HeadingInt = HeadingSum / 10;
  return HeadingInt;
}