public IEnumerator <ITask> DriveAllStopHandler(drive.AllStop allstop) { this.state.DriveState.TimeStamp = DateTime.Now; serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(board.CreatePacket <sbyte>(board.SetFullStopString)); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); allstop.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); allstop.ResponsePort.Post(f); yield break; } // Reflect into our state this.state.DriveState.LeftWheel.MotorState.CurrentPower = GDDriveFullStop; this.state.DriveState.RightWheel.MotorState.CurrentPower = GDDriveFullStop; this.state.DriveState.LeftWheel.WheelSpeed = 0; this.state.DriveState.RightWheel.WheelSpeed = 0; this.state.DriveState.IsEnabled = false; allstop.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public IEnumerator <ITask> DriveDriveDistanceHandler(drive.DriveDistance drivedistance) { if (!this.state.DriveState.IsEnabled) { drivedistance.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Drive not enabled!")); yield break; } this.state.DriveState.TimeStamp = DateTime.Now; short pwr = (short)(board.HBridgeReverseMax + ((drivedistance.Body.Power - GDDriveMin) * DrivePowerScale)); short dist = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * drivedistance.Body.Distance); byte[] cmdpacket = board.CreatePacket <short, short>(board.SetTravelDistanceString, dist, pwr); serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(cmdpacket); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); drivedistance.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); drivedistance.ResponsePort.Post(f); yield break; } this.state.DriveState.LeftWheel.MotorState.CurrentPower = drivedistance.Body.Power; this.state.DriveState.RightWheel.MotorState.CurrentPower = drivedistance.Body.Power; this.state.DriveState.DriveDistanceStage = drive.DriveStage.Completed; drivedistance.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
/// <summary> /// Performs a reset on both encoders in response to /// a reset (update) on either encoder. /// </summary> /// <param name="reset">An encoder Reset operation type</param> /// <returns>An IEnumerator of type ITask</returns> private IEnumerator <ITask> InternalEncoderReset(drive.ResetEncoders reset) { serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Terminator = board.PacketTerminatorString; sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.ResetEncoderTicksString)); PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { reset.ResponsePort.Post(f); yield break; } serialcomservice.Packet p = (serialcomservice.Packet)resultPort; if (this.HasFWError(p)) { f = soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "FW ERROR: Failed to reset encoder tick count!"); reset.ResponsePort.Post(f); yield break; } // update both the encoder partner state as well as the brick state this.state.DriveState.LeftWheel.EncoderState.TicksSinceReset = this.state.DriveState.RightWheel.EncoderState.TicksSinceReset = 0; this.state.DriveState.LeftWheel.EncoderState.CurrentAngle = this.state.DriveState.RightWheel.EncoderState.CurrentAngle = 0; this.state.DriveState.LeftWheel.EncoderState.CurrentReading = this.state.DriveState.RightWheel.EncoderState.CurrentReading = 0; this.state.DriveState.LeftWheel.EncoderState.TimeStamp = this.state.DriveState.RightWheel.EncoderState.TimeStamp = DateTime.Now; // Notify any brick subscribers as well as any drive subscribers this.SendNotification <Replace>(this.submgrPort, new Replace()); this.SendNotification <drive.ResetEncoders>(this.submgrDrivePort, new drive.ResetEncoders()); reset.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public IEnumerator <ITask> DriveSetDriveSpeedHandler(drive.SetDriveSpeed setdrivespeed) { if (!this.state.DriveState.IsEnabled) { setdrivespeed.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Drive not enabled!")); yield break; } this.state.DriveState.TimeStamp = DateTime.Now; short leftWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * setdrivespeed.Body.LeftWheelSpeed); short rightWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Right] * setdrivespeed.Body.RightWheelSpeed); byte[] cmdpacket = board.CreatePacket <short>(board.SetTravelVelocityString, leftWheelSpeed, rightWheelSpeed); serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(cmdpacket); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); setdrivespeed.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); setdrivespeed.ResponsePort.Post(f); yield break; } this.state.DriveState.LeftWheel.WheelSpeed = setdrivespeed.Body.LeftWheelSpeed; this.state.DriveState.RightWheel.WheelSpeed = setdrivespeed.Body.RightWheelSpeed; setdrivespeed.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public IEnumerator <ITask> DriveEnableDriveHandler(drive.EnableDrive enabledrive) { this.state.DriveState.TimeStamp = DateTime.Now; this.state.DriveState.IsEnabled = enabledrive.Body.Enable; // The caller's intent is to disable the drive, so we need to set motor power to zero if (!this.state.DriveState.IsEnabled) { serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.SetFullStopString)); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", board.SetFullStopString)); enabledrive.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); enabledrive.ResponsePort.Post(f); yield break; } // Reflect into state this.state.DriveState.LeftWheel.MotorState.CurrentPower = GDDriveFullStop; this.state.DriveState.RightWheel.MotorState.CurrentPower = GDDriveFullStop; this.state.DriveState.LeftWheel.WheelSpeed = 0; this.state.DriveState.RightWheel.WheelSpeed = 0; } enabledrive.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public IEnumerator <ITask> GpioSetPinHandler(gpio.SetPin setPin) { soap.Fault f; // Pins are labeled on the control board using 1-based indexing int pinIndex = setPin.Body.PinState.Number - 1; if (pinIndex < 0 || pinIndex >= board.GPIOPinCount) { f = soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Invalid GPIO pin index specified!"); setPin.ResponsePort.Post(f); yield break; } int pinMask = 1 << pinIndex; string cmdString = (setPin.Body.PinState.PinDirection == gpio.GpioPinState.GpioPinDirection.Out) ? board.SetGPIODirectionOutString : board.SetGPIODirectionInString; byte[] cmdpacket = board.CreatePacket(cmdString, pinMask); serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(cmdpacket); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); setPin.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); setPin.ResponsePort.Post(f); yield break; } // If we have an OUT pin, then it's appropriate to take action on the HIGH/LOW state // Ignore PinStates that have a HIGH/LOW specifier when direction is set to IN if (setPin.Body.PinState.PinDirection == gpio.GpioPinState.GpioPinDirection.Out) { cmdString = (setPin.Body.PinState.PinState == gpio.GpioPinState.GpioPinSignal.Low) ? board.SetGPIOStateLowString : board.SetGPIOStateHighString; cmdpacket = board.CreatePacket(cmdString, pinMask); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(cmdpacket); resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); setPin.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); setPin.ResponsePort.Post(f); yield break; } } this.state.GpioState.Pins[pinIndex] = setPin.Body.PinState; setPin.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
/// <summary> /// Retrieve the COM port specified in the SerialCOMService state. /// Perform initialization if COM port is correct and available. /// </summary> /// <returns>Enumerator of type ITask</returns> private IEnumerator <ITask> InternalInitialize() { // Make sure we have a valid and open COM port int currentCOMPort = 0; yield return(this.serialCOMServicePort.GetConfiguration().Choice( s => currentCOMPort = s.PortNumber, f => LogError("Failed to retrieve config from Serial Port partner service"))); yield return(this.serialCOMServicePort.OpenPort().Choice( s => LogInfo(string.Format("Opened COM{0}", currentCOMPort)), f => LogError(string.Format("Failed to open COM{0}", currentCOMPort)))); if (currentCOMPort == 0) { LogError("Parallax2011ReferencePlatformIoController Service failed to initialize: Check 'PortNumber' in serialcomservice.config.xml"); this.Shutdown(); yield break; } this.initialized = this.Initialize(); if (this.initialized) { // Make sure we have a live Parallax board on the COM port by retrieving the FW version string serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Terminator = board.PacketTerminatorString; sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetVersionString)); PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f == null) { serialcomservice.Packet p = (serialcomservice.Packet)resultPort; if (p != null) { if (p.Message != null) { string str = Encoding.ASCII.GetString(p.Message); this.state.FWVersion = Convert.ToInt32(str, 16); } } } else { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); LogError("Failed to receive FW version!"); } LogInfo(string.Format("FW Version: {0}", this.state.FWVersion)); serialcomservice.SendAndGetRequest accSendAndGet = new serialcomservice.SendAndGetRequest(); accSendAndGet.Timeout = this.state.DefaultResponsePause; accSendAndGet.Terminator = board.PacketTerminatorString; accSendAndGet.Data = new serialcomservice.Packet(board.CreatePacket <ushort>(board.SetRampingValueString, (ushort)this.state.AccelerationRate)); resultPort = this.serialCOMServicePort.SendAndGet(accSendAndGet); yield return(resultPort.Choice()); f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(accSendAndGet.Data.Message))); LogError("Failed to set acceleration!"); } this.encoderTicksPerMeter = new double[] { this.state.DriveState.LeftWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.LeftWheel.Radius * Math.PI), this.state.DriveState.RightWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.RightWheel.Radius * Math.PI) }; SaveState(this.state); base.Start(); // Make sure the pin polling port is in the main interleave because it modifies service state MainPortInterleave.CombineWith( new Interleave( new ExclusiveReceiverGroup( Arbiter.ReceiveWithIterator(true, this.pinPollingPort, this.PollPins)), new ConcurrentReceiverGroup())); // Start the pin polling interval this.pinPollingPort.Post(DateTime.Now); } }
/// <summary> /// Keep the FW watchdog alive by continuously retrieving pin and encoder values /// </summary> /// <param name="dt">A instance of type DateTime</param> /// <returns>A instance of IEnumerator of type ITask</returns> private IEnumerator <ITask> PollPins(DateTime dt) { try { serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Terminator = board.PacketTerminatorString; sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetADCValuesString)); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); yield break; } serialcomservice.Packet p = (serialcomservice.Packet)resultPort; if (this.HasFWError(p)) { LogError("Error returned reading ADC values from FW!"); yield break; } // Stash the analog pin values into a string string analogValues = Encoding.ASCII.GetString(p.Message); // Retrieve the digital pin values sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetDigitalValuesString)); resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); yield break; } p = (serialcomservice.Packet)resultPort; if (this.HasFWError(p)) { LogError("Error returned reading digital pin values from FW!"); yield break; } // Stash the digital pin values into a string string digitalValues = Encoding.ASCII.GetString(p.Message); // Perform all the parsing this.UpdatePinStatesFromFWString(analogValues, digitalValues); // Reset speed to 0 since it is currently unknown as opposed to having the last value be stuck this.state.DriveState.LeftWheel.WheelSpeed = 0; this.state.DriveState.RightWheel.WheelSpeed = 0; // Retrieve both wheel speeds sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetCurrentSpeedString)); resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); p = (serialcomservice.Packet)resultPort; if (this.HasFWError(p)) { LogError("Error received reading speed values from FW!"); yield break; } f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); yield break; } this.UpdateSpeedStateFromFWString(Encoding.ASCII.GetString(p.Message)); // Retrieve both wheel encoder values sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetEncoderTicksString)); resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); yield break; } p = (serialcomservice.Packet)resultPort; if (this.HasFWError(p)) { LogError("Error received reading encoder values from FW!"); yield break; } // Reflect the returned bytes into encoder and motor states this.UpdateEncoderStatesFromFWString(Encoding.ASCII.GetString(p.Message)); } finally { // Ensure we haven't been droppped if (ServicePhase == ServiceRuntimePhase.Started) { // Issue another polling request Activate(TimeoutPort(this.state.PinPollingInterval).Receive(this.pinPollingPort.Post)); } } }