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