public IEnumerator<ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower) { // Requests come too fast, so dump ones that come in too fast. if (RequestPending > 0) { setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType()); yield break; } //flip direction if necessary double revPow = setMotorPower.Body.TargetPower; if (_state.ReversePolarity) revPow *= -1.0; //update state _state.CurrentPower = revPow; //convert to native units revPow *= 100; revPow += 100; int power = (int)Math.Round(revPow); //send hardware specific motor data brick.SetMotorBody motordata = new brick.SetMotorBody(); motordata.Motor = _state.Name; motordata.Speed = power; RequestPending++; Activate(Arbiter.Choice(_scribblerPort.SetMotor(motordata), delegate(DefaultUpdateResponseType status) { setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance); RequestPending--; }, delegate(soap.Fault failure) { setMotorPower.ResponsePort.Post(failure); RequestPending--; } )); yield break; }
public virtual IEnumerator<ITask> GetHandler(motor.Get get) { get.ResponsePort.Post(_state); yield break; }
public virtual IEnumerator<ITask> ReplaceHandler(motor.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }
void StopMotorWithEncoderHandler(Port<encoder.UpdateTickCount> encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort) { int stopWheelAt; bool ignore; switch (side) { case "left": stopWheelAt = stopLeftWheelAt; ignore = !_leftEncoderTickEnabled; break; default: case "right": stopWheelAt = stopRightWheelAt; ignore = !_rightEncoderTickEnabled; break; } if (!ignore) { #if TRACEDEBUGTICKS Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt); #endif // TRACEDEBUGTICKS if (update.Body.Count >= stopWheelAt) { switch (side) { case "left": _leftEncoderTickEnabled = false; break; default: case "right": _rightEncoderTickEnabled = false; break; } // whatever else got stuck there, we are not interested. Keep the port clear. //Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)]; //port.Clear(); motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 0 }); motorPort.Post(stop); Activate(Arbiter.Choice(stop.ResponsePort, delegate(DefaultUpdateResponseType resp) { #if TRACEDEBUGTICKS Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped by encoder !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); #endif // TRACEDEBUGTICKS }, delegate(Fault fault) { LogError(fault); } )); #if TRACEDEBUGTICKS Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending to completionPort: DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); #endif // TRACEDEBUGTICKS completionPort.Post(drive.DriveStage.Completed); } } }
public virtual IEnumerator<ITask> GenericSetMotorPowerHandler(pxmotor.SetMotorPower genericMotorPower) { if (!ValidateConnection(_state, genericMotorPower.ResponsePort)) yield break; SetMotorRotation setMotorRotation = new SetMotorRotation(new SetMotorRotationRequest(genericMotorPower.Body.TargetPower)); _internalMotorRotationPort.Post(setMotorRotation); yield return Arbiter.Choice(setMotorRotation.ResponsePort, delegate(DefaultUpdateResponseType response) { SendNotification<pxmotor.SetMotorPower>(_subMgrPort, genericMotorPower); genericMotorPower.ResponsePort.Post(response); }, delegate(Fault fault) { genericMotorPower.ResponsePort.Post(fault); }); yield break; }
public virtual IEnumerator<ITask> GenericMotorReplaceHandler(pxmotor.Replace replace) { _state.CopyFromGenericState(replace.Body); replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }
public virtual IEnumerator<ITask> GenericMotorGetHandler(pxmotor.Get get) { get.ResponsePort.Post(_state.ToGenericState(_genericState)); yield break; }
public virtual IEnumerator<ITask> SetMotorPowerHandler(pxmotor.SetMotorPower update) { if (_state.HardwareIdentifier == 0) { throw new InvalidOperationException("TrackRoamer Motor configuration missing"); } powerbrick.MotorSpeed motorSpeed = new powerbrick.MotorSpeed(); // Default -1 which is ignored by the controller. motorSpeed.LeftSpeed = null; motorSpeed.RightSpeed = null; double power = update.Body.TargetPower * _state.PowerScalingFactor / 100.0d; // PowerScalingFactor is percentage, 0 to 100 if (power != 0.0d && Math.Abs(power) < MinimumMotorPower) { //string msg = string.Format("Warning: Motor {0} - requested power {1} less than minimum {2}", _state.HardwareIdentifier, power, MinimumMotorPower); //Tracer.Trace(msg); //LogWarning(msg); power = (power < 0) ? -MinimumMotorPower : MinimumMotorPower; } //Tracer.Trace(string.Format("Motor {0} - setting power to {1}", _state.HardwareIdentifier, power)); if (_state.HardwareIdentifier == 1) { motorSpeed.LeftSpeed = _state.ReversePolarity ? -power : power; } else if (_state.HardwareIdentifier == 2) { motorSpeed.RightSpeed = _state.ReversePolarity ? -power : power; } else { throw new ArgumentException("TrackRoamerMotorService : SetMotorPowerHandler() invalid HardwareIdentifier=" + _state.HardwareIdentifier); } _state.CurrentPower = power; coord.ActuatorCoordination coordination = update.GetHeader<coord.ActuatorCoordination>(); if (coordination == null) { _powerbrickPort.UpdateMotorSpeed(motorSpeed); } else { // TODO: Remove this extra code when we allow headers // to flow with a causality across services. // Pass on the coordination to the trackroamerbot powerbrick.UpdateMotorSpeed updateMotorSpeed = new powerbrick.UpdateMotorSpeed(motorSpeed); updateMotorSpeed.AddHeader(coordination); _powerbrickPort.Post(updateMotorSpeed); } update.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }