static void scan_bus_ids(ref SerialPort port)
{
Console.WriteLine();
Console.WriteLine("Scanning Bus for Device IDs");
Console.WriteLine("on serial port: {0} at {1} bps", port.PortName, port.BaudRate);
Console.WriteLine("============================================");
Console.WriteLine(">Only the scan will be performed; if you specified other actions, they will be ignored.");
Console.WriteLine(">To run an actual benchmark test, explicitly list the Dynamixel IDs to use via the -dynids parameter.");
Console.WriteLine();
Dynamixel1CommandGenerator dync = new Dynamixel1CommandGenerator();
int bytes_received = 0; long reply_time_usecs = 0, reply_time_ticks = 0;
for (byte b = 0; b < Dynamixel1CommandGenerator.ID_BROADCAST; b++)
{
Console.Write("\rScanning ID {0} ...", b);
byte[] dyn_command = dync.generate_ping_packet(b);
port.ReadExisting(); // purge any bytes in the incomming buffer
port.Write(dyn_command, 0, dyn_command.Length);
byte[] reply = dync.get_dyn_reply(port, b, Dynamixel1CommandGenerator.DYN1_REPLY_SZ_PING, 20000, ref bytes_received, ref reply_time_usecs);
if (reply != null)
{
Console.WriteLine("\rFound ID: {0,3}, replied in {1,5} uSecs)", b, reply_time_usecs, reply_time_ticks);
check_return_delay_times(ref port, new List <byte> {
b
});
}
}
Console.WriteLine("\rDone ");
}
static void check_return_delay_times(ref SerialPort port, List <byte> dyn_ids)
{
Dynamixel1CommandGenerator cg = new Dynamixel1CommandGenerator();
int reply_size = 0; long reply_time = 0;
foreach (byte dyn_id in dyn_ids)
{
byte[] packet = cg.generate_read_packet(dyn_id, ADDR_RETURN_DELAY_TIME, 1);
port.ReadExisting(); // purge any bytes in the incomming buffer
port.Write(packet, 0, packet.Length);
byte[] reply = cg.get_dyn_reply(port, dyn_id, Dynamixel1CommandGenerator.DYN1_REPLY_SZ_READ_MIN + 1, 40000, ref reply_size, ref reply_time);
if (reply == null)
{
Console.Write("ERROR: Unable to check Return Delay time configuration for ID {0} (Reason: ", dyn_id);
switch (cg.last_get_dyn_reply_error)
{
case Dynamixel1CommandGenerator.en_reply_result.TIMEOUT:
Console.WriteLine(" Timeout)");
break;
case Dynamixel1CommandGenerator.en_reply_result.MISTMATCHED_DEVICE_ID:
Console.WriteLine(" Device ID mismatch)");
break;
case Dynamixel1CommandGenerator.en_reply_result.INVALID_COMMAND_LEN:
Console.WriteLine(" LEN error)");
break;
default:
Console.WriteLine(" Other/unhandled)");
break;
}
}
else
{
int ret_delay_time = cg.get_byte_value_from_reply(reply);
if (ret_delay_time < 0)
{
Console.WriteLine("ERROR: Unable to check Return Delay time configuration for ID {0} (unable to parse reply)", dyn_id);
}
else if (ret_delay_time != 0)
{
Console.WriteLine("WARNING: Device with ID {0} is not configured for optimal performance. Return Delay time set to {1}({2}ms); for best performance set this to 0.", dyn_id, ret_delay_time, ret_delay_time * 2);
}
}
}
}
static void run_READ_benchmark(ref SerialPort port, ref List <byte> l_dyn_ids, byte b_read_startaddr, byte b_read_len, System.IO.TextWriter out_writter)
{
Console.WriteLine();
Console.WriteLine("#1 Individual READ commands:");
Console.WriteLine("==========================================");
Console.WriteLine();
check_return_delay_times(ref port, l_dyn_ids);
if (out_writter != null)
{
out_writter.Write("READ,Cycle Nr,");
foreach (byte b_id in l_dyn_ids)
{
out_writter.Write("ID {0},", b_id);
}
out_writter.WriteLine("Total Cycle time");
}
List <long> iteration_times = new List <long>(); int failed_runs = 0;
long min_time_innercycle = long.MaxValue, max_time_innercycle = -1;
byte min_time_innercycle_id = 0, max_time_innercycle_id = 0;
long min_time_outercycle = long.MaxValue, max_time_outercycle = -1;
Dynamixel1CommandGenerator dync = new Dynamixel1CommandGenerator();
int bytes_received = 0; long reply_time_innercycle = 0;
StopWatchMicrosecs outer_timer = new StopWatchMicrosecs(); bool run_failed = false; int cycle_nr = 0;
for (cycle_nr = 0; cycle_nr < NUMBER_OF_CYCLES_PER_BENCHMARK; cycle_nr++)
{
outer_timer.Reset();
outer_timer.Start();
if (out_writter != null)
{
out_writter.Write("READ,{0},", cycle_nr);
}
foreach (byte b_id in l_dyn_ids)
{
run_failed = false;
// make READ request
byte[] command = dync.generate_read_packet(b_id, b_read_startaddr, b_read_len);
port.ReadExisting(); // purge any bytes in the incomming buffer
port.Write(command, 0, command.Length);
byte[] reply = dync.get_dyn_reply(port, b_id, (byte)(Dynamixel1CommandGenerator.DYN1_REPLY_SZ_READ_MIN + b_read_len), 40000, ref bytes_received, ref reply_time_innercycle);
if (reply != null)
{
if (out_writter != null)
{
out_writter.Write("{0},", reply_time_innercycle);
}
if (reply_time_innercycle < min_time_innercycle)
{
min_time_innercycle = reply_time_innercycle;
min_time_innercycle_id = b_id;
}
if (reply_time_innercycle > max_time_innercycle)
{
max_time_innercycle = reply_time_innercycle;
max_time_innercycle_id = b_id;
}
}
else
{
// run fails
run_failed = true;
if (out_writter != null)
{
Console.Write("READ fails at ID: {0}. Reason: ", b_id);
switch (dync.last_get_dyn_reply_error)
{
case Dynamixel1CommandGenerator.en_reply_result.TIMEOUT:
Console.WriteLine("Timeout");
break;
case Dynamixel1CommandGenerator.en_reply_result.MISTMATCHED_DEVICE_ID:
Console.WriteLine("ID mismatch");
break;
case Dynamixel1CommandGenerator.en_reply_result.INVALID_COMMAND_LEN:
Console.WriteLine("LEN error");
break;
default:
Console.WriteLine("Other /unhandled");
break;
}
}
break;
}
}
outer_timer.Stop();
if (!run_failed)
{
min_time_outercycle = Math.Min(min_time_outercycle, outer_timer.ElapsedMicroseconds);
max_time_outercycle = Math.Max(max_time_outercycle, outer_timer.ElapsedMicroseconds);
iteration_times.Add(outer_timer.ElapsedMicroseconds);
if (out_writter != null)
{
out_writter.WriteLine(outer_timer.ElapsedMicroseconds);
}
}
else
{
failed_runs++;
}
Console.Write("\rCycle #{0}: Cycle time {1}uSecs. (Fastest so far {2}uSecs; slowest so far {3}uSecs. Failed runs: {4}) ", cycle_nr, (run_failed ? "FAILED" : outer_timer.ElapsedMicroseconds.ToString()), min_time_outercycle, max_time_outercycle, failed_runs);
}
// test cycle ended. run stats
double avg, std_dev;
if (iteration_times.Count > 1)
{
avg = iteration_times.Average();
std_dev = Math.Sqrt(iteration_times.Average(v => Math.Pow(v - avg, 2)));
}
else
{
avg = 0; std_dev = 0;
}
Console.WriteLine("\rStatistics: ");
Console.WriteLine("\tCycles ran : {0,5}", cycle_nr);
Console.WriteLine("\tCycles failed : {0,5} {1}", failed_runs, (failed_runs > 0 ? "(check if the IDs are all connected and responding)" : ""));
Console.WriteLine("\tFastest Cycle time : {0,5} uSec", min_time_outercycle);
Console.WriteLine("\tSlowest Cycle time : {0,5} uSec", max_time_outercycle);
Console.WriteLine("\tAverage Cycle time : {0,5} uSec", avg);
Console.WriteLine("\tStandard Deviation : {0,5}", std_dev);
Console.WriteLine("\t----------------------");
Console.WriteLine("\tFastest Individual Device: ID {0}, reply time {1,5} uSec", min_time_innercycle_id, min_time_innercycle);
Console.WriteLine("\tSlowest Individual Device: ID {0}, reply time {1,5} uSec", max_time_innercycle_id, max_time_innercycle);
Console.WriteLine();
}
static void run_dsyncread_benchmark(ref SerialPort port, ref List <byte> l_dyn_ids, byte b_read_startaddr, byte b_read_len, System.IO.TextWriter out_writter)
{
Console.WriteLine("#3 Read multiple devices using one DSYNC_READ command:");
Console.WriteLine("==========================================");
Console.WriteLine();
check_return_delay_times(ref port, l_dyn_ids);
if (out_writter != null)
{
out_writter.WriteLine("DSYNC_READ {0} IDs, Total Cycle Time", l_dyn_ids.Count);
}
List <long> iteration_times = new List <long>(); int failed_runs = 0;
long min_time_outercycle = long.MaxValue, max_time_outercycle = -1;
Dynamixel1CommandGenerator dync = new Dynamixel1CommandGenerator();
int bytes_received = 0; long inner_cycle_time = 0;
StopWatchMicrosecs outer_timer = new StopWatchMicrosecs(); bool run_failed = false; int cycle_nr = 0;
// build DSYNC_READ command
byte[] command = dync.generate_dsync_read_packet(0xFE, l_dyn_ids, b_read_startaddr, b_read_len);
for (cycle_nr = 0; cycle_nr < NUMBER_OF_CYCLES_PER_BENCHMARK; cycle_nr++)
{
outer_timer.Reset();
outer_timer.Start();
port.Write(command, 0, command.Length);
run_failed = false;
// get reply
byte[] reply = dync.get_dyn_reply(port, 0, (byte)(Dynamixel1CommandGenerator.DYN1_REPLY_SZ_READ_MIN + l_dyn_ids.Count * b_read_len), 40000, ref bytes_received, ref inner_cycle_time);
outer_timer.Stop();
if (reply != null)
{
min_time_outercycle = Math.Min(min_time_outercycle, outer_timer.ElapsedMicroseconds);
max_time_outercycle = Math.Max(max_time_outercycle, outer_timer.ElapsedMicroseconds);
iteration_times.Add(outer_timer.ElapsedMicroseconds);
if (out_writter != null)
{
out_writter.WriteLine("DSYNC_READ {0} IDs, {1}", l_dyn_ids.Count, outer_timer.ElapsedMicroseconds);
}
}
else
{
// run fails
run_failed = true;
port.ReadExisting(); // purge port
failed_runs++;
if (out_writter != null)
{
out_writter.WriteLine("DSYNC_READ {0} IDs, FAILED", l_dyn_ids.Count);
}
}
Console.Write("\rCycle #{0}: Cycle time {1} uSec. (Fastest so far {2} uSec; slowest so far {3} uSec. Failed runs: {4}) ", cycle_nr, (run_failed ? "FAILED" : outer_timer.ElapsedMicroseconds.ToString()), min_time_outercycle, max_time_outercycle, failed_runs);
}
// test cycle ended. run stats
double avg, std_dev;
if (iteration_times.Count > 1)
{
avg = iteration_times.Average();
std_dev = Math.Sqrt(iteration_times.Average(v => Math.Pow(v - avg, 2)));
}
else
{
avg = 0; std_dev = 0;
}
Console.WriteLine("\rStatistics: ");
Console.WriteLine("\tCycles ran : {0,5}", cycle_nr);
Console.WriteLine("\tCycles failed : {0,5} {1}", failed_runs, (failed_runs > 0 ? "(check if the IDs are all connected and responding)" : ""));
Console.WriteLine("\tFastest Cycle time : {0,5} uSec", min_time_outercycle);
Console.WriteLine("\tSlowest Cycle time : {0,5} uSec", max_time_outercycle);
Console.WriteLine("\tAverage Cycle time : {0,5} uSec", avg);
Console.WriteLine("\tStandard Deviation : {0,5}", std_dev);
Console.WriteLine("\t----------------------");
Console.WriteLine("\tIndividual device benchmarks are not available due to the way DSYNC_READ works");
Console.WriteLine();
}
