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));
}
void OnGetRequestTypeSuccess(driveinsquare.DriveInSquareState response)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest();
request.Enable = true;
ObstacleAvoidanceDrivePort.EnableDrive(request);
Decrement();
}
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);
}
));
}
}
/// <summary>
/// Connect to the Obstacle Avoidance Diff Drive for "Drive Forward operation
/// </summary>
/// <returns>An Iterator</returns>
private IEnumerator <ITask> ConnectObstacleAvoidanceDrive()
{
var request = new drive.EnableDriveRequest {
Enable = true
};
if (this._obstacleAvoidanceDrivePort != null)
{
yield return(Arbiter.Choice(this._obstacleAvoidanceDrivePort.EnableDrive(request), EmptyHandler, LogError));
}
}
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);
}
));
}
/// <summary>
/// Handle Motion Commands
/// </summary>
/// <param name="onCommand">The motion command</param>
/// <returns>An Iterator</returns>
private IEnumerator <ITask> OnMotionCommandHandler(OnMotionCommand onCommand)
{
if (onCommand.DashboardForm == this.dashboardForm && this.drivePort != null)
{
switch (onCommand.Command)
{
case MOTIONCOMMANDS.Rotate:
var rotRequest = new drive.RotateDegreesRequest
{
Degrees = onCommand.Parameter, Power = onCommand.Power * MotorPowerSaleFactor
};
yield return(Arbiter.Choice(this.drivePort.RotateDegrees(rotRequest), EmptyHandler, LogError));
break;
case MOTIONCOMMANDS.Translate:
var transRequest = new drive.DriveDistanceRequest
{
Distance = onCommand.Parameter, Power = onCommand.Power * MotorPowerSaleFactor
};
yield return(Arbiter.Choice(this.drivePort.DriveDistance(transRequest), EmptyHandler, LogError));
break;
case MOTIONCOMMANDS.Enable:
var request = new drive.EnableDriveRequest {
Enable = true
};
yield return(Arbiter.Choice(this.drivePort.EnableDrive(request), EmptyHandler, LogError));
break;
default:
LogInfo("Requesting EStop");
var stopRequest = new drive.AllStopRequest();
yield return(Arbiter.Choice(this.drivePort.AllStop(stopRequest), EmptyHandler, LogError));
break;
}
}
yield break;
}
protected void EnableMotors()
{
drive.EnableDriveRequest enableDriveMessage = new drive.EnableDriveRequest();
enableDriveMessage.Enable = true;
drivePort.EnableDrive(enableDriveMessage);
}
/// <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);
}
/// <summary>
/// Connect to the Obstacle Avoidance Diff Drive for "Drive Forward operation
/// </summary>
/// <returns>An Iterator</returns>
private IEnumerator<ITask> ConnectObstacleAvoidanceDrive()
{
var request = new drive.EnableDriveRequest { Enable = true };
if (this._obstacleAvoidanceDrivePort != null)
{
yield return Arbiter.Choice(this._obstacleAvoidanceDrivePort.EnableDrive(request), EmptyHandler, LogError);
}
}
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);
}
);
}
}
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;
}
