#!/usr/bin/env python

import pandas as pd
import matplotlib.pyplot as plt
import math
import sys
import numpy as np

from scipy.interpolate import interp1d
from scipy import signal
from matplotlib.widgets import MultiCursor

filename = 'analysis.csv'

axes = {
  'X' : { 
    'index': 0,
    'mm_per_step': 1/100.0,    
    'visible': True,
    'result': dict()
    },
  'Y' : { 
    'index': 1,
    'mm_per_step': 1/100.0,
    'visible': True,
    'result': dict()
    },
  'Z' : { 
    'index': 2,
    'mm_per_step': 1/400,
    'visible': True,
    'result': dict()
    }
}

visible_axes = {k:v for k,v in axes.items() if v['visible']}

show_motion = True

fs = 5000.0  # (Re-)sampling frequency

def filter(i, fs, fc):
  w = fc / (fs / 2) # Normalize the frequency
  b, a = signal.butter(3, w, 'low')
  return signal.filtfilt(b, a, i)

def savgol_filter(i):
  return signal.savgol_filter(i, 3, 1)

def velocity(x, y):
  return np.append([0], np.diff(y)*fs) 

def acceleration(x, y):
  v = np.append([0], np.diff(y)*fs)
  a = np.append([0], np.diff(v)*fs)
  return a

def maxabs(a):
  return max(np.max(a), abs(np.min(a)))

def calc_axis(axis_data, time, axis_name, time_start, time_end, fs):
  t,p,s,n = zip(*axis_data)
  
  pos = interp1d(t, p, kind='nearest', bounds_error=False, fill_value='extrapolate')(time)  # OK

  vel = velocity(t, pos)
  acc = acceleration(t, pos)

  vel_filtered = filter(vel, fs, 200)
  acc_filtered = filter(acc, fs, 50)

  vel_max = maxabs(vel_filtered)
  acc_max = maxabs(acc_filtered)

  pulse_min = np.min(s)*1000*1000
  pulse_avg = np.mean(s)*1000*1000
  pulse_max = np.max(s)*1000*1000
  
  if len(s) > 0:
    print ('[Axis {}]: Pulse width (min, avg, max): {:.3f}us, {:.3f}us, {:.3f}us. Max velocity = {:f}. Max acceleration = {:f}'.format(axis_name, pulse_min, pulse_avg, pulse_max, vel_max, acc_max))

  steps = []
  for step_time, step_width, step_dir in zip(t, s, n):
    steps.append([step_time, step_dir])
    steps.append([step_time+step_width, 0])

  return { 'pos': pos, 'vel': vel, 'vel_filtered': vel_filtered, 'acc': acc, 'acc_filtered': acc_filtered, 'steps': np.array(steps), 'vel_max': vel_max, 'acc_max': acc_max, 'pulse_min': pulse_min, 'pulse_avg': pulse_avg, 'pulse_max' : pulse_max }

def extract_data(data, timeIdx, stepIdx, dirIdx, mm_per_step):
    position = 0
    pulseStart = None

    for (t, s, d) in zip(data[:,timeIdx], data[:,stepIdx], data[:,dirIdx]):        
      if s:
        if not pulseStart:
          pulseStart = t
          step = -1 if d else 1
          position += mm_per_step * step
      else:
        if pulseStart != None:
          yield (pulseStart, position, t-pulseStart, step)
          pulseStart = None

print ('Loading dataset...')
dtype_dict = {'Time[s]':'float64','X STEP':'bool', 'X DIR':'bool','Y STEP':'bool','Y DIR':'bool','Z STEP':'bool','Z DIR':'bool'}

dataFrame = pd.read_csv(filename, delimiter = ',', skipinitialspace=True, header=0)

data = dataFrame.get_values()

print ('Sample count =', len(data))

time_start = data[0][0]
time_end = data[-1][0]

