+

+The "motion" tests start a full emc in sim mode, with emcrsh as the UI.

+

+They run g-code programs and use halsampler to monitor the motions that

+motmod emits to HAL.

+

+#!/usr/bin/python

+

+import sys

+import os

+from subprocess import *

+

+

+#

+# read some stuff out of the ini file

+#

+

+# servo period in nanoseconds

+servo_period_ns = float(Popen(

+ [

+ "inivar",

+ "-var",

+ "SERVO_PERIOD",

+ "-sec",

+ "EMCMOT",

+ "-ini",

+ "%s/tests/motion/g0/motion-test.ini" % os.getenv("EMC2_HOME")

+ ],

+ stdout=PIPE

+).communicate()[0])

+

+cycles_per_second = 1000000000.0 / servo_period_ns

+

+# max acceleration in inches per second^2

+max_acceleration_ips2 = float(Popen(

+ [

+ "inivar",

+ "-var",

+ "MAX_ACCELERATION",

+ "-sec",

+ "AXIS_0",

+ "-ini",

+ "%s/tests/motion/g0/motion-test.ini" % os.getenv("EMC2_HOME")

+ ],

+ stdout=PIPE

+).communicate()[0])

+

+# max acceleration in inches per second per servo-thread cycle

+# the factor of half at the end is because emc2 reserves half the acceleration for blending

+max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second) * 0.5

+

+# max velocity in inches per second

+max_velocity_ips = float(Popen(

+ [

+ "inivar",

+ "-var",

+ "MAX_VELOCITY",

+ "-sec",

+ "AXIS_0",

+ "-ini",

+ "%s/tests/motion/g0/motion-test.ini" % os.getenv("EMC2_HOME")

+ ],

+ stdout=PIPE

+).communicate()[0])

+

+max_velocity_ipc = max_velocity_ips / cycles_per_second

+

+

+# read the samples file and turn it into an array, where index i is an

+# array of the floats found on line i of the samples file

+samples_filename = sys.argv[1] + ".halsamples"

+lines = file(samples_filename).readlines()

+samples = []

+for line in lines:

+ s = [ float(x) for x in line.split() ]

+ samples.append(s)

+

+

+#

+# here comes the test proper

+#

+

+

+# verify that X starts at 0.000000

+if samples[0][1] != 0.000000:

+ print "sample 0: X starts at %.6f, not at 0.000000" % samples[0][1]

+ sys.exit(1)

+

+print "line 0: X starts at 0.000000"

+

+

+# find where X starts moving (X is the second column, column 1)

+i = 0

+while samples[i][1] == samples[i+1][1]:

+ i += 1

+

+print "line %d: accel phase starts" % i

+

+

+# now i is the sample before it starts moving

+# verify accel does not exceed maxaccel

+# verify vel does not exceed maxvel

+# verify accel phase stops when vel == maxvel

+highest_seen_accel_ipc2 = 0

+old_accel_ipc2 = 0

+old_v_ipc = 0

+for i in range(i, len(samples)):

+

+ new_v_ipc = samples[i+1][1] - samples[i][1]

+ accel_ipc2 = new_v_ipc - old_v_ipc

+

+ highest_seen_accel_ipc2 = max(accel_ipc2, highest_seen_accel_ipc2)

+

+ #print "line %d: X=%.6f, v=%.6f i/c (%.6f i/s), a=%.6f i/c^2 (%.6f i/s^2)" % (i, samples[i][1], new_v_ipc, (new_v_ipc * cycles_per_second), accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second))

+

+ # i hate floating point

+ if ((accel_ipc2 * cycles_per_second) - max_acceleration_ipspc) > 0.0000001:

+ print "line %d: detected accel constraint violation!" % i

+ print "detected accel %.6f i/c^2 (%.6f i/s^2)" % (accel_ipc2, (accel * cycles_per_second * cycles_per_second))

+ print "max accel %.6f i/s^2)" % max_acceleration_ips2

+ sys.exit(1)

+

+ if (new_v_ipc - max_velocity_ipc) > 0.0000001:

