gistfile1.txt

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