Coding a HWIL module

It has become a requirement to be able to detect issues with missions by rerunning the logs while the robot is on the bench. This turns out to be bigger than I thought it would be. What I really had to do, was rewrite my logger so that it did a better job simplifying the xml that was being produced.

interface kit 0 not attached
interface kit 1 not attached
< Message time="0" Received="10" Body="" />
< Message time="0" Received="10" Body="" />
< StateMessage:IMessage,ISerializable time="12" Received="12" >< taskManagerState RxQueueLength="0" QueuableCommandsMissed="0" QueuableCommandsMissedTotal="0" time="12" OverrideWaterMark="0" MessagesReceived="12" MessagesSent="12" MessagesNull="0" histo0="0" histo1="3" histo2="0" histo3="0" histo4="0" histo5="0" histo6="1" histo7="0" histo8="1" histo9="7" histo10="1" /> < /StateMessage:IMessage,ISerializable>

What I needed to do was to check the logged item first before it was logged and check to see if it was an entry itself. This was just to keep the number of recursive loops to a minimum. The default XML reader is easy to use if the entry just has values. It is also just easier if nested records were not necessary to wrestle with.

It has become a requirement to be able to detect issues with missions by rerunning the logs while the robot is on the bench. This turns out to be bigger than I thought it would be. What I really had to do, was rewrite my logger so that it did a better job simplifying the xml that was being produced.

interface kit 0 not attached
interface kit 1 not attached
< Message time="0" Received="10" Body="" />
< Message time="0" Received="10" Body="" />
< StateMessage:IMessage,ISerializable time="12" Received="12" >< taskManagerState RxQueueLength="0" QueuableCommandsMissed="0" QueuableCommandsMissedTotal="0" time="12" OverrideWaterMark="0" MessagesReceived="12" MessagesSent="12" MessagesNull="0" histo0="0" histo1="3" histo2="0" histo3="0" histo4="0" histo5="0" histo6="1" histo7="0" histo8="1" histo9="7" histo10="1" /> < /StateMessage:IMessage,ISerializable>

Ok, now that a reasonable data format is available, let's make a factory that can take the XML data and create messages s that they are in the same form as those created by the system. I built a simple Command pattern system that is configured in my normal configuration files. So if it is a normal mission, it will play normally. If the configuration is to rerun an existing log file, it has the path to the log file to read and whether the mission should begin immediately or with some wait period between completing reading the log and beginning the mission.

Most of the time, I find that it is reasonable to ask for a key input before starting the execution of the HWIL test. Nothing like a 30# car scooting across the table when you are not ready. Make sure that the robot is strapped down, sometimes it is more kinetic than was planned.

Now that we have the robot strapped down to a raised dais, we can hit enter and let it run the run. I made this a simple list of Commands following the pattern and a timer. The invoker checks at each timer tick and executes all of the commands that were logged at that time during the normal run.

I had to make simple ICommands that had a hook member and an execute. The invoker is hooked to the timer and I hooked the commands to the TaskManager. That way the Execute() method, just created a proper IMessage and delivered it directly to the TaskManager. It also makes it so that you can rewind the commands. This method has allowed me to find some linkage issues that would have been hard to find other ways. There are some hysteresis issues with any mechanical system, I was not prepared for the number that were found.

"...But knowing is half of the battle..."

The PhidgetsException #9

For the most part, I really like Phidgets. They are good simple equipment that is inexpensive and just works. That is a trick in my experience in the current robotics technology playing field. Their API is the best I have seen and I have to say does a nice job even if you want to write in c# or flash. Yeah, Flash is supported.

So I was happily banging away at my control code to manage these servos. I kept seeing that the code was running. I sent lots and lots of new servo locations to the servos and it just did not move the servos. After some investigation into my logs and trapping lots of PhidgetExceptions. I figured out what the issue was. Like a good little OOP'er I had contained my reading of the Advanced Servo Controller 8 port's properties in an object and then a different handles the executions. Here in lay the problem, the properties of a given servo such as velocity which has no setter (mistake in the documentation) are not exposed until the motor is in engaged.

So I wrote the following code to fix that. These are not threadsafe, you have to make sure that you are not engaged at the wrong time. It will make all kinds of wacky stuff happen.

Viewing code:

public double GetServoPosition(int servoIndex)
{
double position=-1;
try
{
AdvancedServoServo ass=null;
if(advServo.Attached==true)
{
ass=advServo.Servos[servoIndex];
ass.Engaged=true;
position=ass.Position;
ass.Engaged=false;
}
}catch(Exception exc)
{
throw new Exception(className+" public double GetServoPosition( "+ servoIndex+" ) :: "+exc.Message);
}
return position;
}

Motion code:

///

/// move a servo to a new position
///