+ print "line %d: detected vel constraint violation!" % i

+ print "detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second)

+ print "max vel %.6f i/c (%.6f i/s)" % (max_velocity_ipc, max_velocity_ips)

+ sys.exit(1)

+

+ if accel_ipc2 == 0:

+ print "line %d: accel phase over" % i

+ break

+

+ old_v_ipc = new_v_ipc

+

+# verify highest seen accel is very close to max accel

+if abs(max_acceleration_ipspc - (highest_seen_accel_ipc2 * cycles_per_second)) > 0.0000001:

+ print "accel only reached %.6f i/c^2 (%.6f i/s^2)" % (highest_seen_accel_ipc2, (highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second))

+ print "max accel is %.6f i/s^2" % max_acceleration_ips2

+ sys.exit(1)

+

+print " accel reached but did not exceed 1/2 of max accel of %.6f i/s^2" % max_acceleration_ips2

+

+print "line %d: entering cruise phase, vel=%.6f i/s, max_accel = %.6f i/s^2" % (i, new_v_ipc * cycles_per_second, highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second)

+

+

+# now i is the first sample of the cruise phase

+# wait for vel to drop again

+for i in range(i, len(samples)):

+ new_v_ipc = samples[i+1][1] - samples[i][1]

+

+ if abs(new_v_ipc - old_v_ipc) > 0.0000001:

+ # decel phase starting

+ break

+

+ old_v_ipc = new_v_ipc

+

+print "line %d: decel phase starting, old_v=%.6f i/s, new_v=%.6f i/s" % (i, old_v_ipc * cycles_per_second, new_v_ipc * cycles_per_second)

+

+# now i is the sample before it starts decel

+# verify accel does not exceed maxaccel

+# verify vel does not exceed maxvel

+# verify decel phase stops when vel == 0

+highest_seen_accel_ipc2 = 0

+old_accel_ipc2 = 0

+for i in range(i, len(samples)):

+

+ new_v_ipc = samples[i+1][1] - samples[i][1]

+ accel_ipc2 = new_v_ipc - old_v_ipc

+

+ highest_seen_accel_ipc2 = min(accel_ipc2, highest_seen_accel_ipc2)

+

+ #print "line %d: X=%.6f, v=%.6f i/c (%.6f i/s), a=%.6f i/c^2 (%.6f i/s^2)" % (i, samples[i][1], new_v_ipc, (new_v_ipc * cycles_per_second), accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second))

+

+ # i hate floating point

+ if ((accel_ipc2 * cycles_per_second) - max_acceleration_ipspc) > 0.0000001:

+ print "line %d: detected accel constraint violation!" % i

+ print "detected accel %.6f i/c^2 (%.6f i/s^2)" % (accel_ipc2, accel * cycles_per_second * cycles_per_second)

+ print "max accel %.6f i/s^2)" % max_acceleration_ips2

+ sys.exit(1)

+

+ if (new_v_ipc - max_velocity_ipc) > 0.0000001:

+ print "line %d: detected vel constraint violation!" % i

+ print "detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second)

+ print "max vel %.6f i/c (%.6f i/s)" % (max_velocity_ipc, max_velocity_ips)

+ sys.exit(1)

+

+ if new_v_ipc < -0.0000001:

+ print "line %d: detected vel undershoot!" % i

+ print "detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second)

+ sys.exit(1)

+

+ if new_v_ipc == 0:

+ print "line %d: decel phase over" % i

+ break

+

+ old_v_ipc = new_v_ipc

+

+# verify highest seen accel is very close to max accel

+if abs(max_acceleration_ipspc + (highest_seen_accel_ipc2 * cycles_per_second)) > 0.0000001:

+ print "accel only reached %.6f i/c^2 (%.6f i/s^2)" % (highest_seen_accel_ipc2, (highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second))

+ print "max accel is %.6f i/s^2" % max_acceleration_ips2

+ sys.exit(1)

+

