public IEnumerator <ITask> ResetHandler(pxencoder.Reset update)
{
LogInfo("TrackRoamerEncoder : ResetHandler() m_lastResetTicks=" + m_lastResetTicks);
// reset physical counter in the bot AX2850 controller:
//trackroamerbot.ResetMotorEncoder resetMotorEncoder = new trackroamerbot.ResetMotorEncoder();
//resetMotorEncoder.Body.HardwareIdentifier = _state.HardwareIdentifier;
//_trackroamerbotPort.Post(resetMotorEncoder);
// initialize our local logical counter:
lock (m_lastResetTicksLock)
{
_state.TicksSinceReset = 0;
_state.CurrentReading = 0;
_state.CurrentAngle = 0.0d;
m_lastResetTicks = null;
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator <ITask> DriveUntilDistance(double distance, double power)
{
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, power=" + power + ")");
_leftEncoderTickEnabled = false;
_rightEncoderTickEnabled = false;
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderCmdPort.Post(Lreset);
yield return(Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderCmdPort.Post(Rreset);
yield return(Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
_leftEncoderTickEnabled = true;
_rightEncoderTickEnabled = true;
//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 = drive.DriveStage.Started;
drive.DriveDistance driveUpdate = new drive.DriveDistance();
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started;
_internalDriveOperationsPort.Post(driveUpdate);
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = Pow
});
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = Pow
});
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheel at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - waiting for the first side to complete...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - other side should complete too...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() - both sides completed, send notification of driveDistance complete to subscription manager");
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed;
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = drive.DriveStage.Completed;
}
/// <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)
{
LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")");
_leftEncoderTickEnabled = false;
_rightEncoderTickEnabled = false;
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderCmdPort.Post(Lreset);
yield return(Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderCmdPort.Post(Rreset);
yield return(Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d;
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
_leftEncoderTickEnabled = true;
_rightEncoderTickEnabled = true;
//start moving
// start rotate operation
_state.RotateDegreesStage = drive.DriveStage.Started;
drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = drive.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(new motor.SetMotorPowerRequest()
{
TargetPower = leftPow
});
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = rightPow
});
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - waiting for the first side to complete...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - other side should complete too...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");
// send notification of RotateDegrees complete to subscription manager
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
_internalDriveOperationsPort.Post(rotateUpdate);
_state.RotateDegreesStage = drive.DriveStage.Completed;
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator<ITask> DriveUntilDistance(double distance, double power)
{
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, power=" + power + ")");
#endif
EncoderTicksEnabled = false;
//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
stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
EncoderTicksEnabled = true;
pollEncoderState(); // get starting encoder state
//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 = drive.DriveStage.Started;
drive.DriveDistance driveUpdate = new drive.DriveDistance();
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Started");
#endif
_internalDriveOperationsPort.Post(driveUpdate);
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
#endif
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
#endif
yield return DriveWaitForCompletionDual();
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Completed");
#endif
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = drive.DriveStage.Completed;
}
public IEnumerator<ITask> ResetEncodersHandler(drive.ResetEncoders resetEncoders)
{
encoder.Reset rightResetRequest = null;
encoder.Reset leftResetRequest = null;
if (this._rightEncoderPort != null)
{
rightResetRequest = new encoder.Reset();
this._rightEncoderPort.Post(rightResetRequest);
}
if (this._leftEncoderPort != null)
{
leftResetRequest = new encoder.Reset();
this._leftEncoderPort.Post(leftResetRequest);
}
if (rightResetRequest != null)
{
yield return rightResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler);
}
if (leftResetRequest != null)
{
yield return leftResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler);
}
this.SendNotification<drive.ResetEncoders>(this._subMgrPort, resetEncoders.Body);
resetEncoders.ResponsePort.Post(new DefaultUpdateResponseType());
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)
{
#if TRACELOG
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")");
#endif
EncoderTicksEnabled = false;
//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 * Math.PI / 360.0d;
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
EncoderTicksEnabled = true;
pollEncoderState(); // get starting encoder state
//start moving
// start rotate operation
_state.RotateDegreesStage = drive.DriveStage.Started;
drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Started");
#endif
_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(new motor.SetMotorPowerRequest() { TargetPower = leftPow });
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = rightPow });
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
#endif
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
#endif
yield return DriveWaitForCompletionDual();
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");
#endif
// send notification of RotateDegrees complete to subscription manager
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Completed");
#endif
_internalDriveOperationsPort.Post(rotateUpdate);
_state.RotateDegreesStage = drive.DriveStage.Completed;
}
public IEnumerator <ITask> ResetHandler(encoder.Reset reset)
{
_state.TicksSinceReset = 0;
reset.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
