Trapezoidal velocity profile function, handles special cases too

trapez_profile.py

def trapez_profile(t, accel, vel_max, v_start, v_end, t_total):
    """Trapezoidal velocity profile function, handles special cases too
    
    Parameters:
    t: current time
    accel: the maximum acceleration (and deceleration) of the system
    vel_max: the maximum velocity
    v_start: the starting velocity
    v_end: the desired ending velocity
    t_toal: how long should the motion be
    
    Returns:
    v: the linear velocity for time t
    """
    t_acc = (vel_max - v_start) / accel
    t_dec = (vel_max - v_end) / accel
    t_max = t_total - t_acc - t_dec    
    if t_max < 0: #not enough time
        if t_dec > t_total: #emergency brake
            t_1 = 0
            t_2 = 0
            accel = -(v_end - v_start) / t_total 
        else: #no steady
            #intersection between the lines:
            # y = t*accel + v_start --rising
            # = accel*(t_total - t) + v_end --lowering
            t_1 = (v_end + accel*t_total - v_start)/(2*accel)
            t_2 = t_1

    else: #we can do the whole cycle of accelerate, steady, decelerate
        t_1 = t_acc
        t_2 = t_acc + t_max    
    #print (t_acc, t_dec, t_max)
    #print t_1, t_2
    if t < t_1: #rising
        return t*accel + v_start
    elif t_1 <= t < t_2: #steady
        return vel_max
    elif t_2 <= t < t_total: #lowering
        return accel*(t_total - t) + v_end
    else:
        return v_end
        
#example use (pylab):
f = frompyfunc(trapez_profile, 6, 1)
t_total = 1.5
accel = 0.25
vel_max = 0.3
v_start = 0.25
v_end = 0
ts = linspace(0,t_total, 100)
y = f(ts, accel, vel_max, v_start, v_end, t_total)
figure()
plot(ts, y, '*');