+print " decel reached but did not exceed 1/2 of max accel of %.6f i/s^2" % max_acceleration_ips2

+

+

+# verify X stopped at 1.000000

+if samples[i][1] != 1.000000:

+ print "line %d: X stopped at %.6f, not at 1.000000!" % (i, samples[i][1])

+ sys.exit(1)

+

+print " X reached target of 1.0000"

+

+

+# verify X doesn't move from now on

+for i in range(i, len(samples) - 1):

+ if samples[i][1] != samples[i+1][1]:

+ print "line %d: X moved!" % i

+ sys.exit(1)

+

+

+print "line %d: X did not move after reaching target" % i

+

+print "success!\n"

+sys.exit(0)

+

+

+# core HAL config file for simulation

+

+# first load all the RT modules that will be needed

+# kinematics

+loadrt trivkins

+# motion controller, get name and thread periods from ini file

+loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES

+# load 6 differentiators (for velocity and accel signals

+loadrt ddt count=6

+# load additional blocks

+loadrt hypot count=2

+loadrt comp count=3

+loadrt or2 count=1

+

+# add motion controller functions to servo thread

+addf motion-command-handler servo-thread

+addf motion-controller servo-thread

+# link the differentiator functions into the code

+addf ddt.0 servo-thread

+addf ddt.1 servo-thread

+addf ddt.2 servo-thread

+addf ddt.3 servo-thread

+addf ddt.4 servo-thread

+addf ddt.5 servo-thread

+addf hypot.0 servo-thread

+addf hypot.1 servo-thread

+

+# create HAL signals for position commands from motion module

+# loop position commands back to motion module feedback

+net Xpos axis.0.motor-pos-cmd => axis.0.motor-pos-fb ddt.0.in

+net Ypos axis.1.motor-pos-cmd => axis.1.motor-pos-fb ddt.2.in

+net Zpos axis.2.motor-pos-cmd => axis.2.motor-pos-fb ddt.4.in

+

+# send the position commands thru differentiators to

+# generate velocity and accel signals

+net Xvel ddt.0.out => ddt.1.in hypot.0.in0

+net Xacc <= ddt.1.out

+net Yvel ddt.2.out => ddt.3.in hypot.0.in1

+net Yacc <= ddt.3.out

+net Zvel ddt.4.out => ddt.5.in hypot.1.in0

+net Zacc <= ddt.5.out

+

+# Cartesian 2- and 3-axis velocities

+net XYvel hypot.0.out => hypot.1.in1

+net XYZvel <= hypot.1.out

+

+# estop loopback

+net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in

+

+# create signals for tool loading loopback

+net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared

+net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed

+

+

+# sample where motion tells us to go

+

+loadrt sampler depth=5000 cfg=ff

+

+addf sampler.0 servo-thread

+

+net Xpos => sampler.0.pin.0

+net Ypos => sampler.0.pin.1

+

+# EMC controller parameters for a simulated machine.

+

+[EMC]

+

+# Name of machine, for use with display, etc.

+MACHINE = MOTION-TEST

+

+# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others

+DEBUG = 0x7FFFFFFF

+#DEBUG = 0x10

+

+[DISPLAY]

+

+DISPLAY = emcrsh

+

+#PROGRAM_PREFIX = /home/seb/emc2/nc_files

+

+#MAX_FEED_OVERRIDE = 2.0

+

+[TASK]

+

+TASK = milltask

+CYCLE_TIME = 0.001

+

+[RS274NGC]

+

+# File containing interpreter variables

+PARAMETER_FILE = sim.var

+

+[EMCMOT]

+

+EMCMOT = motmod

+

+# Timeout for comm to emcmot, in seconds

+COMM_TIMEOUT = 1.0

+

+# Interval between tries to emcmot, in seconds

+COMM_WAIT = 0.010

+

+# BASE_PERIOD is unused in this configuration but specified in core_sim.hal

+BASE_PERIOD = 0

+# Servo task period, in nano-seconds

+SERVO_PERIOD = 1000000

+

+[HAL]

+

+HALFILE = core_sim.hal

