public IEnumerator <ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
_state.TimeStamp = DateTime.Now;
PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();
// Add a coordination header to our motor requests
// so that advanced motor implementations can
// coordinate the individual motor reqests.
coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification <drive.Update>(_subMgrPort, update);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
if (successList.Count == 2)
{
setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
}
foreach (Fault fault in failureList)
{
setDrivePower.ResponsePort.Post(fault);
break;
}
}));
yield break;
}
void StopMotorWithEncoderHandler(encoder.EncoderOperations encoderNotificationPort, encoder.UpdateTickCount msg, Motor.MotorOperations motorPort, int stopWheelAt)
{
if (msg.Body.Count >= stopWheelAt)
{
Motor.SetMotorPower stop = new Motor.SetMotorPower(0);
motorPort.Post(stop);
Activate(Arbiter.Choice(stop.ResponsePort,
delegate(DefaultUpdateResponseType resp) { },
delegate(Fault fault) { LogError(fault); }
));
}
else
{
// Call self to continue waiting for notifications
Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, encoderNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(encoderNotificationPort, update, motorPort, stopWheelAt);
}
));
}
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator <ITask> DriveUntilDistance(double distance, double power)
{
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderPort.Post(Lreset);
yield return(Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderPort.Post(Rreset);
yield return(Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
//compute tick to stop at
int stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
int stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
// Subscribe to the encoders on internal ports
encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations();
encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = leftNotificationPort;
_leftEncoderPort.Post(op);
yield return(Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, leftNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
}));
},
delegate(Fault fault) { LogError(fault); }
));
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = rightNotificationPort;
_leftEncoderPort.Post(op2);
yield return(Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, rightNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
//start moving
double Pow;
if (distance > 0)
{
Pow = power;
}
else
{
Pow = -power;
}
PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();
// send notification of driveDistance start to subscription manager
_state.DriveDistanceStage = DriveStage.Started;
DriveDistance driveUpdate = new DriveDistance();
driveUpdate.Body.DriveDistanceStage = DriveStage.Started;
_internalDriveOperationsPort.Post(driveUpdate);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(Pow);
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(Pow);
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = DriveStage.Completed;
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = DriveStage.Completed;
}
void StopMotorWithEncoderHandler(encoder.EncoderOperations encoderNotificationPort, encoder.UpdateTickCount msg, Motor.MotorOperations motorPort, int stopWheelAt)
{
if (msg.Body.Count >= stopWheelAt)
{
Motor.SetMotorPower stop = new Motor.SetMotorPower(0);
motorPort.Post(stop);
Arbiter.Choice(stop.ResponsePort,
delegate(DefaultUpdateResponseType resp) { },
delegate(Fault fault) { LogError(fault); }
);
}
else
{
// Call self to continue waiting for notifications
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, encoderNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(encoderNotificationPort, update, motorPort, stopWheelAt);
}
));
}
}
/// <summary>
/// Rotate the the drive (positive degrees turn counterclockwise)
/// </summary>
/// <param name="degrees">(positive degrees turn counterclockwise)</param>
/// <param name="power">(-1.0 to 1.0)</param>
IEnumerator<ITask> RotateUntilDegrees(double degrees, double power)
{
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderPort.Post(Lreset);
yield return (Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderPort.Post(Rreset);
yield return (Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * 3.14159 / 360;
//compute tick to stop at
int stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
int stopRightWheelAt = (int)Math.Round(arcDistance / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
// Subscribe to the encoders on internal ports
encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations();
encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = leftNotificationPort;
_leftEncoderPort.Post(op);
yield return (Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, leftNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
}));
},
delegate(Fault fault) { LogError(fault); }
));
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = rightNotificationPort;
_leftEncoderPort.Post(op2);
yield return (Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, rightNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
//start moving
// start rotate operation
_state.RotateDegreesStage = DriveStage.Started;
RotateDegrees rotateUpdate = new RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = DriveStage.Started;
_internalDriveOperationsPort.Post(rotateUpdate);
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
double rightPow;
double leftPow;
if (degrees > 0)
{
rightPow = power;
leftPow = -power;
}
else
{
rightPow = -power;
leftPow = power;
}
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(leftPow);
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(rightPow);
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
_state.RotateDegreesStage = DriveStage.Completed;
//complete
rotateUpdate.Body.RotateDegreesStage = DriveStage.Completed;
_internalDriveOperationsPort.Post(rotateUpdate);
}
public IEnumerator<ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
_state.TimeStamp = DateTime.Now;
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
// Add a coordination header to our motor requests
// so that advanced motor implementations can
// coordinate the individual motor reqests.
coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification<drive.Update>(_subMgrPort, update);
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
if (successList.Count == 2)
setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
foreach (Fault fault in failureList)
{
setDrivePower.ResponsePort.Post(fault);
break;
}
}));
yield break;
}