reg_t = np.linspace(time_start, time_end, int((time_end - time_start) // (1/fs) + 1))
print ('Interpolated steps =', len(reg_t))

for axis_name, axis in visible_axes.items():
  axis['result'] = calc_axis(extract_data(data, 0, axis['index']*2+1, axis['index']*2+2, axis['mm_per_step']), reg_t, axis_name, time_start, time_end, fs)

can_show_motion = show_motion and axes['X']['result'] and axes['Y']['result']

rows = 4
cols = (3 if can_show_motion else 2)

figure = plt.figure('Stepper Motion analys')

# POSITION
plot_pos = plt.subplot2grid((rows, cols), (0, 0), colspan = 2)
plot_pos.set_ylabel('position ($mm$)')
plot_pos.set_xlabel('time ($s$)')


# VELOCITY
plot_vel = plt.subplot2grid((rows, cols), (1, 0), colspan = 2, sharex=plot_pos)
plot_vel.set_ylabel('velocity ($mm/s$)')
plot_vel.set_xlabel('time ($s$)')


# ACCELERATION
plot_acc = plt.subplot2grid((rows, cols), (2, 0), colspan = 2, sharex=plot_pos)
plot_acc.set_ylabel('acceleration ($mm/s^2$)')
plot_acc.set_xlabel('time ($s$)')


# STEPS
plot_steps = plt.subplot2grid((rows, cols), (3, 0), colspan = 2, sharex=plot_pos)
plot_steps.set_ylim(-2, 2)
plot_steps.set_ylabel('steps')
plot_steps.set_xlabel('time ($s$)')


# Plot motion
if can_show_motion:
  plot_graph = plt.subplot2grid((rows, cols), (1, 2), colspan = 1, rowspan = 2)
  plt.step(axes['X']['result']['pos'], axes['Y']['result']['pos'])
  plot_graph.set_ylabel('motion ($y$)')
  plot_graph.set_xlabel('motion ($x$)')

props = dict(boxstyle='round', facecolor='wheat', alpha=0.25)

# Plot visible axes
for axis_name, axis in visible_axes.items():
    result = axis['result']
    plot_pos.step(reg_t, result['pos'], label=axis_name)  
    # plot_vel.plot(reg_t, result['vel'], label=axis_name)
    plot_vel.plot(reg_t, result['vel_filtered'], label=axis_name)
    # plot_acc.plot(reg_t, result['acc'], label=axis_name)
    plot_acc.plot(reg_t, result['acc_filtered'], label=axis_name)
    plot_steps.step(result['steps'][:,0], result['steps'][:,1], label=axis_name, where='post')

plot_pos.legend()
plot_pos.grid(True)

plot_vel.legend()
plot_vel.grid(True)

plot_acc.legend()
plot_acc.grid(True)

plot_steps.legend()
plot_steps.grid(True)

plt.subplots_adjust(hspace=0.38, wspace=0.15, left=0.05, top=0.95, bottom=0.05, right=0.98)

multi = MultiCursor(figure.canvas, (plot_pos, plot_vel, plot_acc, plot_steps), useblit=True, color='r', lw=1)

plt.show()

a3f27aa3327e00a79c62e5b284b11f7f704d99e3
<mask token>


def acceleration(x, y):
    v = np.append([0], np.diff(y) * fs)
    a = np.append([0], np.diff(v) * fs)
    return a


def maxabs(a):
    return max(np.max(a), abs(np.min(a)))


<mask token>

<mask token>


def filter(i, fs, fc):
    w = fc / (fs / 2)
    b, a = signal.butter(3, w, 'low')
    return signal.filtfilt(b, a, i)


def savgol_filter(i):
    return signal.savgol_filter(i, 3, 1)


def velocity(x, y):
    return np.append([0], np.diff(y) * fs)


def acceleration(x, y):
    v = np.append([0], np.diff(y) * fs)
    a = np.append([0], np.diff(v) * fs)
    return a


def maxabs(a):
    return max(np.max(a), abs(np.min(a)))


def calc_axis(axis_data, time, axis_name, time_start, time_end, fs):
    t, p, s, n = zip(*axis_data)
    pos = interp1d(t, p, kind='nearest', bounds_error=False, fill_value=
        'extrapolate')(time)
    vel = velocity(t, pos)
    acc = acceleration(t, pos)
    vel_filtered = filter(vel, fs, 200)
    acc_filtered = filter(acc, fs, 50)
    vel_max = maxabs(vel_filtered)
    acc_max = maxabs(acc_filtered)
    pulse_min = np.min(s) * 1000 * 1000
    pulse_avg = np.mean(s) * 1000 * 1000
    pulse_max = np.max(s) * 1000 * 1000
    if len(s) > 0:
        print(
            '[Axis {}]: Pulse width (min, avg, max): {:.3f}us, {:.3f}us, {:.3f}us. Max velocity = {:f}. Max acceleration = {:f}'
            .format(axis_name, pulse_min, pulse_avg, pulse_max, vel_max,
            acc_max))
    steps = []
    for step_time, step_width, step_dir in zip(t, s, n):
        steps.append([step_time, step_dir])
        steps.append([step_time + step_width, 0])
    return {'pos': pos, 'vel': vel, 'vel_filtered': vel_filtered, 'acc':
        acc, 'acc_filtered': acc_filtered, 'steps': np.array(steps),
        'vel_max': vel_max, 'acc_max': acc_max, 'pulse_min': pulse_min,
        'pulse_avg': pulse_avg, 'pulse_max': pulse_max}


def extract_data(data, timeIdx, stepIdx, dirIdx, mm_per_step):
    position = 0
    pulseStart = None
    for t, s, d in zip(data[:, timeIdx], data[:, stepIdx], data[:, dirIdx]):
        if s:
            if not pulseStart:
                pulseStart = t
                step = -1 if d else 1
                position += mm_per_step * step
        elif pulseStart != None:
            yield pulseStart, position, t - pulseStart, step
            pulseStart = None


<mask token>

<mask token>


def filter(i, fs, fc):
    w = fc / (fs / 2)
    b, a = signal.butter(3, w, 'low')
    return signal.filtfilt(b, a, i)


def savgol_filter(i):
    return signal.savgol_filter(i, 3, 1)


def velocity(x, y):
    return np.append([0], np.diff(y) * fs)


def acceleration(x, y):
    v = np.append([0], np.diff(y) * fs)
    a = np.append([0], np.diff(v) * fs)
    return a


def maxabs(a):
    return max(np.max(a), abs(np.min(a)))


def calc_axis(axis_data, time, axis_name, time_start, time_end, fs):
    t, p, s, n = zip(*axis_data)
    pos = interp1d(t, p, kind='nearest', bounds_error=False, fill_value=
        'extrapolate')(time)
    vel = velocity(t, pos)
    acc = acceleration(t, pos)
    vel_filtered = filter(vel, fs, 200)
    acc_filtered = filter(acc, fs, 50)
    vel_max = maxabs(vel_filtered)
    acc_max = maxabs(acc_filtered)
    pulse_min = np.min(s) * 1000 * 1000
    pulse_avg = np.mean(s) * 1000 * 1000
    pulse_max = np.max(s) * 1000 * 1000
    if len(s) > 0:
        print(
            '[Axis {}]: Pulse width (min, avg, max): {:.3f}us, {:.3f}us, {:.3f}us. Max velocity = {:f}. Max acceleration = {:f}'
            .format(axis_name, pulse_min, pulse_avg, pulse_max, vel_max,
            acc_max))
    steps = []
    for step_time, step_width, step_dir in zip(t, s, n):
        steps.append([step_time, step_dir])
        steps.append([step_time + step_width, 0])
    return {'pos': pos, 'vel': vel, 'vel_filtered': vel_filtered, 'acc':
        acc, 'acc_filtered': acc_filtered, 'steps': np.array(steps),
        'vel_max': vel_max, 'acc_max': acc_max, 'pulse_min': pulse_min,
        'pulse_avg': pulse_avg, 'pulse_max': pulse_max}


def extract_data(data, timeIdx, stepIdx, dirIdx, mm_per_step):
    position = 0
    pulseStart = None
    for t, s, d in zip(data[:, timeIdx], data[:, stepIdx], data[:, dirIdx]):
        if s:
            if not pulseStart:
                pulseStart = t
                step = -1 if d else 1
                position += mm_per_step * step
        elif pulseStart != None:
            yield pulseStart, position, t - pulseStart, step
            pulseStart = None


print('Loading dataset...')
<mask token>
print('Sample count =', len(data))
<mask token>
print('Interpolated steps =', len(reg_t))
for axis_name, axis in visible_axes.items():
    axis['result'] = calc_axis(extract_data(data, 0, axis['index'] * 2 + 1,
        axis['index'] * 2 + 2, axis['mm_per_step']), reg_t, axis_name,
        time_start, time_end, fs)
<mask token>
plot_pos.set_ylabel('position ($mm$)')
plot_pos.set_xlabel('time ($s$)')
<mask token>
plot_vel.set_ylabel('velocity ($mm/s$)')
plot_vel.set_xlabel('time ($s$)')
<mask token>
plot_acc.set_ylabel('acceleration ($mm/s^2$)')
plot_acc.set_xlabel('time ($s$)')
<mask token>
plot_steps.set_ylim(-2, 2)
plot_steps.set_ylabel('steps')
plot_steps.set_xlabel('time ($s$)')
if can_show_motion:
    plot_graph = plt.subplot2grid((rows, cols), (1, 2), colspan=1, rowspan=2)
    plt.step(axes['X']['result']['pos'], axes['Y']['result']['pos'])
    plot_graph.set_ylabel('motion ($y$)')
    plot_graph.set_xlabel('motion ($x$)')
<mask token>
for axis_name, axis in visible_axes.items():
    result = axis['result']
    plot_pos.step(reg_t, result['pos'], label=axis_name)
    plot_vel.plot(reg_t, result['vel_filtered'], label=axis_name)
    plot_acc.plot(reg_t, result['acc_filtered'], label=axis_name)
    plot_steps.step(result['steps'][:, 0], result['steps'][:, 1], label=
        axis_name, where='post')
plot_pos.legend()
plot_pos.grid(True)
plot_vel.legend()
plot_vel.grid(True)
plot_acc.legend()
plot_acc.grid(True)
plot_steps.legend()
plot_steps.grid(True)
plt.subplots_adjust(hspace=0.38, wspace=0.15, left=0.05, top=0.95, bottom=
    0.05, right=0.98)
<mask token>
plt.show()

<mask token>
filename = 'analysis.csv'
axes = {'X': {'index': 0, 'mm_per_step': 1 / 100.0, 'visible': True,
    'result': dict()}, 'Y': {'index': 1, 'mm_per_step': 1 / 100.0,
    'visible': True, 'result': dict()}, 'Z': {'index': 2, 'mm_per_step': 1 /
    400, 'visible': True, 'result': dict()}}
visible_axes = {k: v for k, v in axes.items() if v['visible']}
show_motion = True
fs = 5000.0


def filter(i, fs, fc):
    w = fc / (fs / 2)
    b, a = signal.butter(3, w, 'low')
    return signal.filtfilt(b, a, i)


def savgol_filter(i):
    return signal.savgol_filter(i, 3, 1)


def velocity(x, y):
    return np.append([0], np.diff(y) * fs)


def acceleration(x, y):
    v = np.append([0], np.diff(y) * fs)
    a = np.append([0], np.diff(v) * fs)
    return a


def maxabs(a):
    return max(np.max(a), abs(np.min(a)))


def calc_axis(axis_data, time, axis_name, time_start, time_end, fs):
    t, p, s, n = zip(*axis_data)
    pos = interp1d(t, p, kind='nearest', bounds_error=False, fill_value=
        'extrapolate')(time)
    vel = velocity(t, pos)
    acc = acceleration(t, pos)
    vel_filtered = filter(vel, fs, 200)
    acc_filtered = filter(acc, fs, 50)
    vel_max = maxabs(vel_filtered)
    acc_max = maxabs(acc_filtered)
    pulse_min = np.min(s) * 1000 * 1000
    pulse_avg = np.mean(s) * 1000 * 1000
    pulse_max = np.max(s) * 1000 * 1000
    if len(s) > 0:
        print(
            '[Axis {}]: Pulse width (min, avg, max): {:.3f}us, {:.3f}us, {:.3f}us. Max velocity = {:f}. Max acceleration = {:f}'
            .format(axis_name, pulse_min, pulse_avg, pulse_max, vel_max,
            acc_max))
    steps = []
    for step_time, step_width, step_dir in zip(t, s, n):
        steps.append([step_time, step_dir])
        steps.append([step_time + step_width, 0])
    return {'pos': pos, 'vel': vel, 'vel_filtered': vel_filtered, 'acc':
        acc, 'acc_filtered': acc_filtered, 'steps': np.array(steps),
        'vel_max': vel_max, 'acc_max': acc_max, 'pulse_min': pulse_min,
        'pulse_avg': pulse_avg, 'pulse_max': pulse_max}


def extract_data(data, timeIdx, stepIdx, dirIdx, mm_per_step):
    position = 0
    pulseStart = None
    for t, s, d in zip(data[:, timeIdx], data[:, stepIdx], data[:, dirIdx]):
        if s:
            if not pulseStart:
                pulseStart = t
                step = -1 if d else 1
                position += mm_per_step * step
        elif pulseStart != None:
            yield pulseStart, position, t - pulseStart, step
            pulseStart = None


print('Loading dataset...')
dtype_dict = {'Time[s]': 'float64', 'X STEP': 'bool', 'X DIR': 'bool',
    'Y STEP': 'bool', 'Y DIR': 'bool', 'Z STEP': 'bool', 'Z DIR': 'bool'}
dataFrame = pd.read_csv(filename, delimiter=',', skipinitialspace=True,
    header=0)
data = dataFrame.get_values()
print('Sample count =', len(data))
time_start = data[0][0]
time_end = data[-1][0]
reg_t = np.linspace(time_start, time_end, int((time_end - time_start) // (1 /
    fs) + 1))
print('Interpolated steps =', len(reg_t))
for axis_name, axis in visible_axes.items():
    axis['result'] = calc_axis(extract_data(data, 0, axis['index'] * 2 + 1,
        axis['index'] * 2 + 2, axis['mm_per_step']), reg_t, axis_name,
        time_start, time_end, fs)
can_show_motion = show_motion and axes['X']['result'] and axes['Y']['result']
rows = 4
cols = 3 if can_show_motion else 2
figure = plt.figure('Stepper Motion analys')
plot_pos = plt.subplot2grid((rows, cols), (0, 0), colspan=2)
plot_pos.set_ylabel('position ($mm$)')
plot_pos.set_xlabel('time ($s$)')
plot_vel = plt.subplot2grid((rows, cols), (1, 0), colspan=2, sharex=plot_pos)
plot_vel.set_ylabel('velocity ($mm/s$)')
plot_vel.set_xlabel('time ($s$)')
plot_acc = plt.subplot2grid((rows, cols), (2, 0), colspan=2, sharex=plot_pos)
plot_acc.set_ylabel('acceleration ($mm/s^2$)')
plot_acc.set_xlabel('time ($s$)')
plot_steps = plt.subplot2grid((rows, cols), (3, 0), colspan=2, sharex=plot_pos)
plot_steps.set_ylim(-2, 2)
plot_steps.set_ylabel('steps')
plot_steps.set_xlabel('time ($s$)')
if can_show_motion:
    plot_graph = plt.subplot2grid((rows, cols), (1, 2), colspan=1, rowspan=2)
    plt.step(axes['X']['result']['pos'], axes['Y']['result']['pos'])
    plot_graph.set_ylabel('motion ($y$)')
    plot_graph.set_xlabel('motion ($x$)')
props = dict(boxstyle='round', facecolor='wheat', alpha=0.25)
for axis_name, axis in visible_axes.items():
    result = axis['result']
    plot_pos.step(reg_t, result['pos'], label=axis_name)
    plot_vel.plot(reg_t, result['vel_filtered'], label=axis_name)
    plot_acc.plot(reg_t, result['acc_filtered'], label=axis_name)
    plot_steps.step(result['steps'][:, 0], result['steps'][:, 1], label=
        axis_name, where='post')
plot_pos.legend()
plot_pos.grid(True)
plot_vel.legend()
plot_vel.grid(True)
plot_acc.legend()
plot_acc.grid(True)
plot_steps.legend()
plot_steps.grid(True)
plt.subplots_adjust(hspace=0.38, wspace=0.15, left=0.05, top=0.95, bottom=
    0.05, right=0.98)
multi = MultiCursor(figure.canvas, (plot_pos, plot_vel, plot_acc,
    plot_steps), useblit=True, color='r', lw=1)
plt.show()

#!/usr/bin/env python

import pandas as pd
import matplotlib.pyplot as plt
import math
import sys
import numpy as np

from scipy.interpolate import interp1d
from scipy import signal
from matplotlib.widgets import MultiCursor

filename = 'analysis.csv'

axes = {
  'X' : { 
    'index': 0,
    'mm_per_step': 1/100.0,    
    'visible': True,
    'result': dict()
    },
  'Y' : { 
    'index': 1,
    'mm_per_step': 1/100.0,
    'visible': True,
    'result': dict()
    },
  'Z' : { 
    'index': 2,
    'mm_per_step': 1/400,
    'visible': True,
    'result': dict()
    }
}

visible_axes = {k:v for k,v in axes.items() if v['visible']}

show_motion = True

fs = 5000.0  # (Re-)sampling frequency

def filter(i, fs, fc):
  w = fc / (fs / 2) # Normalize the frequency
  b, a = signal.butter(3, w, 'low')
  return signal.filtfilt(b, a, i)

def savgol_filter(i):
  return signal.savgol_filter(i, 3, 1)

def velocity(x, y):
  return np.append([0], np.diff(y)*fs) 

def acceleration(x, y):
  v = np.append([0], np.diff(y)*fs)
  a = np.append([0], np.diff(v)*fs)
  return a

def maxabs(a):
  return max(np.max(a), abs(np.min(a)))

def calc_axis(axis_data, time, axis_name, time_start, time_end, fs):
  t,p,s,n = zip(*axis_data)
  
  pos = interp1d(t, p, kind='nearest', bounds_error=False, fill_value='extrapolate')(time)  # OK

  vel = velocity(t, pos)
  acc = acceleration(t, pos)

  vel_filtered = filter(vel, fs, 200)
  acc_filtered = filter(acc, fs, 50)

  vel_max = maxabs(vel_filtered)
  acc_max = maxabs(acc_filtered)

  pulse_min = np.min(s)*1000*1000
  pulse_avg = np.mean(s)*1000*1000
  pulse_max = np.max(s)*1000*1000
  
  if len(s) > 0:
    print ('[Axis {}]: Pulse width (min, avg, max): {:.3f}us, {:.3f}us, {:.3f}us. Max velocity = {:f}. Max acceleration = {:f}'.format(axis_name, pulse_min, pulse_avg, pulse_max, vel_max, acc_max))

  steps = []
  for step_time, step_width, step_dir in zip(t, s, n):
    steps.append([step_time, step_dir])
    steps.append([step_time+step_width, 0])

  return { 'pos': pos, 'vel': vel, 'vel_filtered': vel_filtered, 'acc': acc, 'acc_filtered': acc_filtered, 'steps': np.array(steps), 'vel_max': vel_max, 'acc_max': acc_max, 'pulse_min': pulse_min, 'pulse_avg': pulse_avg, 'pulse_max' : pulse_max }

def extract_data(data, timeIdx, stepIdx, dirIdx, mm_per_step):
    position = 0
    pulseStart = None

    for (t, s, d) in zip(data[:,timeIdx], data[:,stepIdx], data[:,dirIdx]):        
      if s:
        if not pulseStart:
          pulseStart = t
          step = -1 if d else 1
          position += mm_per_step * step
      else:
        if pulseStart != None:
          yield (pulseStart, position, t-pulseStart, step)
          pulseStart = None

print ('Loading dataset...')
dtype_dict = {'Time[s]':'float64','X STEP':'bool', 'X DIR':'bool','Y STEP':'bool','Y DIR':'bool','Z STEP':'bool','Z DIR':'bool'}

dataFrame = pd.read_csv(filename, delimiter = ',', skipinitialspace=True, header=0)

data = dataFrame.get_values()

print ('Sample count =', len(data))

time_start = data[0][0]
time_end = data[-1][0]

reg_t = np.linspace(time_start, time_end, int((time_end - time_start) // (1/fs) + 1))
print ('Interpolated steps =', len(reg_t))

for axis_name, axis in visible_axes.items():
  axis['result'] = calc_axis(extract_data(data, 0, axis['index']*2+1, axis['index']*2+2, axis['mm_per_step']), reg_t, axis_name, time_start, time_end, fs)

can_show_motion = show_motion and axes['X']['result'] and axes['Y']['result']

rows = 4
cols = (3 if can_show_motion else 2)

figure = plt.figure('Stepper Motion analys')

# POSITION
plot_pos = plt.subplot2grid((rows, cols), (0, 0), colspan = 2)
plot_pos.set_ylabel('position ($mm$)')
plot_pos.set_xlabel('time ($s$)')


# VELOCITY
plot_vel = plt.subplot2grid((rows, cols), (1, 0), colspan = 2, sharex=plot_pos)
plot_vel.set_ylabel('velocity ($mm/s$)')
plot_vel.set_xlabel('time ($s$)')


# ACCELERATION
plot_acc = plt.subplot2grid((rows, cols), (2, 0), colspan = 2, sharex=plot_pos)
plot_acc.set_ylabel('acceleration ($mm/s^2$)')
plot_acc.set_xlabel('time ($s$)')


# STEPS
plot_steps = plt.subplot2grid((rows, cols), (3, 0), colspan = 2, sharex=plot_pos)
plot_steps.set_ylim(-2, 2)
plot_steps.set_ylabel('steps')
plot_steps.set_xlabel('time ($s$)')


# Plot motion
if can_show_motion:
  plot_graph = plt.subplot2grid((rows, cols), (1, 2), colspan = 1, rowspan = 2)
  plt.step(axes['X']['result']['pos'], axes['Y']['result']['pos'])
  plot_graph.set_ylabel('motion ($y$)')
  plot_graph.set_xlabel('motion ($x$)')

props = dict(boxstyle='round', facecolor='wheat', alpha=0.25)

# Plot visible axes
for axis_name, axis in visible_axes.items():
    result = axis['result']
    plot_pos.step(reg_t, result['pos'], label=axis_name)  
    # plot_vel.plot(reg_t, result['vel'], label=axis_name)
    plot_vel.plot(reg_t, result['vel_filtered'], label=axis_name)
    # plot_acc.plot(reg_t, result['acc'], label=axis_name)
    plot_acc.plot(reg_t, result['acc_filtered'], label=axis_name)
    plot_steps.step(result['steps'][:,0], result['steps'][:,1], label=axis_name, where='post')

plot_pos.legend()
plot_pos.grid(True)

plot_vel.legend()
plot_vel.grid(True)

plot_acc.legend()
plot_acc.grid(True)

plot_steps.legend()
plot_steps.grid(True)

plt.subplots_adjust(hspace=0.38, wspace=0.15, left=0.05, top=0.95, bottom=0.05, right=0.98)

multi = MultiCursor(figure.canvas, (plot_pos, plot_vel, plot_acc, plot_steps), useblit=True, color='r', lw=1)

plt.show()
