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.