// Add a clock time to the queue (flushing the queue if needed) public bool append(double step_clock) { double rel_sc = step_clock + clock_offset; if (!(qnext >= qend || rel_sc >= (double)Stepcompress.CLOCK_DIFF_MAX)) { *qnext++ = last_step_clock_32 + (uint)rel_sc; return(false); } // Call queue_append_slow() to handle queue expansion and integer overflow stepcompress sc = this.sc; ulong old_last_step_clock = sc.last_step_clock; sc.queue_next = qnext; int ret = sc.queue_append_slow(rel_sc); if (ret != 0) { return(ret > 0); } qnext = sc.queue_next; qend = sc.queue_end; last_step_clock_32 = (uint)sc.last_step_clock; clock_offset -= sc.last_step_clock - old_last_step_clock; return(false); }
// Set the conversion rate of 'print_time' to mcu clock public void set_time(double time_offset, double mcu_freq) { int i; for (i = 0; i < sc_list.Count; i++) { stepcompress sc = sc_list[i]; sc.set_time(time_offset, mcu_freq); } }
public bool append_set_next_step_dir(bool sdir) { stepcompress sc = this.sc; ulong old_last_step_clock = sc.last_step_clock; sc.queue_next = qnext; int ret = sc.set_next_step_dir(sdir ? 1 : 0); if (ret != 0) { return(ret > 0); } qnext = sc.queue_next; qend = sc.queue_end; last_step_clock_32 = (uint)sc.last_step_clock; clock_offset -= sc.last_step_clock - old_last_step_clock; return(false); }
// Find and transmit any scheduled steps prior to the given 'move_clock' public int flush(ulong move_clock) { // Flush each stepcompress to the specified move_clock int i; for (i = 0; i < sc_list.Count; i++) { var sc = sc_list[i]; int ret = sc.flush(move_clock); if (ret != 0) { return(ret); } } // Order commands by the reqclock of each pending command List <SerialQueue.RawMessage> msgs = new List <SerialQueue.RawMessage>(); while (true) { // Find message with lowest reqclock ulong req_clock = SerialQueue.MAX_CLOCK; var qm = new SerialQueue.RawMessage(); var scFound = default(stepcompress); bool found = false; for (i = 0; i < sc_list.Count; i++) { stepcompress sc = sc_list[i]; if (sc.msg_queue.Count != 0) { var m = sc.msg_queue.Peek(); if (m.req_clock < req_clock) { found = true; scFound = sc; qm = m; req_clock = m.req_clock; } } } if (!found || (qm.min_clock != 0 && req_clock > move_clock)) { break; } ulong next_avail = move_clocks[0]; if (qm.min_clock != 0) { // The qm.min_clock field is overloaded to indicate that // the command uses the 'move queue' and to store the time // that move queue item becomes available. heap_replace(qm.min_clock); } // Reset the min_clock to its normal meaning (minimum transmit time) qm.min_clock = next_avail; qm = scFound.msg_queue.Dequeue(); // Batch this command msgs.Add(qm); } // Transmit commands if (msgs.Count != 0) { sq.send_batch(cq, msgs.ToArray()); } return(0); }
// Generate step times for a stepper during a move public bool itersolve_gen_steps(ref move m) { stepcompress sc = this.sc; double half_step = 0.5 * step_dist; double mcu_freq = sc.get_mcu_freq(); timepos last = new timepos(0.0, commanded_pos), low = last, high = last; double seek_time_delta = 0.000100; bool sdir = sc.get_step_dir(); queue_append qa = sc.queue_append_start(m.print_time, 0.5); bool ret; while (true) { // Determine if next step is in forward or reverse direction double dist = high.position - last.position; if (Math.Abs(dist) < half_step) { if (high.time >= m.move_t) { // At end of move break; } CalcPosition(ref m, last, ref low, ref high, ref seek_time_delta); continue; } bool next_sdir = dist > 0.0; if (next_sdir != sdir) { // Direction change if (Math.Abs(dist) < half_step + .000000001) { // Only change direction if going past midway point if (high.time >= m.move_t) { // At end of move break; } CalcPosition(ref m, last, ref low, ref high, ref seek_time_delta); continue; } if (last.time >= low.time && high.time > last.time) { // Must seek new low range to avoid re-finding previous time high.time = (last.time + high.time) * .5; high.position = calc_position(ref m, high.time); continue; } ret = qa.append_set_next_step_dir(next_sdir); if (ret) { return(ret); } sdir = next_sdir; } // Find step double target = last.position + (sdir ? half_step : -half_step); timepos next = itersolve_find_step(ref m, ref low, ref high, target); // Add step at given time ret = qa.append(next.time * mcu_freq); if (ret) { return(ret); } seek_time_delta = next.time - last.time; if (seek_time_delta < 0.000000001) { seek_time_delta = 0.000000001; } last.position = target + (sdir ? half_step : -half_step); last.time = next.time; low = next; if (last.time >= high.time) { // The high range is no longer valid - recalculate it if (high.time >= m.move_t) { // At end of move break; } CalcPosition(ref m, last, ref low, ref high, ref seek_time_delta); continue; } } qa.queue_append_finish(); commanded_pos = last.position; return(false); }
public void itersolve_set_stepcompress(ref stepcompress sc, double step_dist) { this.sc = sc; this.step_dist = step_dist; }