private void ConfigureInitialState()
{
//configure initial state
if (_state == null)
{
_state = new DriveState();
_state.WheelRadius = 0.052;
_state.DistanceBetweenWheels = 0.165;
_state.LeftMotorPortNumber = 1;
_state.RightMotorPortNumber = 3;
_state.LeftEncoderPortNumber = 4;
_state.RightEncoderPortNumber = 5;
_state.ForwardSpeed = 1.0;
_state.ReverseSpeed = 0.8;
_state.RotateSpeed = 0.5;
}
}
public virtual IEnumerator <ITask> ReplaceHandler(Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
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;
}