public IEnumerator <ITask> UpdateHandler(drive.Update update)
{
_state = update.Body;
_state.TimeStamp = DateTime.Now;
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
if (_state == null)
{
//add default state.
//NOTE: because this is a custom implementation of the drive service, we can hard code values
//also, because there are no encoders, we do not need any of the wheel measurements.
_state = new drive.DriveDifferentialTwoWheelState();
_state.IsEnabled = true;
_state.LeftWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.LeftWheel.Name = "Left Wheel";
_state.LeftWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
_state.LeftWheel.MotorState.Name = "Left Motor";
_state.RightWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.RightWheel.Name = "Right Wheel";
_state.RightWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
_state.RightWheel.MotorState.Name = "Right Motor";
}
// Listen on the main port for requests and call the appropriate handler.
ActivateDsspOperationHandlers();
// Publish the service to the local Node Directory
DirectoryInsert();
// display HTTP service Uri
LogInfo(LogGroups.Console, "Service uri: ");
}
public virtual IEnumerator <ITask> UpdateHandler(drive.Update update)
{
_state = update.Body;
SaveState(_state);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Set up the standard Tri-bot configuration
/// </summary>
void CreateDefaultState()
{
_state = new drive.DriveDifferentialTwoWheelState();
//initialize motor platform specific to Lego NXT
_state.DistanceBetweenWheels = 0.115;
_state.TimeStamp = DateTime.Now;
_state.IsEnabled = false;
_state.LeftWheel = new motor.WheeledMotorState();
_state.LeftWheel.Name = "Medium Wheel";
_state.LeftWheel.Radius = 0.028;
_state.LeftWheel.GearRatio = 1.0;
_state.LeftWheel.MotorState = new motor.MotorState("Left Motor", 3, 100.0, false, new PhysicalModel.Proxy.Pose());
_state.LeftWheel.EncoderState = new Encoder.Proxy.EncoderState(_state.LeftWheel.MotorState.HardwareIdentifier, 6);
_state.RightWheel = new motor.WheeledMotorState();
_state.RightWheel.Name = "Medium Wheel";
_state.RightWheel.Radius = 0.028;
_state.RightWheel.GearRatio = 1.0;
_state.RightWheel.MotorState = new motor.MotorState("Right Motor", 2, 100.0, true, new PhysicalModel.Proxy.Pose());
_state.RightWheel.EncoderState = new Encoder.Proxy.EncoderState(_state.RightWheel.MotorState.HardwareIdentifier, 6);
SaveState(_state);
}
/// <summary>
/// Updates the state of the wheels.
/// </summary>
/// <param name="state">The state.</param>
public void UpdateWheelState(drive.DriveDifferentialTwoWheelState state)
{
this.LeftWheelSpeed.Text = state.LeftWheel.WheelSpeed.ToString("0.00");
this.RightWheelSpeed.Text = state.RightWheel.WheelSpeed.ToString("0.00");
this.LeftEncoderTicks.Text = state.LeftWheel.EncoderState.CurrentReading.ToString();
this.RightEncoderTicks.Text = state.RightWheel.EncoderState.CurrentReading.ToString();
}
/// <summary>
/// Transform generic state to this instance.
/// </summary>
/// <param name="genericState"></param>
public void CopyFromGenericState(pxdrive.DriveDifferentialTwoWheelState genericState)
{
if (genericState.DistanceBetweenWheels > 0.0)
{
this.DistanceBetweenWheels = genericState.DistanceBetweenWheels;
}
this.TimeStamp = genericState.TimeStamp;
if (genericState.LeftWheel != null)
{
if (genericState.LeftWheel.Radius > 0.0)
{
this.LeftWheel.WheelDiameter = genericState.LeftWheel.Radius * 2.0;
}
if (genericState.LeftWheel.MotorState != null)
{
if (this.RuntimeStatistics == null)
{
this.RuntimeStatistics = new RuntimeStatistics();
}
this.RuntimeStatistics.LeftPowerCurrent = genericState.LeftWheel.MotorState.CurrentPower;
this.LeftWheel.ReversePolarity = genericState.LeftWheel.MotorState.ReversePolarity;
if (genericState.LeftWheel.MotorState.HardwareIdentifier > 0 && genericState.LeftWheel.MotorState.HardwareIdentifier <= 3)
{
if (genericState.LeftWheel.MotorState.HardwareIdentifier > 0 && genericState.LeftWheel.MotorState.HardwareIdentifier <= 3)
{
this.LeftWheel.MotorPort = NxtDrive.HardwareIdentifierToMotorPort(genericState.LeftWheel.MotorState.HardwareIdentifier, this.LeftWheel.MotorPort);
}
}
}
}
if (genericState.RightWheel != null)
{
if (genericState.RightWheel.Radius > 0.0)
{
this.RightWheel.WheelDiameter = genericState.RightWheel.Radius * 2.0;
}
if (genericState.RightWheel.MotorState != null)
{
if (this.RuntimeStatistics == null)
{
this.RuntimeStatistics = new RuntimeStatistics();
}
this.RuntimeStatistics.RightPowerCurrent = genericState.RightWheel.MotorState.CurrentPower;
this.RightWheel.ReversePolarity = genericState.RightWheel.MotorState.ReversePolarity;
if (genericState.RightWheel.MotorState.HardwareIdentifier > 0)
{
if (genericState.RightWheel.MotorState.HardwareIdentifier > 0 && genericState.RightWheel.MotorState.HardwareIdentifier <= 3)
{
this.RightWheel.MotorPort = NxtDrive.HardwareIdentifierToMotorPort(genericState.RightWheel.MotorState.HardwareIdentifier, this.RightWheel.MotorPort);
}
}
}
}
}
void CreateDefaultState()
{
_state = new diffdrive.DriveDifferentialTwoWheelState();
_state.LeftWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.RightWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.LeftWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
_state.RightWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
}
public void UpdateMotorData(drive.DriveDifferentialTwoWheelState data)
{
if (data.TimeStamp > _lastMotor)
{
_lastMotor = data.TimeStamp;
TimeSpan lag = DateTime.Now - data.TimeStamp;
lblMotor.Text = data.IsEnabled ? "On" : "Off";
lblLag.Text = string.Format("{0}.{1:D000}",
lag.TotalSeconds, lag.Milliseconds);
}
}
/// <summary>
/// Copy state to generic state.
/// </summary>
/// <param name="genericState"></param>
public pxdrive.DriveDifferentialTwoWheelState CopyToGenericState(pxdrive.DriveDifferentialTwoWheelState genericState)
{
genericState.DistanceBetweenWheels = this.DistanceBetweenWheels;
genericState.IsEnabled = this.LeftWheel.MotorPort != NxtMotorPort.NotConnected && this.RightWheel.MotorPort != NxtMotorPort.NotConnected;
genericState.TimeStamp = this.TimeStamp;
if (genericState.LeftWheel == null)
{
genericState.LeftWheel = new pxmotor.WheeledMotorState();
}
if (genericState.LeftWheel.MotorState == null)
{
genericState.LeftWheel.MotorState = new pxmotor.MotorState();
}
genericState.LeftWheel.MotorState.CurrentPower = this.RuntimeStatistics.LeftPowerCurrent;
genericState.LeftWheel.MotorState.HardwareIdentifier = NxtDrive.MotorPortToHardwareIdentifier(this.LeftWheel.MotorPort);
genericState.LeftWheel.MotorState.PowerScalingFactor = 100.0;
genericState.LeftWheel.MotorState.ReversePolarity = this.LeftWheel.ReversePolarity;
genericState.LeftWheel.Radius = this.LeftWheel.WheelDiameter / 2.0;
if (genericState.RightWheel == null)
{
genericState.RightWheel = new pxmotor.WheeledMotorState();
}
if (genericState.RightWheel.MotorState == null)
{
genericState.RightWheel.MotorState = new pxmotor.MotorState();
}
genericState.RightWheel.MotorState.CurrentPower = this.RuntimeStatistics.RightPowerCurrent;
genericState.RightWheel.MotorState.HardwareIdentifier = NxtDrive.MotorPortToHardwareIdentifier(this.RightWheel.MotorPort);
genericState.RightWheel.MotorState.PowerScalingFactor = 100.0;
genericState.RightWheel.MotorState.ReversePolarity = this.RightWheel.ReversePolarity;
genericState.RightWheel.Radius = this.RightWheel.WheelDiameter / 2.0;
return(genericState);
}
public DriveUpdate(drive.DriveDifferentialTwoWheelState body)
: base(body)
{
}
public void Set(drive.DriveDifferentialTwoWheelState state)
{
throw new NotSupportedException("Setting the entire state is not supported on a drive. Try the other forms of \"set\".");
}
void CreateDefaultState()
{
_state = new diffdrive.DriveDifferentialTwoWheelState();
_state.LeftWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.RightWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.LeftWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
_state.RightWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
}
public IEnumerator<ITask> UpdateHandler(drive.Update update)
{
_state = update.Body;
_state.TimeStamp = DateTime.Now;
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
private void InitState()
{
LogInfo("TrackRoamerDriveService:: InitState() _state=" + _state);
if (_state == null)
{
// no partner-supplied initial state found - create default one:
LogInfo("TrackRoamerDriveService:: InitState() (null) - creating one");
//_state = new TrackRoamerDriveState();
_state = new drive.DriveDifferentialTwoWheelState();
_state.DistanceBetweenWheels = TrackRoamerDriveParams.DistanceBetweenWheels;
_state.LeftWheel = new motor.WheeledMotorState();
_state.LeftWheel.Radius = TrackRoamerDriveParams.WheelRadius;
_state.LeftWheel.GearRatio = TrackRoamerDriveParams.WheelGearRatio;
_state.LeftWheel.MotorState = new motor.MotorState();
_state.LeftWheel.MotorState.HardwareIdentifier = 1;
_state.LeftWheel.MotorState.Name = "Left Motor";
_state.LeftWheel.MotorState.PowerScalingFactor = TrackRoamerDriveParams.MotorPowerScalingFactor;
_state.LeftWheel.MotorState.ReversePolarity = true;
_state.RightWheel = new motor.WheeledMotorState();
_state.RightWheel.Radius = TrackRoamerDriveParams.WheelRadius;
_state.RightWheel.GearRatio = TrackRoamerDriveParams.WheelGearRatio;
_state.RightWheel.MotorState = new motor.MotorState();
_state.RightWheel.MotorState.HardwareIdentifier = 2;
_state.RightWheel.MotorState.Name = "Right Motor";
_state.RightWheel.MotorState.PowerScalingFactor = TrackRoamerDriveParams.MotorPowerScalingFactor;
_state.RightWheel.MotorState.ReversePolarity = true;
_state.LeftWheel.EncoderState = new encoder.EncoderState();
_state.LeftWheel.EncoderState.HardwareIdentifier = 1;
_state.LeftWheel.EncoderState.TicksPerRevolution = TrackRoamerDriveParams.EncoderTicksPerWheelRevolution;
_state.RightWheel.EncoderState = new encoder.EncoderState();
_state.RightWheel.EncoderState.HardwareIdentifier = 2;
_state.RightWheel.EncoderState.TicksPerRevolution = TrackRoamerDriveParams.EncoderTicksPerWheelRevolution;
_state.IsEnabled = true;
_state.TimeStamp = DateTime.Now;
//_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; -- not available in drive.Proxy
LogInfo("TrackRoamerDriveService:: InitState(): saving state");
SaveState(_state);
}
else
{
LogInfo("TrackRoamerDriveService:: InitState() (not null) _state.DistanceBetweenWheels=" + _state.DistanceBetweenWheels);
// + " PowerScalingFactor=" + _state.LeftWheel.MotorState.PowerScalingFactor + "/" + _state.RightWheel.MotorState.PowerScalingFactor);
//_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; -- not available in drive.Proxy
_state.TimeStamp = DateTime.Now;
}
}
public IEnumerator <ITask> ConfigureDrive(DriveState state)
{
PartnerType leftMotorPartner = this.ServiceInfo.PartnerList.Find(delegate(PartnerType partner) { return(partner.Name.Name == "LeftMotor"); });
PartnerType rightMotorPartner = this.ServiceInfo.PartnerList.Find(delegate(PartnerType partner) { return(partner.Name.Name == "RightMotor"); });
// Configure the left motor connection
motorbase.MotorState Lconfigure = new motorbase.MotorState();
Lconfigure.HardwareIdentifier = _state.LeftMotorPortNumber;
Lconfigure.Name = "Left Motor";
Lconfigure.PowerScalingFactor = 1;
yield return(Arbiter.Choice(_leftMotorPort.Replace(Lconfigure),
delegate(DefaultReplaceResponseType success)
{
LogInfo("Left Motor Port set");
},
delegate(Fault fault)
{
LogError(fault);
_initFault = true;
}));
if (_initFault)
{
yield break;
}
// Configure the right motor connection
motorbase.MotorState Rconfigure = new motorbase.MotorState();
Rconfigure.HardwareIdentifier = _state.RightMotorPortNumber;
Rconfigure.Name = "Right Motor";
Rconfigure.PowerScalingFactor = 1;
yield return(Arbiter.Choice(_rightMotorPort.Replace(Rconfigure),
delegate(DefaultReplaceResponseType success)
{
LogInfo("Right Motor Port set");
},
delegate(Fault fault)
{
LogError(fault);
_initFault = true;
}));
if (_initFault)
{
yield break;
}
// Configure the drive service
drivesrv.DriveDifferentialTwoWheelState driveState = new drivesrv.DriveDifferentialTwoWheelState();
driveState.DistanceBetweenWheels = state.DistanceBetweenWheels;
driveState.TimeStamp = DateTime.Now;
driveState.LeftWheel = new motorbase.WheeledMotorState();
driveState.LeftWheel.Radius = _state.WheelRadius;
driveState.LeftWheel.Name = "Left Wheel";
driveState.LeftWheel.MotorState = new motorbase.MotorState();
driveState.LeftWheel.GearRatio = 1.0;
driveState.LeftWheel.EncoderState = new encoderbase.EncoderState();
driveState.RightWheel = new motorbase.WheeledMotorState();
driveState.RightWheel.Radius = _state.WheelRadius;
driveState.RightWheel.Name = "Right Wheel";
driveState.RightWheel.MotorState = new motorbase.MotorState();
driveState.RightWheel.GearRatio = 1.0;
driveState.RightWheel.EncoderState = new encoderbase.EncoderState();
driveState.IsEnabled = true;
yield return(Arbiter.Choice(_drivePort.Update(driveState),
delegate(DefaultUpdateResponseType success)
{
LogInfo("Drive configuration updated");
},
delegate(Fault f)
{
LogError(f);
_initFault = true;
}));
if (_initFault)
{
yield break;
}
drivesrv.EnableDriveRequest en = new drivesrv.EnableDriveRequest();
en.Enable = true;
yield return(Arbiter.Receive <DefaultUpdateResponseType>(false, _drivePort.EnableDrive(en),
delegate(DefaultUpdateResponseType response){}));
yield break;
}
