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