步进电机S曲线生成

s_curve.c

/*
	amax: max of velocity accelerator
	aa: accelerator of v accelerator
	vm: max of velocity
	s: total lenght of distance
	pin: pluse pin of stepper controller

	In order to improve performance, let's re-define the unit system:
	time -- us
	length -- step
	velocity -- step/us
*/
void s_curve(float amax, float aa, float vm, int s_total, int pin) {

#define seg1_dist(aa, t1) (int)((1.0/6)*(aa)*pow((t1), 3))
#define seg2_dist(amax, v1, t2) (int)((v1)*(t2) + 0.5*(amax)*pow((t2), 2))
#define seg3_dist(amax, aa, v2, t3) (int)((v2)*(t3) + 0.5*(amax)*pow((t3), 2) - 1.0/6*(aa)*pow((t3), 3))

	float t[8];
	float v[8];
	int s[8];

	t[1] = t[3] = amax/aa;
	v[1] = 0.5*aa*pow(t[1], 2);
	v[3] = vm;

	t[2] = (v[3] - v[0])/amax - t[1];

	v[2] = v[1] + amax*t[2];

	s[0] = 0;
	s[1] = seg1_dist(aa, t[1]);
	s[2] = seg2_dist(amax, v[1], t[2]);
	s[3] = seg3_dist(amax, aa, v[2], t[3]);

	s[4] = s_total - 2*(s[1] + s[2] + s[3]);
	t[4] = s[4]/vm;

	// here distance is more important than speed
	// if distance is not enough, it will fallback to this mode
	if (s[4] < 0) vm = amax*amax/aa;
	// what if vm is too small, or distance is too short
	if (vm <= amax*amax/aa) {
		/* no 2, 6 segment exists */
		t[1] = t[3] = sqrt(vm/aa);
		t[2] = 0;
		s[2] = 0;
		v[1] = v[2] = 0.5*aa*pow(t[1], 2);
		amax = sqrt(vm*aa);  // new amax

		s[1] = seg1_dist(aa, t[1]);
		s[2] = 0;
		s[3] = seg3_dist(amax, aa, v[2], t[3]);

		s[4] = s_total - 2*(s[1] + s[3]);
		t[4] = s[4]/vm;

		// what if s_total is ver short, it can't reach vmax, neither amax
		if (s[4] < 0) {
			t[1] = t[3] = pow(s_total/aa/2, 1.0/3);
			amax = aa*t[1];
			t[2] = 0;
			v[1] = v[2] = 0.5*aa*pow(t[1], 2);
			vm = aa*pow(t[1], 2);

			s[1] = seg1_dist(aa, t[1]);
			s[2] = 0;
			s[3] = seg3_dist(amax, aa, v[2], t[3]);

			s[4] = 0;
			t[4] = 0;
		}
	}
	v[3] = v[4] = vm;

	// new fill up the remains
	v[5] = v[2];
	v[6] = v[1];

	t[5] = t[3];
	t[6] = t[2];
	t[7] = t[1];

	s[5] = s[3];
	s[6] = s[2];
	s[7] = s[1];

	// aggregate to get the abs stamp
	for (int i = 1; i < 8; i++) {
		t[i] = t[i - 1] + t[i];
		s[i] = s[i - 1] + s[i];
	}

	// new let's start output
	float cur_time = 0;
	for (int i = 0; i < s_total; i++) {
		float vt;
		float dt;
		if (i < s[1]) {
			dt = cur_time - t[0];
			vt = 0.5*aa*pow(dt, 2);
		} else if (i < s[2]) {
			dt = cur_time - t[1];
			vt = v[1] + amax*dt;
		} else if (i < s[3]) {
			dt = cur_time - t[2];
			vt = v[2] + amax*dt - 0.5*aa*pow(dt, 2);
		} else if (i < s[4]) {
			vt = v[3];
		} else if (i < s[5]) {
			dt = cur_time - t[4];
			vt = v[3] - 0.5*aa*pow(dt, 2);
		} else if (i < s[6]) {
			dt = cur_time - t[5];
			vt = v[2] - amax*dt;
		} else if (i < s[7]) {
			dt = cur_time - t[6];
			vt = v[1] - amax*dt + 0.5*aa*pow(dt, 2);
		}

		int interval = (int)1/vt/2;
		digitalWrite(pin, HIGH);
		delayMicroseconds(interval);
		digitalWrite(pin, LOW);
		delayMicroseconds(interval);

		cur_time += interval*2;
	}
}

stepper_motor_s_curve.py

