protected void EnableMotors()
{
drive.EnableDriveRequest enableDriveMessage = new drive.EnableDriveRequest();
enableDriveMessage.Enable = true;
drivePort.EnableDrive(enableDriveMessage);
Console.WriteLine("Enabling motors");
}
/// <summary>
/// Sets the drives enabled state.
/// </summary>
/// <param name="enable">new enables state</param>
/// <returns>response port</returns>
PortSet <DefaultUpdateResponseType, Fault> EnableMotor(bool enable)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest();
request.Enable = enable;
return(_drivePort.EnableDrive(request));
}
IEnumerator <ITask> OnConnectMotorHandler(OnConnectMotor onConnectMotor)
{
if (onConnectMotor.DriveControl == _driveControl)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest(false);
if (_drivePort != null)
{
yield return(Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
));
if (_motorShutdown != null)
{
PerformShutdown(ref _motorShutdown);
}
}
_drivePort = ServiceForwarder <drive.DriveOperations>(onConnectMotor.Service);
_motorShutdown = new Port <Shutdown>();
drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
new ReliableSubscribeRequestType(10)
);
subscribe.NotificationPort = _driveNotify;
subscribe.NotificationShutdownPort = _motorShutdown;
_drivePort.Post(subscribe);
yield return(Arbiter.Choice(
subscribe.ResponsePort,
delegate(SubscribeResponseType response)
{
LogInfo("Subscribed to " + onConnectMotor.Service);
},
delegate(Fault fault)
{
_motorShutdown = null;
LogError(fault);
}
));
request = new drive.EnableDriveRequest(true);
yield return(Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
));
}
}
private Choice EnableMotor()
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest();
request.Enable = true;
return(Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault)
{
LogError(null, "Unable to enable motor", fault);
}
));
}
// 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;
}
protected void EnableMotors()
{
drive.EnableDriveRequest enableDriveMessage = new drive.EnableDriveRequest();
enableDriveMessage.Enable = true;
drivePort.EnableDrive(enableDriveMessage);
}
IEnumerator<ITask> OnConnectMotorHandler(OnConnectMotor onConnectMotor)
{
if (onConnectMotor.DriveControl == _driveControl)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest();
if (_drivePort != null)
{
yield return Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response)
{
},
delegate(Fault f)
{
LogError(f);
}
);
if (_motorShutdown != null)
{
yield return PerformShutdown(ref _motorShutdown);
}
}
_drivePort = ServiceForwarder<drive.DriveOperations>(onConnectMotor.Service);
_motorShutdown = new Port<Shutdown>();
request.Enable = true;
yield return Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
);
drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
new ReliableSubscribeRequestType(10)
);
subscribe.NotificationPort = _driveNotify;
subscribe.NotificationShutdownPort = _motorShutdown;
_drivePort.Post(subscribe);
yield return Arbiter.Choice(
subscribe.ResponsePort,
delegate(SubscribeResponseType response)
{
LogInfo("Subscribed to " + onConnectMotor.Service);
},
delegate(Fault fault)
{
_motorShutdown = null;
LogError(fault);
}
);
}
}
protected void EnableMotors()
{
RSUtils.ReceiveSync(taskQueue, drivePort.EnableDrive(true), Params.DefaultRecieveTimeout);
}
public IEnumerator <ITask> ConfigureDrive(DriveState state)
{
PartnerType leftMotorPartner = this.ServiceInfo.PartnerList.Find(delegate(PartnerType partner) { return(partner.Name.Name == "LeftMotor"); });
PartnerType rightMotorPartner = this.ServiceInfo.PartnerList.Find(delegate(PartnerType partner) { return(partner.Name.Name == "RightMotor"); });
// Configure the left motor connection
motorbase.MotorState Lconfigure = new motorbase.MotorState();
Lconfigure.HardwareIdentifier = _state.LeftMotorPortNumber;
Lconfigure.Name = "Left Motor";
Lconfigure.PowerScalingFactor = 1;
yield return(Arbiter.Choice(_leftMotorPort.Replace(Lconfigure),
delegate(DefaultReplaceResponseType success)
{
LogInfo("Left Motor Port set");
},
delegate(Fault fault)
{
LogError(fault);
_initFault = true;
}));
if (_initFault)
{
yield break;
}
// Configure the right motor connection
motorbase.MotorState Rconfigure = new motorbase.MotorState();
Rconfigure.HardwareIdentifier = _state.RightMotorPortNumber;
Rconfigure.Name = "Right Motor";
Rconfigure.PowerScalingFactor = 1;
yield return(Arbiter.Choice(_rightMotorPort.Replace(Rconfigure),
delegate(DefaultReplaceResponseType success)
{
LogInfo("Right Motor Port set");
},
delegate(Fault fault)
{
LogError(fault);
_initFault = true;
}));
if (_initFault)
{
yield break;
}
// Configure the drive service
drivesrv.DriveDifferentialTwoWheelState driveState = new drivesrv.DriveDifferentialTwoWheelState();
driveState.DistanceBetweenWheels = state.DistanceBetweenWheels;
driveState.TimeStamp = DateTime.Now;
driveState.LeftWheel = new motorbase.WheeledMotorState();
driveState.LeftWheel.Radius = _state.WheelRadius;
driveState.LeftWheel.Name = "Left Wheel";
driveState.LeftWheel.MotorState = new motorbase.MotorState();
driveState.LeftWheel.GearRatio = 1.0;
driveState.LeftWheel.EncoderState = new encoderbase.EncoderState();
driveState.RightWheel = new motorbase.WheeledMotorState();
driveState.RightWheel.Radius = _state.WheelRadius;
driveState.RightWheel.Name = "Right Wheel";
driveState.RightWheel.MotorState = new motorbase.MotorState();
driveState.RightWheel.GearRatio = 1.0;
driveState.RightWheel.EncoderState = new encoderbase.EncoderState();
driveState.IsEnabled = true;
yield return(Arbiter.Choice(_drivePort.Update(driveState),
delegate(DefaultUpdateResponseType success)
{
LogInfo("Drive configuration updated");
},
delegate(Fault f)
{
LogError(f);
_initFault = true;
}));
if (_initFault)
{
yield break;
}
drivesrv.EnableDriveRequest en = new drivesrv.EnableDriveRequest();
en.Enable = true;
yield return(Arbiter.Receive <DefaultUpdateResponseType>(false, _drivePort.EnableDrive(en),
delegate(DefaultUpdateResponseType response){}));
yield break;
}
