public void TestByteToEnum() { Assert.AreEqual(PLCCommandResponseExpectationEnum.UNDEFINED, PLCCommandResponseExpectationConversionHelper.GetFromByte(0x0)); Assert.AreEqual(PLCCommandResponseExpectationEnum.MINOR_RESPONSE, PLCCommandResponseExpectationConversionHelper.GetFromByte(0x1)); Assert.AreEqual(PLCCommandResponseExpectationEnum.FULL_RESPONSE, PLCCommandResponseExpectationConversionHelper.GetFromByte(0x2)); Assert.AreEqual(PLCCommandResponseExpectationEnum.UNDEFINED, PLCCommandResponseExpectationConversionHelper.GetFromByte(0x3)); Assert.AreEqual(PLCCommandResponseExpectationEnum.UNDEFINED, PLCCommandResponseExpectationConversionHelper.GetFromByte(0xFF)); }
public bool ProcessRequest(NetworkStream ActiveClientStream, byte[] query) { int ExpectedSize = query[0] + (256 * query[1]); if (query.Length != ExpectedSize) { throw new ArgumentException( "ScaleModelPLCDriverController read a package specifying a size [" + ExpectedSize.ToString() + "], but the actual size was different [" + query.Length + "]." ); } byte CommandQueryTypeAndExpectedResponseStatus = query[2]; byte CommandQueryTypeByte = (byte)(CommandQueryTypeAndExpectedResponseStatus & 0x3F); byte ExpectedResponseStatusByte = (byte)(CommandQueryTypeAndExpectedResponseStatus >> 6); PLCCommandAndQueryTypeEnum CommandQueryTypeEnum = PLCCommandAndQueryTypeConversionHelper.GetFromByte(CommandQueryTypeByte); PLCCommandResponseExpectationEnum ExpectedResponseStatusEnum = PLCCommandResponseExpectationConversionHelper.GetFromByte(ExpectedResponseStatusByte); byte[] FinalResponseContainer; if (ExpectedResponseStatusEnum == PLCCommandResponseExpectationEnum.FULL_RESPONSE) { FinalResponseContainer = new byte[] { 0x13, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 }; switch (CommandQueryTypeEnum) { case PLCCommandAndQueryTypeEnum.TEST_CONNECTION: { FinalResponseContainer[3] = 0x1; FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_AZEL_POSITIONS: { double TestAzimuth = 180.0; double TestElevation = 42.0; Array.Copy(BitConverter.GetBytes(TestAzimuth), 0, FinalResponseContainer, 3, 8); Array.Copy(BitConverter.GetBytes(TestElevation), 0, FinalResponseContainer, 11, 8); FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_LIMIT_SWITCH_STATUSES: { PLCLimitSwitchStatusEnum StatusAzimuthUnderRotation = PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; PLCLimitSwitchStatusEnum StatusAzimuthOverRotation = PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; PLCLimitSwitchStatusEnum StatusElevationUnderRotation = PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; PLCLimitSwitchStatusEnum StatusElevationOverRotation = PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; int PacketSum = PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusElevationOverRotation) + (PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusElevationUnderRotation) * 0x4) + (PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusAzimuthOverRotation) * 0x10) + (PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusAzimuthUnderRotation) * 0x40) ; FinalResponseContainer[3] = (byte)PacketSum; FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_SAFETY_INTERLOCK_STATUS: { FinalResponseContainer[3] = PLCSafetyInterlockStatusConversionHelper.ConvertToByte(PLCSafetyInterlockStatusEnum.LOCKED); FinalResponseContainer[2] = 0x1; break; } default: { throw new ArgumentException("Invalid PLCCommandAndQueryTypeEnum value seen while expecting a response: " + CommandQueryTypeEnum.ToString()); } } } else if (ExpectedResponseStatusEnum == PLCCommandResponseExpectationEnum.MINOR_RESPONSE) { FinalResponseContainer = new byte[] { 0x3, 0x0, 0x0 }; switch (CommandQueryTypeEnum) { case PLCCommandAndQueryTypeEnum.CANCEL_ACTIVE_OBJECTIVE_AZEL_POSITION: case PLCCommandAndQueryTypeEnum.SHUTDOWN: case PLCCommandAndQueryTypeEnum.CALIBRATE: { FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.SET_OBJECTIVE_AZEL_POSITION: { double NextAZ, NextEL; try { NextAZ = BitConverter.ToDouble(query, 3); NextEL = BitConverter.ToDouble(query, 11); } catch (Exception e) { if ((e is ArgumentException) || (e is ArgumentNullException) || (e is ArgumentOutOfRangeException)) { // This error code means that the data could not be converted into a double-precision floating point FinalResponseContainer[2] = 0x2; break; } else { // Unexpected exception throw e; } } if ((NextAZ < 0) || (NextAZ > 360)) { // This error code means that the objective azimuth position is invalid FinalResponseContainer[2] = 0x3; break; } if ((NextEL < 0) || (NextEL > 90)) { // This error code means that the objective elevation position is invalid FinalResponseContainer[2] = 0x4; break; } // Otherwise, this is valid // TODO: Perform task(s) to set objective orientation! FinalResponseContainer[2] = 0x1; break; } default: { throw new ArgumentException("Invalid PLCCommandAndQueryTypeEnum value seen while NOT expecting a response: " + CommandQueryTypeEnum.ToString()); } } } else { throw new ArgumentException("Invalid PLCCommandResponseExpectationEnum value seen while processing client request in ScaleModelPLCDriver: " + ExpectedResponseStatusEnum.ToString()); } return(AttemptToWriteDataToServer(ActiveClientStream, FinalResponseContainer)); }
/// <summary> /// handle conection data comming from modbus device /// this serves as part of a tcp server along with HandleClientManagementThread /// it apears to have been designed for ethernet/ip which we found out our plc cant do /// this is probably unnecicary in the mcu drivers. /// </summary> /// <param name="ActiveClientStream"></param> /// <param name="query"></param> /// <returns></returns> public bool ProcessRequest(NetworkStream ActiveClientStream, byte[] query) { int ExpectedSize = query[0] + (256 * query[1]); if (query.Length != ExpectedSize) { throw new ArgumentException( "ProductionPLCDriver read a package specifying a size [" + ExpectedSize.ToString() + "], but the actual size was different [" + query.Length + "]." ); } byte CommandQueryTypeAndExpectedResponseStatus = query[2]; byte CommandQueryTypeByte = (byte)(CommandQueryTypeAndExpectedResponseStatus & 0x3F); byte ExpectedResponseStatusByte = (byte)(CommandQueryTypeAndExpectedResponseStatus >> 6); PLCCommandAndQueryTypeEnum CommandQueryTypeEnum = PLCCommandAndQueryTypeConversionHelper.GetFromByte(CommandQueryTypeByte); PLCCommandResponseExpectationEnum ExpectedResponseStatusEnum = PLCCommandResponseExpectationConversionHelper.GetFromByte(ExpectedResponseStatusByte); byte[] FinalResponseContainer; if (ExpectedResponseStatusEnum == PLCCommandResponseExpectationEnum.FULL_RESPONSE) { FinalResponseContainer = new byte[] { 0x13, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 }; switch (CommandQueryTypeEnum) { case PLCCommandAndQueryTypeEnum.TEST_CONNECTION: { // Read the heartbeat register ushort[] inputRegisters = MCUModbusMaster.ReadInputRegisters(MCUConstants.ACTUAL_MCU_READ_INPUT_REGISTER_HEARTBEAT_ADDRESS, 1); ushort resultValue = (ushort)((inputRegisters.Length == 1) ? inputRegisters[0] : 0); FinalResponseContainer[3] = (byte)(((resultValue == 8192) || (resultValue == 24576)) ? 0x1 : 0x0); FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_AZEL_POSITIONS: { PrintReadInputRegsiterContents("Before getting current position"); // Get the MCU's value for the displacement since its power cycle ushort[] inputRegisters = MCUModbusMaster.ReadInputRegisters(MCUConstants.ACTUAL_MCU_READ_INPUT_REGISTER_CURRENT_POSITION_ADDRESS, 2); int currentStepForMCU = (65536 * inputRegisters[0]) + inputRegisters[1]; PrintReadInputRegsiterContents("After getting current position"); // Convert that step change into degrees and write the bytes to return Array.Copy(BitConverter.GetBytes(currentStepForMCU * 360 / 10000000.0), 0, FinalResponseContainer, 3, 8); Array.Copy(BitConverter.GetBytes(0.0), 0, FinalResponseContainer, 11, 8); FinalResponseContainer[2] = 0x1; break; } default: { throw new ArgumentException("Invalid PLCCommandAndQueryTypeEnum value seen while expecting a response: " + CommandQueryTypeEnum.ToString()); } } } else if (ExpectedResponseStatusEnum == PLCCommandResponseExpectationEnum.MINOR_RESPONSE) { FinalResponseContainer = new byte[] { 0x3, 0x0, 0x0 }; switch (CommandQueryTypeEnum) { case PLCCommandAndQueryTypeEnum.SET_CONFIGURATION: { // Copy over data we care about, which for now is only the azimuth // We skip over the data concerning the elevation, hence the gap in element access for query ushort[] DataToWrite = { (ushort)((256 * query[3]) + query[4]), (ushort)((256 * query[5]) + query[6]), (ushort)((256 * query[7]) + query[8]), (ushort)((256 * query[9]) + query[10]), (ushort)((256 * query[15]) + query[16]), 0, 0, 0, 0, 0 }; MCUModbusMaster.WriteMultipleRegisters(MCUConstants.ACTUAL_MCU_WRITE_REGISTER_START_ADDRESS, DataToWrite); PrintReadInputRegsiterContents("After setting configuration"); if (SendResetErrorsCommand()) { Console.WriteLine("[ProductionPLCDriver] Successfully sent reset command."); PrintReadInputRegsiterContents("After sending reset command"); FinalResponseContainer[2] = 0x1; } else { // Send an error code Console.WriteLine("[ProductionPLCDriver] ERROR sending reset command."); FinalResponseContainer[2] = 0x2; } break; } case PLCCommandAndQueryTypeEnum.CONTROLLED_STOP: { // There was already a helper function to execute a controlled stop, so just call that // Send an error code if there's a failure for some reason FinalResponseContainer[2] = (byte)(SendEmptyMoveCommand() ? 0x1 : 0x2); break; } case PLCCommandAndQueryTypeEnum.IMMEDIATE_STOP: { // There was already a helper function to execute a controlled stop, so just call that // Send an error code if there's a failure for some reason FinalResponseContainer[2] = (byte)(SendImmediateStopCommand() ? 0x1 : 0x2); break; } case PLCCommandAndQueryTypeEnum.SET_OBJECTIVE_AZEL_POSITION: { PrintReadInputRegsiterContents("Before setting objective position"); // Copy over data we care about, so skip over the data concerning the elevation double discrepancyMultiplier = 1.0; double objectiveAZDouble = BitConverter.ToDouble(query, 3); int stepChange = (int)(discrepancyMultiplier * Math.Pow(2, MCUConstants.ACTUAL_MCU_AZIMUTH_ENCODER_BIT_RESOLUTION) * objectiveAZDouble / 360); ushort stepChangeUShortMSW = (ushort)((stepChange >> 16) & 0xFFFF); ushort stepChangeUShortLSW = (ushort)(stepChange & 0xFFFF); int programmedPeakSpeed = BitConverter.ToInt32(query, 7); ushort programmedPeakSpeedUShortMSW = (ushort)((programmedPeakSpeed >> 16) & 0xFFFF); ushort programmedPeakSpeedUShortLSW = (ushort)(programmedPeakSpeed & 0xFFFF); ushort[] DataToWrite = { 0x2, // Denotes a relative move in command mode 0x3, // Denotes a Trapezoidal S-Curve profile stepChangeUShortMSW, stepChangeUShortLSW, programmedPeakSpeedUShortMSW, programmedPeakSpeedUShortLSW, MCUConstants.ACTUAL_MCU_MOVE_ACCELERATION_WITH_GEARING, MCUConstants.ACTUAL_MCU_MOVE_ACCELERATION_WITH_GEARING, 0, 0 }; MCUModbusMaster.WriteMultipleRegisters(MCUConstants.ACTUAL_MCU_WRITE_REGISTER_START_ADDRESS, DataToWrite); PrintReadInputRegsiterContents("After setting objective position"); FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.START_JOG_MOVEMENT: { PrintReadInputRegsiterContents("Before starting jog command"); // Make sure the command is intended for the azimuth if (query[3] != 0x1) { throw new ArgumentException("Unsupported value for axis specified in jog command for ProductionPLCDriver: " + query[3].ToString()); } ushort programmedPeakSpeedUShortMSW = (ushort)((256 * query[4]) + query[5]); ushort programmedPeakSpeedUShortLSW = (ushort)((256 * query[6]) + query[7]); ushort commandCode; switch (query[8]) { case 0x1: { commandCode = 0x80; break; } case 0x2: { commandCode = 0x100; break; } default: { throw new ArgumentException("Unsupported value for motor movement direction in jog command for ProductionPLCDriver: " + query[8].ToString()); } } ushort[] DataToWrite = { commandCode, // Denotes a jog move, either CW or CCW, in command mode 0x3, // Denotes a Trapezoidal S-Curve profile 0, // Reserved to 0 for a jog command 0, // Reserved to 0 for a jog command programmedPeakSpeedUShortMSW, programmedPeakSpeedUShortLSW, MCUConstants.ACTUAL_MCU_MOVE_ACCELERATION_WITH_GEARING, MCUConstants.ACTUAL_MCU_MOVE_ACCELERATION_WITH_GEARING, 0, 0 }; MCUModbusMaster.WriteMultipleRegisters(MCUConstants.ACTUAL_MCU_WRITE_REGISTER_START_ADDRESS, DataToWrite); PrintReadInputRegsiterContents("After starting jog command"); FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.TRANSLATE_AZEL_POSITION: { PrintReadInputRegsiterContents("Before starting relative move"); // Make sure the command is intended for the azimuth if (query[3] != 0x1) { throw new ArgumentException("Unsupported value for axis specified in move relative command for ProductionPLCDriver: " + query[3].ToString()); } ushort programmedPeakSpeedUShortMSW = (ushort)((256 * query[4]) + query[5]); ushort programmedPeakSpeedUShortLSW = (ushort)((256 * query[6]) + query[7]); short programmedPositionUShortMSW = (short)((256 * query[8]) + query[9]); short programmedPositionUShortLSW = (short)((256 * query[10]) + query[11]); ushort[] DataToWrite = { 0x2, // Denotes a relative move 0x3, // Denotes a Trapezoidal S-Curve profile (ushort)programmedPositionUShortMSW, // MSW for position (ushort)programmedPositionUShortLSW, // LSW for position programmedPeakSpeedUShortMSW, programmedPeakSpeedUShortLSW, MCUConstants.ACTUAL_MCU_MOVE_ACCELERATION_WITH_GEARING, MCUConstants.ACTUAL_MCU_MOVE_ACCELERATION_WITH_GEARING, 0, 0 }; MCUModbusMaster.WriteMultipleRegisters(MCUConstants.ACTUAL_MCU_WRITE_REGISTER_START_ADDRESS, DataToWrite); PrintReadInputRegsiterContents("After starting relative move command"); FinalResponseContainer[2] = 0x1; break; } default: { throw new ArgumentException("Invalid PLCCommandAndQueryTypeEnum value seen while NOT expecting a response: " + CommandQueryTypeEnum.ToString()); } } } else { throw new ArgumentException("Invalid PLCCommandResponseExpectationEnum value seen while processing client request in ProductionPLCDriver: " + ExpectedResponseStatusEnum.ToString()); } return(AttemptToWriteDataToServer(ActiveClientStream, FinalResponseContainer)); }
public bool ProcessRequest(NetworkStream ActiveClientStream, byte[] query) { int ExpectedSize = query[0] + (256 * query[1]); if (query.Length != ExpectedSize) { throw new ArgumentException( "TestPLCDriverController read a package specifying a size [" + ExpectedSize.ToString() + "], but the actual size was different [" + query.Length + "]." ); } byte CommandQueryTypeAndExpectedResponseStatus = query[2]; byte CommandQueryTypeByte = (byte)(CommandQueryTypeAndExpectedResponseStatus & 0x3F); byte ExpectedResponseStatusByte = (byte)(CommandQueryTypeAndExpectedResponseStatus >> 6); PLCCommandAndQueryTypeEnum CommandQueryTypeEnum = PLCCommandAndQueryTypeConversionHelper.GetFromByte(CommandQueryTypeByte); PLCCommandResponseExpectationEnum ExpectedResponseStatusEnum = PLCCommandResponseExpectationConversionHelper.GetFromByte(ExpectedResponseStatusByte); byte[] FinalResponseContainer; if (ExpectedResponseStatusEnum == PLCCommandResponseExpectationEnum.FULL_RESPONSE) { FinalResponseContainer = new byte[] { 0x13, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 }; switch (CommandQueryTypeEnum) { case PLCCommandAndQueryTypeEnum.TEST_CONNECTION: { FinalResponseContainer[3] = 0x1; FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_AZEL_POSITIONS: { Array.Copy(BitConverter.GetBytes(CurrentOrientation.Azimuth), 0, FinalResponseContainer, 3, 8); Array.Copy(BitConverter.GetBytes(CurrentOrientation.Elevation), 0, FinalResponseContainer, 11, 8); FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_LIMIT_SWITCH_STATUSES: { double CurrentAZ = CurrentOrientation.Azimuth; double CurrentEL = CurrentOrientation.Elevation; double ThresholdAZ = MiscellaneousHardwareConstants.LIMIT_SWITCH_AZ_THRESHOLD_DEGREES; double ThresholdEL = MiscellaneousHardwareConstants.LIMIT_SWITCH_EL_THRESHOLD_DEGREES; // Subtracting out those 2 degrees is because of our actual rotational limits of (-2 : 362) and (-2 : 92) degrees in azimuth and elevation respectively PLCLimitSwitchStatusEnum StatusAzimuthUnderRotation = (CurrentAZ < (ThresholdAZ - 2.0)) ? PLCLimitSwitchStatusEnum.WITHIN_WARNING_LIMITS : PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; PLCLimitSwitchStatusEnum StatusAzimuthOverRotation = (CurrentAZ > (360 + ThresholdAZ - 2.0)) ? PLCLimitSwitchStatusEnum.WITHIN_WARNING_LIMITS : PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; PLCLimitSwitchStatusEnum StatusElevationUnderRotation = (CurrentEL < (ThresholdEL - 2.0)) ? PLCLimitSwitchStatusEnum.WITHIN_WARNING_LIMITS : PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; PLCLimitSwitchStatusEnum StatusElevationOverRotation = (CurrentEL > (90 + ThresholdEL - 2.0)) ? PLCLimitSwitchStatusEnum.WITHIN_WARNING_LIMITS : PLCLimitSwitchStatusEnum.WITHIN_SAFE_LIMITS; int PacketSum = PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusElevationOverRotation) + (PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusElevationUnderRotation) * 0x4) + (PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusAzimuthOverRotation) * 0x10) + (PLCLimitSwitchStatusConversionHelper.ConvertToByte(StatusAzimuthUnderRotation) * 0x40) ; FinalResponseContainer[3] = (byte)PacketSum; FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.GET_CURRENT_SAFETY_INTERLOCK_STATUS: { FinalResponseContainer[3] = PLCSafetyInterlockStatusConversionHelper.ConvertToByte(PLCSafetyInterlockStatusEnum.LOCKED); FinalResponseContainer[2] = 0x1; break; } default: { throw new ArgumentException("Invalid PLCCommandAndQueryTypeEnum value seen while expecting a response: " + CommandQueryTypeEnum.ToString()); } } } else if (ExpectedResponseStatusEnum == PLCCommandResponseExpectationEnum.MINOR_RESPONSE) { FinalResponseContainer = new byte[] { 0x3, 0x0, 0x0 }; switch (CommandQueryTypeEnum) { case PLCCommandAndQueryTypeEnum.CANCEL_ACTIVE_OBJECTIVE_AZEL_POSITION: case PLCCommandAndQueryTypeEnum.SHUTDOWN: case PLCCommandAndQueryTypeEnum.CALIBRATE: { FinalResponseContainer[2] = 0x1; break; } case PLCCommandAndQueryTypeEnum.SET_OBJECTIVE_AZEL_POSITION: { double NextAZ, NextEL; try { NextAZ = BitConverter.ToDouble(query, 3); NextEL = BitConverter.ToDouble(query, 11); } catch (Exception e) { if ((e is ArgumentException) || (e is ArgumentNullException) || (e is ArgumentOutOfRangeException)) { // This error code means that the data could not be converted into a double-precision floating point FinalResponseContainer[2] = 0x2; break; } else { // Unexpected exception throw e; } } if ((NextAZ < 0) || (NextAZ > 360)) { // This error code means that the objective azimuth position is invalid FinalResponseContainer[2] = 0x3; break; } if ((NextEL < 0) || (NextEL > 90)) { // This error code means that the objective elevation position is invalid FinalResponseContainer[2] = 0x4; break; } // Otherwise, this is valid CurrentOrientation = new Orientation(NextAZ, NextEL); logger.Info("[TestPLCDriver] Setting current orientation to {" + CurrentOrientation.Azimuth.ToString() + ", " + CurrentOrientation.Elevation.ToString() + "}"); FinalResponseContainer[2] = 0x1; break; } default: { throw new ArgumentException("Invalid PLCCommandAndQueryTypeEnum value seen while NOT expecting a response: " + CommandQueryTypeEnum.ToString()); } } } else { throw new ArgumentException("Invalid PLCCommandResponseExpectationEnum value seen while processing client request in ScaleModelPLCDriver: " + ExpectedResponseStatusEnum.ToString()); } return(AttemptToWriteDataToServer(ActiveClientStream, FinalResponseContainer)); }