/// <summary>
/// Start the timer
/// </summary>
private void StartTimer(int tickInterval)
{
// if no baseline state has been established, do it now
if (_hardwareState == null)
{
_hardwareState = (irobottypes.RoombaState)_state.Clone();
}
if (_tickInterval != tickInterval)
{
_tickInterval = tickInterval;
}
if (_nextTimer == DateTime.MaxValue)
{
_nextTimer = DateTime.Now.AddMilliseconds(_tickInterval);
Activate(Arbiter.Receive(false, TimeoutPort(_tickInterval), TimerHandler));
}
else
{
// skip any backlog of timer events
_nextTimer = DateTime.Now.AddMilliseconds(_tickInterval);
// Don't need to activate because there is still a pending receive.
}
}
private void UpdateHardwareStateFromEntity(irobottypes.RoombaState _hardwareState)
{
DateTime now = DateTime.Now;
_hardwareState.Pose.ButtonsCreate = 0;
if (_entity.PlayButtonPressed)
{
_hardwareState.Pose.ButtonsCreate |= create.ButtonsCreate.Play;
}
if (_entity.AdvanceButtonPressed)
{
_hardwareState.Pose.ButtonsCreate |= create.ButtonsCreate.Advance;
}
// Update the sensor timestamps
_hardwareState.LastUpdated = now;
_hardwareState.CliffDetail.Timestamp = now;
_hardwareState.Pose.Timestamp = now;
_hardwareState.Power.Timestamp = now;
_hardwareState.Sensors.Timestamp = now;
_hardwareState.Telemetry.Timestamp = now;
_hardwareState.CliffDetail.CliffLeftSignal = _entity.CliffLeft;
_hardwareState.CliffDetail.CliffFrontLeftSignal = _entity.CliffFrontLeft;
_hardwareState.CliffDetail.CliffRightSignal = _entity.CliffRight;
_hardwareState.CliffDetail.CliffFrontRightSignal = _entity.CliffFrontRight;
if (_irEntity != null)
{
// convert distance value to something close to what the Create reports
float distance = _irEntity.Distance;
if (distance > 0.08f) // range is close to 8 cm
{
distance = 0.08f;
}
// distance now ranges from 0 to 0.08, make it range from 1 to 0;
distance = 1f - (distance / 0.08f);
// add a little noise to make the signal more realistic and to cause it to update more often
_hardwareState.CliffDetail.WallSignal = (int)(distance * 200) + (int)(now.Ticks) % 2;
}
// bumpers
_hardwareState.Sensors.BumpsWheeldrops = 0;
if (_entity.BumperLeft)
{
_hardwareState.Sensors.BumpsWheeldrops |= BumpsWheeldrops.BumpLeft;
}
if (_entity.BumperRight)
{
_hardwareState.Sensors.BumpsWheeldrops |= BumpsWheeldrops.BumpRight;
}
}
/// <summary>
/// The Timer Handler fires every _tickInterval milliseconds
/// </summary>
/// <param name="time"></param>
private void TimerHandler(DateTime time)
{
// Stop the timer if we are shutting down
if (_state.Mode == irobottypes.RoombaMode.Shutdown)
{
return;
}
// ignore timer if the Create is not initialized.
if (_state.Mode == irobottypes.RoombaMode.Uninitialized)
{
WaitForNextTimer();
return;
}
// Is Polling turned off?
if (_state.PollingInterval < 0)
{
WaitForNextTimer();
return;
}
LogVerbose(LogGroups.Console, "Timer: Starting query");
bool dataChanged = false;
UpdateHardwareStateFromEntity(_hardwareState);
// check each block of state for differences
if (StateIsDifferent(_hardwareState.Pose, _state.Pose))
{
SendNotification <irobotupdates.UpdatePose>(_subMgrPort, _hardwareState.Pose);
dataChanged = true;
}
if (StateIsDifferent(_hardwareState.Sensors, _state.Sensors))
{
SendNotification <irobotupdates.UpdateBumpsCliffsAndWalls>(_subMgrPort, _hardwareState.Sensors);
dataChanged = true;
}
if (StateIsDifferent(_hardwareState.Power, _state.Power))
{
SendNotification <irobotupdates.UpdatePower>(_subMgrPort, _hardwareState.Power);
dataChanged = true;
}
if (StateIsDifferent(_hardwareState.CliffDetail, _state.CliffDetail))
{
SendNotification <irobotupdates.UpdateCliffDetail>(_subMgrPort, _hardwareState.CliffDetail);
dataChanged = true;
}
if (dataChanged)
{
_state = _hardwareState;
_hardwareState = (irobottypes.RoombaState)_state.Clone();
}
else
{
_state.LastUpdated = _hardwareState.LastUpdated;
_state.Pose.Timestamp = _hardwareState.Pose.Timestamp;
_state.Power.Timestamp = _hardwareState.Power.Timestamp;
_state.Sensors.Timestamp = _hardwareState.Sensors.Timestamp;
_state.CliffDetail.Timestamp = _hardwareState.CliffDetail.Timestamp;
_state.Telemetry.Timestamp = _hardwareState.Telemetry.Timestamp;
}
WaitForNextTimer();
}
public virtual IEnumerator <ITask> ConfigureHandler(irobottypes.Configure configure)
{
_state = configure.Body;
configure.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
private void ValidateState()
{
if (_state == null)
{
_state = new irobottypes.RoombaState();
}
// Default connection is wired RS232
_state.ConnectionType = irobottypes.iRobotConnectionType.RoombaSerialPort;
_state.IRobotModel = irobottypes.IRobotModel.Create;
if (_state.CliffDetail == null)
{
_state.CliffDetail = new create.ReturnCliffDetail();
}
if (_state.Telemetry == null)
{
_state.Telemetry = new create.ReturnTelemetry();
_state.Telemetry.OIMode = _state.Mode;
}
if (_state.CreateNotifications == null)
{
_state.CreateNotifications = new List <create.CreateSensorPacket>();
_state.CreateNotifications.Add(create.CreateSensorPacket.OIMode);
_state.CreateNotifications.Add(create.CreateSensorPacket.AllBumpsCliffsAndWalls);
_state.CreateNotifications.Add(create.CreateSensorPacket.Buttons);
}
if (_state.Pose == null)
{
_state.Pose = new irobottypes.ReturnPose();
}
if (_state.Power == null)
{
_state.Power = new irobottypes.ReturnPower();
}
if (_state.Sensors == null)
{
_state.Sensors = new irobottypes.ReturnSensors();
}
// Give the robot a name.
if (string.IsNullOrEmpty(_state.Name))
{
_state.Name = "Hiro";
}
if (_state.SongDefinitions == null)
{
_state.SongDefinitions = new List <irobottypes.CmdDefineSong>();
}
bool song1Defined = false;
bool song2Defined = false;
if (_state.SongDefinitions == null)
{
_state.SongDefinitions = new List <irobottypes.CmdDefineSong>();
}
else
{
foreach (irobottypes.CmdDefineSong cmdDefineSong in _state.SongDefinitions)
{
if (cmdDefineSong.SongNumber == 1)
{
song1Defined = true;
}
if (cmdDefineSong.SongNumber == 2)
{
song2Defined = true;
}
}
}
irobottypes.CmdDefineSong song;
if (!song1Defined)
{
song = DefineAscendingSong(1);
_state.SongDefinitions.Add(song);
}
if (!song2Defined)
{
song = DefineDecendingSong(2);
_state.SongDefinitions.Add(song);
}
}
