Complete motion ramp checks
- Add additional information in the output generated by rampgen in order to allow recalculating the acceleration curves independently - Implement motion ramp checks inside test_motion_ramp.py test_motion_ramp reads the output of a merged stepping sequence and splits the motion of each axis, checking the acceleration curves independently. This ensures both that the acceleration curves are correct (as generated by the PulseGen class) and that the multiplexed moves are too. The nominal rate is checked exactly, while the acceleration/deceleration segment allow for some deviation from an ideal curve. This is currently 5% for both expected speed and acceleration, with an absolute limit of 20mm/s of maximum difference in each point.pull/87/head
Yuri D'Elia
DRracer
parent
89ab29dbde
commit
240f4c28ab
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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())
|
||||
|
|
|
|||
Loading…
Reference in New Issue