/// index of the servo to move /// new position, should be 0-100 /// bool if the servo is now in the correct position
public bool MoveServo(int ServoIndex, int newpos)
{
bool movedOk = false;
try
{
AdvancedServoServo curr = null;

double pos = 0.01 * newpos * (basicServoMax - basicServoMin) + basicServoMin;

if (pos > basicServoMax){pos = basicServoMax - 1;}
if (pos < basicServoMin){pos = basicServoMin + 1;} if (aServo0.Attached && ServoIndex<4) { curr = aServo0.servos[ServoIndex]; } if (aServo1.Attached && ServoIndex > 3)
{
curr = aServo0.servos[ServoIndex];
}

if (curr != null)
{
curr.Engaged = true;
if (Math.Abs((curr.Position - pos) / curr.Position) > 0.03)
{

curr.Position = pos;

//disengage the servo when we are not moving it
curr.Engaged = false;
}
}
}
catch (Exception exc)
{
if (LogEvent != null)
{
LogEvent(className + " public bool MoveServo( " + ServoIndex + " , " + newpos + ") :: " +
exc.Message + "\n", false);
}
else
{
throw new Exception(className + " public bool MoveServo( " + ServoIndex + " , " + newpos +
") :: " +
exc.Message + "\n");
}
}
return movedOk;
} //MoveServo

Servo reset code:

///

/// try to reset the servo, when it throws phidgets exceptions, set it to a safe midpoint
///

/// servo to reset void resetServo(AdvancedServoServo srvo)
{
try
{
srvo.Engaged = true;
double midpoint = (basicServoMax - basicServoMin) / 2 + basicServoMin;
srvo.Acceleration = 1000;
srvo.Position =midpoint;
srvo.Engaged = false;

}catch(Exception exc)
{
LogEvent(className + " void resetServo(AdvancedServoServo) :: "+exc.Message, false);
}
}

Another thing, that you have to make sure that you consider is that there are hard to document dead zones in each servo. They are sometimes inherent to the servo design, but each servo can have multiple in different places. I would make sure that if you hear this tell-tale clicking sound when the motor tries to move to the right place and then corrects ad nauseum... that you trap this in your code. My experience is that this burns up your motor pretty quickly. These are toy or better than toy motors. Better motors are available but are more expensive.

Ok, I guess it should not bother me

I have been researching the Android 2 as a robotics platform. Several people have weighed in to say that the only big advantage of Google's Android 2 is that it is essentially free to distribute. However, it has other overhead that has to be overcome. It has to be recompiled per processor, so there is more to sourcing cpus and instruments for it.

I did find in my rolodex that Toradex has a Limestone PDA kit that claims to boot Android 1.6. I have to keep looking. One of my mad scientist ideas is the multiple chips flying closely together to distribute computing projects into manageable chunks. I will keep looking for more. I am not an EE so it is important these solutions be mostly plug and play.

The only "robot" stuff, that I have found before are canned apps from canned boards. I guess I was thinking robot not rc kit. Yeah, I use a lot of rc stuff, but I am not looking to directly control the devices at every turn. They should have increasing levels of autonomy. However, I do have to say that I like that some companies have the same idea that I do. The kit I have found that has the most coverage is the Surveyor SRV-1. It looks interesting enough, but PIC sucks. ;) An interesting couple of "rc toys" on Android movies can be found at robots.net.

Phidgets UltraSonic RangeFinder

The rangefinder is not a Phidgets. They are reselling a LV Max Sonar EZ1 from MaxBotix. You get in the bag the sensor (transceiver) and a small wire bundle. The wires need to be soldered into the proper pins. Just for reference from the pin closest to the mounting hole,

black
red
open
green
white
open
open

wire meanings

black - ground
red +5V
green signal to trip the ultrasonic ping, +5V I think, but I do not use it
white 5V signal

This will cable your phidgets three wire signal cable correctly. Remember this is not a normal sensor, it is one of their analog sensors. So you will need one of their a/d boards or interface boards in their terminology. The event that you will want to hook, the SensorChange event. Everything the system reads a new voltage on the port it fires sensor change. See my post on the 888 interface kit to see how to set up the interface kit.

Here is my way of switching the data so you only need one method for any port change.