def aac(amax, aa, vm, s_total):
    # s_total is import than vm
    def seg1_distance(aa, t1):
        # // v1 = 0.5*aa*t^2
        return 1.0/6*aa*pow(t1, 3)

    def seg2_distance(amax, v1, t2):
        # // v2 = v1 + amax*t
        # // s(t) = v1*t + 1/2*amax*t^2
        st = lambda t: v1*t + 0.5*amax*pow(t, 2)
        return st(t2)

    def seg3_distance(amax, aa, v2, t3):
        # // v3 = v2 + amax*t - 1/2*aa*t^2
        # // s(t) = v2*t + 1/2*amax*t^2 - 1/6*aa*t^3
        st = lambda t: v2*t + 0.5*amax*pow(t, 2) - 1.0/6*aa*pow(t, 3)
        return st(t3)

    t = [0]*8
    v = [0]*8

    t[1] = t[3] = amax/aa
    v[1] = 0.5*aa*pow(t[1], 2)
    v[3] = vm

    t[2] = (v[3] - v[0])/amax - t[1]

    v[2] = v[1] + amax*t[2]

    s = [0]*8
    s[0] = 0
    s[1] = seg1_distance(aa, t[1])
    s[2] = seg2_distance(amax, v[1], t[2])
    s[3] = seg3_distance(amax, aa, v[2], t[3])

    saac = sum(s[:4])
    s[4] = s_total - 2*saac
    t[4] = s[4]/vm

    # here distance is more important than speed
    # if distance is not enough, it will fallback to this mode
    if (s[4] < 0):
        vm = amax*amax/aa
    # what if vm is too small, or distance is too short
    if (vm <= amax*amax/aa):
        print "lenght too short"
        # /* no 2, 6 segment exists */
        # cal the time
        t[1] = t[3] = pow(vm/aa, 0.5)
        t[2] = 0
        s[2] = 0
        v[1] = v[2] = 0.5*aa*pow(t[1], 2)
        amax = pow(vm*aa, 0.5)  # new amax

        # distance
        s[1] = seg1_distance(aa, t[1])
        s[2] = 0
        s[3] = seg3_distance(amax, aa, v[2], t[3])

        saac = sum(s[:4])
        s[4] = s_total - 2*saac
        t[4] = s[4]/vm

        # what if s_total is ver short, it can't reach vmax, neither amax
        if s[4] < 0:
            print "too too short"
            t[1] = t[3] = pow(s_total/aa/2, 1.0/3)
            amax = aa*t[1]
            t[2] = 0
            v[1] = v[2] = 0.5*aa*pow(t[1], 2)
            vm = aa*pow(t[1], 2)

            s[1] = seg1_distance(aa, t[1])
            s[2] = 0
            s[3] = seg3_distance(amax, aa, v[2], t[3])

            s[4] = 0
            t[4] = 0
    v[3] = vm

    # expand to full array
    tmp = list(v[0:4])
    tmp.reverse()
    v = v[0:4] + tmp
    tmp = list(t[1:4])
    tmp.reverse()
    t = t[0:5] + tmp
    tmp = list(s[1:4])
    tmp.reverse()
    s = s[0:5] + tmp

    # aggregate to abs value
    s_abs = [0]*8
    t_abs = [0]*8
    for i in range(1, 8):
        s_abs[i] += s_abs[i-1] + s[i]
        t_abs[i] += t_abs[i-1] + t[i]

    print 'v', v
    print 't', t
    print 't_abs', t_abs
    print 's', s
    print 's_abs', s_abs

    step = (t_abs[-1])/1000
    times = map(lambda i: step*i, xrange(1000))
    velocity = []
    for cur_t in times:
        if cur_t < t_abs[1]:
            v_t = 0.5*aa*pow(cur_t, 2)
        elif cur_t < t_abs[2]:
            dt = cur_t - t_abs[1]
            v_t = v[1] + amax*(dt)
        elif cur_t < t_abs[3]:
            dt = cur_t - t_abs[2]
            v_t = v[2] + amax*dt - 0.5*aa*pow(dt, 2)
        elif cur_t < t_abs[4]:
            v_t = v[3]
        elif cur_t < t_abs[5]:
            dt = cur_t - t_abs[4]
            v_t = v[3] - 0.5*aa*pow(dt, 2)
        elif cur_t < t_abs[6]:
            dt = cur_t - t_abs[5]
            v_t = v[2] - amax*dt
        elif cur_t < t_abs[7]:
            dt = cur_t - t_abs[6]
            v_t = v[1] - amax*dt + 0.5*aa*pow(dt, 2)
        velocity.append(v_t)

    import numpy as np
    import matplotlib.pyplot as plt

    x_axis = np.array(times)
    y_axis = np.array(velocity)
    plt.plot(x_axis, y_axis)
    plt.show()


if __name__ == '__main__':
    aac(3.0, 1.0, 20.1, 500)
    aac(3.0, 1.0, 20.1, 100)
    aac(3.0, 1.0, 20.1, 50)