/// <summary>
/// Internal rotate degrees handler
/// </summary>
/// <param name="rotateDegrees"></param>
/// <returns></returns>
public virtual IEnumerator <ITask> InternalRotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
switch (rotateDegrees.Body.RotateDegreesStage)
{
case drive.DriveStage.InitialRequest:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees;
SpawnIterator <double, double>(rotateDegrees.Body.Degrees, rotateDegrees.Body.Power, RotateUntilDegrees);
break;
case drive.DriveStage.Started:
SendNotification <drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Completed:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification <drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Canceled:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification <drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
}
yield break;
}
public void DriveRotateHandler(diffdrive.RotateDegrees rotate)
{
if (this.driveEntity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!this.state.DriveState.IsEnabled)
{
rotate.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("RotateDegrees request to disabled drive.");
return;
}
this.state.DriveState.RotateDegreesStage = rotate.Body.RotateDegreesStage;
if (rotate.Body.RotateDegreesStage == diffdrive.DriveStage.InitialRequest)
{
var entityResponse = new Port <engine.OperationResult>();
Activate(
Arbiter.Receive(
false,
entityResponse,
result =>
{
// post a message to ourselves indicating that the drive distance has completed
var req = new diffdrive.RotateDegreesRequest(0, 0);
switch (result)
{
case engine.OperationResult.Error:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case engine.OperationResult.Canceled:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case engine.OperationResult.Completed:
req.RotateDegreesStage = diffdrive.DriveStage.Completed;
break;
}
this.drivePort.Post(new diffdrive.RotateDegrees(req));
}));
this.driveEntity.RotateDegrees((float)rotate.Body.Degrees, (float)rotate.Body.Power, entityResponse);
var req2 = new diffdrive.RotateDegreesRequest(0, 0)
{
RotateDegreesStage = diffdrive.DriveStage.Started
};
this.drivePort.Post(new diffdrive.RotateDegrees(req2));
}
else
{
SendNotification(this.subMgrPort, rotate);
}
rotate.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
public void DriveRotateDegreesHandler(drive.RotateDegrees rotatedegrees)
{
this.controllerDrivePort.Post(
new drive.RotateDegrees()
{
Body = rotatedegrees.Body, ResponsePort = rotatedegrees.ResponsePort
});
}
public IEnumerator <ITask> DriveRotateDegreesHandler(drive.RotateDegrees rotatedegrees)
{
if (!this.state.DriveState.IsEnabled)
{
rotatedegrees.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Drive not enabled!"));
yield break;
}
this.state.DriveState.TimeStamp = DateTime.Now;
short pwr = (short)(board.HBridgeReverseMax +
((rotatedegrees.Body.Power - GDDriveMin)
* DrivePowerScale));
short deg = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * rotatedegrees.Body.Degrees);
byte[] cmdpacket = board.CreatePacket <short, short>(board.SetRotateInPlaceString, deg, pwr);
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Data = new serialcomservice.Packet(cmdpacket);
var resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
soap.Fault f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
rotatedegrees.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Error received from FW!");
rotatedegrees.ResponsePort.Post(f);
yield break;
}
this.state.DriveState.LeftWheel.MotorState.CurrentPower = rotatedegrees.Body.Power;
this.state.DriveState.RightWheel.MotorState.CurrentPower = rotatedegrees.Body.Power;
rotatedegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
public IEnumerator <ITask> RotateDegreesHandler(RotateDegrees rotateDegrees)
{
ValidateDriveConfiguration(true);
if (_state.DistanceBetweenWheels <= 0)
{
rotateDegrees.ResponsePort.Post(new Fault());
throw new InvalidOperationException("The wheel encoders are not properly configured");
}
else
{
_state.TimeStamp = DateTime.Now;
// send immediate response
rotateDegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send immediate response
rotateDegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// post request to internal port.
_internalDriveOperationsPort.Post(rotateDegrees);
}
yield break;
}
/// <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> RotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
LogError("Rotate degrees is not implemented");
yield break;
}
