void TimerHandler(DateTime signal)
{
legoNXT.LegoGetBatteryLevel getBatt = new legoNXT.LegoGetBatteryLevel();
getBatt.RequireResponse = true;
Activate(Arbiter.Choice(_legoPort.SendLegoCommand(getBatt),
delegate(legoNXT.LegoResponse status)
{
legoNXT.LegoResponseGetBatteryLevel legoResponseGetBatteryLevel = status as legoNXT.LegoResponseGetBatteryLevel;
if (legoResponseGetBatteryLevel != null && legoResponseGetBatteryLevel.ErrorCode == legoNXT.LegoErrorCode.Success)
{
double voltage = legoResponseGetBatteryLevel.Voltage;
_state.PercentBatteryPower = voltage / 1000.0 / _state.MaxBatteryPower;
}
},
delegate(Fault failure)
{
LogError(failure);
}
));
//re-call ourselves
Activate(
Arbiter.Receive(false, TimeoutPort(_state.PollDelayTime),
delegate(DateTime time)
{
_timerPort.Post(time);
}
)
);
}
public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
{
// For now, just dump requests that come in too fast.
if (_requestPending > 0)
{
setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
//flip direction if nessisary
double revPow = setMotorPower.Body.TargetPower;
if (_state.ReversePolarity)
{
revPow = (-1) * revPow;
}
//update state
_state.CurrentPower = revPow;
//convert power
int power = (int)Math.Round(revPow * _state.PowerScalingFactor);
//send hardware specific motor data
legoNXT.LegoSetOutputState motordata = new legoNXT.LegoSetOutputState();
motordata.OutputPort = _state.HardwareIdentifier - 1;
motordata.PowerSetPoint = power;
motordata.Mode = legoNXT.LegoOutputMode.PowerControl;
motordata.RegulationMode = legoNXT.LegoRegulationMode.MotorSpeed;
motordata.TurnRatio = 0;
motordata.RunState = legoNXT.LegoRunState.Running;
motordata.RotationLimit = 0; //run forever
motordata.RequireResponse = false;
_requestPending++;
Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motordata),
delegate(legoNXT.LegoResponse status)
{
_requestPending--;
},
delegate(Fault failure)
{
_requestPending--;
}
));
//reply to sender
setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
IEnumerator <ITask> ToneHandler()
{
beeping = true;
legoNXT.LegoPlayTone tone = new legoNXT.LegoPlayTone();
tone.Frequency = _state.Frequency;
tone.Duration = _state.PlayDuration;
tone.RequireResponse = false;
_legoPort.SendLegoCommand(tone);
//wait play time + pause time
yield return(Arbiter.Receive(false, TimeoutPort(_state.PlayDuration + _state.PauseDuration),
delegate(DateTime t) { }));
beeping = false;
//_legoPort.Get(new GetRequestType());
yield return(Arbiter.Receive <legoNXT.LegoNxtState>(false, _legoPort.Get(new GetRequestType()),
delegate(legoNXT.LegoNxtState legoState)
{
SensorNotificationHandler(new legoNXT.Configure(legoState));
}
));
}
public virtual IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower update)
{
// For now, just dump requests that come in too fast.
if (_requestPending > 0)
{
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
//flip direction if necessary
double revPow = update.Body.LeftWheelPower;
if (_state.LeftWheel.MotorState.ReversePolarity)
{
revPow = (-1) * revPow;
}
//send left motor command
nxt.LegoSetOutputState motors = new nxt.LegoSetOutputState();
motors.OutputPort = _state.LeftWheel.MotorState.HardwareIdentifier - 1;
motors.PowerSetPoint = (int)(update.Body.LeftWheelPower * _state.LeftWheel.MotorState.PowerScalingFactor);
motors.Mode = nxt.LegoOutputMode.PowerControl;
motors.RegulationMode = nxt.LegoRegulationMode.MotorSpeed;
motors.TurnRatio = 0;
motors.RunState = nxt.LegoRunState.Running;
motors.RotationLimit = 0; //run forever
motors.RequireResponse = false;
_requestPending++;
PortSet <nxt.LegoResponse, Fault> resp1Port = _legoPort.SendLegoCommand(motors);
/* Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
* delegate(nxt.LegoResponse response)
* {
* _requestPending--;
* },
* delegate(Fault fault)
* {
* _requestPending--;
* }
* ));
*/
//send right motor command
motors = new nxt.LegoSetOutputState();
motors.OutputPort = _state.RightWheel.MotorState.HardwareIdentifier - 1;
motors.PowerSetPoint = (int)(update.Body.RightWheelPower * _state.RightWheel.MotorState.PowerScalingFactor);
motors.Mode = nxt.LegoOutputMode.PowerControl;
motors.RegulationMode = nxt.LegoRegulationMode.MotorSpeed;
motors.TurnRatio = 0;
motors.RunState = nxt.LegoRunState.Running;
motors.RotationLimit = 0; //run forever
motors.RequireResponse = false;
_requestPending++;
//Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
// delegate(nxt.LegoResponse response)
// {
// _requestPending--;
// },
// delegate(Fault fault)
// {
// _requestPending--;
// }
//));
yield return(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
delegate(nxt.LegoResponse response)
{
_requestPending--;
},
delegate(Fault fault)
{
_requestPending--;
}
));
yield return(Arbiter.Choice(resp1Port,
delegate(nxt.LegoResponse response)
{
_requestPending--;
},
delegate(Fault fault)
{
_requestPending--;
}
));
LogInfo(update.Body.LeftWheelPower * 100 + ":" + update.Body.RightWheelPower * 100);
_state.TimeStamp = DateTime.Now;
// forward notification
SendNotification(_subMgrPort, new drive.Update(_state));
//reply to sender
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}