diff --git a/tests/unit/modules/motion/rampgen.cpp b/tests/unit/modules/motion/rampgen.cpp index 7048abf..84b3d33 100644 --- a/tests/unit/modules/motion/rampgen.cpp +++ b/tests/unit/modules/motion/rampgen.cpp @@ -19,42 +19,53 @@ int main(int argc, const char *argv[]) { } // common settings - const int idlerSteps = 100; - const int selectorSteps = 80; + const Axis ax_a = Idler; + const int steps_a = 100; + const Axis ax_b = Selector; + const int steps_b = 80; const int maxFeedRate = 1000; + const int maxJerk = 1; + + // write common parameters + fprintf(fd, "{\"timebase\": %lu}\n", F_CPU / config::stepTimerFrequencyDivider); for (int accel = 2000; accel <= 50000; accel *= 2) { - // first axis using nominal values - motion.SetPosition(Idler, 0); - motion.SetAcceleration(Idler, accel); - motion.PlanMoveTo(Idler, idlerSteps, maxFeedRate); + // first axis defines the nominal values + motion.SetJerk(ax_a, maxJerk); + motion.SetPosition(ax_a, 0); + motion.SetAcceleration(ax_a, accel); + motion.PlanMoveTo(ax_a, steps_a, maxFeedRate); - fprintf(fd, "[{\"steps\": %d, \"accel\": %d, \"maxrate\": %d}, ", - idlerSteps, accel, maxFeedRate); + fprintf(fd, "[{\"steps\": %d, \"jerk\": %d, \"accel\": %d, \"maxrate\": %d}, ", + steps_a, maxJerk, accel, maxFeedRate); // second axis finishes slightly sooner at triple acceleration to maximize the // aliasing effects int accel_3 = accel * 3; - motion.SetPosition(Selector, 0); - motion.SetAcceleration(Selector, accel_3); - motion.PlanMoveTo(Selector, selectorSteps, maxFeedRate); + motion.SetJerk(ax_b, 1); + motion.SetPosition(ax_b, 0); + motion.SetAcceleration(ax_b, accel_3); + motion.PlanMoveTo(ax_b, steps_b, maxFeedRate); - fprintf(fd, "{\"steps\": %d, \"accel\": %d, \"maxrate\": %d}]\n", - selectorSteps, accel_3, maxFeedRate); + fprintf(fd, "{\"steps\": %d, \"jerk\": %d, \"accel\": %d, \"maxrate\": %d}]\n", + steps_b, maxJerk, accel_3, maxFeedRate); + + // initial state + unsigned long ts = 0; + st_timer_t next = 0; + fprintf(fd, "%lu %u %d %d\n", ts, next, motion.CurPosition(ax_a), motion.CurPosition(ax_b)); // step and output time, interval and positions - unsigned long ts = 0; - st_timer_t next; do { next = motion.Step(); - pos_t pos_idler = motion.CurPosition(Idler); - pos_t pos_selector = motion.CurPosition(Selector); + pos_t pos_idler = motion.CurPosition(ax_a); + pos_t pos_selector = motion.CurPosition(ax_b); fprintf(fd, "%lu %u %d %d\n", ts, next, pos_idler, pos_selector); ts += next; } while (next); - fprintf(fd, "\n\n"); + fprintf(fd, "\n"); } return EX_OK; diff --git a/tests/unit/modules/motion/test_motion_ramp.py b/tests/unit/modules/motion/test_motion_ramp.py index 3a3f56f..58f8f14 100755 --- a/tests/unit/modules/motion/test_motion_ramp.py +++ b/tests/unit/modules/motion/test_motion_ramp.py @@ -1,5 +1,174 @@ #!/usr/bin/env python3 import numpy as np -import pandas as pd +import argparse +import json -exit(0) +import pandas as pd +pd.options.mode.chained_assignment = None + + +def load_data(data): + runs = [] + + # read all sets + lines = open(data).readlines() + info = json.loads(lines[0]) + + i = 1 + while i < len(lines): + # first line in each set is a json description + run_info = json.loads(lines[i]) + + run_data = [] + for j in range(i + 1, len(lines)): + # read until an empty line (data terminator) + line = lines[j].rstrip() + if len(line) == 0: + break + + # parse the line + tokens = list(map(int, line.split(' '))) + run_data.append(tokens) + + runs.append([run_info, run_data]) + i = j + 1 + + return info, runs + + +def check_axis(info, ax_info, data): + tb = info['timebase'] + + # remove duplicate positions (meaning another axis was moved, not the current) + data = data[data['pos'].diff() != 0] + + # recalculate intervals just for this axis + data['int'] = data['ts'].diff() + + # check start/ending position + assert (data['pos'].iat[0] == 0) + assert (data['pos'].iat[-1] == ax_info['steps']) + + # check first null timestamp/interval/position values + assert ((data['ts'][0:2] == 0).all()) + assert ((data['int'][0:2].dropna() == 0).all()) + + # ensure timestamps and positions are monotonically increasing + assert ((data['ts'].diff()[2:] > 0).all()) + assert ((data['pos'].diff()[1:] > 0).all()) + + # convert timestamps to seconds + data['ts_s'] = data['ts'] / tb + data['int_s'] = data['int'] / tb + data['rate'] = 1 / data['int_s'] + + # ensure we never _exceed_ max feedrate + assert ((data['rate'][2:] <= ax_info['maxrate']).all()) + + # recalculate independently the acceleration parameters + acc_dist = (ax_info['maxrate']**2 - + ax_info['jerk']**2) / (2 * ax_info['accel']) + if acc_dist * 2 > ax_info['steps']: + # no cruising, calculate intersection (equal start/end speed) + acc_dist = ax_info['steps'] / 2 + maxrate = np.sqrt(2 * ax_info['accel'] * acc_dist + ax_info['jerk']**2) + else: + # cruising possible, get distance + c_dist = ax_info['steps'] - 2 * acc_dist + maxrate = ax_info['maxrate'] + + # check cruising speed + cruise_data = data[(data['pos'] > acc_dist + 2) + & (data['pos'] < acc_dist + 2 + c_dist)] + assert ((cruise_data['rate'] - maxrate).abs().max() < 1) + + # checking acceleration segments require a decent number of samples for good results + if acc_dist < 10: + return + + # TODO: minrate is currently hardcoded in the FW as a function of the timer type (we + # can't represent infinitely-long intervals, to the slowest speed itself is limited). + # We recover the minrate here directly from the trace, but perhaps we shouldn't + startrate = data['rate'].iat[2] # skip first two null values + endrate = data['rate'].iat[-1] + + maxdev_coarse = (maxrate - startrate) / 20 # 5% speed deviation + maxdev_fine = 20 # absolute maximum deviation + maxdev_acc = 0.05 # 5% acceleration deviation + + # check acceleration segment (coarse) + acc_data = data[(data['pos'] < acc_dist)][2:] + acc_data['ts_s'] -= acc_data['ts_s'].iat[0] + acc_time = acc_data['ts_s'].iat[-1] + acc_data['exp_rate'] = startrate + acc_data['ts_s'] \ + / acc_time * (maxrate - startrate) + assert ((acc_data['exp_rate'] - acc_data['rate']).abs().max() < + maxdev_coarse) + + # acceleration (fine) + acc_data['exp_fine'] = acc_data['rate'].iat[0] + acc_data['ts_s'] \ + / acc_time * (acc_data['rate'].iat[-1] - startrate) + assert ((acc_data['exp_fine'] - acc_data['rate']).abs().max() < + maxdev_fine) + + # check effective acceleration rate + acc_vel = (acc_data['rate'].iat[-1] - acc_data['rate'].iat[0]) / acc_time + assert (abs(acc_vel - ax_info['accel']) / ax_info['accel'] < 0.05) + + # deceleration (coarse) + dec_data = data[(data['pos'] > (data['pos'].iat[-1] - acc_dist))][2:] + dec_data['ts_s'] -= dec_data['ts_s'].iat[0] + dec_time = dec_data['ts_s'].iat[-1] + dec_data['exp_rate'] = maxrate - dec_data['ts_s'] \ + / dec_time * (maxrate - endrate) + assert ((dec_data['exp_rate'] - dec_data['rate']).abs().max() < + maxdev_coarse) + + # deceleration (fine) + dec_data['exp_fine'] = dec_data['rate'].iat[0] - dec_data['ts_s'] \ + / dec_time * (dec_data['rate'].iat[0] - endrate) + assert ((dec_data['exp_fine'] - dec_data['rate']).abs().max() < + maxdev_fine) + + # check effective deceleration rate + dec_vel = (dec_data['rate'].iat[-1] - dec_data['rate'].iat[0]) / dec_time + print(abs(dec_vel - ax_info['accel']) / ax_info['accel'] < 0.05) + + +def check_run(info, run): + # unpack the axis data + ax_info, data = run + + # split axis information + ax_data = [] + for ax in range(2): + ax_info[ax]['name'] = ax + tmp = [] + for i in range(len(data)): + row = data[i] + tmp.append([row[0], row[1], row[2 + ax]]) + + ax_data.append(pd.DataFrame(tmp, columns=['ts', 'int', 'pos'])) + + # check each axis independently + for ax in range(2): + check_axis(info, ax_info[ax], ax_data[ax]) + + +def main(): + # parse arguments + ap = argparse.ArgumentParser() + ap.add_argument('data') + args = ap.parse_args() + + # load data runs + info, runs = load_data(args.data) + + # test each set + for run in runs: + check_run(info, run) + break + + +if __name__ == '__main__': + exit(main())