//Request a value From memory
public void EEP_ReadParam(SerialPort port, byte pID, SrvRegAddr memoryAddress)
{
byte packetSize = (byte)(9); //reading only one value so fixed length of 9 bytes in the packet (data[0] = addr data[1] = len@addr)
byte[] data = { (byte)memoryAddress, GetMemoryAddrLength((byte)memoryAddress) };
byte[] packet = new byte[packetSize]; //initializing packet to the right length
packet[0] = 0xFF; //SOF1
packet[1] = 0xFF; //SOF2
packet[2] = packetSize; //packet size
packet[3] = pID; //servo ID
packet[4] = (byte)ToServoCommandSet.EEP_READ; //CMD is EEP_READ
packet[5] = GetChecksum(packetSize, pID, (byte)ToServoCommandSet.EEP_READ, data)[0]; //calc Check Sum1
packet[6] = GetChecksum(packetSize, pID, (byte)ToServoCommandSet.EEP_READ, data)[1]; //calc Check Sum2
packet[7] = data[0];
packet[8] = data[1];
//redirect data to OnControllerCommandSentToRedirectorBridge
RedirectSentData(packet);
port.Write(packet, 0, packet.Length); //sending packet with 0 bytes offset
}
//writes a value to the specified memory address
public void EEP_WriteParam(SerialPort port, byte pID, SrvRegAddr memoryAddress, byte[] dataToWrite)
{
byte packetSize = (byte)(7 + dataToWrite.Length);
byte[] packet = new byte[packetSize];
packet[0] = 0xFF; //SOF1
packet[1] = 0xFF; //SOF2
packet[2] = packetSize;
packet[3] = pID;
packet[4] = (byte)ToServoCommandSet.EEP_WRITE; //CMD is EEP_WRITE
packet[5] = (byte)GetChecksum(packetSize, pID, (byte)ToServoCommandSet.EEP_WRITE, dataToWrite)[0]; //calc Check Sum1
packet[5] = (byte)GetChecksum(packetSize, pID, (byte)ToServoCommandSet.EEP_WRITE, dataToWrite)[1]; //calc Check Sum2
packet[7] = dataToWrite[0];
if (packetSize == 9) //if dataToWrite is two bytes long
{
packet[8] = dataToWrite[1];
}
//redirect data to OnControllerCommandSentToRedirectorBridge
RedirectSentData(packet);
port.Write(packet, 0, packet.Length); //sending packet with 0 bytes offset
}
