public static byte ConvertToByte(PLCCommandResponseExpectationEnum input)
{
if (!Enum.IsDefined(typeof(PLCCommandResponseExpectationEnum), input))
{
return(ConvertToByte(PLCCommandResponseExpectationEnum.UNDEFINED));
}
else
{
return((byte)input);
}
}
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));
}