Commit 17ceb08c authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Fix setpoint generator behaviour at high speeds.

Also make predictions more reliable by using position/velocity algorithms
parent d92ca07d
......@@ -107,6 +107,13 @@ struct controller_block_private {
float precision_v_sec;
float precision_a_sec;
/* state at last change */
double start_x;
double start_v;
double start_a;
double start_j;
int start_t;
/* current internal state */
double cur_x;
double cur_v;
......@@ -208,6 +215,8 @@ static void calculate(struct controller_block *spg)
struct controller_block_private *priv = spg->private;
bool ignore_x = false;
bool must_brake = false;
bool good_x;
bool good_v;
if (*priv->reset) {
priv->cmd_x = *priv->reset_x;
......@@ -219,16 +228,23 @@ static void calculate(struct controller_block *spg)
priv->cur_v = 0.0;
priv->cur_a = 0.0;
priv->cur_j = 0.0;
priv->start_x = priv->cur_x;
priv->start_v = 0.0;
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
}
if (priv->current_command.done) {
if (!priv->next_command.done) {
printf("%d next\n", controller_time_samplenr);
priv->current_command = priv->next_command;
priv->next_command.done = 1;
priv->current_command.start = 0;
} else {
if (priv->current_command.type ==
BLOCK_SPG_SETPOINT_TIME) {
printf("%d v = 0.0\n", controller_time_samplenr);
priv->cmd_v = 0.0;
priv->cmd_x =
priv->current_command.setpoint;
......@@ -266,8 +282,12 @@ static void calculate(struct controller_block *spg)
} else {
priv->cmd_v =
(priv->current_command.setpoint -
priv->cmd_x) / t;
priv->cmd_x) / (t+1);
priv->current_command.start = 1;
printf("%d t: %g cmd_v: %.8g diff: %.8g\n",
controller_time_samplenr,
t, priv->cmd_v,
priv->current_command.setpoint - priv->cmd_x);
}
}
......@@ -277,6 +297,7 @@ static void calculate(struct controller_block *spg)
priv->cmd_x =
priv->current_command.setpoint -
priv->cmd_v;
printf("%d done\n", controller_time_samplenr);
}
break;
}
......@@ -292,8 +313,6 @@ static void calculate(struct controller_block *spg)
if (priv->current_command.type == BLOCK_SPG_SPEED) {
ignore_x = true;
priv->cmd_x = priv->cur_x;
} else {
priv->cmd_x += priv->cmd_v;
}
if (priv->cmd_x > priv->max_x)
......@@ -301,13 +320,21 @@ static void calculate(struct controller_block *spg)
if (priv->cmd_x < priv->min_x)
priv->cmd_x = priv->min_x;
if (!almost_equal(priv->cur_v, priv->cmd_v) ||
!almost_equal(priv->cur_x, priv->cmd_x)) {
double error_x = priv->cmd_x - priv->cur_x;
good_x = almost_equal(priv->cur_x, priv->cmd_x);
good_v = almost_equal(priv->cur_v, priv->cmd_v);
if (!good_v || !good_x) {
double error_x_cur;
double error_x;
double req_x_1;
double error_x_jpos;
double error_x_j0;
double error_x_jneg;
double error_v = priv->cmd_v - priv->cur_v;
double x, v, a, j, t;
double req_x, req_v, t_max_a, v_delta_from_max_a;
double error_v_after_a, error_x_at_v;
double j_from_pos;
bool state_at_max_a = false;
bool state_to_max_a = false;
......@@ -321,13 +348,32 @@ static void calculate(struct controller_block *spg)
req_x = priv->cmd_x;
req_v = priv->cmd_v;
req_x_1 = req_x + req_v;
error_x_jpos = req_x_1 - x_after_ticks(priv, x, v, a, priv->max_j, 1);
error_x_j0 = req_x_1 - x_after_ticks(priv, x, v, a, 0, 1);
error_x_jneg = req_x_1 - x_after_ticks(priv, x, v, a, -priv->max_j, 1);
if (fabs(error_x_jpos) < fabs(error_x_jneg)) {
error_x = error_x_jpos;
j_from_pos = priv->max_j;
} else {
error_x = error_x_jneg;
j_from_pos = -priv->max_j;
}
if (fabs(error_x_j0) < fabs(error_x)) {
error_x = error_x_j0;
j_from_pos = 0;
}
error_x_cur = req_x - x;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(priv, 0, priv->max_a);
v_delta_from_max_a = v_after_ticks(priv, 0, 0, priv->max_j, t_max_a);
j = copysign(priv->max_j, error_v);
if (fabs(a) > priv->max_j) {
if (fabs(a) >= priv->max_j) {
/* Not at constant velocity */
if (signbit(a) != signbit(error_v)) {
......@@ -362,6 +408,7 @@ static void calculate(struct controller_block *spg)
t_a_to_0 = ticks_to_a(priv, a, 0);
} else if (fabs(a_peak) > priv->max_a) {
/* We are going to hit maximum acc */
a_peak = copysign(priv->max_a, a_peak);
t = ticks_to_a(priv, 0, a_peak);
......@@ -394,6 +441,7 @@ static void calculate(struct controller_block *spg)
if (t_to_max_a >= 1.0) {
state_to_max_a = true;
/* accelerate to max a */
x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
v = v_after_ticks(priv, v, a, j, t_to_max_a);
a = a_after_ticks(priv, a, j, t_to_max_a);
......@@ -402,9 +450,9 @@ static void calculate(struct controller_block *spg)
state_at_max_a = true;
}
}
/* decellerate to a==0 */
x = x_after_ticks(priv, x, v, a, -j, t_a_to_0);
v = v_after_ticks(priv, v, a, -j, t_a_to_0);
a = a_after_ticks(priv, a, -j, t_a_to_0);
req_x = req_x + req_v * t_a_to_0;
a = 0;
}
......@@ -427,7 +475,7 @@ static void calculate(struct controller_block *spg)
x = x_after_ticks(priv, x, v, 0, j, t_max_a);
v = v_after_ticks(priv, v, 0, j, t_max_a);
a = a_after_ticks(priv, 0, j, t_max_a);
a = copysign(priv->max_a, j);
req_x = req_x + req_v * t_max_a;
t_at_max_a = (fabs(error_v_after_a) - v_delta_from_max_a*2) * priv->inv_max_a;
......@@ -439,7 +487,6 @@ static void calculate(struct controller_block *spg)
x = x_after_ticks(priv, x, v, a, -j, t_max_a);
v = v_after_ticks(priv, v, a, -j, t_max_a);
req_x = req_x + req_v * t_max_a;
a = a_after_ticks(priv, a, -j, t_max_a);
a = 0;
} else {
/* Simple profile: triangle
......@@ -463,28 +510,41 @@ static void calculate(struct controller_block *spg)
error_x_at_v = req_x - x;
if (fabs(error_x_at_v) < fabs(error_x) || ignore_x) {
if (fabs(error_x_at_v) > req_v && !ignore_x &&
printf("%s req_x_1=%.8g req_v=%.8g cur_x=%.8g cur_j=%g %g %g %g %g %g", spg->name, req_x_1, req_v, priv->cur_x, priv->cur_j, error_x_cur, error_x_at_v, error_x, j, j_from_pos);
if (fabs(error_x_at_v) < fabs(error_x) ||
(signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) ||
ignore_x) {
printf(" diff=%g", fabs(error_x_at_v) - fabs(error_x));
if (!ignore_x &&
// fabs(error_x_at_v) >= fabs(priv->cur_v) &&
signbit(error_x_at_v) == signbit(error_x) ) {
/* position got better, but not yet enough */
priv->cur_j = copysign(priv->max_j, error_x);
priv->cur_j = j_from_pos;
printf(" j_from_pos");
// /* position got better, but not yet enough */
// priv->cur_j = copysign(priv->max_j, error_x);
} else {
if (state_to_max_a) {
priv->cur_j = j;
printf(" to max a");
} else if (state_at_max_a) {
priv->cur_j = 0;
printf(" at max a");
} else {
priv->cur_j = -j;
printf(" acc down");
}
}
} else {
/* going to requested speed would make position error
bigger, first go to position.
*/
priv->cur_j = copysign(priv->max_j, error_x);
priv->cur_j = j_from_pos;
}
printf(" j=%g\n", priv->cur_j);
}
/* When moving can we brake before the position limits? */
if (fabs(priv->cur_v) > 0.0) {
double t, x, v, a, j;
......@@ -585,6 +645,7 @@ static void calculate(struct controller_block *spg)
}
}
/* If accelerating, can we decelerate before going beyond our max vel */
if (fabs(priv->cur_a) > 0.0) {
double t, v, a, j;
......@@ -615,6 +676,11 @@ static void calculate(struct controller_block *spg)
}
}
if (!ignore_x) {
priv->cmd_x += priv->cmd_v;
}
/* Is the difference between spg and command small enough?
If so, make outputs equal to command */
if (!must_brake &&
......@@ -627,14 +693,35 @@ static void calculate(struct controller_block *spg)
priv->cur_x = priv->cmd_x;
}
priv->cur_a += priv->cur_j;
/* new jerk? */
if (!almost_equal(priv->cur_j, priv->start_j)) {
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = priv->cur_v;
priv->start_x = priv->cur_x;
priv->start_t = 0;
}
priv->start_t++;
priv->cur_a = a_after_ticks(priv,
priv->start_a, priv->start_j, priv->start_t);
priv->cur_v = v_after_ticks(priv,
priv->start_v, priv->start_a, priv->start_j, priv->start_t);
priv->cur_x = x_after_ticks(priv,
priv->start_x, priv->start_v, priv->start_a, priv->start_j,
priv->start_t);
if (fabs(priv->cur_a) > priv->max_a) {
priv->cur_a = copysign(priv->max_a, priv->cur_a);
priv->cur_j = 0;
}
priv->cur_v += priv->cur_a;
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = priv->cur_v;
priv->start_x = priv->cur_x;
priv->start_t = 0;
}
if (fabs(priv->cur_v) >= priv->max_v) {
/* prevent further acceleration beyond max v */
......@@ -645,21 +732,37 @@ static void calculate(struct controller_block *spg)
priv->cur_j = 0.0;
}
priv->cur_v = copysign(priv->max_v, priv->cur_v);
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = priv->cur_v;
priv->start_x = priv->cur_x;
priv->start_t = 0;
}
priv->cur_x += priv->cur_v;
if (priv->cur_x > priv->max_x) {
priv->cur_x = priv->max_x;
priv->cur_v = 0;
priv->cur_a = 0;
priv->cur_j = 0;
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = priv->cur_v;
priv->start_x = priv->cur_x;
priv->start_t = 0;
}
if (priv->cur_x < priv->min_x) {
priv->cur_x = priv->min_x;
priv->cur_v = 0;
priv->cur_a = 0;
priv->cur_j = 0;
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = priv->cur_v;
priv->start_x = priv->cur_x;
priv->start_t = 0;
}
priv->cur_x_out = priv->cur_x;
......@@ -887,6 +990,11 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
spg->private->cur_v = 0.0;
spg->private->cur_a = 0.0;
spg->private->cur_j = 0.0;
spg->private->start_x = 0.0;
spg->private->start_v = 0.0;
spg->private->start_a = 0.0;
spg->private->start_j = 0.0;
spg->private->start_t = 0;
spg->private->cur_x_out = 0.0;
spg->private->cur_v_out = 0.0;
spg->private->cur_a_out = 0.0;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment