public void LOG_SETUP_moveSpeed(byte INSTRUCTION_BYTE, C_Value _angle, C_Value _speed)
{
string prefix = "ODD_MOVE";
switch (INSTRUCTION_BYTE)
{
case (C_DynAdd.INS_WRITE): prefix = "ORDER_move"; break;
case (C_DynAdd.INS_REG_WRITE): prefix = "REGISTER_move"; break;
case (C_DynAdd.INS_READ): prefix = "READ_move"; break;
}
if (_speed.Dec != C_DynVal.SET_MOV_SPEED_NOCONTROL)
{
LOG(String.Format("{0}:[angle];[speed] =\t[{1}]; [{3}] =\t{2:0.00}°;\t{4:0.00}%, [angle-default]=\t{5:0.00}°",
prefix,
C_CONV.byteArray2strHex_space(_angle.Hex.Reverse().ToArray()), _angle.Dec,
C_CONV.byteArray2strHex_space(_speed.Hex.Reverse().ToArray()), _speed.Dec,
_angle.Dec_FromDefault
));
}
else
{
LOG(String.Format("{0}: [angle];[speed] = [{1}];[{3}] = {2}°; No speed control",
prefix,
C_CONV.byteArray2strHex_space(_angle.Hex.Reverse().ToArray()), _angle.Dec,
C_CONV.byteArray2strHex_space(_speed.Hex.Reverse().ToArray())
));
}
}
public static string GET_errorByteInfo(C_Packet packet)
{
StringBuilder str = new StringBuilder();
for (int b = 0; b < 7; b++)
{
if (C_CONV.GET_bit(packet.byteInstructionOrError, b) == true)
{
str.AppendLine(string.Format("ID[{0}] error: {1}", packet.byteId, errStr[b]));
}
}
return(str.ToString());
}
public static void ACTUALIZE_motorRegistersFromStatusPacket(C_Packet received, C_Packet pairedLastSent)
{
try
{
if (received.byteId == pairedLastSent.byteId)
{
// we have no error in statusPacket
if (pairedLastSent.byteInstructionOrError == C_DynAdd.INS_WRITE)
{
// actualize the parameters which were written into motors - and we know they were written good
C_MotorControl.ACTUALIZE_motorRegister(
pairedLastSent.rotMotor,
e_regByteType.seenValue, // as we received statusMessage after we written the value
pairedLastSent.Par);
}
else if (pairedLastSent.byteInstructionOrError == C_DynAdd.INS_READ)
{
// actualize the parameters which were read from motors
C_MotorControl.ACTUALIZE_motorRegister(
pairedLastSent.rotMotor,
e_regByteType.seenValue,
C_CONV.listOfObjects2listOfBytes(new List <object>()
{
pairedLastSent.Par[0], received.Par
})
);
}
else
{
C_SPI.LOG_debug(string.Format(
"lastSent packet: {0}\nPaired with: {1}\n{2}",
pairedLastSent.PacketBytes_toString,
received.PacketBytes_toString,
"wasn't read neither write and yet still it was supposed to be processed as a statusPacket. Not processing!"
));
}
}
else
{
LOG_statusPacket(string.Format(
"The received status packet :\t{0}\nDoes not belong to the lastSent: \t{1}",
received.PacketBytes_toString, pairedLastSent.PacketBytes_toString
));
}
}
catch (Exception e)
{
C_SPI.LOG_debug("tak tady to padá! " + e.Message);
}
}
public void LOAD_examples(string fname, char del)
{
C_Motor.cmdPackets = new List <C_Packet>();
C_Motor.cmdEx_str = new List <string>();
string strHex_concantenated;
string name;
string[] strArr;
string[] lines;
if (!System.IO.File.Exists(fname))
{
LOG_err(string.Format("{0}\n{1}",
"File with command examples not found!Searched in:",
fname.ToString()
));
}
else
{
LOG("File with command examples found, starting to load cmdInners");
lines = System.IO.File.ReadAllLines(fname, Encoding.ASCII);
foreach (string line in lines)
{
if (string.IsNullOrEmpty(line))
{
continue;
}
strArr = line.Split(del);
strHex_concantenated = strArr[0];
name = strArr[1];
cmdEx_str.Add(string.Format("{0} - {1}", name, strHex_concantenated));
// this can be added as a new constructor of C_Packet(mot, insAndPar)
List <byte> insAndParams = new List <byte>();
//byte[] insAndParams =
insAndParams.AddRange(C_CONV.strHex2byteArray(strHex_concantenated));
C_Motor.cmdPackets.Add(
new C_Packet(
this, insAndParams[0],
insAndParams.GetRange(1, insAndParams.Count - 1)
));
}
LOG("Command examples loaded succesfully!");
cmdinEx_initialized = true;
}
}
private void txRawBytes_TextChanged(object sender, TextChangedEventArgs e)
{
if (C_State.FURTHER(e_stateProg.initialized))
{
try
{
raw = new C_Packet(C_CONV.strHex2byteArray(txRawBytes.Text, " "));
}
finally
{
btnSendRawBytes.IsEnabled = raw.IsConsistent;
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endregion Writing
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#region Reading
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
public static bool START_withPacketStartBytes(List <byte> packetBytes)
{
int q = 0;
foreach (byte by in C_DynAdd.PACKETSTART)
{
if (packetBytes[q] != by)
{
LOG_unimportant(string.Format(
"This packet does not start with PACKETSTART bytes: [{0}]",
C_CONV.byteArray2strHex_space(packetBytes.ToArray())
));
return(false);
}
}
return(true);
}
public static void ACTUALIZE_motorRegister(e_rot rot, e_regByteType type, List <byte> pars)
{
if (pars.Count > 0)
{
byte addressByte = pars[0];
byte[] parValues = pars.Skip(1).ToArray();
C_Packet.LOG_statusPacket(string.Format(
"Status OK - actualizing mot[{0}] register type[{1}]: From address[{2}]=[{3}], these values[{4}]",
rot, type, addressByte, MainWindow.Ms[rot].Reg.GET_name(addressByte),
C_CONV.byteArray2strHex_space(parValues)));
foreach (byte byteValue in parValues)
{
C_SPI.LOG_unimportant(string.Format(
"going to acualize mot[{0}] register on address [{1}]",
rot, addressByte
));
MainWindow.Ms[rot].ACTUALIZE_register(addressByte, byteValue, type);
addressByte++;
}
}
}
public static bool PROCESS_receivedPacket(List <byte> receivedBytes)
{
// we have received one whole packet
/*
* -> is Consistent ?
* -> pair with best from lastSent queues
* -> PROCESS_statusPacket(received, paired)
*/
try
{
C_Packet received = new C_Packet(receivedBytes); // constructor checks consistency
received.statusReceivedTime = DateTime.Now;
return(PAIR_andProcessStatusPacket(received));
}
catch (Exception e)
{
LOG_debug(string.Format(
"Will not process received packet because of its consistency error: {0} Exception:{1}",
C_CONV.byteArray2strHex_space(receivedBytes), e.Message
));
return(false);
}
}
public void INIT_byteRegisterDefaultValues()
{
// load it from txt file
string txt = Properties.Resources.ResourceManager.GetString("registerByteDefault");
string[] lines = txt.Split(new string[] { Environment.NewLine }, StringSplitOptions.None);
char sep = '|';
byte add = 0;
foreach (string line in lines)
{
if (line.Length > 2)
{
string[] splited = line.Split(sep);
// syntax of each line:
// XX A #num name
// legend:
// splited[0]= XX - hexa
// splited[1]= A - [X]registered [R]ead [B]oth ([W]rite)
// splited[2]= # - number symbol
// splited[2]= num - address in register - not needed
// splited[3]= name - address byte name string
e_readWrite rwShadow = e_readWrite.registered;
e_readWrite rwActual = e_readWrite.registered;
switch (splited[1])
{
//case ("X"): rw = e_readWrite.registered; break;
//case ("B"): rw = e_readWrite.readWrite; break;
//case ("R"): rw = e_readWrite.readOnly; break;
//case ("W"): rw = e_readWrite.writeOnly; break;
case ("X"):
rwShadow = e_readWrite.registered;
rwActual = e_readWrite.registered;
break;
case ("B"):
rwShadow = e_readWrite.readWrite;
rwActual = e_readWrite.readWrite;
break;
case ("R"):
rwShadow = e_readWrite.readWrite;
rwActual = e_readWrite.readOnly;
break;
case ("W"):
rwShadow = e_readWrite.readWrite;
rwActual = e_readWrite.writeOnly;
break;
}
reg.Add(new C_RegByte(
splited[3], rwActual, rwShadow, add, C_CONV.strHex2byte(splited[0])
));
//reg[reg.Count-1].
add++;
}
}
i_maxReg = reg.Count;
}
private static void SPI_DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
{
lock (spiGot_locker)
{
SerialPort sp = (SerialPort)sender;
Queue <byte> readBuffer = new Queue <byte>();
byte receivedByte = 0;
LOG_unimportant("Start to read packet");
try
{
// when item is true -> lastSent packet for this motor (index) was already processed
// load new lastSent packet to this index from queueSent[index]
//int cnt = 0;
bool INCOMING_PACKET = false;//= START_withPacketStartBytes(packetBytes);
if (receivingPacketBytes.Count <= GET_packetLength(receivingPacketBytes))
{
if (PACKETSTART_detector(receivingPacketBytes) == 0)
{
INCOMING_PACKET = true;
}
}
int packetLength = GET_packetLength(receivingPacketBytes);
// read all the bytes to read
while (0 != sp.BytesToRead)
{
receivedByte = (byte)C_SPI.spi.ReadByte();
readBuffer.Enqueue(receivedByte);
}
while (readBuffer.Count != 0)
{
receivedByte = readBuffer.Dequeue();
receivingPacketBytes.Add(receivedByte);
//cnt = packetBytes.Count;
if (receivingPacketBytes.Count >= 2)
{
// PACKETSTART detector - detect whether the start sequence is not only in the first byte but wherever in the packetBytes array
int i_start = PACKETSTART_detector(receivingPacketBytes);
if (i_start == 0)
{
INCOMING_PACKET = true;
}
else if (i_start > 0)
{
LOG_debug(string.Format(
"Found another PACKETSTART sequence inside recieving packetBytes on index [{0}] in : {1}"
+ "\nthis is not allowed - skip this packet and use the next PACKETSTART sequence",
i_start, C_CONV.byteArray2strHex_space(receivingPacketBytes.ToArray())
));
receivingPacketBytes.RemoveRange(0, receivingPacketBytes.Count - i_start - 1); // -1??
// try to use the now trimmed part of packet as a full packet ?
INCOMING_PACKET = true;
continue;
}
else if (i_start == -1)
{
LOG_debug("Did not found any PACKETSTART sequence in receiving packet : " +
C_CONV.byteArray2strHex_space(receivingPacketBytes.ToArray()).ToString());
INCOMING_PACKET = false;
continue;
}
else
{
LOG_debug("Not allowed state, the PACKETSTART not detected nor detected - INCOMING_PACKET=false");
INCOMING_PACKET = false;
}
if (INCOMING_PACKET == true)
{
if (receivingPacketBytes.Count - 1 == C_DynAdd.INDEXOF_ID_IN_STATUSPACKET)
{
if (receivedByte > C_DynAdd.ID_BROADCAST)
{
LOG_debug("Not allowed value of id : " + receivedByte.ToString());
receivingPacketBytes.RemoveAt(0); // remove the first byte as it disables the PACKETSTART detection with positioning the id byte as this
INCOMING_PACKET = false;
continue; // it will be catched by PACKETSTART detector
}
}
if (receivingPacketBytes.Count - 1 == C_DynAdd.INDEXOF_LENGTH_IN_STATUSPACKET) // get the LENGTH_BYTE
{
packetLength = GET_packetLength(receivingPacketBytes);
if (packetLength > C_DynAdd.MAX_BYTES_OF_PACKET)
{
// too long packet
LOG_debug(string.Format(
"Packet length byte greater than allowed = {0} > {1}",
packetLength, C_DynAdd.MAX_BYTES_OF_PACKET
));
receivingPacketBytes.RemoveAt(0); // remove the first byte as it disables the PACKETSTART detection with positioning the id byte as this
INCOMING_PACKET = false;
continue;
}
if (packetLength < C_DynAdd.MIN_BYTES_OF_PACKET)
{
// too short packet
LOG_debug(string.Format(
"Packet length byte smaller than allowed = {0} < {1}",
packetLength, C_DynAdd.MIN_BYTES_OF_PACKET
));
receivingPacketBytes.RemoveAt(0); // remove the first byte as it disables the PACKETSTART detection with positioning the id byte as this
INCOMING_PACKET = false;
continue;
}
// LENGTH_BYTE = N*[Params] + 1*[LEN] + 1*[INS/ERROR]
// packetLength = [LENGTH_BYTE] + 1*[ID] + 1*[CheckSum] + 2*[PacketStart]
//packetLength = (int)receivedByte + 4;
}
packetLength = GET_packetLength(receivingPacketBytes);
if (receivingPacketBytes.Count >= packetLength)
{
// the last added byte to packetBytes was the most likely last byte of this package
INCOMING_PACKET = false;
LOG_cmd(receivingPacketBytes.ToArray(), e_cmd.received);
LOG_unimportant("end of packet");
// chose lastSent by receivedPacketBytes idByte
List <byte> statusBytes = new List <byte>(receivingPacketBytes);
receivingPacketBytes.Clear();
LOG_debug("Sent packetBytes to process and 'cleared from readBuffer' :" +
C_CONV.byteArray2strHex_space(statusBytes.ToArray()));
// process it
PROCESS_receivedPacket(statusBytes);
}
if (receivingPacketBytes.Count > packetLength)
{
// trim the packet
LOG_err(String.Format(
"Packet too long, there were more bytes read from packet than the length byte proposes: " +
"{0} from {1}! LENBYTE={2}! \nIt will by cutted! : {3}",
receivingPacketBytes.Count, packetLength,
receivingPacketBytes[C_DynAdd.INDEXOF_LENGTH_IN_STATUSPACKET],
C_CONV.byteArray2strHex_space(receivingPacketBytes.ToArray())
));
receivingPacketBytes = receivingPacketBytes.GetRange(0, GET_packetLength(receivingPacketBytes));
}
}
}
if (C_State.prog == e_stateProg.closing)
{
return;
}
}
}
catch (Exception ex)
{
LOG_err("Exception in packet read algorithm :" + GET_exInfo(ex));
//FLUSH_forNextIncomingPackage("exception");
}
// try if there is not a full packet in the packetBytes
if (receivingPacketBytes.Count == GET_packetLength(receivingPacketBytes))
{
if (PACKETSTART_detector(receivingPacketBytes) == 0)
{
PROCESS_receivedPacket(receivingPacketBytes);
receivingPacketBytes.Clear();
}
}
}
}
