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.

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.

Phidgets Accelerometers, the Magic of Three-Axes

The three-axis accelerometer is a piezo-electric accelerometer that is about 1" square. It is pretty good and have never seen any issues with drift or orientation issues. It measures each axis in units of g Do not forget to convert to your units, so a reading of 1.2 is actually an acceleration of 38.6 ft/s/s. I just make a little helper function to convert the readings when the phidgets_accelerationchanged method fires.

Hopefully, that will help clean up your code by reducing the risk of double converting units. Even NASA makes this mistake. One of the first things that I set up is a library of unit converstion factors. That way it is less risky if the user wishes to see the measurements in , mks or cgs and my software interanlly uses US Customary slug-ft-lbf. I do not do any conversions in the code. Just read the electrical signals from the transducers and convert them to real units, consistent with the system. Do not try and convert back and forth within the code, it will just be miserable to find.

You convert to any system that is different than your base unit system until you display the data. You can easily set a flag in the display object that shows the data to the user and multiply out the measurements at presentation time via the decoration pattern. To be honest, the Phidgets API is awesome. It makes short work of connecting and managing their instruments, so the code to start an interface kit is not so different from the accelerometer.

do
{
//System.Threading.Thread.Sleep(10);
if (acc0.Attached)
{

Console.WriteLine("_acc0 attached");
//accelerometer events
acc0.Attach += phidgets_Attach;
acc0.Detach += phidgets_Detach;
acc0.Error += phidgets_Error;
acc0.AccelerationChange += _acc0_AccelerationChange;
}
else
{
Console.WriteLine("retry : " + retry + " waiting for acc0 attach");

}
retry++;
} while (retry < 10 && !acc0.Attached); #region helperMethods #region Phidgets event handlers ///

/// handle the phidget device discovery events
///

/// /// protected void phidgets_Attach(object Sender, AttachEventArgs Args)
{
try
{
Console.WriteLine(Args.Device.Type + " attached.");
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Attach( Sender, Args) :: " + _exc.Message +
"\n");
}
} //phidgets_Attach
///

/// handle the phidget device discovery events
///

/// /// protected void phidgets_Detach(object Sender, DetachEventArgs Args)
{
try
{
Console.WriteLine(Args.Device.Type + " detached.");
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Detach( Sender, Args) :: " + _exc.Message +"\n");
}
} //phidgets_Detach
protected void phidgets_Error(object Sender, ErrorEventArgs Args)
{
try
{
Console.WriteLine("phidgets error : " + Args.Code + " " + Args.Description);
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Error( Sender, Args) :: " + _exc.Message +
"\n");
}
} //phidgets_Error
///

/// reads the acceleration from the Phidgets accelerometer
///

/// accelerometer object/// essentially an array of three doubles, one for each direction measuredprotected void _acc0_AccelerationChange(object Sender, AccelerationChangeEventArgs Args)
{
try
{
rawAcc[Args.Index] = Args.Acceleration;
}
catch (Exception _exc)
{
Console.WriteLine(
className + " protected void _acc0_AccelerationChange( Sender, Args) :: " + _exc.Message + "\n"
);
}
} //_acc0_AccelerationChange
#endregion

Another helper method that is constantly requested is converting from accelerations to roll and pitch. You can do the trigonometry yourself, but if gravitation is assumed to act in the -Z direction you can work out the basic orientation of the accelerating object. This can be fooled by large or quick orientation changes, but for the most part sampling frequency can fix this. So I would make sure that you do as little as possible that may muddy the event handler system. They are really fast and that is a good thing in this case.

///

/// calculate the euler angles from the local accelerations
///

/// acceleration toward the right wing, g [gravity multiples]/// acceleration toward the nose, g [gravity multiples]/// acceleration toward the ground, g [gravity multiples]/// headingprivate static void accel2euler(double Ax, double Ay, double Az, double Compass, out double[] EulerAngles)
{
EulerAngles = new double[3];
try
{
double g = Math.Sqrt(Ax * Ax + Ay * Ay + Az * Az);
/* Roll */
if (g != 0)
{
//EulerAngles[0]=Math.Atan2(Ay,Az);
EulerAngles[0] = Math.Atan2(Ay/g, -Az/g);
}else
{
EulerAngles[0] = Math.Atan2(Ay / 1, -Az / 1);
}
/* Pitch */
if (g != 0)
{
//EulerAngles[1] = Math.Asin(Ax/-g);
EulerAngles[1] = Math.Atan2(Ax / g, -Az / g);
}
else
{
EulerAngles[1] = Math.Atan2(Ax / 1, -Az / 1);
}
EulerAngles[2] = Compass; /* Yaw */


}
catch (Exception _exc)
{
throw new Exception(className + " public static void accel2euler( , " + Ax.ToString("0.000") + " , " +
Ay.ToString("0.000") + " , " + Az.ToString("0.000") + " , " +
Compass.ToString("0.000") + " ) :: " + _exc.Message + "\n");
}
}

