/// <summary>
/// Post Set Motor Power with body and return the response port.
/// </summary>
public virtual PortSet <DefaultUpdateResponseType, Fault> SetMotorRotation(pxmotor.SetMotorPowerRequest body)
{
pxmotor.SetMotorPower op = new pxmotor.SetMotorPower();
op.Body = body ?? new pxmotor.SetMotorPowerRequest();
this.PostUnknownType(op);
return(op.ResponsePort);
}
public virtual IEnumerator <ITask> SetMotorPowerHandler(pxmotor.SetMotorPower update)
{
if (_state.HardwareIdentifier == 0)
{
throw new InvalidOperationException("TrackRoamer Motor configuration missing");
}
powerbrick.MotorSpeed motorSpeed = new powerbrick.MotorSpeed();
// Default -1 which is ignored by the controller.
motorSpeed.LeftSpeed = null;
motorSpeed.RightSpeed = null;
double power = update.Body.TargetPower * _state.PowerScalingFactor / 100.0d; // PowerScalingFactor is percentage, 0 to 100
if (power != 0.0d && Math.Abs(power) < MinimumMotorPower)
{
//string msg = string.Format("Warning: Motor {0} - requested power {1} less than minimum {2}", _state.HardwareIdentifier, power, MinimumMotorPower);
//Tracer.Trace(msg);
//LogWarning(msg);
power = (power < 0) ? -MinimumMotorPower : MinimumMotorPower;
}
//Tracer.Trace(string.Format("Motor {0} - setting power to {1}", _state.HardwareIdentifier, power));
if (_state.HardwareIdentifier == 1)
{
motorSpeed.LeftSpeed = _state.ReversePolarity ? -power : power;
}
else if (_state.HardwareIdentifier == 2)
{
motorSpeed.RightSpeed = _state.ReversePolarity ? -power : power;
}
else
{
throw new ArgumentException("TrackRoamerMotorService : SetMotorPowerHandler() invalid HardwareIdentifier=" + _state.HardwareIdentifier);
}
_state.CurrentPower = power;
coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>();
if (coordination == null)
{
_powerbrickPort.UpdateMotorSpeed(motorSpeed);
}
else
{
// TODO: Remove this extra code when we allow headers
// to flow with a causality across services.
// Pass on the coordination to the trackroamerbot
powerbrick.UpdateMotorSpeed updateMotorSpeed = new powerbrick.UpdateMotorSpeed(motorSpeed);
updateMotorSpeed.AddHeader(coordination);
_powerbrickPort.Post(updateMotorSpeed);
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
void StopMotorWithEncoderHandler(encoder.EncoderOperations encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort)
{
//LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt);
int stopWheelAt;
bool ignore;
switch (side)
{
case "left":
stopWheelAt = stopLeftWheelAt;
ignore = !_leftEncoderTickEnabled;
break;
default:
case "right":
stopWheelAt = stopRightWheelAt;
ignore = !_rightEncoderTickEnabled;
break;
}
if (!ignore && update.Body.Count >= stopWheelAt)
{
switch (side)
{
case "left":
_leftEncoderTickEnabled = false;
break;
default:
case "right":
_rightEncoderTickEnabled = false;
break;
}
// whatever else got stuck there, we are not interested. Keep the port clear.
//Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)];
//port.Clear();
motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = 0
});
motorPort.Post(stop);
Arbiter.Choice(stop.ResponsePort,
delegate(DefaultUpdateResponseType resp) {
LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
},
delegate(Fault fault) { LogError(fault); }
);
LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending internal DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
completionPort.Post(drive.DriveStage.Completed);
}
}
public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
{
// Requests come too fast, so dump ones that come in too fast.
if (RequestPending > 0)
{
setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
//flip direction if necessary
double revPow = setMotorPower.Body.TargetPower;
if (_state.ReversePolarity)
{
revPow *= -1.0;
}
//update state
_state.CurrentPower = revPow;
//convert to native units
revPow *= 100;
revPow += 100;
int power = (int)Math.Round(revPow);
if (power == 0)
{
power = 1;
}
//send hardware specific motor data
brick.SetMotorData motordata = new brick.SetMotorData();
motordata.Motor = _state.Name;
motordata.Speed = power;
RequestPending++;
Activate(Arbiter.Choice(_scribblerPort.SetMotor(motordata),
delegate(DefaultUpdateResponseType status)
{
setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
},
delegate(soap.Fault failure)
{
setMotorPower.ResponsePort.Post(failure);
RequestPending--;
}
));
yield break;
}
public IEnumerator <ITask> SetDrivePowerHandler(drive.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();
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = setDrivePower.Body.LeftWheelPower
});
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = setDrivePower.Body.RightWheelPower
});
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
Update update = new Update(_state);
SendNotification <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;
}
public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
{
// For now, just dump requests that come in too fast.
if (_requestPending > 0)
{
setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
//flip direction if nessisary
double revPow = setMotorPower.Body.TargetPower;
if (_state.ReversePolarity)
{
revPow = (-1) * revPow;
}
//update state
_state.CurrentPower = revPow;
//convert power
int power = (int)Math.Round(revPow * _state.PowerScalingFactor);
//send hardware specific motor data
legoNXT.LegoSetOutputState motordata = new legoNXT.LegoSetOutputState();
motordata.OutputPort = _state.HardwareIdentifier - 1;
motordata.PowerSetPoint = power;
motordata.Mode = legoNXT.LegoOutputMode.PowerControl;
motordata.RegulationMode = legoNXT.LegoRegulationMode.MotorSpeed;
motordata.TurnRatio = 0;
motordata.RunState = legoNXT.LegoRunState.Running;
motordata.RotationLimit = 0; //run forever
motordata.RequireResponse = false;
_requestPending++;
Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motordata),
delegate(legoNXT.LegoResponse status)
{
_requestPending--;
},
delegate(Fault failure)
{
_requestPending--;
}
));
//reply to sender
setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower update)
{
//Console.WriteLine("In set motor power handler .Got " + update.Body.TargetPower);
if (_state.HardwareIdentifier == 0)
{
throw new InvalidOperationException("Surveyor SRV-1 Motor is not configured");
}
_state.CurrentPower = update.Body.TargetPower;
float srv1Power = EnsureMinimumPower(_state.CurrentPower);
actuator.ActuatorValueRequest req = new actuator.ActuatorValueRequest();
// Leave the other motors unchanged
req.activateA = false;
req.activateB = false;
switch (_state.HardwareIdentifier)
{
case 1:
req.actuatorA = srv1Power;
req.activateA = true;
break;
case 2:
req.actuatorB = srv1Power;
req.activateB = true;
break;
}
// Post a message to the Surveyor SRV-1 Actuator
//Console.WriteLine("==== Posting to actuator: " + req.actuatorA.Value + "," + req.actuatorB.Value);
actuator.SetActuatorValues msg = new actuator.SetActuatorValues(req);
_actuator.Post(msg);
//_actuator.SetActuatorValues(req);
yield return(Arbiter.Receive <DefaultUpdateResponseType>(false, msg.ResponsePort, delegate(DefaultUpdateResponseType resp){}));
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> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
#if TRACELOG
Tracer.Trace("TR Drive: SetDrivePowerHandler() =============================== TR handler ==================================== left=" + setDrivePower.Body.LeftWheelPower + " right=" + setDrivePower.Body.RightWheelPower);
#endif
cancelCurrentOperation();
_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(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower });
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 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;
}
}));
pollEncoderState();
yield break;
}
void StopMotorWithEncoderHandler(Port<encoder.UpdateTickCount> encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort)
{
int stopWheelAt;
bool ignore;
switch (side)
{
case "left":
stopWheelAt = stopLeftWheelAt;
ignore = !_leftEncoderTickEnabled;
break;
default:
case "right":
stopWheelAt = stopRightWheelAt;
ignore = !_rightEncoderTickEnabled;
break;
}
if (!ignore)
{
#if TRACEDEBUGTICKS
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt);
#endif // TRACEDEBUGTICKS
if (update.Body.Count >= stopWheelAt)
{
switch (side)
{
case "left":
_leftEncoderTickEnabled = false;
break;
default:
case "right":
_rightEncoderTickEnabled = false;
break;
}
// whatever else got stuck there, we are not interested. Keep the port clear.
//Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)];
//port.Clear();
motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 0 });
motorPort.Post(stop);
Activate(Arbiter.Choice(stop.ResponsePort,
delegate(DefaultUpdateResponseType resp)
{
#if TRACEDEBUGTICKS
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped by encoder !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
#endif // TRACEDEBUGTICKS
},
delegate(Fault fault) { LogError(fault); }
));
#if TRACEDEBUGTICKS
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending to completionPort: DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
#endif // TRACEDEBUGTICKS
completionPort.Post(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)
{
#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;
}
