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