The one thing you will notice is that you cannot get the yaw from the accelerations. That makes sense if you think about it, flat rotation perpendicular to gravity would not be measured. I usually run a compass in the systems too. That makes the 3-1-3 rotation easy to move between body reference frames and global reference frames. I would suggest that you multiply out the cells for the rotations in a separate method each so that you can just multiply them by calling each method in turn with an argument of the last rotation.

How to Connect and Communicate With a Phidgets Interface Kit in C#

I have a bunch of there 8/8/8 kits. They are great. First, I would suggest getting their newest API package for your favorite platform.

I use visual studio, so I will have biased examples. I could regurgitate their examples, but mine hopefully don't suck.

From a simple command line application. You may get into some static thread issues, but they are generally avoidable.

in your main()...

try
{
InterfaceKit ik1 = new InterfaceKit();
ik1.open();
}
catch (Exception _exc)
{
Console.WriteLine("interface kit error : " + _exc.Message + "\n",);
}

This is pretty simple, and just opens the IK for management. You have to set up some events. In a constructor method, I dump the following code after you execute the Open() method.

do
{

if (ik1.Attached)
{

Console.WriteLine("ik1 attached.");

//interface kit events
ik1.Attach += phidgets_Attach;
ik1.Detach += phidgets_Detach;
ik1.Error += phidgets_Error;
ik1.SensorChange += _ik1_SensorChange;
ik1.OutputChange += _ik1_OutputChange;
ik1.InputChange += _ik1_InputChange;
}
else
{
Console.WriteLine(("retry : " + retry + " waiting for ik1 attach");
}
retry++;
} while (retry < 10 && !ik1.Attached);

I like these do loops for the initialization setups. Why, because it retries and does not fail just because. It says which instrument is having an issue before it proceeds. I use the phidgets_xxx methods to have a standard system for handling the major Phidgets methods .

#region Phidgets event handlers
///

/// handle the phidget device discovery events
///

/// /// protected void phidgets_Attach(object Sender, AttachEventArgs Args)
{
try
{
Console.WriteLine(Args.Device.Type + " attached.");
}
catch (Exception _exc)
{
throw new Exception(className +
" protected void phidgets_Attach( Sender, Args) :: " + _exc.Message +"\n");
}
} //phidgets_Attach
///

/// handle the phidget device discovery events
///

/// /// protected void phidgets_Detach(object Sender, DetachEventArgs Args)
{
try
{
Console.WriteLine(Args.Device.Type + " detached.");
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Detach( Sender, Args) :: " + _exc.Message +"\n");
}
} //phidgets_Detach
protected void phidgets_Error(object Sender, ErrorEventArgs Args)
{
try
{
Console.WriteLine("phidgets error : " + Args.Code + " " + Args.Description);
}
catch (Exception _exc)
{
throw new Exception(className +
" protected void phidgets_Error( Sender, Args) :: " + _exc.Message +"\n");
}
} //phidgets_Error

Then some simple methods to catch changes per port. I connect different instruments to each port. I have to calibrate the readings for each port so that the readings make sense directly from the Read() methods.

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

switch (Args.Index)
{
case 7:
_type = InstrumentType.notset;
_index = 4;
break;
case 6:
_type = InstrumentType.notset;
_index = 3;
break;
case 5:
_type = InstrumentType.notset;
_index = 2;
break;
case 4:
_type = InstrumentType.notset;
_index = 1;
break;
case 3:
_type = InstrumentType.notset;
_index = 0;
break;
case 2:
_type = InstrumentType.notset;
break;
case 1:
_type = InstrumentType.gyro;
gyro.SetRollData(
convertVoltageToUnit(InstrumentType.gyro, Args.Value, 1));
break;
case 0:
_type = InstrumentType.gyro;
gyro.SetPitchData(
convertVoltageToUnit(InstrumentType.gyro, Args.Value, 0));
break;
}
if (_type == InstrumentType.ultrasonicRangeFinder)
{
rangeFinders[_index] = convertVoltageToUnit(
InstrumentType.ultrasonicRangeFinder, Args.Value, _index);
}
Console.WriteLine(className +
" instrument change " + _type + " value: " + Args.Value);
}
catch (Exception _exc)
{
Console.WriteLine(className +
" protected void _ik1_SensorChange( Sender, Args) :: " + _exc.Message + "\n");

}
} //_ik0_SensorChange
protected void _ik1_InputChange(object Sender, InputChangeEventArgs Args)
{
try
{
}
catch (Exception _exc)
{
Console.WriteLine(className +
" protected void _ik1_InputChange( Sender, Args) :: " + _exc.Message + "\n");
}
} //_ik0_InputChange
protected void _ik1_OutputChange(object Sender, OutputChangeEventArgs Args)
{
try
{
}
catch (Exception _exc)
{
Console.WriteLine(className +
" protected void _ik1_OutputChange( Sender, Args) :: " +
_exc.Message + "\n");
}
} //_ik1_OutputChange