protected void SensorChange(object Sender, SensorChangeEventArgs Args)
{
try
{
InstrumentType _type = InstrumentType.notset;
int _index = 0;

switch (Args.Index)
{
case 7:
_type = InstrumentType.ultrasonicRangeFinder;
_index = 4;
break;
case 6:
_type = InstrumentType.ultrasonicRangeFinder;
_index = 3;
break;
case 5:
_type = InstrumentType.ultrasonicRangeFinder;
_index = 2;
break;
case 4:
_type = InstrumentType.ultrasonicRangeFinder;
_index = 1;
break;
case 3:
_type = InstrumentType.ultrasonicRangeFinder;
_index = 0;
break;
case 2:
_type = InstrumentType.pressureTotal;
totalPressure = convertVoltageToUnit(_type, Args.Value, _index);
speed = getPressureSpeed(totalPressure, staticPressure, staticTemperature);
break;
case 1:
_type = InstrumentType.temperature;
staticTemperature = convertVoltageToUnit(_type, Args.Value, _index);
speed = getPressureSpeed(totalPressure, staticPressure, staticTemperature);
break;
case 0:
_type = InstrumentType.pressureStatic;
staticPressure = convertVoltageToUnit(_type, Args.Value, _index);
speed = getPressureSpeed(totalPressure, staticPressure, staticTemperature);
pressureAltitude = getGeoPotentialAltitude(staticPressure / units.kPaToPsf);
break;
}
if (_type == InstrumentType.ultrasonicRangeFinder)
{
rangeFinders[_index] = convertVoltageToUnit(InstrumentType.ultrasonicRangeFinder, Args.Value, _index);
}

}
catch (Exception _exc)
{

throw new Exception(className + " protected void SensorChange( Sender, Args) :: " + _exc.Message + "\n");

}
}

There are some other details in this code. Since I use a bunch of helper methods to calibrate the data coming out. Remember the values (Args.Value) come out as a double so you can write a simple linear interpolation for you that will get your data back into usable units from Voltage. For simplicity's sake, I have the interpolation points in an xml config file that is read in at start time to handle each of the instruments. That way the values can easily be changed for recalibration.

The code I have presented is good simple code that is quick and should work for almost any purpose and uses solid error trapping techniques.

Do I want to see both sides of your pretty face?

This week I am still waiting for motors. It turns out my motor supplier has been on vacation this week, how dare he? Sorry Dave I just needed to get in the jab. So I had this new idea for how to progress, I was thinking about something someone said to me. They were explaining how someone did not like to fly their great looking soarer because of how much it cost to replace.

Then I sat there looking at the School Girl UAS on the table and thought. ZOMG! It costs like 2-3x as much as the guy's soarer. What the hell am I thinking? I have to make sure that this thing can be showable to other people. It does have to fly, to demonstrate that those Engineering school was not a waste of time. I just want to make sure I do not throw two months of work and a thousand dollars into the ground due to bad luck and clean living. All of my best ideas are inspiration this time, I swear there was no barley soda involved. Ok, maybe some, but it was good stuff. The decision was made to make a new version of the truck so that I could work more on my version of the IMU and control system as well as make a safer platform for developing technologies for the UAS.

Long story short, a new beastie was conceived. Like the sign said, always well conceived, unlike people. Now this new ground guy lets me do lots of experiments with much less of the z-axis problem. The z-axis problem is acceleration due to gravity. Something that drives along the ground can still have great mishaps but it is easier to teach a car to drift a corner than it is to teach a plane to avoid turbulence.

So one of the first things about this idea is how to navigate the buggy. Actually, this is less of an issue than it may seem and allows us to do some much more interesting things. Monitoring the instruments and planning a heading to a goal is mostly done with the code that I have. What does this have to do with my pretty face? Navigation is not as interesting as deciding how to see. Many times navigation is more than careening off in the direction that leads you to the goal the fastest. What if there is a reason to follow a path? How do you find the path?

I sat back and ran through a series of thought experiments on how to execute this. The following questions came to me that needed an answer:

1. If there is a reason to follow a path, then how do I find the path?
2. If I find a path how often do I have to look for the path?
3. Maybe I want the video stream to get back to an operator and still be able to work on the data, how do I do it?
4. How much processing is really necessary to find the important elements of a scene?
5. An element of a scene is important, how do I decide if it is important for driving or for identifying to an operator?
6. This is a lot of processing, how can I reduce it to use lower-powered systems to reduce processing costs?

There is a path and I want to follow it, and still go in my course direction. Let's assume that the path is in the general direction that I want to go and that it is relatively constant in inclination. Basic range finding can keep us from running into objects in the immediate area, and our navigator will have a limited number of states. The point of this is to limit the amount of storage needed for reprocessing what happened in the case of an accident. If the path changes constantly, then basic range finding can keep things going and it reduces the need for stereoscopic depth perception.

As a matter of fact, the machine itself cannot make a lot of use of stereoscopic vision. Not that it is not useful, but that is an operator technology not a machine technology. The machine can use the stereoscopic vision to detect objects that may cause negative performance impacts . A really shiny thing in the way could "flash" and make a very small thing seem very big and that would possibly confuse the system, or a shadow may seem like a hole and cause unnecessary course correction. Coordinating objects in the two-camera system will allow false behaviors to be avoided or at least identified. It could also be used to do simple comparisons... is this blob like that one? and then reduce useful processing by discarding one of the copies.

