latest multi sim

new-accleration-sim-multi.rb

require 'bigdecimal'

DOPLOT_TRAPEZOID= true
DOPLOT_MOVE= false
TOFILE= false

STEPS_PER_MM= 80.0
STEP_TICKER_FREQUENCY= 100000.0

# set these
$xdistance= 1 # total distance to move for single axis in mm
$ydistance= 0 # total distance to move for single axis in mm
$acceleration= 2000.0 # mm/sec^2
$nominal_speed= 100.0 # mm/sec

$xsteps_to_move= ($xdistance * STEPS_PER_MM).round
$ysteps_to_move= ($ydistance * STEPS_PER_MM).round
$steps_event_count= [$xsteps_to_move, $ysteps_to_move].max
$millimeters= Math.sqrt($xdistance**2 + $ydistance**2)
$nominal_rate= ($steps_event_count*$nominal_speed/$millimeters).ceil

puts "xdistance: #{$xdistance}mm, ydistance: #{$ydistance}mm, acceleration: #{$acceleration}mm/sec^2, nominal speed: #{$nominal_speed}mm/sec"
puts "total steps: #{$steps_event_count}, nominal rate: #{$nominal_rate}steps/sec, millimeters moved: #{$millimeters}"

if DOPLOT_TRAPEZOID
  # for graphing
  $xpoints= []
  $ypoints= []
  $txpoints= []
  $typoints= []
elsif DOPLOT_MOVE
  $xpoints= []
  $ypoints= []
end

class Block

  attr_accessor :total_move_ticks, :accelerate_until, :decelerate_after, :acceleration_per_tick, :deceleration_per_tick
  attr_accessor :steps_per_tick, :maximum_rate

  def initialize
  end

  # 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

end

class StepperMotor
  # step ticker interrupt

  def initialize(axis)
    @axis= axis
    @counter= 0.0
    @current_step= 0
    @block= nil
    @acceleration_change= nil
    @moved= false
  end

  def moved?
    @moved
  end

  def pos
    @current_step/STEPS_PER_MM
  end

  def tick(current_TICK)

    @steps_per_tick += @acceleration_change

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

      if current_TICK == @block.decelerate_after # We start decelerating
        @acceleration_change = [email protected]_per_tick*@ratio
      end
    end


    if @steps_per_tick <= 0
      if @counter < 0.9
        puts "#{@axis} - 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}"
        return false
      end
      @counter= 1.0
      steps_per_tick= 0
    end

    @counter += @steps_per_tick

    if @counter >= 1.0
      @counter -= 1.0
      @current_step += 1
      puts "#{@axis} - #{current_TICK}: Do STEP #{@current_step}, steps_per_tick: #{@steps_per_tick}, #{@steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM} mm/sec"

      if DOPLOT_TRAPEZOID
        # points to plot time vs speed
        if @axis == 'X'
          $txpoints << current_TICK*1000.0/STEP_TICKER_FREQUENCY # time in milliseconds
          $xpoints << @steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM
        else
          $typoints << current_TICK*1000.0/STEP_TICKER_FREQUENCY # time in milliseconds
          $ypoints << @steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM
        end
      end

      if @current_step == @steps_to_move
        puts "#{@axis} - stepping finished"
        return false
      end
      @moved= true
    else
      @moved= false
    end
    return true
  end

  def move( direction, steps, initial_rate, ratio, block )
    # set direction pin
    @direction = direction
    @steps_to_move = steps
    @block = block
    @ratio= ratio

    @next_accel_event = block.total_move_ticks + 1 # Do nothing by default ( cruising/plateau )
    @acceleration_change = 0
    if block.accelerate_until != 0 # If the next accel event is the end of accel
      @next_accel_event = block.accelerate_until
      @acceleration_change = block.acceleration_per_tick

    elsif block.accelerate_until == 0 && block.decelerate_after == 0
      # handle case where deceleration is from step 0
      @acceleration_change = -block.deceleration_per_tick

    elsif block.decelerate_after != block.total_move_ticks && block.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 = block.decelerate_after
    end

    @steps_per_tick = (initial_rate*ratio) / STEP_TICKER_FREQUENCY # steps/sec / tick frequency to get steps per tick
    @acceleration_change *= ratio
    puts "#{@axis} - acceleration change: #{@acceleration_change}, ratio: #{ratio}"

    if DOPLOT_TRAPEZOID
      if @axis == 'X'
          $txpoints << 0
          $xpoints << @steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM
      else
          $typoints << 0
          $ypoints << @steps_per_tick*STEP_TICKER_FREQUENCY/STEPS_PER_MM
      end
    end

  end

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, $millimeters)
puts "maxspeed: #{maxspeed}"

xstepper= StepperMotor.new('X')
ystepper= StepperMotor.new('Y')

block= Block.new

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

# sets up the move
primary_axis_move= [$xsteps_to_move, $ysteps_to_move].max
ratio= 1.0/primary_axis_move
xstepper.move(0, $xsteps_to_move, initial_rate, $xsteps_to_move*ratio, block)
ystepper.move(0, $ysteps_to_move, initial_rate, $ysteps_to_move*ratio, block)

# stepticker
current_TICK= 0

xr= true
yr= true
while true do
  current_TICK+=1
  xr= xstepper.tick(current_TICK) if xr
  yr= ystepper.tick(current_TICK) if yr
  break if !xr && !yr

  if DOPLOT_MOVE
    if xstepper.moved? || ystepper.moved?
      $xpoints << xstepper.pos
      $ypoints << ystepper.pos
    end
  end
end


if DOPLOT_TRAPEZOID
  # 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: #{$millimeters}"
      plot.ylabel "speed mm/sec"
      plot.xlabel "time msec"

      plot.data << Gnuplot::DataSet.new( [$txpoints, $xpoints] ) do |ds|
        ds.with = "lines"
        ds.title = "X speed"
      end

      plot.data << Gnuplot::DataSet.new( [$typoints, $ypoints] ) do |ds|
        ds.with = "lines"
        ds.title = "Y speed"
      end

    end
  end

elsif DOPLOT_MOVE

 # plot the results
  require "gnuplot"

  Gnuplot.open do |gp|
    Gnuplot::Plot.new( gp ) do |plot|
      if TOFILE
        plot.terminal "png"
        plot.output "move.png"
      end
      plot.title "move: X #{$xdistance} Y #{$ydistance}"
      plot.ylabel "Y mm"
      plot.xlabel "X mm"

      plot.data << Gnuplot::DataSet.new( [$xpoints, $ypoints] ) do |ds|
        ds.with = "lines"
        ds.title = "move"
      end

    end
  end
end