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.