///

/// converts the read value, probably in volts to the correct units
/// this will also apply a 6:4 smooth, old value*0.6 + new value*0.4 to help
/// smooth out the readings
///

/// type of instrument read, to get the correct coeefficients/// reading of the instrument/// some instruments are indexed/// the calibrated value
protected double convertVoltageToUnit(InstrumentType Type, double Reading, int Index)
{
double _value;
try
{
int _InstrumentTypeIndex = 0;
double _old = 0;
switch (Type)
{
case InstrumentType.accelerometer:
_InstrumentTypeIndex = 2;
_old = rawAcc[Index];
break;
case InstrumentType.gyro:
_InstrumentTypeIndex = 3;
_old = gyration[Index];
break;
case InstrumentType.pressureStatic:
_InstrumentTypeIndex = 0;
break;
case InstrumentType.temperature:
_InstrumentTypeIndex = 1;
break;
case InstrumentType.ultrasonicRangeFinder:
_InstrumentTypeIndex = 4;
_old = rangeFinders[Index];
break;
case InstrumentType.servo:
_InstrumentTypeIndex = 5;
_old = servos[Index];
break;
case InstrumentType.notset:
break;
}
double A = double.Parse(
AppSettings["instrumentCalibration" + _InstrumentTypeIndex + "A"]);
double B = double.Parse(
AppSettings["instrumentCalibration" + _InstrumentTypeIndex + "B"]);
double C = double.Parse(
AppSettings["instrumentCalibration" + _InstrumentTypeIndex + "C"]);
//apply basic filter to smooth the data
// value = Ax^2+Bx+C
if (_old != 0)
{
_value = 0.6*_old + 0.4*(A*Reading*Reading + B*Reading + C);
}
else
{
_value = A*Reading*Reading + B*Reading + C;
}
}
catch (Exception _exc)
{
throw new Exception(className +
" protected double convertVoltageToUnit( " + Type.ToString("0.000") +
" , " + Reading.ToString("0.000") + " ) :: " + _exc.Message + "\n");
}
return _value;
} //convertVoltageToUnit

It is pretty simple. I then just use events to manage the reading of the ports or inputs. It is really easy at this point. I put the whole session into a System.Timers.Timer loop or a Console.ReadLine system to control the starting and stopping of the application.

A Stability Manager is Good to Have, But You Could Wing It

More about the loathe and planning of your own autopilot. I used a hopefully simple system to handle the stability manager. I set up a system that uses a simple interface for strategies to get to keep the plane in the air. From this strategy interface, I developed several strategies that specialized in different aspects of managing the airplane's performance.

The strategies then calculate a solution and then pass it to the task manager. The servo manager pops the solution off the task manager's stack and gives. From the solution the servo manager converts the new inputs to a servo position map. It converts the map to servo motion and executes the changes. The Phidgets controllers that we use allow for several settings to be set as it moves the servos into position.

It may seem a bit "math nerd", but the cool thing about the Phidgets controller is that you can set the final position, the speed and the magnitude of the acceleration used to make the changes. In general, I leave it to figure out the acceleration. However, in coordinated systems it may be important to set the jerk of the servo arms so that you do not lock up linkages or other mechanical interfaces to the servos.

