void dual_quadramp_do_filter(struct dual_quadramp_filter *q) {
int32_t x_speed, y_speed;
int32_t x_acc, y_acc;
int x_distance, y_distance;
x_distance = abs(q->x_pos - q->x_consign);
y_distance = abs(q->y_pos - q->y_consign);
x_speed = (int32_t)(q->speed * x_distance / (float)(x_distance + y_distance));
y_speed = (int32_t)(q->speed * y_distance / (float)(x_distance + y_distance));
x_acc = (int32_t)(q->acc * x_distance / (float)(x_distance + y_distance));
y_acc = (int32_t)(q->acc * y_distance / (float)(x_distance + y_distance));
quadramp_set_1st_order_vars(&q->x_quadramp, x_speed, x_speed);
quadramp_set_2nd_order_vars(&q->x_quadramp, x_acc, x_acc);
quadramp_set_1st_order_vars(&q->y_quadramp, y_speed, y_speed);
quadramp_set_2nd_order_vars(&q->y_quadramp, y_acc, y_acc);
q->x_pos = quadramp_do_filter((void *)&q->x_quadramp, q->x_consign);
q->y_pos = quadramp_do_filter((void *)&q->y_quadramp, q->y_consign);
}
int main(void)
{
struct quadramp_filter q;
int32_t in=0, out=0, prev_out=0;
int32_t i=0;
quadramp_init(&q);
for (i=0 ; i<4000 ; i++) {
switch(i) {
case 0:
quadramp_set_1st_order_vars(&q, 50, 100);
quadramp_set_2nd_order_vars(&q, 1, 2);
in = 10000;
break;
case 600:
in = 9000;
break;
case 630:
in = 10000;
break;
case 1000:
in = -5000;
break;
case 1500:
in = -4000;
break;
case 2000:
quadramp_set_1st_order_vars(&q, 10, 10);
quadramp_set_2nd_order_vars(&q, 2, 2);
in = 10000;
break;
case 3000:
quadramp_set_1st_order_vars(&q, 100, 100);
break;
default:
break;
}
out = quadramp_do_filter(&q, in);
printf("%" SCNu32 " %" SCNu32 " %" SCNu32 " %" SCNu32 "\n", i, in, out, out-prev_out);
prev_out = out;
}
return 0;
}
