public static int GetParamCount(NuriMode m) { switch (m) { case NuriMode.NURI_SET_POS_VEL: return(3); case NuriMode.NURI_SET_POS_RAMP: return(3); case NuriMode.NURI_SET_VEL_RAMP: return(3); case NuriMode.NURI_SET_POS_PID: return(4); case NuriMode.NURI_SET_VEL_PID: return(4); case NuriMode.NURI_SET_INFO: return(5); case NuriMode.NURI_FACTORY_RESET: return(0); } return(0); }
public static int GetDataSize(NuriMode m) { switch (m) { case NuriMode.NURI_SET_POS_VEL: return(0x06); case NuriMode.NURI_SET_POS_RAMP: return(0x05); case NuriMode.NURI_SET_VEL_RAMP: return(0x04); case NuriMode.NURI_SET_POS_PID: return(0x04); case NuriMode.NURI_SET_VEL_PID: return(0x04); case NuriMode.NURI_SET_INFO: return(0x05); case NuriMode.NURI_FACTORY_RESET: return(0x00); } return(0); }
public static byte[] GetPacket(int id, NuriMode mode, params double[] numbers) { int dataSize = GetDataSize(mode); int paramCount = GetParamCount(mode); List <byte> rawByteList = new List <byte>(); rawByteList.Add((byte)id); rawByteList.Add((byte)mode); rawByteList.Add((byte)dataSize); if (numbers.Length != paramCount) { Console.WriteLine("Data Count is Wrong required : {0} / input : {1}", paramCount, numbers.Length); return(null); } switch (mode) { case NuriMode.NURI_SET_POS_VEL: { int direction = (int)numbers[0]; int pos = Math.Min((int)numbers[1], 65533); int pos_frac = (int)(numbers[1] * 100) % 100; int vel = Math.Min((int)numbers[2], 30); int vel_frac = (int)(numbers[2] * 100) % 100; rawByteList.Add((byte)direction); // Direction 0 : Right, 1 : Left rawByteList.Add((byte)((pos >> 8) & 0xFF)); rawByteList.Add((byte)(pos & 0xFF)); rawByteList.Add((byte)pos_frac); rawByteList.Add((byte)((vel >> 8) & 0xFF)); rawByteList.Add((byte)(vel & 0xFF)); rawByteList.Add((byte)vel_frac); break; } case NuriMode.NURI_SET_POS_RAMP: { int direction = (int)numbers[0]; int pos = Math.Min((int)numbers[1], 65533); // *[1 Deg] int pos_frac = (int)(numbers[1] * 100) % 100; // *[0.01 Deg] int time = Math.Min((int)numbers[2], 250); // *[0.1s] rawByteList.Add((byte)direction); // Direction 0 : Right, 1 : Left rawByteList.Add((byte)((pos >> 8) & 0xFF)); rawByteList.Add((byte)(pos & 0xFF)); rawByteList.Add((byte)pos_frac); rawByteList.Add((byte)time); break; } case NuriMode.NURI_SET_VEL_RAMP: { int direction = (int)numbers[0]; int vel = Math.Min((int)numbers[1], 30); // *[1 RPM] int vel_frac = (int)(numbers[1] * 100) % 100; // *[0.01 RPM] int time = Math.Min((int)numbers[2], 250); // *[0.1s] rawByteList.Add((byte)direction); // Direction 0 : Right, 1 : Left rawByteList.Add((byte)((vel >> 8) & 0xFF)); rawByteList.Add((byte)(vel & 0xFF)); rawByteList.Add((byte)vel_frac); rawByteList.Add((byte)time); break; } case NuriMode.NURI_SET_POS_PID: { int Kp = Math.Min((int)numbers[0], 250); int Ki = Math.Min((int)numbers[1], 250); int Kd = Math.Min((int)numbers[2], 250); int Imax = Math.Min((int)numbers[3], 80); // *[100mA] rawByteList.Add((byte)Kp); rawByteList.Add((byte)Ki); rawByteList.Add((byte)Kd); rawByteList.Add((byte)Imax); break; } case NuriMode.NURI_SET_VEL_PID: { int Kp = Math.Min((int)numbers[0], 250); int Ki = Math.Min((int)numbers[1], 250); int Kd = Math.Min((int)numbers[2], 250); int Imax = Math.Min((int)numbers[3], 80); // *[100mA] rawByteList.Add((byte)Kp); rawByteList.Add((byte)Ki); rawByteList.Add((byte)Kd); rawByteList.Add((byte)Imax); break; } case NuriMode.NURI_SET_INFO: { int deviceID = Math.Min((int)numbers[0], 253); int baud = Math.Min((int)numbers[1], 10); // 0x0A = 115200 int encReset = Math.Min((int)numbers[2], 1); // 1: Reset int ctrlOnOff = Math.Min((int)numbers[3], 1); // 0:on , 1:off int responseTime = Math.Min((int)numbers[4], 250); // *[100us] rawByteList.Add((byte)deviceID); rawByteList.Add((byte)baud); rawByteList.Add((byte)encReset); rawByteList.Add((byte)ctrlOnOff); rawByteList.Add((byte)responseTime); break; } } return(PacketFinishing(rawByteList.ToArray())); }