My strategies are simple. I tried to decorate a base object with virtualized methods, that way if I missed or the method's implementation did not just come to me the methods would have something by way of implementation. This seems to be a good way to handle this kind of "how the heck do I do this" programming. I am sure there are more professional ways of doing this, but here is how I did it.

///

/// used in the strategy design pattern, so that you can use different strategies and change
/// them by delegate
///

public interface IStrategy
{
///

/// execute the strategy but do not return any results
///

void Execute();
///

/// exectute the strategy, return the result
///

/// result of the strategy
object Execute(bool bResults);
///

/// exectute the strategy, return the result and a object
///

/// return a second resultant object /// result of the strategy
object Execute(out object Result);

}

public interface IStrategyResult
{
object ShowResult();
}

Now these are interfaces that I used to make a series of strategies to balance the different flight aspects and how to keep the system on course. These are run through at about 20Hz and the results are compared at 10Hz. That way decisions can be made at 5Hz and you can keep the whole thing from bumping into the data collection and servo motion phases. It happens, but if the frequency is high enough, we can miss a solution or two and still make it in time to decide which solution was best.

A simple markup model then decides which result is most successful and passes it down the chain. My biggest troubles are always how to correctly smooth the position and orientation data. They are effected by several different coupling and acceleration balances. I have heard about these Kalman filters that everyone loves so much, but have yet to be successful implementing one myself from scratch. I usually keep a simple integrator system to keep the changes small(er) and tend to stay in constant speed-level flight if at all possible. That is better for overall system performance anyway. You have to pay the piper for every energy change.

Gyroscopes Are Fun!

Piezo-electric gyroscopes are fun. However, I certainly had to learn a lot of things to get a usable signal from them. Remember that  gyros report a rate of change not just an absolute value. This rate presents an interesting issue, that we have to integrate the signal at each step. You may want to use a more advanced curve fitting integration scheme than the one that I will present here.

Integration for those not in the know is just finding the area under a curve. The catch is that, you will never be given a curve by the gyroscope. Not having a curve means that we must discretize the signal, from these step-wise readings we can write a formula that will help us back out what the absolute angle is in that direction. However, since we are integrating these formulae will not take into account small changes that happen to the signal between samples.

The simplest form of integration is, just calculating a rectangle. Your software will have to remember the last reading from the gyroscope and the last calculated  position.

angle change =dAngle* step length

so the basic formula would be,

Angle i = Angle i-1+ angle change

A better technique would be simply approximating the changes in the curve as linear segments and then calculating the differences as triangles or trapezoids.

This is a relatively bad way to do this, but is easy to understand. I would suggest using a multi-order approach to try and take into account some of the variations in these signals. The nuts and bolts of it is, is that the equation only works if the change in angle is relatively small compared to the current angle. If you are executing maneuvers and sampling relatively slowly this formula will lead your software astray. I would use a gyroscope sampling rate that is at half of the gyroscope's sample acquisition rate. Halving the sampling rate should keep the software getting into too much trouble from landing between readings and getting garbage.

Remember to read your manufacturer's guidance on this. It will tell you how quickly it can measure changes. We use several of these:

http://www.sparkfun.com/commerce/product_info.php?products_id=9094

They are 500 deg/s sampling rate at 2mV/deg. That is usually too small for our analog to digital coverter boards (A/D boards), so we lower that to 150 deg/s and get a better signal rate of 9mV/deg. This means that we can get a nice clean signal that is several bits wide from our A/D board. Remember that your I/O system will determine a lot of things about your code.

If you sample to fast, you can have lock issues, too slow and you do not get a representative sample. I would suggest that you shoot around 200Hz for samples, and then apply your filtering to bring it down to a rate to allow your other software to make decision on the filtered values as if they were the just read values.

There are better boards available, but the ones from Phidgets.com have a great API for almost any platform. Their 8/8/8 boards are great all-around boards, but do not have the highest sampling precision. For most things, it is plenty.

I really like the .NET support and the usb interface. Even I could figure it out from their examples in only a few minutes. Next time, I will show you how easy it is to connect to the A/D board and begin acquiring signals. The code samples from Phidgets are effective and describe most of the general functions that you will need to implement a system with their instruments.