cos curve for stepper monitor

ardino_cos_curve.c

void cos_curve(float a_max, float w_start, float w_max, int total, int pin) {
#define STATE_ACC 0
#define STATE_CONST 1
#define STATE_DEC 2

	if (w_start > w_max) w_start = w_max;

	float w_i = w_start;
	float t_i = 0.000001;
	int count = 0;
	int state = STATE_ACC;
	int flat_point = 0;

	/* temp variable */
	float w_next;
	float t_next;
	float a_i;
	float tmp;

	while (count < total) {
		if (state == STATE_ACC) {
			tmp = (1 - pow(1 - 2.0*w_i/w_max, 2));
			if (count < total/2 && tmp > 0) {
				a_i = a_max * sqrt(tmp);
				w_next = w_i + a_i * t_i;
			} else {
				w_next = w_i;
				flat_point = count;
				state = STATE_CONST;
			}
		} else if (state == STATE_CONST) {
			w_next = w_i;
			if (count >= total - flat_point) {
				state = STATE_DEC;
				w_next -= 0.1;  // workaround to go next state
			}
		} else if (state == STATE_DEC) {
			tmp = (1 - pow(1 - 2*w_i/w_max, 2));
			a_i = a_max * sqrt(tmp);
			w_next = w_i - a_i * t_i;
			if (w_next <= w_start) w_next = w_start;
		}

		t_next = 1/w_next;
		int sleep_us = int(t_next*1000000/2);
		digitalWrite(pin, HIGH);
		delayMicroseconds(sleep_us);
		digitalWrite(pin, LOW);
		delayMicroseconds(sleep_us);
		// prepare for next
		w_i = w_next;
		t_i = t_next;
		count += 1;
	}
}

cos_curve.py

import numpy as np
import matplotlib.pyplot as plt


def cos_curve(a_max, w_start, w_max, total):
    w_prev = w_start
    t_i = 0.000001
    count = 0
    state = "aac"
    flat_point = 0

    times = []
    ws = []

    while count < total:
        ws.append(w_prev)
        if count == 0:
            times.append(t_i)
        else:
            times.append(times[-1] + t_i)

        if state == "aac":
            tmp = (1 - (1 - 2.0*w_prev/w_max)**2)
            if count < total/2 and tmp > 0:
                a_i = a_max * tmp**0.5
                w_next = w_prev + a_i * t_i
            else:
                w_next = w_prev
                flat_point = count
                state = "const"
        elif state == "const":
            w_next = w_prev
            if count >= total - flat_point:
                state = "dec"
                w_next -= 0.1
        elif state == "dec":
            tmp = (1 - (1 - 2*w_prev/w_max)**2)
            a_i = a_max * (tmp)**0.5
            w_next = w_prev - a_i * t_i
            if w_next <= w_start:
                w_next = w_start

        t_next = 1/w_next
        # prepare for next
        w_prev = w_next
        t_i = t_next
        count += 1

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


cos_curve(20, 20, 80, 1000)

Ref:
http://www.cmemo.org.cn/CN/article/downloadArticleFile.do?attachType=PDF&id=3489