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