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); }