protected List <NaoCommand> GetCommands(NaoState state)
{
var commands = new List <NaoCommand>();
if (state <= NaoState.Initialized)
{
commands.Add(NaoCommand.WalkToCheckpoint);
}
if (state <= NaoState.AtCheckpoint)
{
commands.Add(NaoCommand.GoToGrabLocation);
}
if (state <= NaoState.AtGrabLocation)
{
commands.Add(NaoCommand.GoToLiftPosition);
}
if (state <= NaoState.InLiftPosition)
{
commands.Add(NaoCommand.LiftObject);
}
if (state <= NaoState.InWalkPosition)
{
commands.Add(NaoCommand.WalkWithObject);
}
return(commands);
}
void _stateMachine_TransitionCompleted(object sender, TransitionCompletedEventArgs <NaoState, NaoCommand> e)
{
this._logger.InfoFormat("Transition completed. Current state: {0}, new state: {1}, command: {2}.", e.StateId.ToString(), e.NewStateId.ToString(), e.EventId.ToString());
if (!this._forcedStop)
{
this.CurrentState = e.NewStateId;
}
}
/// <summary>
/// Constructor.
/// </summary>
public Pose()
{
NaoState.Instance.OnConnect += BuildProxies;
NaoState.Instance.OnDisconnect += ResetProxies;
NaoState state = NaoState.Instance;
if (state.Connected)
{
BuildProxies(state.IP.ToString(), state.Port);
}
}
public void Initialize()
{
this.CurrentState = NaoState.Initialized;
this._transitions = new Dictionary <NaoStateTransition, NaoState>
{
{ new NaoStateTransition(NaoState.Initialized, NaoCommand.WalkToCheckpoint), NaoState.AtCheckpoint },
{ new NaoStateTransition(NaoState.Initialized, NaoCommand.Stop), NaoState.Initialized },
{ new NaoStateTransition(NaoState.AtCheckpoint, NaoCommand.GoToGrabLocation), NaoState.AtGrabLocation },
{ new NaoStateTransition(NaoState.AtGrabLocation, NaoCommand.GoToLiftPosition), NaoState.InLiftPosition },
{ new NaoStateTransition(NaoState.InLiftPosition, NaoCommand.LiftObject), NaoState.InWalkPosition },
{ new NaoStateTransition(NaoState.InWalkPosition, NaoCommand.WalkWithObject), NaoState.Terminated },
{ new NaoStateTransition(NaoState.InWalkPosition, NaoCommand.Stop), NaoState.InWalkPosition }
};
}
/// <summary>
/// Gets the commands to be executed based on the specified state
/// </summary>
/// <param name="state"></param>
/// <returns></returns>
protected List <NaoCommand> GetCommands(NaoState state)
{
var commands = new List <NaoCommand>();
var index = StateCommands.IndexOf(state);
if (index != -1)
{
for (int i = index; i < StateCommands.Count; i++)
{
commands.AddRange(StateCommands[i].Keys);
}
}
return(commands);
}
public NaoStateTransition(NaoState currentState, NaoCommand command)
{
this._currentState = currentState;
this._command = command;
}
/// <summary>
/// Starts the robot at the specified state
/// </summary>
/// <param name="startState"></param>
public void Start(NaoState startState)
{
this.CurrentState = startState;
this.Resume();
}
/// <summary>
/// Stops the robot
/// </summary>
public void Stop()
{
this.Pause();
this.CurrentState = NaoState.Terminated;
}
protected void ChangeState(NaoState newState)
{
}
public NaoState MoveNext(NaoCommand command)
{
this.CurrentState = GetNext(command);
return(CurrentState);
}