+HALFILE = motion-test.hal

+#HALFILE = core_sim.hal

+

+

+[TRAJ]

+

+AXES = 3

+COORDINATES = X Y Z

+HOME = 0 0 0

+LINEAR_UNITS = inch

+ANGULAR_UNITS = degree

+CYCLE_TIME = 0.010

+DEFAULT_VELOCITY = 1.2

+MAX_LINEAR_VELOCITY = 4

+

+# Axes sections ---------------------------------------------------------------

+

+# First axis

+[AXIS_0]

+

+TYPE = LINEAR

+HOME = 0.000

+MAX_VELOCITY = 4

+MAX_ACCELERATION = 100.0

+BACKLASH = 0.000

+INPUT_SCALE = 4000

+OUTPUT_SCALE = 1.000

+MIN_LIMIT = -40.0

+MAX_LIMIT = 40.0

+FERROR = 0.050

+MIN_FERROR = 0.010

+HOME_OFFSET = 0.0

+HOME_SEARCH_VEL = 0.0

+HOME_LATCH_VEL = 0.0

+HOME_USE_INDEX = NO

+HOME_IGNORE_LIMITS = NO

+HOME_SEQUENCE = 1

+

+# Second axis

+[AXIS_1]

+

+TYPE = LINEAR

+HOME = 0.000

+MAX_VELOCITY = 4

+MAX_ACCELERATION = 100.0

+BACKLASH = 0.000

+INPUT_SCALE = 4000

+OUTPUT_SCALE = 1.000

+MIN_LIMIT = -40.0

+MAX_LIMIT = 40.0

+FERROR = 0.050

+MIN_FERROR = 0.010

+HOME_OFFSET = 0.0

+HOME_SEARCH_VEL = 0.0

+HOME_LATCH_VEL = 0.0

+HOME_USE_INDEX = NO

+HOME_IGNORE_LIMITS = NO

+HOME_SEQUENCE = 1

+

+# Third axis

+[AXIS_2]

+

+TYPE = LINEAR

+HOME = 0.0

+MAX_VELOCITY = 4

+MAX_ACCELERATION = 100.0

+BACKLASH = 0.000

+INPUT_SCALE = 4000

+OUTPUT_SCALE = 1.000

+MIN_LIMIT = -4.0

+MAX_LIMIT = 4.0

+FERROR = 0.050

+MIN_FERROR = 0.010

+HOME_OFFSET = 1.0

+HOME_SEARCH_VEL = 0.0

+HOME_LATCH_VEL = 0.0

+HOME_USE_INDEX = NO

+HOME_IGNORE_LIMITS = NO

+HOME_SEQUENCE = 0

+

+# section for main IO controller parameters -----------------------------------

+[EMCIO]

+

+# Name of IO controller program, e.g., io

+EMCIO = io

+

+# cycle time, in seconds

+CYCLE_TIME = 0.100

+

+# tool table file

+TOOL_TABLE = simpockets.tbl

+TOOL_CHANGE_POSITION = 0 0 2

+RANDOM_TOOLCHANGER = 1

+#!/bin/bash

+

+#export PATH=$PATH:$EMC2_HOME/tests/helpers

+#source $EMC2_HOME/tests/helpers/test-functions.sh

+

+emc motion-test.ini &

+

+# let emc come up

+sleep 4

+

+(

+ echo starting to capture data

+ halsampler -t -n 20000 >| result.halsamples

+ echo finished capturing data

+) &

+

+(

+ echo hello EMC mt 1.0

+ echo set enable EMCTOO

+

+ echo set mode manual

+ echo set estop off

+ echo set machine on

+

+ echo set home 0

+ echo set home 1

+ echo set home 2

+

+ # give emc a second to home

+ sleep 1.0

+

+ echo set mode mdi

+ echo set mdi g0x1

+

+ # give emc a half second to move

+ sleep 0.5

+

+ echo shutdown

+) | telnet localhost 5007

+

+

+# wait for emc to finish

+wait

+

+exit 0

+