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