Skip to content

Instantly share code, notes, and snippets.

@arthurwolf
Created February 3, 2015 23:46
Show Gist options
  • Save arthurwolf/78b9284191c773140fef to your computer and use it in GitHub Desktop.
Save arthurwolf/78b9284191c773140fef to your computer and use it in GitHub Desktop.
require 'bigdecimal'
DOPLOT= false
TOFILE= false
STEPS_PER_MM= 80.0
STEP_TICKER_FREQUENCY= 100000.0
# set these
distance= 3 # total distance to move for single axis in mm
@acceleration= 5000.0 # mm/sec^2
@nominal_speed= 100.0 # mm/sec
@steps_event_count= (distance*STEPS_PER_MM).round
@nominal_rate= (@steps_event_count*@nominal_speed/distance).ceil
@millimeters= distance # for single axis move the same as distance
@steps_to_move= (distance * STEPS_PER_MM).round
puts "distance: #{distance}mm, acceleration: #{@acceleration}mm/sec^2, nominal speed: #{@nominal_speed}mm/sec"
puts "total steps: #{@steps_event_count}, nominal rate: #{@nominal_rate}steps/sec"
@next_accel_change= 0
@acceleration_change= 0
@next_accel_event= 0
@accelerate_until= 0
@decelerate_after= 0
@acceleration_per_tick= 0
@deceleration_per_tick = 0
@direction= 0
@steps_per_tick= 0
@total_move_ticks= 0
# mm/sec
def calculate_trapezoid(entryspeed, exitspeed)
# Note : went from int to float as we do rounding later
initial_rate= @nominal_rate * (entryspeed / @nominal_speed) # steps/sec
final_rate= @nominal_rate * (exitspeed / @nominal_speed)
# How many steps ( can be fractions of steps, we need very precise values ) to accelerate and decelerate
# Note : went from int to float
acceleration_per_second= (@acceleration * @steps_event_count) / @millimeters # This is a simplification to get rid of rate_delta and get the steps/s² accel directly from the mm/s² accel
maximum_possible_rate = Math.sqrt( ( @steps_event_count * acceleration_per_second ) + ( ( initial_rate**2 + final_rate**2) / 2 ) )
puts "maximum_possible_rate: #{maximum_possible_rate} steps/sec, #{maximum_possible_rate/STEPS_PER_MM} mm/sec"
# Now this is the maximum rate we'll achieve this move, either because it's the higher we can achieve, or because it's the higher we are allowed to achieve
maximum_rate = [maximum_possible_rate, @nominal_rate].min
# Now figure out how long it takes to accelerate in seconds
time_to_accelerate = ( maximum_rate - initial_rate ) / acceleration_per_second
# Now figure out how long it takes to decelerate
time_to_decelerate = ( final_rate - maximum_rate ) / -acceleration_per_second
# Now we know how long it takes to accelerate and decelerate, but we must also know how long the entire move takes so we can figure out how long is the plateau if there is one
plateau_time = 0;
# Only if there is actually a plateau ( we are limited by nominal_rate )
if maximum_possible_rate > @nominal_rate
# Figure out the acceleration and deceleration distances ( in steps )
acceleration_distance = ( ( initial_rate + maximum_rate ) / 2.0 ) * time_to_accelerate
deceleration_distance = ( ( maximum_rate + final_rate ) / 2.0 ) * time_to_decelerate
# Figure out the plateau steps
plateau_distance = @steps_event_count - acceleration_distance - deceleration_distance
# Figure out the plateau time in seconds
plateau_time = plateau_distance / maximum_rate
end
# Figure out how long the move takes total ( in seconds )
total_move_time = time_to_accelerate + time_to_decelerate + plateau_time
puts "total move time: #{total_move_time}s time to accelerate: #{time_to_accelerate}, time to decelerate: #{time_to_decelerate}"
# We now have the full timing for acceleration, plateau and deceleration, yay \o/
# Now this is very important :Â these are in seconds, and we need to round them into ticks.
# This means instead of accelerating in 100.23 ticks we'll accelerate in 100 ticks.
# Which means to reach the exact speed we want to reach, we must figure out a new/slightly different acceleration/deceleration to be sure we accelerate and decelerate at the exact rate we want
# First off round total time, acceleration time and deceleration time in ticks
acceleration_ticks = ( time_to_accelerate * STEP_TICKER_FREQUENCY ).floor
deceleration_ticks = ( time_to_decelerate * STEP_TICKER_FREQUENCY ).floor
total_move_ticks = ( total_move_time * STEP_TICKER_FREQUENCY ).floor
# Now deduce the plateau time for those new values expressed in tick
plateau_ticks = total_move_ticks - acceleration_ticks - deceleration_ticks
# Now we figure out the acceleration value to reach EXACTLY maximum_rate(steps/s) in EXACTLY acceleration_ticks(ticks) amount of time in seconds
acceleration_time = acceleration_ticks / STEP_TICKER_FREQUENCY # This can be moved into the operation bellow, separated for clarity, note :Â we need to do this instead of using time_to_accelerate(seconds) directly because time_to_accelerate(seconds) and acceleration_ticks(seconds) do not have the same value anymore due to the rounding
deceleration_time = deceleration_ticks / STEP_TICKER_FREQUENCY
acceleration_in_steps = ( maximum_rate - initial_rate ) / acceleration_time
deceleration_in_steps = ( maximum_rate - final_rate ) / deceleration_time
# Note :Â we use this value for acceleration as well as for deceleration, if that doesn't work, we can also as well compute the deceleration value this way :
# float deceleration(steps/s²) = ( final_rate(steps/s) - maximum_rate(steps/s) ) / acceleration_time(s);
# and store that in the block and use it for deceleration, which -will- yield better results, but may not be useful. If the moves do not end correctly, try computing this value, adding it to the block, and then using it for deceleration in the step generator
# Now figure out the two acceleration ramp change events in ticks
@accelerate_until = acceleration_ticks
@decelerate_after = total_move_ticks - deceleration_ticks
# Now figure out the acceleration PER TICK, this should ideally be held as a float, even a double if possible as it's very critical to the block timing
# steps/tick^2
#@acceleration_per_tick = BigDecimal.new(acceleration_in_steps, 10)
#@acceleration_per_tick = @acceleration_per_tick / BigDecimal.new(STEP_TICKER_FREQUENCY**2, 10)
@acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY**2
@deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY**2
# We now have everything we need for this block to call a Steppermotor->move method !!!!
# Theorically, if accel is done per tick, the speed curve should be perfect.
# We need this to call move()
@total_move_ticks = total_move_ticks
puts "accelerate_until: #{@accelerate_until}, decelerate_after: #{@decelerate_after}, acceleration_per_tick: #{@acceleration_per_tick}, total_move_ticks: #{@total_move_ticks}"
initial_rate
end
def move( direction, steps, total_move_ticks, accelerate_until, decelerate_after, acceleration_per_tick, initial_rate )
# set direction pin
@direction = direction
@next_accel_change = steps + 1 # Do nothing by default ( cruising/plateau )
@acceleration_change = 0
if accelerate_until != 0 # If the next accel event is the end of accel
@next_accel_event = @accelerate_until
@acceleration_change = acceleration_per_tick
end
if decelerate_after != total_move_ticks && accelerate_until == 0 # If the next accel even is the start of decel ( don't set this if the next accel event is accel end )
@next_accel_event = decelerate_after
@acceleration_change = -deceleration_per_tick
end
@steps_per_tick = initial_rate / STEP_TICKER_FREQUENCY # steps/sec / tick frequency to get steps per tick
puts "acceleration change: #{@acceleration_change}"
end
# sets up the math entry/exit speed are 0
initial_rate= calculate_trapezoid(0, 0)
# sets up the move
move(0, @steps_to_move, @total_move_ticks, @accelerate_until, @decelerate_after, @acceleration_per_tick, initial_rate)
# step ticker interrupt
current_TICK= 0
counter= 0.0
current_step= 0
# for graphing
@xpoints= []
@ypoints= []
while true do
current_TICK+=1
if current_TICK == @next_accel_event
if current_TICK == @accelerate_until # We are done accelerating, decceleration becomes 0 : plateau
@steps_per_tick = @nominal_rate / STEP_TICKER_FREQUENCY # steps/sec / tick frequency to get steps per tick
@acceleration_change = 0
if @decelerate_after < @total_move_ticks
@next_accel_event = @decelerate_after
end
end
if current_TICK == @decelerate_after # We start decelerating
@acceleration_change = -@deceleration_per_tick
@steps_per_tick += -@acceleration_change
end
end
@steps_per_tick += @acceleration_change
if @steps_per_tick <= 0
puts "ERROR: finished too early still have #{@steps_to_move-current_step} steps to go at tick: #{current_TICK}, steps_per_tick: #{@steps_per_tick}"
break
end
counter += @steps_per_tick
if counter >= 1.0
counter -= 1.0
current_step += 1
puts "#{current_TICK}: Do STEP #{current_step}, steps_per_tick: #{@steps_per_tick}, #{@steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM} mm/sec"
# points to plot time vs speed
@xpoints << current_TICK*1000.0/STEP_TICKER_FREQUENCY # time in milliseconds
@ypoints << @steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM
if current_step == @steps_to_move
puts "stepping finished"
current_TICK= 0
break
end
end
end
if DOPLOT
# plot the results
require "gnuplot"
Gnuplot.open do |gp|
Gnuplot::Plot.new( gp ) do |plot|
if TOFILE
plot.terminal "png"
plot.output "trapezoid.png"
end
plot.title "trapezoid: acc #{@acceleration}mm/sec^2 nominal speed #{@nominal_speed} mm/sec distance: #{@distance}"
plot.ylabel "speed mm/sec"
plot.xlabel "time msec"
x = @xpoints
y = @ypoints
plot.data << Gnuplot::DataSet.new( [x, y] ) do |ds|
ds.with = "lines"
ds.title = "speed"
end
# plot.data << Gnuplot::DataSet.new( [x, @xd] ) do |ds|
# ds.with = "lines"
# ds.title = "distance"
# end
end
end
end
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment