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;
}
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;
}
