english version "1.0" identify "xyz" # Copyright (c) 2003-2006 by Wayne C. Gramlich. #, All rights reserved. module steps #: This procedure implements code for explicity stepping a machine tool. import address character command_parse command_types file_set float format in_stream integer logical memory out_stream vector string system time unix_system unix_termios unsigned #: The basic concept behind the steps file format is that a dedicated #, microcontroller generates interrupts at regular intervals. At #, each interrupt interval, the microcontroller can increment, #, decrement, or leave alone each axis -- X, Y, Z, or A. The steps #, software reads in RS-274 code and figures out how what the #, correcmat sequence and time of pulses should be for eaxh axis. #, All of the heavy math is done on the host computer and the #, CNC controller firmware can be quite simple. See the HTML #, documentation file for complete format. #, #, A {chunk} represents a piece of a tool path. The tool path #, can either be a straight line between two points or some #, sort of helical/circular path between two points. In the #, helical/circular case, center axis of the helix or circle #, is specified. Each {chunk} has a total distance that the #, tool traverses between the end points. The {chunk} objects #, are strung together to form the entire tool path. The end #, point of one {chunk} must match the start point of the next #, {chunk}. #, #, A typical chunk is represented by the following velocity versus #, time chart: #, #, v(t) #, ^ #, | | #, | | #, +--------------------------------+ #, | | #, | | #, | chunk | #, | | #, +--------------------------------+--->t #, Velocity vs. Time #, #, In general, chunks are initialized to run at the maximum #, possible velocity. However, if there are two chunks that #, are adjacent to one another but running at different maximum #, velocity, one of the chunks needs to have a velocity ramp up #, or down added to ensure that no attempt is made to have an #, instantaneous change in velocity. Here are a couple of chunks #, that have been spliced together: #, #, v(t) #, ^ #, | | | #, +--------------+ | | #, | \| | #, | +-------------+ #, | chunk 1 | chunk 2 | #, | | | #, +----------------+-------------+--->t #, #, The process of joining two chunks together causes the chunk #, with the ramp to take longer to traverse than a simple flat #, velocity. #, #, Sometimes, three chunks are spliced together such that the #, chunk in the midde needs both a ramp up and a ramp down: #, #, v(t) #, ^ #, | | | | #, | | +--------+ | | #, | | / \ | | #, | |/ \| | #, +-----------+ +----------+ #, | chunk 1 | chunk 2 | chunk 3 | #, | | | | #, +-----------+--------------+----------+----> t #, #, The example above is called a trapazoidal ramp. The #, trapazoidal ramp can represent all the cases that we #, need to. #, #, Sometimes the chunk in the middle is too short and it #, is not posssible to get up to the desired speed before #, it necessary to ramp down again: #, #, v(t) #, ^ #, | | | | #, | | /\ | | #, | | / \ | | #, | |/ \| | #, +-----------+ +----------+ #, | chunk 1 |chk 2 | chunk 3 | #, | | | | #, +-----------+------+----------+----> t #, #, The example above has a symetric ramp up and ramp down. #, An asymetric one is possible as well: #, #, v(t) #, ^ #, | | /\ | | #, | |/ \ | | #, +-----------+ \ | | #, | | \| | #, | | +----------+ #, | chunk 1 |chk 2 | chunk 3 | #, | | | | #, +-----------+------+----------+----> t #, #, Lastly, there is a case where the ramp up or ramp down sequence #, needs to span more than one chunk: #, #, v(t) #, ^ #, | | | | | #, +-------+-+ | | | #, | | \ | | | #, | | \| | | #, | | + | | #, | | |\ | | #, | | | \| | #, | | | +-------+ #, |chunk 1| c2 |c3|chunk 4| #, +-------+----+--+-------+----> t #, #, That wraps up all of the pictures. #, #, Each chunk specifies an input and output direction. If the #, output of one {chunk} is not within approximately ten degrees #, of the input of another {chunk}, the velocity is forced to #, zero between the {chunk} objects. #, #, The distance, velocity, acceleration and time are all #, interrelated by the standard kinimatic equations for Newtonian #, mechanics: #, #, d(t) = 1/2 * a * t^2 + vi * t + di (1) #, #, v(t) = a * t + vi (2) #, #, vf^2 = vi^2 + 2 * a * d (3) #, #, d = 1/2 * (vi + vf) * t (4) #, #, ------------------------------------------------------------------- #, define axis #: One axis of machine record axis_type axis_type #: Type of axis backlash float #: Backlash for axis chunk chunk #: Current chunk direction logical #: {true}=>one way; {false}=>the other label string #: Axis label length float #: Axis Length maximum_velocity float #: Maximum velocity (in units/sec) maximum_acceleration float #: Maximum accel. (in units/sec^2) offset float #: Offset to original origin step integer #: Current step value step_minimum integer #: Minimum recorded step value step_maximum integer #: Maximum recorded step value steps_per_unit unsigned #: Steps per unit time unsigned #: Current time time_steps vector[time_step] #: Time steps queue for axis generate allocate, erase, identical, print define axis_type #: Type of axis enumeration a #: A Axis x #: X Axis y #: Y Axis z #: Z Axis generate equal, print define chunk #: One chunk of tool path record acceleration float #: Maximum allowed acceleration center xpoint #: Point that helix axis goes thru chunk_type chunk_type #: Chunk type debug unsigned #: (For debugging only) distance_ramp_down float #: Ramp down distance (or 0) distance_ramp_up float #: Ramp up distance (or 0) distance_total float #: Total distance in chunk end xpoint #: End point in xpoint #: In direction vector (magnitude = 1) out xpoint #: Out direction vector (magnitude = 1) quadrant unsigned #: Quadrant that helix is in (1-4) start xpoint #: Start point time_ramp_down float #: Time required to ramp down time_ramp_up float #: Time required to ramp up time_total float #: Total chunk traversal time velocity_desired float #: Desired (maximum) velocity velocity_final float #: Ramp down velocity (end) velocity_initial float #: Ramp up velocity (start) generate address_get, allocate, erase, identical, print define chunk_type enumeration line #: Linear segment xy_helix #: Helix/circle around Z axis xz_helix #: Helix/circle around Y axis yz_helix #: Helix/circle around Z axis generate equal, print define machine #: A machine tool record baud_rate unsigned #: Baud rate path string #: Path to machine time_accuracy float #: Time per tick x axis #: X axis y axis #: Y axis z axis #: Z axis a axis #: A axis generate allocate, erase, print define xpoint #: Point record x float #: X coordinate y float #: Y coordinate z float #: Z coordinate generate allocate, erase, identical, print define rs274 record #: RS274 state f float #: Feedrate g float #: G-code i float #: I arc radius center (X axis) j float #: J arc radius center (Y axis) k float #: K arc radius center (Z axis) m float #: M-code n float #: Block number r float #: R arc radius s float #: Spindle speed t float #: Tool number x float #: X coordinate y float #: Y coordinate z float #: Z coordinate generate allocate, erase, print define steps #: Steps record current xpoint #: Current point chunk_enables unsigned #: Number of enabled blocks chunk_after chunk #: Chunk after chunk_before chunk #: Chunk before chunks vector[chunk] #: Chunks buffer array d_register unsigned #: D register delay_current unsigned #: Current delay delay_previous unsigned #: Previous delay direction_mask unsigned #: Direction mask (x=1, y=2, z=4, a=8) file_set file_set #: File set for select() call file_counter unsigned #: A counter for dump files io_stream out_stream #: I/O stream machine machine #: Machine definition maximum_velocity float #: Maximum velocity maximum_acceleration float #: Maximum acceleration mem memory #: Small memory buffer memory string #: Memory buffer for commands port_mode logical #: {true}=>Open communications port repeat logical #: {true}=>Repeat 1st block forever rs274 rs274 #: RS274 state sent_total unsigned #: Total bytes of data sent serial_fd unsigned #: File desc. to talk to controller serial_in_stream in_stream #: Serial port input stream serial_out_stream out_stream #: Serial port output stream steps_stream out_stream #: Steps stream sequential_rapid logical #: {true}=>rapids are axis sequential summary logical #: {true}=>generate summary time_end time #: End time time float #: Current time time_out_seconds unsigned #: Seconds time out for select() time_out_microseconds unsigned #: Microseconds time out for select() time_start time #: Start time time_steps vector[time_step] #: Spare time steps time_previous unsigned #: Previous time trace_bytes logical #: {true}=>trace bytes trace_comment logical #: {true}=>trace RS274 comments trace_g logical #: {true}=>trace RS274 "G"/"M" codes trace_time logical #: {true}=>trace time generate allocate, erase, identical, print define time_step #: Time-step pair record chunk chunk #: Associated chunk time unsigned #: Time step integer #: Step generate allocate, erase, identical, print #: {axis} procedures: procedure create@axis takes axis_type axis_type minimum float maximum float maximum_velocity float maximum_acceleration float label string returns axis #: This procedure will create and return a new {axis} object. zero :@= float_convert@(0) izero :@= integer_convert@(0) initialize axis:: axis := allocate@axis() axis.axis_type := axis_type axis.backlash := zero axis.chunk := ?? axis.direction := false axis.label := label axis.length := zero axis.maximum_velocity := maximum_velocity axis.maximum_acceleration := maximum_acceleration axis.offset := zero axis.step := izero axis.step_minimum := izero axis.step_maximum := izero #axis.steps_per_unit := 20 * 200 * 8 axis.steps_per_unit := 20 * 200 * 2 axis.steps_per_unit := 20 * 200 #axis.steps_per_unit := 50 axis.time := 0 axis.time_steps := allocate@vector[time_step]() return axis procedure steps_compute@axis takes axis axis axis_start float axis_end float time_start float time_accuracy float chunk chunk steps steps returns_nothing #: This procedure computes the (time, step) pairs required to get #, from {axis_start} to {axis_end} for {axis}. system :@= standard@system() debug_stream :@= system.error_out_stream #format@format5[string, float, float, float, chunk](debug_stream, # "=>steps_compute@axis(a:%ds%, as:%f%, ae:%f%, ts:%f%, %c%, *)\n\", # axis.label, axis_start, axis_end, time_start, chunk) # Some constants: ione :@= integer_convert@(1) zero :@= float_convert@(0) two :@= float_convert@(2) pi :@= float_convert@("3.14159265") pi2 :@= two * pi half_pi :@= pi / two # Extract some {axis} fields: axis_type :@= axis.axis_type steps_per_unit :@= float_convert@(axis.steps_per_unit) label :@= axis.label time_accuracy :@= steps.machine.time_accuracy itime_start :@= unsigned_convert@(time_start / time_accuracy) time_end :@= time_start + chunk.time_total itime_end :@= unsigned_convert@(time_end / time_accuracy) # Extract some {chunk} fields: chunk_type :@= chunk.chunk_type distance_total :@= chunk.distance_total start :@= chunk.start start_x :@= start.x start_y :@= start.y start_z :@= start.z end :@= chunk.end end_x :@= end.x end_y :@= end.y end_z :@= end.z center :@= chunk.center center_x :@= center.x center_y :@= center.y center_z :@= center.z quadrant :@= chunk.quadrant # The following fields are used in the {distance} to {time} computation: acceleration :@= chunk.acceleration acceleration_half :@= acceleration / two distance_ramp_down :@= chunk.distance_ramp_down distance_ramp_up :@= chunk.distance_ramp_up velocity_initial :@= chunk.velocity_initial velocity_final :@= chunk.velocity_final velocity_desired :@= chunk.velocity_desired time_ramp_down :@= chunk.time_ramp_down time_ramp_up :@= chunk.time_ramp_up time_total :@= chunk.time_total time_current :@= 0 # Compute some deltas: end_minus_start_x :@= end_x - start_x end_minus_start_y :@= end_y - start_y end_minus_start_z :@= end_z - start_z # Make sure that we round the span of steps towards each other: step_start:: integer := ?? step_end:: integer := ?? direction :@= ione steps_size :@= 0 if axis_start <= axis_end step_start := integer_convert@(ceiling@(axis_start * steps_per_unit)) step_end :@= integer_convert@(floor@(axis_end * steps_per_unit)) steps_size := unsigned_convert@(step_end - step_start + ione) else step_start := integer_convert@(floor@(axis_start * steps_per_unit)) step_end :@= integer_convert@(ceiling@(axis_end * steps_per_unit)) steps_size := unsigned_convert@(step_start - step_end + ione) direction := -ione #format@format3[float, integer, integer](debug_stream, # "spu:%f% ss:%d% se:%d%\n\", # steps_per_unit, step_start, step_end) # Make sure we step through in the correct direction: step_time :@= zero distance :@= zero time_delta :@= zero step :@= step_start step_index :@= 0 loop while step_index < steps_size # Now we need to find the distance through {chunk} that corresponds #, to {step} for {axis}. fraction :@= zero axis_step :@= float_convert@(step) / steps_per_unit #format@format3[unsigned, integer, float](debug_stream, # "[%d%]: step=%d% pos=%f%\n\", step_index, step, axis_step) do_linear:: logical := false switch chunk_type case line do_linear := true case xy_helix # We need to do a linear interpolation of the angle along the arc: switch axis_type case x, y # Compute {start_angle}, {end_angle}, and {total_angle}: dx :@= start_x - center_x dy :@= start_y - center_y radius :@= square_root@(dx * dx + dy * dy) start_angle :@= arc_tangent2@(dy, dx) end_angle :@= arc_tangent2@(end_y - center_y, end_x - center_x) # Make sure that {start_angle} and {end_angle} are in the # correct quadrant: if quadrant = 2 if start_angle < zero start_angle :+= pi2 if end_angle < zero end_angle :+= pi2 else_if quadrant = 3 if start_angle > zero start_angle :-= pi2 if end_angle >= zero end_angle :-= pi2 total_angle :@= start_angle - end_angle if total_angle < zero total_angle := -total_angle if total_angle > half_pi #format@format5[float, float, xpoint, xpoint, # xpoint](debug_stream, # "sa=%f% ea=%f% s=%p% c=%p% e=%p%\n\", # start_angle, end_angle, start, center, end) #format@format4[float, float, float, float](debug_stream, # "dx=%f% dy=%f% ey-cy=%f%, ex-cx=%f%\n\", # dx, dy, end_y - center_y, end_x - center_x) assert false # Now compute {step_angle} and place it into the correct #, quadrant. Use {chunk}.{quadrant} to make life easier: switch axis_type case x # {arc_cosine} returns a result between {pi} and 0 which #, is in quadrants I and II: step_angle :@= safe_arc_cosine@((axis_step - center_x) / radius) if quadrant = 3 || quadrant = 4 # Move {step_angle} into quadrants III and IV: step_angle := -step_angle case y # {arc_sine} returns a result between -{pi}/2 to {pi}/2 #, which is in quadrants I and IV: step_angle :@= safe_arc_sine@((axis_step - center_y) / radius) if quadrant = 2 step_angle := pi - step_angle else_if quadrant = 3 step_angle := -pi - step_angle # Check for consistency: if start_angle >= end_angle # Clockwise: if step_angle > start_angle || step_angle < end_angle #format@format5[axis_type, unsigned, # float, float, float](debug_stream, # "a=%a% q=%d% start=%f% step=%f% end=%f%\n\", # axis_type, quadrant, # start_angle, step_angle, end_angle) #assert false else # Counter clockwise: if step_angle < start_angle || step_angle > end_angle #format@format5[axis_type, unsigned, # float, float, float](debug_stream, # "a=%a% q=%d% start=%f% step=%f% end=%f%\n\", # axis_type, quadrant, # start_angle, step_angle, end_angle) assert false fraction := (step_angle - start_angle) / total_angle if fraction < zero fraction := -fraction if fraction > one fraction := one case z do_linear := true case xz_helix assert false case yz_helix assert false if do_linear # We just do a simple linear interpolation to find the distance: fraction :@= (axis_step - axis_start) / (axis_end - axis_start) # Note, numerator and denominator will will always have the #, same sign, thus {fraction} is always positive: if fraction < zero #format@format4[float, float, float, integer](debug_stream, # "as=%f% stp=%f% ae=%f% dir=%d%\n\", # axis_start, axis_step, axis_end, direction) assert false if fraction < zero fraction := zero else_if fraction >= one fraction := one distance_previous :@= distance distance := fraction * distance_total assert zero <= distance && distance <= distance_total # Now that we {distance}, the time can be computed: #format@format3[float, float, float](debug_stream, # "dt:%f% dru:%f% drd:%f%\n\", # distance_total, distance_ramp_up, distance_ramp_down) #format@form`at3[float, float, float](debug_stream, # "vi:%f% vd:%f% vf:%f%\n\", # velocity_initial, velocity_desired, velocity_final) #format@format5[float, float, float, float, float](debug_stream, # "f=%f% d=%f%, dt=%f%, dru=%f%, drd=%f%\n\", # fraction, distance, distance_total, # distance_ramp_up, distance_ramp_down) step_time :@= zero where :@= "none" if distance < distance_ramp_up # We are in a ramp up section: #, #, The Newtonian distance equation is in effect here: #, #, dist = a * t^2 / 2 + vi * t + di (1) #, #, where di = {axis_start} = 0. Solve for t: #, #, t = quadratic(a/2, vi, di - dist) (2) where := "ramp_up" time_delta :@= positive_quadratic@(acceleration_half, velocity_initial, -distance) step_time := time_start + time_delta #if time_start > step_time || step_time > time_end # format@format4[float, float, float, float](debug_stream, # "time_strt:%f% time_delta:%f% step_time:%f% time_end:%f%\n\", # time_start, time_delta, step_time, time_end) # assert false else_if distance <= distance_total - distance_ramp_down # We are in a flat velocity section: #, #, We can use a simple equation: #, #, dist = vd * t + di (3) #, #, where di = distance_ramp_up + axis_start #, #, t = (dist - di) / vd (4) where := "flat" time_delta :@= (distance - distance_ramp_up) / velocity_desired step_time := time_start + time_ramp_up + time_delta assert time_start <= step_time && step_time <= time_end else_if distance <= distance_total # We are in a ramp down section: #, #, This one is a little trickier since we only know #, the exit velocity. Conceptually, we run the #, Newtonian equations forward and then patch everything #, up aftwards. #, #, The Newtonian distance equation is in effect here: #, #, dist = a * t^2 / 2 + vf * t + di (5) #, #, where di = 0. #, #, Solve for #, t = quadratic(a/2, vf, - dist) (6) where := "ramp_down" time_delta :@= positive_quadratic@(acceleration_half, velocity_final, -(distance_total - distance)) if zero > time_delta || time_delta > time_total format@format2[float, float](debug_stream, "distance_total:%f% distance:%f%\n\", distance_total, distance) format@format1[float](debug_stream, "distance_ramp_down:%f%\n\", distance_ramp_down) format@format2[float, float](debug_stream, "time_delta:%f% time_total:%f%\n\", time_delta, time_total) format@format1[chunk](debug_stream, "chunk: %d%\n\", chunk) assert false assert distance_total >= distance step_time := time_start + time_total - time_delta if time_start > step_time || step_time > time_end format@format1[chunk](debug_stream, "chunk:%c%\n\", chunk) format@format2[float, float](debug_stream, "distance_ramp_up:%f% distance_ramp_down:%f%\n\", distance_ramp_up, distance_ramp_down) format@format3[float, float, float](debug_stream, "velocity_final:%f% distance_total:%d% distance:%f%\n\", velocity_final, distance_total, distance) format@format4[float, float, float, float](debug_stream, "time_strt:%f% time_delta:%f% step_time:%f% time_end:%f%\n\", time_start, time_delta, step_time, time_end) assert false else # We have a problem: assert false time_current := unsigned_convert@(step_time / time_accuracy) #format@format6[string, string, float, unsigned, float, # unsigned](debug_stream, # "time: %s%:%s%: start:%f%(%d%) step:%f%(%d%)\n\", # label, where, # time_start, unsigned_convert@(time_start / time_accuracy), # step_time, unsigned_convert@(step_time / time_accuracy)) #if itime_start > time_current || time_current > itime_end # format@format4[unsigned, unsigned, unsigned, chunk](debug_stream, # "Time problem: start:%d% current:%d% end:%d% chunk:%c%\n\", # itime_start, time_current, itime_end, chunk) # format@format2[unsigned, unsigned](debug_stream, # "step_index:%d% steps_size:%d%\n\", step_index, steps_size) # format@format2[integer, integer](debug_stream, # "step_start:%d% step:%d%\n\", step_start, step) # assert false time_step_append@(axis, time_current, step, chunk, steps, step_start, step_end) step := step + direction step_index :+= 1 #format@format5[string, float, float, float, chunk](debug_stream, # "<=steps_compute@axis(a:%ds%, as:%f%, ae:%f%, ts:%f%, %c%, *)\n\", # axis.label, axis_start, axis_end, time_start, chunk) procedure summary@axis takes axis axis out_stream out_stream returns_nothing #: This procedure will output summary information about {axis} #, to {out_stream}. steps_per_unit :@= float_convert@(axis.steps_per_unit) step_minimum :@= float_convert@(axis.step_minimum) / steps_per_unit step_maximum :@= float_convert@(axis.step_maximum) / steps_per_unit length :@= axis.length hundred :@= float_convert@(100) format@format4[string, float, float, float](out_stream, "%s% Axis: Minimum:%f% Maximum:%f% Travel Used:%f%%%\n\", axis.label, step_minimum, step_maximum, (step_maximum - step_minimum) / length * hundred) procedure time_show@axis takes axis axis machine machine out_stream out_stream returns_nothing #: This procedure will output how much time is required for #, {axis} on {out_stream}. put@(axis.label, out_stream) time_steps :@= axis.time_steps size :@= time_steps.size if size != 0 first :@= time_steps[0] last :@= time_steps[size - 1] first_time :@= first.time last_time :@= last.time time :@= last_time - first_time format@format2[unsigned, float](out_stream, " delta_time:%d%(=%f%) ", time, float_convert@(time) * machine.time_accuracy) else put@(" empty ", out_stream) procedure time_step_append@axis takes axis axis time unsigned step integer chunk chunk steps steps step_start integer step_end integer returns_nothing #: This procedure will enqueue a {time}/{step} pair onto the #, queue of {axis} using {steps}. system :@= standard@system() debug_stream :@= system.error_out_stream #format@format3[axis_type, unsigned, integer](debug_stream, # "=>time_step_append@axis(%a%, %d%, %s%, *, *)\n\", # axis.axis_type, time, step) time_steps :@= axis.time_steps time_steps_size :@= time_steps.size previous_step :@= axis.step previous_time :@= axis.time if axis.chunk !== chunk axis.chunk := chunk #format@format1[chunk](debug_stream, "chunk:%c%\n\", chunk) ione :@= integer_convert@(1) if step = previous_step + ione || step = previous_step - ione initialize time_step:: time_step := time_step_allocate@(steps) time_step.time := time time_step.step := step time_step.chunk := chunk if time_steps.size != 0 && time_steps[time_steps.size - 1].time >= time time_steps_size := time_steps.size previous_time_step :@= time_steps[time_steps_size - 1] format@format3[time_step, time_step, unsigned](debug_stream, "previous:%t% new:%t% time_steps_size:%d%\n\", previous_time_step, time_step, time_steps_size) format@format2[integer, integer](debug_stream, "step_start:%d% step_end:%d%\n\", step_start, step_end) format@format2[chunk, chunk](debug_stream, "previous_chunk:%c%\n\new_chunk:%c%\n\", previous_time_step.chunk, time_step.chunk) assert false append@(time_steps, time_step) if step > axis.step_maximum axis.step_maximum := step else_if step < axis.step_minimum axis.step_minimum := step #if chunk.chunk_type = xy_helix # switch axis.axis_type # case y # put@("\t,t\", debug_stream) # case z # put@("\t,t,t,t\", debug_stream) # format@format3[axis_type, unsigned, float](debug_stream, # "%a%: dt:%d% s:%d%\n\", # axis.axis_type, time - axis.time, # float_convert@(step) / float_convert@(axis.steps_per_unit)) else_if step != previous_step format@format5[integer, integer, unsigned, unsigned, chunk](debug_stream, "ps=%d% ns=%d% pt=%d% nt=%d% chunk=%c%\n\", previous_step, step, previous_time, time, chunk) #assert false axis.step := step axis.time := time procedure time_stamp_release@axis takes axis axis steps steps returns_nothing #: This procedure will release all the {time_stamp} objects #, from {axis} back into {steps}. time_steps :@= axis.time_steps size :@= time_steps.size index :@= 0 loop while index < size time_step :@= time_steps[index] time_step_release@(steps, time_step) index :+= 1 truncate@(time_steps, 0) #: {axis_type} routines: procedure format@axis_type takes axis_type axis_type out_stream out_stream format string offset unsigned returns_nothing #: This procedure will output {axis_type} to {out_stream} using the #, characters {format} starting at {offset} to control formatting. print@(axis_type, out_stream) #: {chunk} routines: procedure create@chunk takes chunk_type chunk_type start xpoint end xpoint in xpoint out xpoint distance_total float acceleration float velocity_desired float center xpoint quadrant unsigned returns chunk #: This procedure will create and return a new {chunk} object. #, {chunk_type} specifies either a linear or helical chunk. #, {start} and {end} specifiy the staring and endling location. #, {in} and {out} specify the corresponding veclocity vectors #, for {start} and {end} respectively. {distance_total} specifies #, the total distance traversed. {acceleration} is the maximum #, acceleration and {velocitiy_desired} is the desired velocity #, of the move. {center} is the center of any helical arc and #, {quadrant} specifies which quadrant (1-4) the arc is in. #, A helical arc is constrained to live in a single quadrant #, for the specified plane. system :@= standard@system() #debug_stream :@= system.error_out_stream #format@format9[chunk_type, # xpoint, xpoint, xpoint, xpoint, float, float, float, # xpoint](debug_stream, # "create@chunk:ct=%c%,s=%p%,e=%p%,i=%p%,o=%p%,d=%f%,a=%f%,v=%f%,c=%p%\n\", # chunk_type, start, end, in, out, # distance_total, acceleration, velocity_desired, center) switch chunk_type case line assert center == ?? case xy_helix, xz_helix, yz_helix assert center !== ?? initialize chunk:: chunk := allocate@chunk() chunk.acceleration := acceleration chunk.center := center chunk.chunk_type := chunk_type chunk.debug := 123 chunk.distance_ramp_up := zero chunk.distance_ramp_down := zero chunk.distance_total := distance_total chunk.end := end chunk.in := in chunk.out := out chunk.quadrant := quadrant chunk.start := start chunk.time_ramp_down := zero chunk.time_ramp_up := zero chunk.time_total := distance_total / velocity_desired chunk.velocity_desired := velocity_desired chunk.velocity_initial := velocity_desired chunk.velocity_final := velocity_desired assert chunk.time_total >= zero return chunk procedure format@chunk takes chunk chunk out_stream out_stream format string offset unsigned returns_nothing #: This procedure will output {chunk} to {out_stream} using the characters #, {format} starting at {offset} to control formatting. format@format3[chunk_type, unsigned, float](out_stream, "(type=%t% dbg=%d% a=%f%", chunk.chunk_type, chunk.debug, chunk.acceleration) format@format3[float, float, float](out_stream, " dt=%f% dru=%f% drd=%f%", chunk.distance_total, chunk.distance_ramp_up, chunk.distance_ramp_down) format@format3[float, float, float](out_stream, " tt=%f% tru=%f% trd=%f%", chunk.time_total, chunk.time_ramp_up, chunk.time_ramp_down) format@format3[float, float, float](out_stream, " vi=%f% vd=%f% vf=%f%", chunk.velocity_initial, chunk.velocity_desired, chunk.velocity_final) format@format4[xpoint, xpoint, xpoint, xpoint](out_stream, " s:%p% e:%p% in:%p% out:%p%", chunk.start, chunk.end, chunk.in, chunk.out) put@(")", out_stream) procedure linear_append@chunk takes steps steps end xpoint velocity_desired float acceleration float indent unsigned returns chunk #: This procedure will create and return a new {chunk} for a #, linear path from {start} to {end} with a desired feed rate #, of {velocity} and a maximum accleration of {accleration}. system :@= standard@system() debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format3[xpoint, xpoint, float](debug_stream, # "=>linear_append@chunk(s:%p%, e:%p%, vd:%f%)\n\", # start, end, velocity_desired) zero :@= float_convert@(0) start :@= steps.current chunk:: chunk := ?? dx :@= end.x - start.x dy :@= end.y - start.y dz :@= end.z - start.z distance_total :@= square_root@(dx * dx + dy * dy + dz * dz) # KLUDGE: The 128K buffer only allows movements of about 1 inch. #, Until that issue is addressed, break motions that are greater #, than 1 inch into smaller chunks. if distance_total > zero nx :@= dx / distance_total ny :@= dy / distance_total nz :@= dz / distance_total in :@= create@xpoint(nx, ny, nz) out :@= in chunk_count :@= unsigned_convert@(distance_total) + 1 if chunk_count = 1 # Only one chunk: chunk := create@chunk(line, start, end, in, out, distance_total, acceleration, velocity_desired, ??, 0) chunk_append@(steps, chunk, indent + 1) else # Multiple chunks: flush@(steps, indent + 1) x :@= start.x y :@= start.y z :@= start.z denominator :@= float_convert@(chunk_count) sub_distance :@= distance_total / denominator format@format2[float, float](debug_stream, "distance_total=%f% sub_distance=%f%\n\", distance_total, sub_distance) index :@= 0 loop while index < chunk_count fraction1 :@= float_convert@(index) / denominator fraction2 :@= float_convert@(index + 1) / denominator point1 :@= create@xpoint(x + dx * fraction1, y + dy * fraction1, z + dz * fraction1) point2 :@= create@xpoint(x + dx * fraction2, y + dy * fraction2, z + dz * fraction2) chunk := create@chunk(line, point1, point2, in, out, sub_distance, acceleration, velocity_desired, ??, 0) #format@format2[unsigned, chunk](debug_stream, # "[%d%]: chunk=%c%\n\", index, chunk) chunk_append@(steps, chunk, indent + 1) flush@(steps, indent + 1) index :+= 1 #pad@(debug_stream, indent) #format@format4[xpoint, xpoint, float, address](debug_stream, # "<=linear_append@chunk(s:%p%, e:%p%, vd:%f%)=>0x%x%\n\", # start, end, velocity_desired, chunk.address) return chunk procedure steps_compute@chunk takes chunk chunk time float steps steps returns float #: This procedure will compute the steps needed for {chunk} and #, append them to {steps}. system :@= standard@system() debug_stream :@= system.error_out_stream #format@format1[float](debug_stream, # "=>steps_compute@chunk(*, %f%, *)\n\", time) start :@= chunk.start end :@= chunk.end start_x :@= start.x start_y :@= start.y start_z :@= start.z end_x :@= end.x end_y :@= end.y end_z :@= end.z machine :@= steps.machine time_accuracy :@= machine.time_accuracy steps_compute@(machine.x, start_x, end_x, time, time_accuracy, chunk, steps) steps_compute@(machine.y, start_y, end_y, time, time_accuracy, chunk, steps) steps_compute@(machine.z, start_z, end_z, time, time_accuracy, chunk, steps) time_after :@= time + chunk.time_total machine :@= steps.machine x_axis :@= machine.x y_axis :@= machine.y z_axis :@= machine.z #format@format4[unsigned, unsigned, unsigned, chunk](debug_stream, # "x_steps:%d% y_steps:%d% z_steps:%d% chunk:%c%\n\", # x_axis.time_steps.size, y_axis.time_steps.size, # z_axis.time_steps.size, chunk) #time_show@(x_axis, machine, debug_stream) #time_show@(y_axis, machine, debug_stream) #time_show@(z_axis, machine, debug_stream) #put@("\n\", debug_stream) time_steps_flush@(steps) memory_flush@(steps) #format@format2[float, float](debug_stream, # "<=steps_compute@chunk(*, %f%, *) => %f%\n\", time, time_after) return time_after procedure tight_turn@chunk takes previous chunk next chunk indent unsigned returns logical #: This procedure will return {true}@{logical} if the turn #, required between {previous} and {next} is too tight to be #, made without bringing the tool velocity down to zero. #, Otherwise, {false}@{logical} is returned. #system :@= standard@system() #debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format2[xpoint, xpoint](debug_stream, # "=>tight_turn@chunk(): po:%p% ni:%p%\n\", # previous.in, next.out) one_eighty :@= float_convert@(180) pi :@= float_convert@("3.14159265") ten :@= float_convert@(10) in :@= next.in out :@= previous.out dot_product :@= dot_product@(out, in) cosine_theta :@= dot_product / (length@(in) * length@(out)) angle :@= arc_cosine@(cosine_theta) * one_eighty / pi result :@= angle > ten #pad@(debug_stream, indent) #format@format3[xpoint, xpoint, logical](debug_stream, # "=>tight_turn@chunk(): po:%p% ni:%p% => %l%\n\", # previous.in, next.out, result) return result procedure velocity_change@chunk takes chunk chunk velocity_initial float velocity_final float indent unsigned returns logical #: This procedure will set the start and end velocity of {chunk} #, {velocity_initial} and {velocity_final} with a maximum acceleration #, of {acclleration}. If either of the velocity's is actually set #, to a low value than requested, {true} is returned; otherwise, #, {false} is returned. #system :@= standard@system() #debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format4[address, float, float, float](debug_stream, # "=>velocity_change@chunk(c:0x%x%, vi:%f%, vf:%f%, a:%f%)\n\", # chunk.address, velocity_initial, velocity_final, acceleration) indent1 :@= indent + 1 #pad@(debug_stream, indent1) #format@format1[chunk](debug_stream, "chunk_before=%c%\n\", chunk) velocity_desired :@= chunk.velocity_desired assert velocity_initial <= velocity_desired assert velocity_final <= velocity_desired # Yank all of the initial values out: two :@= float_convert@(2) acceleration :@= chunk.acceleration acceleration2 :@= acceleration * two distance_ramp_down :@= chunk.distance_ramp_down distance_ramp_up :@= chunk.distance_ramp_up distance_total :@= chunk.distance_total time_ramp_down :@= chunk.time_ramp_down time_ramp_up :@= chunk.time_ramp_up time_total :@= chunk.time_total result:: logical := false assert distance_ramp_up >= zero assert distance_ramp_down >= zero assert distance_ramp_up + distance_ramp_down <= distance_total # The velocity profile is trapazoidal. From {velocity_inital} (vi) we #, ramp up to the desired velocity (vd), hold it flat, then ramp #, down to {velocity_final} (vf). #, #, The time required to ramp from vi to vd is determined from: #, #, vd = a*t + vi (1a) #, vd = a*t + vf (1b) #, #, Solve for t: #, #, t = (vd - vi) / a (2a) #, t = (vd - vf) / a (2b) time_ramp_up :@= (velocity_desired - velocity_initial) / acceleration time_ramp_down :@= (velocity_desired - velocity_final) / acceleration assert time_ramp_up >= zero assert time_ramp_down >= zero #, The distance formula is: #, #, d = (vi + vd)/2 * t (3a) #, d = (vd + vf)/2 * t (3a) #, #, Substitute (2x) into (3x) to get the distance traversed. distance_ramp_up :@= time_ramp_up * (velocity_initial + velocity_desired) / two distance_ramp_down :@= time_ramp_down * (velocity_desired + velocity_final) / two assert distance_ramp_up >= zero assert distance_ramp_down >= zero #pad@(debug_stream, indent1) #format@format5[float, float, float, float, float](debug_stream, # "tru:%f% trd:%f% dru:%f% drd:%f% dt:%f%\n\", # time_ramp_up, time_ramp_down, distance_ramp_up, distance_ramp_down, # distance_total) # Now figure out if we have a flat portion of the trapazoid. debug :@= 0 if distance_ramp_up + distance_ramp_down <= distance_total # This is the simple trapazoid case: #, #, The equation that relates distance to time for a constaant #, velocity is: #, #, d(t) = v * t (4) #, #, Solve for t: #, #, t = d / v (5) debug := 1 distance_flat :@= distance_total - distance_ramp_up - distance_ramp_down time_flat :@= distance_flat / velocity_desired time_total :@= time_ramp_up + time_flat + time_ramp_down #pad@(debug_stream, indent1) #format@format2[float, float](debug_stream, # "trapazoid: time_flat:%f% distance_flat%f%\n\", # time_flat, distance_flat) else #, There is no flat portion of the trapazoid. We need to figure #, out if one the two velocity ramps meet in the middle of the #, chunk somewhere or one ramp dominates the entire chunk. #, #, If we are accelerating from the beginning for the entire #, distance: #, #, vx^2 = vi^2 + 2 * a * d (6a) #, vx^2 = vf^2 + 2 * a * d (6b) #, #, where d is the total distance. Solve for vx: #, #, vxi = sqrt(vi^2 + 2 * a * d) (7a) #, vxf = sqrt(vf^2 + 2 * a * d) (7b) #, #, If vxi < vf, then the entire chunk is dominated by the #, ramp up. Similarly, if vxf < vi, the entire chunk is #, dominated by ramp down. We only have to compute one #, of vxi or vxf depending upon whether vf or vi is lower: if velocity_initial < velocity_final velocity_exit_initial :@= square_root@(velocity_initial * velocity_initial + two * acceleration * distance_total) if velocity_exit_initial <= velocity_final # Ramp-up dominates: #, #, d = (vi + vxi)/2 * t (8) #, #, Solve for t: #, #, t = (2 * d)/(vi + vxi) (9) debug := 2 time_total := (two * distance_total) / (velocity_final + velocity_exit_initial) time_ramp_up := time_total time_ramp_down := zero distance_ramp_up := distance_total distance_ramp_down := zero # We are lowering {velocity_final}: if velocity_final != velocity_exit_initial velocity_final := velocity_exit_initial result := true assert distance_ramp_up >= zero assert distance_ramp_down >= zero assert distance_ramp_up + distance_ramp_down <= distance_total else # The ramps meet in the middle of the chunk somewhere. #, Since vi < vf, we know that we have to accelerate #, from vi to vf. Any distance left over gets split #, evenly between ramping up and ramping down: #, #, vf^2 = vi^2 + 2 * a * d (10) #, #, Solve for d: #, #, d = (vf^2 - vi^2) / (2 * a) (11) debug := 3 distance_ramp_up_partial :@= (velocity_final * velocity_final - velocity_initial * velocity_initial) / acceleration2 distance_remaining :@= distance_total - distance_ramp_up_partial distance_remaining2 :@= distance_remaining / two distance_ramp_up := distance_ramp_up_partial + distance_remaining2 distance_ramp_down := distance_remaining2 assert distance_ramp_up >= zero assert distance_ramp_down >= zero # Now we can compute the peak velocity: #, #, vp^2 = vi^2 + 2 * a * d (12) #, #, where d is the total ramp up distance: #, #, vp = sqrt(vi^2 + 2 * a * d) (13) velocity_peak :@= square_root@(velocity_initial * velocity_initial + acceleration2 * distance_ramp_up) #, Now that we have the peak velocity, we can compute #, the ramp up and down times: #, #, d = (vi + vp)/2 * t (14) #, #, Solve for t: #, #, t = (2 * d) / (vi + vp) (15) time_ramp_up := (two * distance_ramp_up) / (velocity_initial + velocity_peak) time_ramp_down := (two * distance_ramp_down) / (velocity_initial + velocity_peak) time_total := time_ramp_up + time_ramp_down assert distance_ramp_up >= zero assert distance_ramp_down >= zero assert distance_ramp_up + distance_ramp_down <= distance_total else # {velocity_initial} >= {velocity_final}: velocity_exit_final :@= square_root@(velocity_final * velocity_final + acceleration2 * distance_total) if velocity_exit_final <= velocity_initial # Ramp-down dominates: #, #, d = (vf + vxf)/2 * t (16) #, #, Solve for t: #, #, t = (2 * d)/(vf + vxf) (17) debug := 4 time_total := (two * distance_total) / (velocity_final + velocity_exit_final) time_ramp_up := zero time_ramp_down := time_total distance_ramp_up := zero distance_ramp_down := distance_total # We are lowering {velocity_initial}. velocity_initial := velocity_exit_final if velocity_initial != velocity_exit_final velocity_initial := velocity_exit_final result := true assert distance_ramp_up >= zero assert distance_ramp_down >= zero assert distance_ramp_up + distance_ramp_down <= distance_total else # The ramps meet in the middle of the chunk somewhere. #, Since vf <= vi, we know that we have to deccelerate #, from vi to vf. Any distance left over gets split #, evenly between ramping up and ramping down: #, #, vi^2 = vf^2 + 2 * a * d (18) #, #, Solve for d: #, #, d = (vi^2 - vf^2) / (2 * a) (19) debug := 5 distance_ramp_down_partial :@= (velocity_initial * velocity_initial - velocity_final * velocity_final) / acceleration2 assert distance_ramp_down_partial >= zero distance_remaining :@= distance_total - distance_ramp_down_partial distance_remaining2 :@= distance_remaining / two distance_ramp_down := distance_ramp_down_partial + distance_remaining2 distance_ramp_up := distance_remaining2 if distance_ramp_up < zero || distance_ramp_down < zero || distance_ramp_up + distance_ramp_down > distance_total #format@format2[float, float](debug_stream, # "velocity_initial:%f% velocity_final:%f%\n\", # velocity_initial, velocity_final) #format@format1[float](debug_stream, # "distance_total:%f%\n\", distance_total) #format@format1[float](debug_stream, # "distance_ramp_down_partial:%f%\n\", # distance_ramp_down_partial) #format@format1[float](debug_stream, # "distance_remaining:%f%\n\", # distance_remaining) #format@format2[float, float](debug_stream, # "distance_ramp_up:%f% distance_ramp_down:%f%\n\", # distance_ramp_up, distance_ramp_down) assert false # Now we can compute the peak velocity: #, #, vp^2 = vf^2 + 2 * a * d (20) #, #, where d is the total ramp down distance: #, #, vp = sqrt(vf^2 + 2 * a * d) (21) velocity_peak :@= square_root@(velocity_final * velocity_final + acceleration2 * distance_ramp_down) #, Now that we have the peak velocity, we can compute #, the ramp up and down times: #, #, d = (vf + vp)/2 * t (22) #, #, Solve for t: #, #, t = (2 * d) / (vf + vp) (23) time_ramp_up := (two * distance_ramp_up) / (velocity_initial + velocity_peak) time_ramp_down := (two * distance_ramp_down) / (velocity_initial + velocity_peak) time_total := time_ramp_up + time_ramp_down assert distance_ramp_up >= zero assert distance_ramp_down >= zero #assert distance_ramp_up + distance_ramp_down <= distance_total # Fill up the chunk: assert distance_ramp_up >= zero assert distance_ramp_down >= zero #assert distance_ramp_up + distance_ramp_down <= distance_total #if result # format@format1[chunk](debug_stream, # "Chunk before reduction:%c%\n\", chunk) chunk.debug := debug chunk.distance_ramp_down := distance_ramp_down chunk.distance_ramp_up := distance_ramp_up chunk.time_ramp_down := time_ramp_down chunk.time_ramp_up := time_ramp_up chunk.time_total := time_total chunk.velocity_initial := velocity_initial chunk.velocity_final := velocity_final #if result # format@format1[chunk](debug_stream, # "Chunk after reduction:%c%\n,n\", chunk) #pad@(debug_stream, indent1) #format@format1[chunk](debug_stream, # "chunk_after=%c%\n\", chunk) #pad@(debug_stream, indent) #format@format5[address, float, float, float, logical](debug_stream, # "<=velocity_change@chunk(c:0x%x%, vi:%f%, vf:%f%, a:%f%)=>%l%\n\", # chunk.address, velocity_initial, velocity_final, acceleration, # result) return result #: {chunk_type} routines: procedure format@chunk_type takes chunk_type chunk_type out_stream out_stream format string offset unsigned returns_nothing #: This procedure will output {chunk_type} to {out_stream} using the #, characters at in {format} starting at {offset} to control formatting. print@(chunk_type, out_stream) #: {float} routines: procedure safe_arc_cosine@float takes cosine float returns float #: This procedure will return the arc cosine of {cosine}. #, If {cosine} is not in the range -1 through 1, it is forced #, into range. one :@= float_convert@(1) negative_one :@= -one if cosine > one cosine := one else_if cosine < negative_one cosine := one return arc_cosine@(cosine) procedure safe_arc_sine@float takes sine float returns float #: This procedure will return the arc sine of {sine}. #, If {sine} is not in the range -1 through 1, it is forced #, into range. one :@= float_convert@(1) negative_one :@= -one if sine > one sine := one else_if sine < negative_one sine := one return arc_sine@(sine) procedure negative_quadratic@float takes a float b float c float returns float #: This procedure will return the negative quadratic equation #, for {a}, {b}, and {c}. four :@= float_convert@(4) return (-b - square_root@(b * b - four * a * c)) / (a + a); procedure positive_quadratic@float takes a float b float c float returns float #: This procedure will return the positive quadratic equation #, for {a}, {b}, and {c}. four :@= float_convert@(4) return (-b + square_root@(b * b - four * a * c)) / (a + a); #: {machine} routines: procedure create@machine takes x axis y axis z axis a axis returns machine #: This procedure will create and return a new {machine} object #, containing {x}, {y}, and {z}. initialize machine:: machine := allocate@machine() machine.baud_rate := 115200 machine.path := "/dev/ttyUSB0" machine.time_accuracy :@= float_convert@(".00001") #machine.time_accuracy :@= float_convert@(".0001") machine.a := a machine.x := x machine.y := y machine.z := z return machine #: {xpoint} routines: procedure create@xpoint takes x float y float z float returns xpoint #: This procedure will return a point containing {x}, {y}, and {z}. initialize point:: xpoint := allocate@xpoint() point.x := x point.y := y point.z := z return point procedure dot_product@xpoint takes point1 xpoint point2 xpoint returns float #: This procedure will return the dot product of {point1} and {point2}. return point1.x * point2.x + point1.y * point2.y + point1.z * point2.z procedure length@xpoint takes point xpoint returns float #: This procedure will return the length the segement from the origin #, to {point}. return square_root@(dot_product@(point, point)) procedure format@xpoint takes point xpoint out_stream out_stream format string offset unsigned returns_nothing #: This procedure will output {point} to {out_stream} using the #, characters in {format} starting at {offset} to control formatting. format@format3[float, float, float](out_stream, "(%f%:%f%:%f%)", point.x, point.y, point.z) #: {rs274} routines: procedure create@rs274 takes_nothing returns rs274 #: This procedure will create and return a new {rs274} object. zero :@= float_convert@(0) initialize rs274:: rs274 := allocate@rs274() rs274.f := zero rs274.g := zero rs274.i := zero rs274.j := zero rs274.k := zero rs274.m := zero rs274.n := zero rs274.r := zero rs274.s := zero rs274.x := zero rs274.y := zero rs274.z := zero return rs274 procedure process@rs274 takes rs274 rs274 cnc_stream in_stream steps steps configure_only logical skip logical returns string #: This procedure will process {cnc_stream} using {rs274} and {steps}. #, If {configure_only} is {true}, only the configuration information #, from {cnc_stream} is processed. {skip} is {true}@{logical} if #, we are in skip mode skipping forward until the next tool change. #, If the processing pauses before the end of the file, a message #, explaining why is returned; otherwise, ??@{string} is returned. system :@= standard@system() error_stream :@= system.error_out_stream debug_stream :@= system.error_out_stream #put@("=>process@rs274()\n\", debug_stream) zero :@= float_convert@(0) one :@= float_convert@(1) ten :@= float_convert@(10) close_parenthesis :@= ")"[0] sixty :@= float_convert@(60) trace_comment :@= steps.trace_comment trace_g :@= steps.trace_g #format@format2[logical, logical](debug_stream, # "trace_comment:%l% trace_g:%l%\n\", trace_comment, trace_g) # Ready to process: end_of_file :@= character_convert@(0xffffffff) new_line :@= "\n\"[0] open_parenthesis :@= "("[0] dot :@= "."[0] minus :@= "-"[0] left_parenthesis :@= "("[0] right_parenthesis :@= ")"[0] capital_t :@= "T"[0] field_letter:: character := new_line number :@= allocate@string() comment :@= allocate@string() line_number :@= 0 loop # Process a line: line_number :+= 1 #format@format1[unsigned](debug_stream, # "Line %d%\n\", line_number) trim@(number, 0) field_letter :@= new_line loop character :@= character_read@(cnc_stream) while is_white_space@(character) #format@format1[unsigned](debug_stream, # "chr:%d%\n\", unsigned_convert@(character)) until character = end_of_file if character = open_parenthesis # Comment: trim@(comment, 0) loop #format@format1[character](debug_stream, # "%c%", character) character := character_read@(cnc_stream) until character = new_line || character = end_of_file if character != close_parenthesis character_append@(comment, character) if trace_comment format@format1[string](debug_stream, "(%s%)\n\", comment) if sub_string_equal@(comment, 0, 6, "steps:", 0, 6) # We've got some configuration information: machine :@= steps.machine x_axis :@= machine.x y_axis :@= machine.y z_axis :@= machine.z axis:: axis := ?? value :@= -one if sub_string_equal@(comment, 6, 8, "Cycles: ", 0, 8) # We have cycles: delete@(comment, 0, 14) value :@= one / float_convert@(comment) machine.time_accuracy := value else_if sub_string_equal@(comment, 6, 6, "Path: ", 0, 6) # We have path: delete@(comment, 0, 12) string_append@(comment, "") machine.path :@= copy@(comment) else_if sub_string_equal@(comment, 6, 11, "Baud_Rate: ", 0, 11) # We have baud rate: delete@(comment, 0, 17) machine.baud_rate := unsigned_convert@(comment) else_if sub_string_equal@(comment, 6, 13, "Buffer_Size: ", 0, 13) # We have buffer size: delete@(comment, 0, 19) #buffer_resize@(steps, unsigned_convert@(comment)) else_if sub_string_equal@(comment, 6, 4, "Done", 0, 4) # We are done configuring: if configure_only #put@("=>process@rs274(): true\n\", debug_stream) return 'Done processing configuration information' else # Look for Axis information: if sub_string_equal@(comment, 6, 2, "X_", 0, 2) # We have an X axis: axis := x_axis else_if sub_string_equal@(comment, 6, 2, "Y_", 0, 2) # We have an Y axis: axis := y_axis else_if sub_string_equal@(comment, 6, 2, "Z_", 0, 2) # We have an Z axis: axis := z_axis else format@format1[string](debug_stream, "Unrecognized configuration (%s%\n\", comment) if axis !== ?? #format@format1[string](debug_stream, # "Axis configuration (%s%\n\", comment) error:: logical := false if sub_string_equal@(comment, 8, 14, "Acceleration: ", 0, 14) delete@(comment, 0, 22) value := float_convert@(comment) axis.maximum_acceleration := value else_if sub_string_equal@(comment, 8, 10, "Backlash: ", 0, 10) delete@(comment, 0, 18) value := float_convert@(comment) axis.backlash := float_convert@(comment) else_if sub_string_equal@(comment, 8, 11, "Direction: ", 0, 11) delete@(comment, 0, 19) string_append@(comment, "") one_or_zero :@= unsigned_convert@(comment) axis.direction := one_or_zero = 1 else_if sub_string_equal@(comment, 8, 8, "Length: ", 0, 8) delete@(comment, 0, 16) value := float_convert@(comment) axis.length := value else_if sub_string_equal@(comment, 8, 16, "Rapid_Feedrate: ", 0, 16) delete@(comment, 0, 24) value := float_convert@(comment) / sixty axis.maximum_velocity := value else_if sub_string_equal@(comment, 8, 7, "Steps: ", 0, 7) delete@(comment, 0, 15) value := float_convert@(comment) axis.steps_per_unit := unsigned_convert@(value) else format@format1[string](debug_stream, "Bad axis configuration (%s%\n\", comment) axis_update@(steps) #format@format2[float, string](debug_stream, # "value:%f% comment:(%s%\n\", value, comment) #put@("\n\", debug_stream) else_if character = new_line # Blank line: else # Block: tool_change:: logical := false in_comment:: logical := false trim@(comment, 0) loop until character = new_line || character = end_of_file if in_comment if character = right_parenthesis in_comment := false character_append@(comment, character) else if is_letter@(character) # Field letter: field_letter := character if character = capital_t tool_change := true else_if is_digit@(character) || character = dot || character = minus # Number: character_append@(number, character) else_if is_white_space@(character) && number.size != 0 dispatch@(rs274, field_letter, number) trim@(number, 0) else_if character = left_parenthesis # Comment on same line: in_comment := true character_append@(comment, character) character := character_read@(cnc_stream) if number.size != 0 dispatch@(rs274, field_letter, number) trim@(number, 0) if tool_change format@format2[float, string](debug_stream, "Tool change: T%f% %s%\n\", rs274.t, comment) tool_change := false return comment # We now have a new block: #print@(rs274, debug_stream) #put@("\n\", debug_stream) g_code :@= unsigned_convert@(rs274.g) if g_code = 0 # Rapid: if trace_g format@format4[unsigned, float, float, float](debug_stream, "%d%: G0 X%f% Y%f% Z%f%\n\", line_number, rs274.x, rs274.y, rs274.z) if !skip rapid@(steps, rs274.x, rs274.y, rs274.z, 0) else_if g_code = 1 # Linear: if trace_g format@format5[unsigned, float, float, float, float](debug_stream, "%d%: G1 X%f% Y%f% Z%f% F%f%\n\", line_number, rs274.x, rs274.y, rs274.z, rs274.f) if !skip linear_cut@(steps, rs274.x, rs274.y, rs274.z, rs274.f / sixty, 0) else_if g_code = 2 # Clockwise circular: if trace_g format@format8[unsigned, float, float, float, float, float, float, float](debug_stream, "%d%: G2 X%f% Y%f% Z%f% I%f% J%f% K%f% F%f%\n\", line_number, rs274.x, rs274.y, rs274.z, rs274.i, rs274.j, rs274.k, rs274.f) if !skip helix_cut@(steps, rs274.x, rs274.y, rs274.z, rs274.i, rs274.j, rs274.k, true, rs274.f / sixty, 0) else_if g_code = 3 # Counter-clockwise circular: if trace_g format@format8[unsigned, float, float, float, float, float, float, float](debug_stream, "%d%: G3 X%f% Y%f% Z%f% I%f% J%f% K%f% F%f%\n\", line_number, rs274.x, rs274.y, rs274.z, rs274.i, rs274.j, rs274.k, rs274.f) if !skip helix_cut@(steps, rs274.x, rs274.y, rs274.z, rs274.i, rs274.j, rs274.k, false, rs274.f / sixty, 0) else_if g_code = 20 # Do nothing!!! else assert false time_accuracy :@= steps.machine.time_accuracy #buffer_time_minimum :@= # float_convert@(steps.buffer_time_minimum) * time_accuracy #if options.summary # format@format1[float](debug_stream, # "Minimum buffer cycle time: %f%\n\", buffer_time_minimum) one :@= float_convert@(1) ten :@= float_convert@(10) #buffer_size :@= float_convert@(steps.buffer_size) #hundred :@= float_convert@(100) #baud_rate :@= machine.baud_rate #bit_width :@= one / float_convert@(baud_rate) #buffer_fill_time :@= bit_width * ten * buffer_size #put@("=>process@rs274(): false\n\", debug_stream) return ?? procedure dispatch@rs274 takes rs274 rs274 field_letter character number string returns_nothing #: This procedure will fill the field for {field_letter} into #, {rs274) with {number}. value :@= float_convert@(number) if field_letter = "F"[0] rs274.f := value else_if field_letter = "G"[0] rs274.g := value else_if field_letter = "I"[0] rs274.i := value else_if field_letter = "J"[0] rs274.j := value else_if field_letter = "K"[0] rs274.k := value else_if field_letter = "M"[0] rs274.m := value else_if field_letter = "N"[0] rs274.n := value else_if field_letter = "R"[0] rs274.r := value else_if field_letter = "S"[0] rs274.s := value else_if field_letter = "T"[0] rs274.t := value else_if field_letter = "X"[0] rs274.x := value else_if field_letter = "Y"[0] rs274.y := value else_if field_letter = "Z"[0] rs274.z := value else system :@= standard@system() debug_stream :@= system.error_out_stream format@format1[character](debug_stream, "Unrecognized character %c%\n\", field_letter) flush@(debug_stream) assert false #: {steps} routines: procedure byte_read@steps takes steps steps returns unsigned #: This procedure will read the next character from the #, serial port attched to {steps}. If no character arrives #, after the default timeout, 0xffffffff is returned to #, indicate a timeout. return byte_read_timeout@(steps, steps.time_out_seconds, steps.time_out_microseconds) procedure byte_read_timeout@steps takes steps steps seconds unsigned microseconds unsigned returns unsigned #: This procedure will read the next character from the #, serial port attched to {steps}. If no character arrives #, after {seconds} seconds and {microseconds} microseconds, #, 0xffffffff is returned to indicate a timeout. mem :@= steps.mem serial_fd :@= steps.serial_fd read_file_set :@= steps.file_set clear@(read_file_set) enter@(read_file_set, serial_fd) unix_system :@= one_and_only@unix_system() select_result :@= select@(unix_system, read_file_set, ??, ??, seconds, microseconds) assert unix_system.status = ok if select_result = 0 # Timeout: return 0xffffffff # There is supposed to be data: if !is_in@(read_file_set, serial_fd) # This is weird: assert false return 0xffffffff # There is data on the serial amount_read :@= read@(unix_system, serial_fd, mem, 0, 1) assert unix_system.status = ok assert amount_read = 1 return mem[0] procedure byte_show@steps takes byte unsigned d_register unsigned out_stream out_stream returns unsigned #: This procedure will produce a human readable decodding of {byte} #, where the value of the D register coming in is {d_register}. #, The new value of the D register is returned. The output #, is placed on {out_stream}. format@format1[unsigned](out_stream, "0x%x% ", byte) if byte & 0x80 = 0 # 0ddc axyz (Step): hyphen :@= "-"[0] if byte & 0x10 = 0 # Leave D<13:2> alone: d_register := d_register & 0xfffc | (byte >> 5) else # Clear D<13:2>: d_register := byte >> 5 format@format1[unsigned](out_stream, "Step: 0x%x% ", d_register) character :@= hyphen if byte & 8 != 0 character := "A"[0] put@(character, out_stream) character := hyphen if byte & 4 != 0 character := "X"[0] put@(character, out_stream) character := hyphen if byte & 2 != 0 character := "Y"[0] put@(character, out_stream) character := hyphen if byte & 1 != 0 character := "Z"[0] put@(character, out_stream) put@("\n\", out_stream) else # 1xxx xxxx: if byte & 0x40 = 0 # 10xx xxxx: if byte & 0x20 = 0 # 100x xxxx: if byte & 0x10 = 0 #: 1000 aaaa (SetA): d_register := (d_register & 0xc3ff) | (byte & 0xf) << 10 format@format2[unsigned, unsigned](out_stream, "\t,t\SetA %d%: D=0x%x%\n\", byte & 0xf, d_register) else #: 1001 bbbb (SetB): d_register := (d_register & 0xfc3f) | (byte & 0xf) << 6 format@format2[unsigned, unsigned](out_stream, "\t\SetB %d%: D=0x%x%\n\", byte & 0xf, d_register) else # 101x xxxx: if byte & 0x10 = 0 #: 1010 cccc (SetC): d_register := (d_register & 0xffc3) | (byte & 0xf) << 2 format@format2[unsigned, unsigned](out_stream, "SetC %d%: D=0x%x%\n\", byte & 0xf, d_register) else #: 1011 axyz (Set Direction): put@("Direction: ", out_stream) text :@= " -A" if byte & 8 != 0 text := " +A" put@(text, out_stream) text :@= " -X" if byte & 4 != 0 text := " +X" put@(text, out_stream) text :@= " -Y" if byte & 2 != 0 text := " +Y" put@(text, out_stream) text :@= " -Z" if byte & 1 != 0 text := " +Z" put@(text, out_stream) put@("\n\", out_stream) else # 11xx xxxx: if byte & 0x20 = 0 # 110x xxxx: if byte & 0x10 = 0 # 1100 rrrr (Repeat): format@format2[unsigned, unsigned](out_stream, "Repeat %d%: D=0x%x%\n\", byte & 0xf, d_register) else # 1100 xxxx: put@("Reserved:\n\", out_stream) else # 111x xxxx: put@("Reserved:\n\", out_stream) return d_register procedure memory_dump@steps takes steps steps memory string returns_nothing #: This proceudre will dump {memory} out. system :@= standard@system() debug_stream :@= system.error_out_stream steps.file_counter :+= 1 file_name :@= allocate@string() buffer_append@("/tmp/yikes", file_name) buffer_append@(steps.file_counter, file_name) buffer_append@(".dmp", file_name) out_stream :@= open@out_stream(file_name) memory_show@steps(memory, out_stream) close@(out_stream) format@format1[string](debug_stream, "Wrote out file %ds%\n\", file_name) procedure memory_show@steps takes memory string out_stream out_stream returns_nothing #: This procedure will output {memory} to {out_stream}. size :@= memory.size d_register :@= 0xffffffff index :@= 0 loop while index < size byte :@= unsigned_convert@(memory[index]) format@format1[unsigned](out_stream, "[%d%] ", index) d_register :@= byte_show@steps(byte, d_register, out_stream) index :+= 1 procedure checksum_read@steps takes steps steps returns unsigned # This procedure will probe the controller and get the current checksum. # If now checksum is available after {seconds} seconds and system :@= standard@system() debug_stream :@= system.error_out_stream checksum1 :@= 0xffffffff checksum2 :@= 0xffffffff index :@= 0 loop while index < 1 checksum1 := send_receive@(steps, 0xeb) checksum2 := send_receive@(steps, 0xec) index :+= 1 if checksum1 | checksum2 = 0xffffffff format@format2[unsigned, unsigned](debug_stream, "cs1:0x%x% cs2:0x%x%\n\", checksum1, checksum2) return 0xffffffff checksum :@= (checksum2 << 8) | checksum1 format@format1[unsigned](debug_stream, "checksum_read@steps()=>%d%\n\", checksum) return checksum procedure checksum_read_helper@steps takes steps steps checksum unsigned byte_send unsigned receive_mask unsigned returns unsigned #: This procedure will read a checksum nibble. if checksum = 0xffffffff checksum := send_receive@(steps, byte_send) if checksum = 0xffffffff || (checksum & 0xf0) != receive_mask return 0xffffffff checksum :&= 0xf return checksum procedure pointer_read@steps takes steps steps base unsigned returns unsigned # This procedure will probe the controller and get the current pointer. # If now pointer is available after {seconds} seconds and #system :@= standard@system() #debug_stream :@= system.error_out_stream pointer1 :@= 0xffffffff pointer2 :@= 0xffffffff pointer3 :@= 0xffffffff pointer1 := send_receive@(steps, base) pointer2 := send_receive@(steps, base + 1) pointer3 := send_receive@(steps, base + 2) if pointer1 | pointer2 | pointer3 = 0xffffffff return 0xffffffff pointer :@= (pointer3 << 16) | (pointer2 << 8) | pointer1 #format@format1[unsigned](debug_stream, # "pointer_read@steps()=>%d%\n\", pointer) return pointer procedure send_receive@steps takes steps steps byte_send unsigned returns unsigned #: This procedure will send {byte_send} to the controller #, return the response byte. serial_out_stream :@= steps.serial_out_stream put@(character_convert@(byte_send), serial_out_stream) flush@(serial_out_stream) return byte_read@(steps) procedure sleep@steps takes steps steps seconds unsigned microseconds unsigned returns_nothing #: This procedure will pause {seconds} seconds and {microseconds} #, nanoseconds. file_set :@= steps.file_set clear@(file_set) unix_system :@= one_and_only@unix_system() select_result :@= select@(unix_system, file_set, ??, ??, seconds, microseconds) assert unix_system.status = ok assert select_result = 0 procedure close@steps takes steps steps returns_nothing #: This procedure will close {steps}. system :@= standard@system() debug_stream :@= system.error_out_stream #put@("=>close@steps()\n\", debug_stream) sixty :@= float_convert@(60) unix_system:: unix_system := one_and_only@unix_system() flush@(steps, 0) memory_flush@(steps) serial_fd :@= steps.serial_fd if serial_fd != 0xffffffff close@(unix_system, serial_fd) if unix_system.status != ok put@("Problem closing port connection: ", debug_stream) print@(unix_system.status, debug_stream) put@("\n\", debug_stream) # Shut down {io_stream} if appropriate. io_stream :@= steps.io_stream if io_stream !== ?? put@("\n\", io_stream) flush@(io_stream) assert steps.memory.size = 0 if steps.summary #format@format3[float, unsigned, float](debug_stream, # "Buffer (size = %f%) fill time at %d% baud: %f%\n\", # buffer_size, baud_rate, buffer_fill_time) #if buffer_time_minimum >= buffer_fill_time # format@format1[float](debug_stream, # "Projected buffer 'low water' fraction: %f%%%\n\", # ((buffer_time_minimum - buffer_fill_time) / # buffer_fill_time) * hundred) #else # format@format1[float](debug_stream, # "Buffer underun by %f%%%\n\", # ((buffer_fill_time - buffer_time_minimum) / # buffer_fill_time) * hundred) # format@format1[chunk](debug_stream, # "Chunk before: %c%\n\", steps.chunk_before) # format@format1[chunk](debug_stream, # "Chunk after: %c%\n\", steps.chunk_after) machine :@= steps.machine summary@(machine.x, debug_stream) summary@(machine.y, debug_stream) summary@(machine.z, debug_stream) format@format1[float](debug_stream, "Time required: %f% (minute)\n\", steps.time / sixty) #put@("<=close@steps()\n\", debug_stream) procedure cnc_process@steps takes steps steps cnc_stream in_stream configure_only logical skip logical returns string #: This procedure will process {cnc_stream} using {steps}. If #, {configure_only} is {true}, only the configuration information #, in {cnc_stream} will be processed. If {skip} is {true}@{logical}, #, actual processing is skipped over until we reach the next tool #, change. If the processing paused before reaching the end of the #, file, a non-empty string is returned; otherwise, ??@{string} is #, returned. return process@(steps.rs274, cnc_stream, steps, configure_only, skip) procedure memory_flush@steps takes steps steps returns_nothing system :@= standard@system() debug_stream :@= system.error_out_stream #put@("=>memory_flush@steps()\n\", debug_stream) memory :@= steps.memory memory_size :@= memory.size if memory_size != 0 # If {steps_stream} is open, output the buffer to it. steps_stream :@= steps.steps_stream if steps_stream !== ?? format@format1[unsigned](debug_stream, "memory_flush(): %d% bytes flushed\n\", memory_size) put@(memory, steps_stream) format@format1[unsigned](debug_stream, "Send %d% bytes\n\", memory_size) assert unsigned_convert@(memory[memory_size - 1]) = 0xff # Output to the serial port if -p option specified: if steps.port_mode checksum :@= 0 byte :@= 0 serial_out_stream :@= steps.serial_out_stream serial_in_stream :@= steps.serial_in_stream if serial_out_stream == ?? # Attempt to open the serial port: put@("Open serial port\n\", debug_stream) machine :@= steps.machine path :@= machine.path baud_rate :@= machine.baud_rate serial_fd :@= terminal_open@termios(path, baud_rate, false) if serial_fd >= 0xfffffff0 format@format2[string, unsigned](debug_stream, "Unable to open communications port %ds% at %d% baud\n\", path, baud_rate) exit@system(1) # Success: serial_out_stream := descriptor_open@out_stream(serial_fd) serial_in_stream := descriptor_open@in_stream(serial_fd) steps.serial_fd := serial_fd steps.serial_out_stream := serial_out_stream steps.serial_in_stream := serial_in_stream # Flush any stale input: loop byte :@= byte_read_timeout@(steps, 0, 100000) until byte = 0xffffffff format@format1[unsigned](debug_stream, "Flushing 0x%x% as stale data\n\", byte) # Establish communication: loop byte := send_receive@(steps, 0xfe) if byte = 0xffffffff put@('Serial connection time out\n\', debug_stream) else if (byte & 0xc0) = 0 # Reasonable response: chunk_enables :@= byte & 7 chunk_current :@= byte >> 3 if chunk_enables = chunk_current # System is idle: steps.chunk_enables := chunk_enables break put@("#", debug_stream) flush@(debug_stream) #format@format2[unsigned, unsigned](debug_stream, # "Controller is not idle (%d% != %d%)\n\", # chunk_current, chunk_enables) else # Got wierd response: format@format1[unsigned](debug_stream, "Got 0x%x% for Read Chunk Counters\n\", byte) put@("Proper handshake occured\n\", debug_stream) # If in repeat mode, send the repeat command: if steps.repeat assert false #put@(character_convert@(0xee), serial_out_stream) #flush@(serial_out_stream) time_start :@= steps.time_start time_end :@= steps.time_end # Compute the checksum: checksum := 0 memory_size :@= memory.size index :@= 0 loop while index < memory_size checksum :+= unsigned_convert@(memory[index])) index :+= 1 checksum :&= 0xffff memory_dump@(steps, memory) # Get the data block down there: first:: logical := true loop if first first := false else # Do a rollback: loop put@("Doing a rollback\n\", debug_stream) byte := send_receive@(steps, 0xfd) until byte = 0xaa format@format1[unsigned](debug_stream, "Rollback failed; got 0x%x% (not 0xaa)\n\", byte) # Get the data on the move: put@(memory, serial_out_stream) flush@(serial_out_stream) controller_checksum :@= checksum_read@(steps) until checksum = controller_checksum format@format2[unsigned, unsigned](debug_stream, "Checksum (0x%x%) != Controller Checksum (0x%x%)\n\", checksum, controller_checksum) # Commit the block: loop byte :@= send_receive@(steps, 0xfc) until byte = 0xa5 format@format1[unsigned](debug_stream, "Commit failed; got 0x%x% (not 0xf5)\n\", byte) steps.sent_total :+= memory_size # Now fire off the block to be executed: current_set@(time_start) loop chunk_enables :@= steps.chunk_enables byte := send_receive@(steps, 0xf0 | chunk_enables) if byte = 0x81 # Success: steps.chunk_enables := (chunk_enables + 1) & 7 break else format@format1[unsigned](debug_stream, "Increment Chunk failed 0x%x%\n\", byte) assert false # Now poll to ensure that we are done: index := 0 loop sleep@(steps, 0, 100000) byte := send_receive@(steps, 0xfe) if byte = 0xffffffff put@('Serial connection time out\n\', debug_stream) else if (byte & 0xc0) = 0 # Reasonable response: chunk_enables :@= byte & 7 chunk_current :@= byte >> 3 if chunk_enables = chunk_current # System is idle: steps.chunk_enables := chunk_enables break if (chunk_current + 1) & 7 = (chunk_enables & 7) put@("#", debug_stream) flush@(debug_stream) else #format@format2[unsigned, unsigned](debug_stream, # "Controller is not idle (%d% != %d%)\n\", # chunk_current, chunk_enables) else # Got wierd response: format@format1[unsigned](debug_stream, "Got 0x%x% for Read Chunk Counters\n\", byte) index :+= 1 if index & 0xff = 0 memory_in :@= pointer_read@(steps, 0xe5) memory_out :@= pointer_read@(steps, 0xe8) format@format3[unsigned, unsigned, unsigned](debug_stream, "\n\memory_in=0x%x% memory_out=0x%x% total=0x%x%\n\", memory_in, memory_out, steps.sent_total) memory_in :@= pointer_read@(steps, 0xe0) memory_out :@= pointer_read@(steps, 0xe8) format@format3[unsigned, unsigned, unsigned](debug_stream, "\n\memory_in=0x%x% memory_out=0x%x% total=0x%x%\n\", memory_in, memory_out, steps.sent_total) current_set@(time_end) seconds :@= time_end.seconds - time_start.seconds time_end_nanoseconds :@= time_end.nanoseconds time_start_nanoseconds :@= time_start.nanoseconds if time_end_nanoseconds < time_start_nanoseconds seconds :+= 1 time_end_nanoseconds :+= 1000000000 nanoseconds :@= time_end_nanoseconds - time_start_nanoseconds format@format2[unsigned, unsigned](debug_stream, "Time elapsed: %d%.%w9p0d%\n\", seconds, nanoseconds) io_stream :@= steps.io_stream if io_stream !== ?? put@(" ", io_stream) index :@= 0 loop while index < memory_size format@format1[unsigned](io_stream, " %x%", unsigned_convert@(memory[index])) index :+= 1 put@("\n\", io_stream) put@(" \n\", io_stream) trim@(memory, 0) #put@("<=memory_flush@steps()\n\", debug_stream) procedure comment@steps takes steps steps text string returns_nothing #: This procedure will output {text} to {steps}. system :@= standard@system() debug_stream :@= system.error_out_stream format@format1[string](debug_stream, "comment@steps(%ds%)\n\", text) #format@format1[string](steps.step_stream, # 'Comment: %ds%\n\', text) procedure create@steps takes steps_stream out_stream io_stream out_stream trace_g logical trace_comment logical summary logical port_mode logical trace_time logical trace_bytes logical repeat logical sequential_rapid logical returns steps #: This procedure will create and return a new {steps} object: system :@= standard@system() debug_stream :@= system.error_out_stream five :@= float_convert@(5) three :@= float_convert@(3) one :@= float_convert@(1) two :@= float_convert@(2) zero :@= float_convert@(0) izero :@= integer_convert@(0) maximum_acceleration :@= float_convert@(1) maximum_velocity :@= float_convert@(".25") x :@= create@axis(x, -five, five, maximum_velocity, maximum_acceleration, 'X') y :@= create@axis(y, -three, three, maximum_velocity, maximum_acceleration, 'Y') z :@= create@axis(z, -three, one, maximum_velocity, maximum_acceleration, 'Z') a :@= create@axis(a, zero, zero, zero, zero, 'A') machine :@= create@machine(x, y, z, a) initialize steps:: steps := allocate@steps() steps.current := create@xpoint(zero, zero, zero) steps.chunk_after := ?? steps.chunk_before := ?? steps.chunk_enables := 0 steps.chunks := allocate@vector[chunk]() steps.d_register := 0 steps.delay_current := 0 steps.delay_previous := 0 steps.direction_mask := 0xffffffff steps.file_set := create@file_set() steps.file_counter := 0 steps.io_stream := io_stream steps.machine := machine steps.maximum_velocity := minimum@(x.maximum_velocity, minimum@(y.maximum_velocity, z.maximum_velocity)) steps.maximum_acceleration := minimum@(z.maximum_acceleration, minimum@(y.maximum_acceleration, z.maximum_acceleration)) steps.mem := create@memory(10) steps.memory := allocate@string() steps.port_mode := port_mode steps.repeat := repeat steps.rs274 := create@rs274() steps.sequential_rapid := sequential_rapid steps.sent_total := 0 steps.serial_fd := 0xffffffff steps.serial_in_stream := ?? steps.serial_out_stream := ?? steps.steps_stream := steps_stream steps.summary := summary steps.time := zero steps.time_end := allocate@time() steps.time_out_seconds := 2 steps.time_out_microseconds := 0 steps.time_start := allocate@time() steps.time_steps := allocate@vector[time_step]() steps.time_previous := 0 steps.trace_bytes := trace_bytes steps.trace_comment := trace_comment steps.trace_g := trace_g steps.trace_time := trace_time io_stream :@= steps.io_stream if io_stream !== ?? put@("\n\", io_stream) put@(" \n\", io_stream) put@(" ef\n\", io_stream) put@(" \n\", io_stream) return steps procedure chunk_append@steps takes steps steps chunk chunk indent unsigned returns_nothing #: This procedure will append {chunk} to the buffer inside of {steps}. #system :@= standard@system() #debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format1[address](debug_stream, # "=>chunk_append@steps(*, 0x%x%)\n\", chunk.address) indent1 :@= indent + 1 chunks :@= steps.chunks size :@= chunks.size if size != 0 && tight_turn@(chunks[size - 1], chunk, indent1) flush@(steps, indent + 1) append@(steps.chunks, chunk) steps.current := chunk.end #pad@(debug_stream, indent) #format@format1[address](debug_stream, # "<=chunk_append@steps(*, 0x%x%)\n\", chunk.address) procedure flush@steps takes steps steps indent unsigned returns_nothing #: Flush out chunks buffer inside of {steps}. Have the #, tool decellerated to a velocity of zero by the end. system :@= standard@system() debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #put@("=>flush@steps()\n\", debug_stream) indent1 :@= indent + 1 #: Propagate the velocity conditions forward and backward: zero :@= float_convert@(0) chunks :@= steps.chunks size :@= chunks.size again:: logical := true loop while again again := false index :@= 0 loop while index < size chunk :@= chunks[index] #pad@(debug_stream, indent1) #format@format2[unsigned, address](debug_stream, # "Propagate[%d%]: c=0x%x%\n\", index, chunk.address) velocity_initial :@= zero velocity_final :@= zero if index != 0 velocity_initial := chunks[index - 1].velocity_final if index + 1 < size velocity_final := chunks[index + 1].velocity_initial if velocity_initial >= chunk.velocity_initial velocity_initial := chunk.velocity_initial if velocity_final >= chunk.velocity_final velocity_final := chunk.velocity_final if velocity_change@(chunk, velocity_initial, velocity_final, indent1) again := true index :+= 1 # Show all of the {chunk}'s: index := 0 loop while index < size chunk :@= chunks[index] #pad@(debug_stream, indent1) #format@format2[unsigned, chunk](debug_stream, # "SHOW[%d%]:%c%\n\", index, chunk) index :+= 1 # Now compute all of the steps: time :@= steps.time index := 0 loop while index < size chunk :@= chunks[index] time := steps_compute@(chunk, time, steps) index :+= 1 #steps.time := time steps.time := zero steps.time_previous := 0 time_steps_flush@(steps) truncate@(chunks, 0) #pad@(debug_stream, indent) #put@("<=flush@steps()\n\", debug_stream) procedure helix_cut@steps takes steps steps x float y float z float i float j float k float clockwise logical velocity float indent unsigned returns_nothing #: This procedure will cause a circular cut from the current location #, to ({x}, {y}, {z}) at {feed_rate} with a radius center of #, ({i}, {j}, {k}). If {clockwise} is {true}, the cut is clockwise; #, otherwise, it it is counter-clockwise. system :@= standard@system() debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format8[float, # float, float, float, float, float, logical, float](debug_stream, # "=>helix_cut@steps(%f%:%f%:%f%, %f%:%f%:%f%, cw=%l%, v=%f%)\n\", # x, y, z, i, j, k, clockwise, velocity) zero :@= float_convert@(0) two :@= float_convert@(2) eight :@= float_convert@(8) pi :@= float_convert@("3.14159265") pi2 :@= two * pi half_pi :@= pi / two # Print out some common values of pi: #ix :@= 1 #loop # while ix < 8 # format@format2[unsigned, float](debug_stream, # "%d%*pi/8=%f% ", ix, pi * float_convert@(ix) / eight) # ix :+= 1 #put@("\n\", debug_stream) end :@= create@xpoint(x, y, z) center :@= create@xpoint(i, j, k) start :@= steps.current #format@format3[xpoint, xpoint, xpoint](debug_stream, # "start=%p%, center=%p%, end=%p%\n\", start, center, end) # Helical {chunk} objects should not span more than one quadrant #, of the plane orthogonal to the helix center line. Any circular #, cut that spans more than one quardrant needs to be broken up #, into quadrant sized pieces. We start by finding the start angle #, and the end angle (in radians). sx :@= start.x sy :@= start.y sz :@= start.z ex :@= end.x ey :@= end.y ez :@= end.z cx :@= center.x cy :@= center.y cz :@= center.z scx :@= sx - cx scy :@= sy - cy scz :@= sz - cz ecx :@= ex - cx ecy :@= ey - cy ecz :@= ez - cz # Compute {radius}: radius :@= zero helix_height :@= zero chunk_type:: chunk_type := xy_helix switch chunk_type case xy_helix radius := square_root@(ecx * ecx + ecy * ecy) helix_height := ez - sz case xz_helix radius := square_root@(ecx * ecx + ecz * ecz) helix_height := ey - sy case yz_helix radius := square_root@(ecy * ecy + ecz * ecz) helix_height := ex - sx #format@format3[float, float, float](debug_stream, # "radius=%f%, helix_height=%f%\n\", radius, helix_height) # Now compute the start and end angles (in radians): start_angle :@= zero end_angle :@= zero switch chunk_type case xy_helix start_angle := arc_tangent2@(scy, scx) end_angle := arc_tangent2@(ecy, ecx) case xz_helix start_angle := arc_tangent2@(scz, scx) end_angle := arc_tangent2@(ecz, ecx) case yz_helix start_angle := arc_tangent2@(scz, scy) end_angle := arc_tangent2@(ecz, ecy) #format@format2[float, float](debug_stream, # "start_angle=%f% end_angle=%f%\n\", start_angle, end_angle) # Now figure out the total angle: total_angle :@= zero if clockwise if start_angle < end_angle if start_angle <= zero start_angle :+= pi2 else_if end_angle >= zero end_angle :-= pi2 else assert false assert pi2 >= start_angle && start_angle >= end_angle && end_angle >= -pi2 total_angle := start_angle - end_angle else if start_angle > end_angle if start_angle >= zero start_angle :-= pi2 else_if end_angle <= zero end_angle :+= pi2 else assert false assert -pi2 <= start_angle && start_angle <= end_angle && end_angle <= pi2 total_angle := -(start_angle - end_angle) if total_angle <= zero total_angle :@= pi2 assert zero <= total_angle && total_angle <= pi2 arc_distance :@= total_angle * radius total_distance :@= square_root@(arc_distance * arc_distance + helix_height * helix_height) #format@format3[float, float, float](debug_stream, # "total_angle=%f% arc_distance=%f% total_distance=%f%\n\", # total_angle, arc_distance, total_distance) # Now comes the fun of breaking the angle traversed into chunks #, that do not span a quadrant: # We'll be needing these variables below: begin_x :@= zero begin_y :@= zero begin_z :@= zero finish_x :@= zero finish_y :@= zero finish_z :@= zero in_x :@= zero in_y :@= zero in_z :@= zero out_x :@= zero out_y :@= zero out_z :@= zero # We want to span 8 quadrants - four with positive angles and #, four with negative: ione :@= integer_convert@(1) ifour :@= integer_convert@(4) index :@= 0 loop while index < 8 iindex :@= integer_convert@(index) # {begin_angle} and {end_angle} specify a quadrant of angle pi/2: begin_angle :@= zero finish_angle :@= zero quadrant :@= 0 quadrant_overlap:: logical := false if clockwise # First quadrant is {begin_angle}=2*pi to {finish_angle}=3*pi/4, # second quadrant is {begin_angle}=3*pi/4 to {finish_angle}=pi, # third quadrant is {begin_angle}=pi to {finish_angle}=pi/2, and # fourth quadrant is {begin_angle}=pi/2 to {finish_angle}=0: begin_angle :@= float_convert@(ifour - iindex) * half_pi finish_angle :@= float_convert@(ifour - iindex - ione) * half_pi # If {begin_angle} to {finish_angle} does not span the quadrant #, specified by {begin_angle} to {finish_angle}, skip to the #, next quadrant: if start_angle >= finish_angle && end_angle <= begin_angle # Now constrain {begin_angle} and {finish_angle} so that #, they do not extend over arc specified from {start_angle} #, to {end_angle}. if begin_angle > start_angle begin_angle := start_angle if finish_angle < end_angle finish_angle := end_angle quadrant_overlap := true quadrant := unsigned_convert@("\4,3,2,1,4,3,2,1\"[index]) #format@format3[unsigned, float, float](debug_stream, # "[%d%]: begin_angle=%f% finish_angle=%f%\n\", # index, begin_angle, finish_angle) else # First quadrant is {begin_angle}=-2*pi to {finish_angle}=-3*pi/4, # second quadrant is {begin_angle}=-3*pi/4 to {finish_angle}=-pi, begin_angle :@= float_convert@(iindex - ifour) * half_pi finish_angle :@= float_convert@(iindex - ifour + ione) * half_pi if start_angle <= finish_angle && end_angle >= begin_angle # Now constrain {begin_angle} and {finish_angle} so that #, they do not extend over arc specified from {start_angle} #, to {end_angle}. if begin_angle < start_angle begin_angle := start_angle if finish_angle > end_angle finish_angle := end_angle quadrant_overlap := true quadrant := unsigned_convert@("\1,2,3,4,1,2,3,4\"[index]) #format@format3[unsigned, float, float](debug_stream, # "[%d%]: begin_angle=%f% finish_angle=%f%\n\", # index, begin_angle, finish_angle) if quadrant_overlap # Some chunk of this quadrant needs to be output: #format@format5[float, float, float, float, float](debug_stream, # "start=%f% begin=%f% end=%f% finish=%f% total=%f%\n\", # start_angle, begin_angle, end_angle, finish_angle, total_angle) # Figure out the angular fraction for the begin and finish: quadrant_angle :@= begin_angle - finish_angle begin_fraction :@= (start_angle - begin_angle) / total_angle finish_fraction :@= (start_angle - finish_angle) / total_angle if !clockwise quadrant_angle := -quadrant_angle begin_fraction := -begin_fraction finish_fraction := -finish_fraction assert begin_fraction >= zero assert finish_fraction >= zero if clockwise actual_angle :@= begin_angle - finish_angle assert zero <= actual_angle && actual_angle <= half_pi else actual_angle :@= finish_angle - begin_angle assert zero <= actual_angle && actual_angle <= half_pi #format@format3[float, float, float](debug_stream, # "qa:%f% bf:%f% ff:%f%\n\", # quadrant_angle, begin_fraction, finish_fraction) # Only output a chunk if we have more than a sliver of #, {quadrant_angle}: if quadrant_angle > zero switch chunk_type case xy_helix begin_x := cx + cosine@(begin_angle) * radius begin_y := cy + sine@(begin_angle) * radius begin_z := sz + helix_height * begin_fraction finish_x := cx + cosine@(finish_angle) * radius finish_y := cy + sine@(finish_angle) * radius finish_z := sz + helix_height * finish_fraction in_angle :@= pi - begin_angle in_x :@= sine@(in_angle) in_y :@= cosine@(in_angle) in_z :@= zero out_angle :@= pi - finish_angle out_x :@= sine@(out_angle) out_y :@= cosine@(out_angle) out_z :@= zero if !clockwise in_x := -in_x in_y := -in_y out_x := -out_x out_y := -out_y case xz_helix assert false case yz_helix assert false #FIXME: We need to make sure that the requested velocity #, does not exceed the maximum accelleration!!! # Create the quardrant chunk and append it: begin :@= create@xpoint(begin_x, begin_y, begin_z) finish :@= create@xpoint(finish_x, finish_y, finish_z) in :@= create@xpoint(in_x, in_y, in_z) out :@= create@xpoint(out_x, out_y, out_z) #format@format4[xpoint, xpoint, xpoint, xpoint](debug_stream, # "b:%p% f:%p% i:%p% o:%p%\n\", begin, finish, in, out) chunk :@= create@chunk(chunk_type, begin, finish, in, out, quadrant_angle * radius, steps.maximum_acceleration, velocity, center, quadrant) chunk_append@(steps, chunk, indent + 1) index :+= 1 #pad@(debug_stream, indent) #format@format8[float, # float, float, float, float, float, logical, float](debug_stream, # "<=helix_cut@steps(%f%:%f%:%f%, %f%:%f%:%f%, cw=%l%, v=%f%)\n\", # x, y, z, i, j, k, clockwise, velocity) #put@("\n\", debug_stream) procedure linear_cut@steps takes steps steps x float y float z float velocity float indent unsigned returns_nothing #: This procedure will cause a linear cut from the current location #, to ({x}, {y}, {z}) at {feed_rate}. #system :@= standard@system() #debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format4[float, float, float, float](debug_stream, # "=>cut@steps(%f%, %f%, %f%, %f%)\n\", x, y, z, velocity) end :@= create@xpoint(x, y, z) linear_append@chunk(steps, end, velocity, steps.maximum_acceleration, indent + 1) #pad@(debug_stream, indent) #format@format4[float, float, float, float](debug_stream, # "<=cut@steps(%f%, %f%, %f%, %f%)\n\", x, y, z, velocity) procedure rapid@steps takes steps steps x float y float z float indent unsigned returns_nothing #: This procedure will cause the tool to move for the current location #, ({x}, {y}, {z}) at the maximum possible feed rate. #system :@= standard@system() #debug_stream :@= system.error_out_stream #pad@(debug_stream, indent) #format@format3[float, float, float](debug_stream, # "=>rapid@steps(%f%, %f%, %f%)\n\", x, y, z) velocity :@= steps.maximum_velocity maximum_acceleration :@= steps.maximum_acceleration current :@= steps.current if steps.sequential_rapid if current.x != x end :@= create@xpoint(x, current.y, current.z) linear_append@chunk(steps, end, velocity, steps.maximum_acceleration, indent + 1) if current.y != y end :@= create@xpoint(x, y, current.z) linear_append@chunk(steps, end, velocity, steps.maximum_acceleration, indent + 1) if current.z != z end :@= create@xpoint(x, y, z) linear_append@chunk(steps, end, velocity, steps.maximum_acceleration, indent + 1) else end :@= create@xpoint(x, y, z) linear_append@chunk(steps, end, velocity, steps.maximum_acceleration, indent + 1) #pad@(debug_stream, indent) #format@format3[float, float, float](debug_stream, # "<=rapid@steps(%f%, %f%, %f%)\n\", x, y, z) procedure axis_update@steps takes steps steps returns_nothing #: This procedure will update the maximum velocity and acceleration #, for {steps} using the {axis} data structures. system :@= standard@system() debug_stream :@= system.error_out_stream machine :@= steps.machine x_axis :@= machine.x y_axis :@= machine.y z_axis :@= machine.z steps.maximum_velocity :@= minimum@(x_axis.maximum_velocity, minimum@(y_axis.maximum_velocity, z_axis.maximum_velocity)) steps.maximum_acceleration :@= minimum@(x_axis.maximum_acceleration, minimum@(y_axis.maximum_acceleration, z_axis.maximum_acceleration)) #format@format2[float, float](debug_stream, # "max_vel=%f% max_accel=%f%\n\", # steps.maximum_velocity, steps.maximum_acceleration) procedure time_step_allocate@steps takes steps steps returns time_step #: This procedure will return an unused {time_step} object from {steps}. time_steps :@= steps.time_steps size :@= time_steps.size time_step:: time_step := ?? if size = 0 initialize time_step:: time_step := allocate@time_step() time_step.time := 0xffffffff time_step.step := integer_convert@(0x7fffffff) time_step.chunk := ?? return time_step size :-= 1 time_step :@= time_steps[size] truncate@(time_steps, size) return time_step procedure byte_put@steps takes steps steps byte unsigned time unsigned trace logical returns_nothing #: This procedure will output {byte} to {steps} keeping #, track of of how much time is needed. system :@= standard@system() debug_stream :@= system.error_out_stream assert byte <= 0xff #buffer_chunk :@= steps.buffer_chunk #buffer_time :@= steps.buffer_time #buffer_index :@= steps.buffer_index #buffer_size :@= steps.buffer_size #previous_time :@= buffer_time[buffer_index] #previous_chunk :@= buffer_chunk[buffer_index] #previous_index :@= buffer_index #if previous_index = 0 # previous_index := buffer_size - 1 #else # previous_index :-= 1 #xxx :@= buffer_time[previous_index] #if xxx != 0xffffffff # assert time > xxx #buffer_time[buffer_index] := time #buffer_chunk[buffer_index] := chunk #buffer_index :+= 1 #if buffer_index >= buffer_size # buffer_index := 0 #steps.buffer_index := buffer_index #if previous_time != 0xffffffff # #format@format3[unsigned, unsigned, unsigned](debug_stream, # # "[%d%]: before:%d% after:%d%\n\", # # buffer_index, previous_time, time) # # delta_time :@= time - previous_time # # # For debugging purposes only: # #time_accuracy :@= steps.machine.time_accuracy # #baud_rate :@= steps.machine.baud_rate # #buffer_fill_time :@= # # float_convert@(10 * buffer_size) / float_convert@(baud_rate) # #buffer_residence_time :@= float_convert@(delta_time) * time_accuracy # #if buffer_residence_time < buffer_fill_time # # trace := true # # format@format2[float, float](debug_stream, # # "%f% < %f% ", buffer_residence_time, buffer_fill_time) # # if delta_time = 0 # format@format3[unsigned, unsigned, unsigned](debug_stream, # "DELTA_TIME=0: time:%d% previous_time:%d% index:%d%\n\", # time, previous_time, buffer_index) # #if delta_time < steps.buffer_time_minimum # # steps.buffer_time_minimum := delta_time # # steps.chunk_before := previous_chunk # # steps.chunk_after := chunk if trace steps.d_register := byte_show@steps(byte, steps.d_register, debug_stream) character_append@(steps.memory, character_convert@(byte)) procedure time_steps_flush@steps takes steps steps returns_nothing #: This procedure will flush out all of the timed steps in {steps}. system :@= standard@system() debug_stream :@= system.error_out_stream trace_time :@= steps.trace_time #put@("=>time_steps_flush@steps()\n\", debug_stream) memory :@= steps.memory memory_size :@= memory.size trace_time :@= steps.trace_time trace_bytes :@= steps.trace_bytes chunk:: chunk := ?? machine :@= steps.machine x_axis :@= machine.x y_axis :@= machine.y z_axis :@= machine.z x_time_steps :@= x_axis.time_steps y_time_steps :@= y_axis.time_steps z_time_steps :@= z_axis.time_steps x_size :@= x_time_steps.size y_size :@= y_time_steps.size z_size :@= z_time_steps.size # Figure out the minimum time in all of the axes: x_time :@= 0 y_time :@= 0 z_time :@= 0 time_minimum :@= 0xffffffff if x_size != 0 x_time := x_time_steps[0].time format@format2[integer, unsigned](debug_stream, "Initial X step=%d% time=%d%\n\", x_time_steps[0].step, x_time) time_minimum := x_time if y_size != 0 y_time := y_time_steps[0].time format@format2[integer, unsigned](debug_stream, "Initial Y step=%d% time=%d%\n\", y_time_steps[0].step, y_time) if y_time < time_minimum time_minimum := y_time if z_size != 0 format@format2[integer, unsigned](debug_stream, "Initial Z step=%d% time=%d%\n\", z_time_steps[0].step, z_time) z_time := z_time_steps[0].time if z_time < time_minimum time_minimum := z_time #if time_minimum != 0xffffffff # format@format1[unsigned](debug_stream, # "time_start:%d% ", time_minimum) repeat :@= 0 previous_step_mask :@= 0xffffffff time_previous :@= steps.time_previous delay_current :@= steps.delay_current delay_previous :@= steps.delay_previous direction_mask :@= steps.direction_mask x_direction :@= x_axis.direction y_direction :@= y_axis.direction z_direction :@= z_axis.direction x_step :@= x_axis.step y_step :@= y_axis.step z_step :@= z_axis.step chunk_new :@= chunk #: Compute {direction_xor}: direction_xor :@= 0 if x_direction direction_xor :|= 4 if y_direction direction_xor :|= 2 if z_direction direction_xor :|= 1 if time_previous = time_minimum time_previous :-= 1 clear_d:: logical := true x_index :@= 0 y_index :@= 0 z_index :@= 0 loop have_x :@= x_index < x_size have_y :@= y_index < y_size have_z :@= z_index < z_size while have_x || have_y || have_z x_step_new :@= x_step y_step_new :@= y_step z_step_new :@= z_step time_minimum :@= 0xffffffff if have_x x_time_step :@= x_time_steps[x_index] x_time := x_time_step.time x_step_new := x_time_step.step chunk_new := x_time_step.chunk time_minimum := x_time if have_y y_time_step :@= y_time_steps[y_index] y_time := y_time_step.time y_step_new := y_time_step.step chunk_new := y_time_step.chunk if y_time < time_minimum time_minimum := y_time if have_z z_time_step :@= z_time_steps[z_index] z_time := z_time_step.time z_step_new := z_time_step.step chunk_new := z_time_step.chunk if z_time < time_minimum time_minimum := z_time if trace_time if have_x format@format1[unsigned](debug_stream, " xt:%d%", x_time) if have_y format@format1[unsigned](debug_stream, " yt:%d%", y_time) if have_z format@format1[unsigned](debug_stream, " zt:%d%", z_time) put@("\t\", debug_stream) if clear_d # Before we do anything we will set the delay to 4. This #, means A=0, B=0, and C=1. clear_d := false byte_put@(steps, 0x80, time_minimum, trace_bytes) byte_put@(steps, 0x90, time_minimum, trace_bytes) byte_put@(steps, 0xa1, time_minimum, trace_bytes) delay_current := 4 delay_previous := 4 # We know know the minimum time: strobe_x :@= time_minimum = x_time strobe_y :@= time_minimum = y_time strobe_z :@= time_minimum = z_time if strobe_x x_index :+= 1 assert x_index >= x_size || x_time_steps[x_index].time > time_minimum if strobe_y y_index :+= 1 assert y_index >= y_size || y_time_steps[y_index].time > time_minimum if strobe_z z_index :+= 1 assert z_index >= z_size || z_time_steps[z_index].time > time_minimum #if time_minimum <= time_previous # format@format6[unsigned, # unsigned, unsigned, unsigned, unsigned, chunk](debug_stream, # "time_min:%d% time_prev:%d% xi:%d% yi:%d% zi:%d% chk:%c%\n\", # time_minimum, time_previous, # x_index, y_index, z_index, chunk_new) if chunk !== chunk_new chunk := chunk_new # format@format1[chunk](debug_stream, "chunk: %c%\n\", chunk) # format@format3[chunk_type, xpoint, xpoint](debug_stream, # "chunk: %t% s=%p% e=%p%", # chunk.chunk_type, chunk.start, chunk.end) # switch chunk.chunk_type # case xy_helix, xz_helix, yz_helix # format@format2[xpoint, unsigned](debug_stream, # " c:%p% q:%d%", chunk.center, chunk.quadrant) # put@("\n\", debug_stream) # Figure out if we need to set directions: #format@format2[unsigned, unsigned](debug_stream, # "time_minimum:%d% time_previous:%d%\n\", # time_minimum, time_previous) # We have a code to emit: delay :@= time_minimum - time_previous - 1 if delay >= 0xffffffff0 format@format1[unsigned](debug_stream, "delay=0x%x%\n\", delay) format@format3[unsigned, unsigned, unsigned](debug_stream, "xi:%d% yi:%d% zi:%d%\n\", x_index, y_index, z_index) format@format3[unsigned, unsigned, unsigned](debug_stream, "xs:%d% ys:%d% zs:%d%\n\", x_size, y_size, z_size) assert false if trace_time format@format6[unsigned, unsigned,float, integer, integer, integer](debug_stream, "t:%d% dt:%d%(=%f%ms) x:%d% y:%d% z:%d%\n\", time_minimum, delay, float_convert@(delay * 1000) * machine.time_accuracy, x_step_new, y_step_new, z_step_new) new_direction_mask :@= direction_mask & 0xf step_mask :@= 0 if strobe_x if x_step_new < x_step new_direction_mask :&= 0xb step_mask :|= 4 else_if x_step_new > x_step new_direction_mask :|= 4 step_mask :|= 4 if strobe_y if y_step_new < y_step new_direction_mask :&= 0xd step_mask :|= 2 else_if y_step_new > y_step new_direction_mask :|= 2 step_mask :|= 2 if strobe_z if z_step_new < z_step new_direction_mask :&= 0xe step_mask :|= 1 else_if z_step_new > z_step new_direction_mask :|= 1 step_mask :|= 1 if new_direction_mask != direction_mask byte_put@(steps, (new_direction_mask ^ direction_xor ) | 0xb0, time_minimum, trace_bytes) format@format2[unsigned, unsigned](debug_stream, "new_direction_mask: 0x%x% direction_mask: 0x%x%\n\", new_direction_mask, direction_mask) direction_mask := new_direction_mask #format@format2[unsigned, unsigned](debug_stream, # " Direction Mask: 0x%x% => 0x%x%", # direction_mask, new_direction_mask) #direction_mask := new_direction_mask #if delay != 0 # delay :-= 1 #if direction_mask & 4 = 0 # put@(" -X", debug_stream) #else # put@(" +X", debug_stream) #if direction_mask & 2 = 0 # put@(" -Y", debug_stream) #else # put@(" +Y", debug_stream) #if direction_mask & 1 = 0 # put@(" -Z", debug_stream) #else # put@(" +Z", debug_stream) #put@("\n\", debug_stream) set_a :@= (delay_current & 0x3c00) != (delay & 0x3c00) set_b :@= (delay_current & 0x3c0) != (delay & 0x3c0) set_c :@= (delay_current & 0x3c) != (delay & 0x3c) # If {delay} < 4, we can do everything in a single step command: if delay & 0xfffc = 0 step_mask :|= 0x10 set_a := false set_b := false set_c := false if delay > 0x3fff #format@format3[unsigned, unsigned, unsigned](debug_stream, # "delay (0x%x%) > 0x3fff, tm:%d% tp:%d%\n\", # delay, time_minimum, time_previous) if delay >= 0xf0000000 trace_bytes := true if set_a value :@= (delay >> 10) & 0xf if value >= 8 format@format3[unsigned, unsigned, unsigned](debug_stream, "Bogus delay: delay=0x%x% time_min=0x%x% time_prev=0x%x%\n\", delay, time_minimum, time_previous) assert false byte_put@(steps, value | 0x80, time_minimum, trace_bytes) if set_b byte_put@(steps, ((delay >> 6) & 0xf) | 0x90, time_minimum, trace_bytes) if set_c byte_put@(steps, ((delay >> 2) & 0xf) | 0xa0, time_minimum, trace_bytes) byte_put@(steps, ((delay & 3) << 5) | step_mask, time_minimum, trace_bytes) previous_step_mask := step_mask & 0xf delay_current := delay #format@format2[unsigned, unsigned](debug_stream, # "STEP delay=0x%x% mask=0x%x%\n\", # delay, step_mask) time_previous := time_minimum if memory.size != 0 #put@("time_steps_flush@steps(): Writing 0xff into buffer\n\", # debug_stream) byte_put@(steps, 0xff, time_minimum, trace_bytes) # Restore state into data structures: x_axis.step := x_step y_axis.step := y_step z_axis.step := z_step steps.delay_current := delay_current steps.delay_previous := delay_previous steps.time_previous := time_previous steps.direction_mask := direction_mask time_stamp_release@(x_axis, steps) time_stamp_release@(y_axis, steps) time_stamp_release@(z_axis, steps) #if time_minimum != 0xffffffff # format@format1[unsigned](debug_stream, # " time_end:%d%\n\", time_minimum) #put@("<=time_steps_flush@steps()\n\", debug_stream) procedure time_step_release@steps takes steps steps time_step time_step returns_nothing #: This procedure will release {time_step} back into {steps}. time_step.time := 0xffffffff time_step.step := integer_convert@(0x7fffffff) time_step.chunk := ?? append@(steps.time_steps, time_step) #: {time_step} procedures: procedure format@time_step takes time_step time_step out_stream out_stream format string offset unsigned returns_nothing #: This procedure will output {time_step} to {out_stream} using #, the characters in {format} starting at {offset} to control formatting. format@format2[unsigned, integer](out_stream, "{time:%d% step:%d%)", time_step.time, time_step.step)