If we can process the scene in such a way that we can slow the effective frame rate down by not checking for the path as often it is one less object in the scene to be processed. Optical flow algorithms will help with this because you can quickly get velocity estimates for a given object relative to your position. As that changes you can say... same element do not reprocess it. If we identify the objects in the scene and discard the ones that are clearly not as important then we can process many fewer regions of the scene and or pass the processing back to an operator.

This is going to be a multi-part post about basic vision systems and how I think it should be brought together and used in many applications without changing the code. How a few observations about what we want to do and what it really means may make a lot of decisions easier in the design. This code will focus on commodity components and instrumentation and control electronics from Phidgets and SparkFun.

Before we get to actual code...

Before we hop off into any actual implementation, I would suggest that we sit and think about some basics. The very first thing that should be done is to come up with a basic skeleton of the code that you would like to write. This may impact many decisions down the line. Basically, a UGV and a UAS/UAV are the same from the code's perspective. The UAV/UAS use different actual algorithms for certain processes, but the data flow and data collected are not so different. Some data is read, a decision is made, a servo solution is calculated, servos are moved into position and then the system loops again.

One thing that I did, was to develop an object that was a sensor, ISensor. This will be the part that actually interact with any device. ISensorReading will also act as a fundamental contract for data. I would make sure that this object follows in general an observable pattern. That way it is easy to allow other objects to read its data, or to be notified when new data is available. This will make it easy for filtering algorithms to execute and work on the newest data as soon as it arrives. It will also make it easier to make the data collection to be asynchronously collected and processed.

Then I made an instrumentation manager, IM. The IM will have factories for each kind of sensor and observe instances of each of the instruments. This way if something happens the IM can close an instrument and try to reconnect with a new object. In turn, the IM presents data to other parts of the code as needed. No other parts of the code need to see the sensors or interact with them directly.

On top of the instrumentation system, should be a director layer. This will actually be the layer that does all of the work. It makes a nice break line between the device and the presentation, or intelligence. Separating the systems is an important object-oriented technique that is necessary for keeping the code healthy.

More about the director later... I will see if I can get some pictures in to make it easier to see.

Basic Quantities and Some Trigonometry

So You Want to Build a DIY Autopilot

Accelerations

One of the most important quantities for you to measure are accelerations. If your two or more-axis accelerometer is mounted along the traditional axes of the aircraft it will be the easiest to code for. The three traditional axes are:

• From the nose through the center of gravity on the line of symmetry, the Y-axis, roll
• From the center of gravity out of the fuselage toward the tip of the right wing, the X-axis, pitch
• From the center of gravity away from the earth, the Z-axis, yaw
  
     

Figure of Rigid Aircraft Axes

So, now for some basic Trigonometry, everyone remembers SOH CAH TOA.

In general, the following picture is true for a vehicle moving through space.

Figure of the Direction of the Force of Gravitation on a Body

As you can see from the image, the angle of pitch relative to the surface of the earth is the same angle offset of the weight vector relative to the z-axis in the body frame of reference. If we put our accelerometer so that one of its axes is parallel to the body's z-axis at its center of gravity we are measuring this offset vector. Which is really neat, because it means that we can express the angle of the body in level, non-accelerating motion as ratios of the accelerations

Pitch: 

The angle theta between the actual gravity vector and the measured gravity is related to the pitch of the aircraft (pitch = theta + 90°). If we know theta, we know our pitch! Since we know the magnitude of the earth’s gravity, simple calculus gives us our pitch angle:

accelerometer = cos (theta) * gravity

theta = acos (accelerometer / gravity)

And since pitch = theta + 90°

pitch = asin (accelerometer / gravity)

Woot, we calculated the pitch orientation of our airplane using an accelerometer. Pretty easy, huh?

The real formula that we need to use for the software looks like this:

pitch = atan2(accelerometer / gravity, z / gravity)

Common piezo-electric accelerometers return in units of, g, 32.17 ft/s^2 or 9.81 m/s^2. We also know some more things about the flight that let us calculate the angles relative to the ground. More on this later, it is a bit more than basic trigonometry to describe. These equations assume non-accelerating flight. You can use a magnetometer to get the relative plane in space with less math, but magnetometers generally take more interface programming in my experience.

Roll:

Roll needs a second accelerometer with an axis perpendicular to the first so that we can figure out the resultant vector between them and then the angle. Essentially the vector between the accelerometers becomes the "gravitational" acceleration and the relative readings lets us calculate the angle with an atan2 function. The second accelerometer will have some other things to manage such as the effects of the distance between them on the accelerations measured. Physics fun and none of the boring class.

Yaw:

Yaw is the hardest of the angles to measure. The only answer is to use a magnetometer or a compass. In many ways, yaw can be solved by dead reckoning. Dead reckoning is all that is important for most of the projects in the DIY garage. They will be covered later.

Next we will discuss gyroscopes and the beauty of rates and integral calculus