bad case

new-accleration-sim.rb

require 'bigdecimal'

DOPLOT= true
TOFILE= false

STEPS_PER_MM= 80.0
STEP_TICKER_FREQUENCY= 100000.0

# set these
distance= 1 # total distance to move for single axis in mm
@acceleration= 2000.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
@maximum_rate = 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}, deceleration_per_tick: #{@deceleration_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, deceleration_per_tick, initial_rate )
  # set direction pin
  @direction = direction

  @next_accel_change = total_move_ticks + 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

  # handle case where deceleration is from step 0
  if accelerate_until == 0 && decelerate_after == 0
    @acceleration_change = -deceleration_per_tick
  elsif 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 = 0 # -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} next_accel_event: #{@next_accel_event}"
end

def max_allowable_speed(acceleration, target_velocity, distance)
  Math.sqrt(target_velocity**2 - 2.0*acceleration*distance)
end

maxspeed= max_allowable_speed(-@acceleration, 0, distance)
puts "maxspeed: #{maxspeed}"

# sets up the math entry/exit speed are 0
initial_rate= calculate_trapezoid(maxspeed, 0.0)

# sets up the move
move(0, @steps_to_move, @total_move_ticks, @accelerate_until, @decelerate_after, @acceleration_per_tick, @deceleration_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

  @steps_per_tick += @acceleration_change

  if current_TICK == @next_accel_event
    if current_TICK == @accelerate_until # We are done accelerating, decceleration becomes 0 : plateau
      @acceleration_change = 0
      if @decelerate_after < @total_move_ticks
        @next_accel_event = @decelerate_after
        if current_TICK != @decelerate_after # We start decelerating
          @steps_per_tick = @maximum_rate / STEP_TICKER_FREQUENCY # steps/sec / tick frequency to get steps per tick
        end
      end
    end

    if current_TICK == @decelerate_after # We start decelerating
      @acceleration_change = -@deceleration_per_tick
    end
  end


  if @steps_per_tick <= 0
    puts "ERROR: finished too fast still have #{@steps_to_move-current_step} steps to go at tick: #{current_TICK}, steps_per_tick: #{@steps_per_tick}, counter: #{counter}"
    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