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 double randomPower = _randomGen.NextDouble(); drive.SetDrivePowerRequest setPower = new drive.SetDrivePowerRequest(randomPower, -randomPower); bool success = false; yield return (Arbiter.Choice( _drivePort.SetDrivePower(setPower), delegate(DefaultUpdateResponseType rsp) { 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; }
protected PortSet <DefaultUpdateResponseType, Fault> MoveForward(double speed) { LogInfo(string.Format("DriveBehaviorServiceBase:: MoveForward() speed={0} mm/sec", speed)); Console.WriteLine(speed == 0.0d ? "Stop Moving" : ("Move Forward speed " + speed)); //if (speed != 0.0d) //{ // EnableDrive(); //} // use power equivallent: double wheelPower = Math.Max(-1.0d, Math.Min(1.0d, (double)speed / 1000.0d)); // get it to -1...1 range, as wheel power must be in this range if (speed == 0.0d) { Console.WriteLine("Behavior Base: MoveForward() --------------------------- to TR ---------------------------------------- speed=" + speed + " wheelPower=" + wheelPower); _obstacleAvoidanceDrivePort.SetDrivePower(wheelPower, wheelPower); return(_drivePort.SetDrivePower(wheelPower, wheelPower)); } else { Console.WriteLine("Behavior Base: MoveForward() =========================== to OA ======================================== speed=" + speed + " wheelPower=" + wheelPower); //var request = new drive.SetDrivePowerRequest //{ // LeftWheelPower = wheelPower, // RightWheelPower = wheelPower //}; //return _obstacleAvoidanceDrivePort.SetDrivePower(request); return(_obstacleAvoidanceDrivePort.SetDrivePower(wheelPower, wheelPower)); } }
/// <summary> /// Implements a simple sequential state machine that makes the robot wander /// </summary> /// <param name="turnAmount">Amount of time to turn left. If value is negative, the robot will turn right</param> IEnumerator <ITask> BackUpTurnAndMove(int turnAmount) { // start moving backwards _drivePort.SetDrivePower(new drive.SetDrivePowerRequest(-0.9, -0.9)); //wait while reversing yield return(Arbiter.Receive(false, TimeoutPort(800), delegate(DateTime t) { })); // start turning if (turnAmount > 0) { _drivePort.SetDrivePower(new drive.SetDrivePowerRequest(-0.7, 0.7)); } else { _drivePort.SetDrivePower(new drive.SetDrivePowerRequest(0.7, -0.7)); } //wait turn time yield return(Arbiter.Receive(false, TimeoutPort(Math.Abs(turnAmount)), delegate(DateTime t) { })); // continue forwards _drivePort.SetDrivePower(new drive.SetDrivePowerRequest(0.9, 0.9)); //update state _state.CurrentAction = "Driving forward"; // done yield break; }
public virtual IEnumerator <ITask> StopHandler(Stop stop) { drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest(); request.LeftWheelPower = 0; request.RightWheelPower = 0; yield return(Arbiter.Choice( _drivePort.SetDrivePower(request), delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(null, "Unable to stop", fault); } )); }
public void SetMotors(double leftPower, double rightPower) { if (!motorsOn) { EnableMotors(); } //drive.SetDrivePowerRequest drivePowerReq = new drive.SetDrivePowerRequest(); //drivePowerReq.LeftWheelPower = leftPower; //drivePowerReq.RightWheelPower = rightPower; //drive.SetDrivePower setDrivePower = new drive.SetDrivePower(drivePowerReq); //drivePort.Post(setDrivePower); //ManualResetEvent signal = new ManualResetEvent(false); //Arbiter.Activate(DssEnvironment.TaskQueue, // Arbiter.Choice<DefaultUpdateResponseType, Fault>( // drivePort.SetDrivePower((double)leftPower, (double)rightPower), // delegate(DefaultUpdateResponseType state) // { // signal.Set(); // }, // delegate(Fault failure) // { // Console.WriteLine("*** Fault in SetMotors: "); // foreach (var r in failure.Reason) // Console.WriteLine("*** " + r.Value); // signal.Set(); // })); //signal.WaitOne(); RSUtils.ReceiveSync(taskQueue, drivePort.SetDrivePower(leftPower, rightPower), Myro.Utilities.Params.DefaultRecieveTimeout); }
/// <summary> /// Set the motor power. /// </summary> /// <param name="leftPower"></param> /// <param name="rightPower"></param> public void SetMotors(double leftPower, double rightPower) { if (!motorsOn) { EnableMotors(); } RSUtils.ReceiveSync(taskQueue, drivePort.SetDrivePower(leftPower, rightPower), Myro.Utilities.Params.DefaultRecieveTimeout); }
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 override void SetMotors(float leftPower, float rightPower) { if (!motorsOn) { EnableMotors(); } drive.SetDrivePowerRequest drivePowerReq = new drive.SetDrivePowerRequest(); drivePowerReq.LeftWheelPower = leftPower; drivePowerReq.RightWheelPower = rightPower; drivePort.SetDrivePower(drivePowerReq); }
IEnumerator <ITask> DrawIt() { //drawing parameters int DriveStraightTime = 1000; double DriveStraightPower = 1.0; int TurnTime = 750; double TurnPowerOut = 1.0; double TurnPowerIn = 0.0; //wait some time before starting _state.CurrentAction = "Waiting"; Console.WriteLine(_state.CurrentAction); System.Threading.Thread.Sleep(1000); for (int i = 0; i < 4; i++) { // set motors straight _state.CurrentAction = "Driving Straight, edge " + i; Console.WriteLine(_state.CurrentAction); _drivePort.SetDrivePower(new drive.SetDrivePowerRequest(DriveStraightPower, DriveStraightPower)); //wait straight time System.Threading.Thread.Sleep(DriveStraightTime); // set motors turning _state.CurrentAction = "Turning, corner " + i; Console.WriteLine(_state.CurrentAction); _drivePort.SetDrivePower(new drive.SetDrivePowerRequest(TurnPowerIn, TurnPowerOut)); //wait turn time System.Threading.Thread.Sleep(TurnTime); } // done yield break; }
IEnumerator<ITask> CheckLight() { int lefteye = 0; int centereye = 0; int righteye = 0; double leftpower, rightpower; //first get light readings yield return Arbiter.Receive<light.ScribblerLightSensorState>(false, _lightPort.Get(), delegate(light.ScribblerLightSensorState reading) { lefteye = (int)reading.LeftSensor.RawMeasurement; centereye = (int)reading.CenterSensor.RawMeasurement; righteye = (int)reading.RightSensor.RawMeasurement; } ); //do 'smarts' //not enough variation to turn if (Math.Max(Math.Max(lefteye, centereye), righteye) - Math.Min(Math.Min(lefteye, centereye), righteye) < 25) { leftpower = 0.9; rightpower = 0.9; _state.CurrentAction = "Driving Straight"; } else if (Math.Min(Math.Min(lefteye, centereye), righteye) == lefteye) //bright light to left { leftpower = 0.3; rightpower = 1.0; _state.CurrentAction = "Turning Left"; } else if (Math.Min(Math.Min(lefteye, centereye), righteye) == righteye) //bright light to right { leftpower = 1.0; rightpower = 0.3; _state.CurrentAction = "Turning Right"; } else //bright light to center { leftpower = 0.9; rightpower = 0.9; _state.CurrentAction = "Driving Straight"; } // send motor messages drive.SetDrivePowerRequest setPower = new drive.SetDrivePowerRequest(leftpower, rightpower); _drivePort.SetDrivePower(setPower); // done yield break; }
protected void DriveOutputHandler(DriveOutputState output) { if (output.AllStop) { Arbiter.Activate(TaskQueue, Arbiter.Choice( _drivePort.AllStop(new drive.AllStopRequest()), delegate(DefaultUpdateResponseType success) { }, delegate(W3C.Soap.Fault failure) { LogError("Failed to Stop!"); } ) ); } else { _drivePort.SetDrivePower(output); } }
/// <summary> /// Handles the recognition event notification /// </summary> /// <param name="recognition"></param> #region CODECLIP 02-2 void OnSRRecognition(sr.SpeechRecognized recognition) { if (recognition.Body.Semantics != null) { switch (recognition.Body.Semantics.ValueString) { case "Forward": _drivePort.SetDrivePower(0.5, 0.5); break; case "Backward": _drivePort.SetDrivePower(-0.5, -0.5); break; case "Left": _drivePort.SetDrivePower(-0.3, 0.3); break; case "Right": _drivePort.SetDrivePower(0.3, -0.3); break; case "Stop": _followMe = false; _drivePort.SetDrivePower(0.0, 0.0); break; case "FollowMe": _followMe = !_followMe; if (!_followMe) { _drivePort.SetDrivePower(0.0, 0.0); } break; } } }
public void Backward(float power) { drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest(-power, -power); _mainPort.SetDrivePower(request); }
// Iterator to execute the Behavior // It is important to use an Iterator so that it can relinquish control // when there is nothing to do, i.e. yield return IEnumerator <ITask> Behavior() { // Wait for the robot to initialize, otherwise it will // miss the initial command for (int i = 10; i > 0; i--) { LogInfo(LogGroups.Console, i.ToString()); yield return(Timeout(1000)); } if (useTimers) { LogInfo(LogGroups.Console, "Starting now using Timers ..."); } else { LogInfo(LogGroups.Console, "Starting now using Controlled Moves ..."); } // Make sure that the drive is enabled first! _drivePort.EnableDrive(true); for (int times = 0; times < repeatCount; times++) { // Drive along the four sides of a square for (int side = 0; side < 4; side++) { // Display progress info for the user LogInfo(LogGroups.Console, "Side " + side); if (useTimers) { // This appoach just uses SetDrivePower and sets timers // to control the drive and rotate motions. This is not // very accurate and the exact timer parameters will be // different for different types of robots. // Start driving and then wait for a while _drivePort.SetDrivePower(drivePower, drivePower); yield return(Timeout(driveTime)); // Stop the motors and wait for robot to settle _drivePort.SetDrivePower(0, 0); yield return(Timeout(settlingTime)); // Now turn left and wait for a different amount of time _drivePort.SetDrivePower(-rotatePower, rotatePower); yield return(Timeout(rotateTime)); // Stop the motors and wait for robot to settle _drivePort.SetDrivePower(0, 0); yield return(Timeout(settlingTime)); } else { // This code uses the DriveDistance and RotateDegrees // operations to control the robot. These are not precise, // but they should be better than using timers and they // should also work regardless of the type of robot. bool success = true; Fault fault = null; // Drive straight ahead yield return(Arbiter.Choice( _drivePort.DriveDistance(driveDistance, drivePower), delegate(DefaultUpdateResponseType response) { success = true; }, delegate(Fault f) { success = false; fault = f; } )); // If the DriveDistance was accepted, then wait for it to complete. // It is important not to wait if the request failed. // NOTE: This approach only works if you always wait for a // completion message. If you send any other drive request // while the current one is active, then the current motion // will be canceled, i.e. cut short. if (success) { yield return(WaitForCompletion()); } else { LogError("Error occurred on DriveDistance: " + fault); } // Wait for settling time yield return(Timeout(settlingTime)); // Now turn left yield return(Arbiter.Choice( _drivePort.RotateDegrees(rotateAngle, rotatePower), delegate(DefaultUpdateResponseType response) { success = true; }, delegate(Fault f) { success = false; fault = f; } )); // If the RotateDegrees was accepted, then wait for it to complete. // It is important not to wait if the request failed. if (success) { yield return(WaitForCompletion()); } else { LogError("Error occurred on RotateDegrees: " + fault); } // Wait for settling time yield return(Timeout(settlingTime)); } } } // And finally make sure that the robot is stopped! _drivePort.SetDrivePower(0, 0); LogInfo(LogGroups.Console, "Finished"); yield break; }