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(); }