public virtual IEnumerator <ITask> TurnRightHandler(TurnRight forward)
{
if (!_state.MotorEnabled)
{
yield return(EnableMotor());
}
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest();
request.RightWheelPower = 0.5;
request.LeftWheelPower = -0.5;
yield return(Arbiter.Choice(
_drivePort.SetDrivePower(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault)
{
LogError(null, "Unable to turn right", fault);
}
));
}
/// <summary>
/// Handle Motion Commands
/// </summary>
/// <param name="onMove">The motion command</param>
/// <returns>An Iterator</returns>
private IEnumerator <ITask> OnMoveHandler(OnMove onMove)
{
var p = (Port <OnMove>) this.eventsPort[typeof(OnMove)];
if (p.ItemCount > 10)
{
Console.WriteLine("OnMove backlog: " + p.ItemCount);
}
if (onMove.DashboardForm == this.dashboardForm && this.drivePort != null)
{
var request = new drive.SetDrivePowerRequest
{
LeftWheelPower = onMove.Left * MotorPowerSaleFactor,
RightWheelPower = onMove.Right * MotorPowerSaleFactor
};
yield return(Arbiter.Choice(this.drivePort.SetDrivePower(request), EmptyHandler, LogError));
}
}
public virtual IEnumerator <ITask> ForwardHandler(Forward forward)
{
if (!_state.MotorEnabled)
{
yield return(EnableMotor());
}
// This sample sets the power to 75%.
// Depending on your robotic hardware,
// you may wish to change these values.
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest();
request.LeftWheelPower = 0.75;
request.RightWheelPower = 0.75;
yield return(Arbiter.Choice(
_drivePort.SetDrivePower(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault)
{
LogError(null, "Unable to drive forwards", fault);
}
));
}
public void Backward(float power)
{
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest(-power, -power);
_mainPort.SetDrivePower(request);
}
public void TurnRight(float power)
{
drive.SetDrivePowerRequest right = new drive.SetDrivePowerRequest(power, -power);
_mainPort.SetDrivePower(right);
}
public void HttpPostHandler(HttpPost httpPost)
{
HttpPostRequestData formData = httpPost.GetHeader <HttpPostRequestData>();
try
{
DsspOperation operation = formData.TranslatedOperation;
if (operation is drive.DriveDistance)
{
_mainPort.Post((drive.DriveDistance)operation);
}
else if (operation is drive.SetDrivePower)
{
_mainPort.Post((drive.SetDrivePower)operation);
}
else if (operation is drive.RotateDegrees)
{
_mainPort.Post((drive.RotateDegrees)operation);
}
else if (operation is drive.EnableDrive)
{
_mainPort.Post((drive.EnableDrive)operation);
}
else if (operation is drive.AllStop)
{
_mainPort.Post((drive.AllStop)operation);
}
else
{
NameValueCollection parameters = formData.Parameters;
if (parameters["StartDashboard"] != null)
{
string Dashboardcontract = "http://schemas.microsoft.com/robotics/2006/01/simpledashboard.html";
ServiceInfoType info = new ServiceInfoType(Dashboardcontract);
cons.Create create = new cons.Create(info);
create.TimeSpan = DsspOperation.DefaultShortTimeSpan;
ConstructorPort.Post(create);
Arbiter.Choice(
create.ResponsePort,
delegate(CreateResponse createResponse) { },
delegate(Fault f) { LogError(f); }
);
}
else if (parameters["DrivePower"] != null)
{
double power = double.Parse(parameters["Power"]);
drive.SetDrivePowerRequest drivepower = new drive.SetDrivePowerRequest();
drivepower.LeftWheelPower = power;
drivepower.RightWheelPower = power;
_mainPort.Post(new drive.SetDrivePower(drivepower));
}
else
{
throw new InvalidOperationException();
}
}
HttpPostSuccess(httpPost);
}
catch
{
HttpPostFailure(httpPost);
}
}
public void Stop()
{
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest(0, 0);
_mainPort.SetDrivePower(request);
}
public void TurnLeft(float power)
{
drive.SetDrivePowerRequest left = new drive.SetDrivePowerRequest(-power, power);
_mainPort.SetDrivePower(left);
}
public void Forward(float power)
{
drive.SetDrivePowerRequest forwards = new drive.SetDrivePowerRequest(power, power);
_mainPort.SetDrivePower(forwards);
}
public void SetMotors(float leftPower, float rightPower)
{
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest(leftPower, rightPower);
_mainPort.SetDrivePower(request);
}
IEnumerator<ITask> OnMoveHandler(OnMove onMove)
{
if (onMove.DriveControl == _driveControl && _drivePort != null)
{
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest();
request.LeftWheelPower = (double)onMove.Left * MOTOR_POWER_SCALE_FACTOR;
request.RightWheelPower = (double)onMove.Right * MOTOR_POWER_SCALE_FACTOR;
yield return Arbiter.Choice(
_drivePort.SetDrivePower(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
);
}
}
public IEnumerator<ITask> HandleEnable(gendrive.EnableDrive enable)
{
this.pendingSetDrivePower = null;
this.state.Controller.Reset();
var responsePort = this.robotDrivePort.EnableDrive(enable.Body.Enable);
yield return responsePort.Choice();
Fault f = responsePort;
if (f != null)
{
enable.ResponsePort.Post(f);
yield break;
}
enable.ResponsePort.Post(DefaultUpdateResponseType.Instance);
this.SendNotification(this.submgr, enable);
}
/// <summary>
/// MoveStraight
/// </summary>
/// <param name="done">Port to post success message to</param>
/// <param name="powerLevel">Power level for the drive</param>
/// <returns>True/False depending on whether SetDrivePower succeeded</returns>
// IMPORTANT NOTE: MoveStraight() uses ValidatePowerLevel()
// which sets a MINIMUM power level. In other words, you
// can't stop the robot using MoveStraight!!!
IEnumerator<ITask> MoveStraight(Port<bool> done, double powerLevel)
{
// Proxies dont currently have initialization constructors so we have to
// explicitly set fields. This will be fixed post June 20th CTP
drive.SetDrivePowerRequest setPower = new drive.SetDrivePowerRequest();
setPower.LeftWheelPower = ValidatePowerLevel(powerLevel);
setPower.RightWheelPower = ValidatePowerLevel(powerLevel);
string msg;
msg = "Move Straight " + setPower.LeftWheelPower.ToString() + ", " + setPower.RightWheelPower.ToString();
LogInfo(LogGroups.Console, msg);
yield return
Arbiter.Choice(
_drivePort.SetDrivePower(setPower),
delegate(DefaultUpdateResponseType success)
{
_state.DrivePowerLeft = setPower.LeftWheelPower;
_state.DrivePowerRight = setPower.RightWheelPower;
_state.MoveState = MoveStates.MoveStraight;
done.Post(true);
},
delegate(W3C.Soap.Fault failure)
{
// report error but report done anyway. we will attempt
// to do the next step in wander behavior even if turn failed
LogError("Failed setting drive power in MoveStraight()");
done.Post(false);
});
}
/// <summary>
/// RandomTurn
/// </summary>
/// <param name="done">Port to post success message to</param>
/// <returns>Ture/False depending on whether SetDrivePower succeeded</returns>
IEnumerator<ITask> RandomTurn(Port<bool> done)
{
// We turn by issuing motor commands, using reverse polarity for left and right
// We could just issue a Rotate command but since its a higher level function
// We cant assume (yet) all our implementations of differential drives support it
// TT -- In fact, the simulated Pioneer does NOT support rotate even in the
// September 2006 CTP.
drive.SetDrivePowerRequest setPower = new drive.SetDrivePowerRequest();
setPower.LeftWheelPower = ValidatePowerLevel(_randomGen.NextDouble());
setPower.RightWheelPower = ValidatePowerLevel(-setPower.LeftWheelPower);
string msg;
msg = "Random Turn " + setPower.LeftWheelPower.ToString() + ", " + setPower.RightWheelPower.ToString();
LogInfo(LogGroups.Console, msg);
bool success = false;
yield return
Arbiter.Choice(
_drivePort.SetDrivePower(setPower),
delegate(DefaultUpdateResponseType rsp)
{
_state.DrivePowerLeft = setPower.LeftWheelPower;
_state.DrivePowerRight = setPower.RightWheelPower;
_state.MoveState = MoveStates.Turn;
success = true;
},
delegate(W3C.Soap.Fault failure)
{
// report error but report done anyway. we will attempt
// to do the next step in wander behavior even if turn failed
LogError("Failed setting drive power");
});
done.Post(success);
yield break;
}
public void HttpPostHandler(HttpPost httpPost)
{
HttpPostRequestData formData = httpPost.GetHeader<HttpPostRequestData>();
try
{
DsspOperation operation = formData.TranslatedOperation;
if (operation is drive.DriveDistance)
{
_mainPort.Post((drive.DriveDistance)operation);
}
else if (operation is drive.SetDrivePower)
{
_mainPort.Post((drive.SetDrivePower)operation);
}
else if (operation is drive.RotateDegrees)
{
_mainPort.Post((drive.RotateDegrees)operation);
}
else if (operation is drive.EnableDrive)
{
_mainPort.Post((drive.EnableDrive)operation);
}
else if (operation is drive.AllStop)
{
_mainPort.Post((drive.AllStop)operation);
}
else if (operation is drive.ResetEncoders)
{
_mainPort.Post((drive.ResetEncoders)operation);
}
else
{
NameValueCollection parameters = formData.Parameters;
if (parameters["StartDashboard"] != null)
{
string Dashboardcontract = "http://schemas.microsoft.com/robotics/2006/01/simpledashboard.user.html";
ServiceInfoType info = new ServiceInfoType(Dashboardcontract);
cons.Create create = new cons.Create(info);
create.TimeSpan = DsspOperation.DefaultShortTimeSpan;
ConstructorPort.Post(create);
Activate(Arbiter.Choice(
create.ResponsePort,
delegate(CreateResponse createResponse) { },
delegate(Fault f) { LogError(f); }
));
}
else if (parameters["DrivePower"] != null)
{
double power = double.Parse(parameters["Power"]);
drive.SetDrivePowerRequest drivepower = new drive.SetDrivePowerRequest();
drivepower.LeftWheelPower = power;
drivepower.RightWheelPower = power;
_mainPort.Post(new drive.SetDrivePower(drivepower));
}
else
{
throw new InvalidOperationException();
}
}
HttpPostSuccess(httpPost);
}
catch
{
HttpPostFailure(httpPost);
}
}
IEnumerator<ITask> OnMoveHandler(OnMove onMove)
{
if (onMove.DriveControl == _driveControl && _drivePort != null)
{
drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest();
request.LeftWheelPower = (double)onMove.Left * MOTOR_POWER_SCALE_FACTOR;
request.RightWheelPower = (double)onMove.Right * MOTOR_POWER_SCALE_FACTOR;
_drivePort.SetDrivePower(request);
}
yield break;
}
public void HandleSetPower(gendrive.SetDrivePower setPower)
{
// If either wheel power is greater than the max power per wheel
// scale both wheels down so that the higher of them is equal to max power
double higherPower = Math.Max(
Math.Abs(setPower.Body.LeftWheelPower),
Math.Abs(setPower.Body.RightWheelPower));
if (higherPower > this.state.MaxPowerPerWheel)
{
double scalingFactor = higherPower / this.state.MaxPowerPerWheel;
setPower.Body.LeftWheelPower /= scalingFactor;
setPower.Body.RightWheelPower /= scalingFactor;
}
this.NormalizeRotationSpeed(setPower);
this.pendingSetDrivePower = setPower.Body;
if (this.pendingSetDrivePower.LeftWheelPower == 0 &&
this.pendingSetDrivePower.RightWheelPower == 0)
{
this.SetPowerWithAcceleration(0, 0);
this.pendingSetDrivePower = null;
}
// simple cache power request. We will apply proper control and do sensor fusion
// for obstacle avoidance, as part of our sampling loop
setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
