Skip to content

GitLab

Commit 509f9d68 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Some small improvements.

parent 4863fafa
Hide whitespace changes
Inline Side-by-side
controller/block/block_setpoint_generator.c
View file @ 509f9d68
......@@ -97,6 +97,8 @@ struct controller_block_private {
/* conversion factor for seconds/tick and its inverse */
float tick; /* seconds per tick */
float freq; /* ticks per second */
float freq2;
float freq3;
/* parameters in real world format (time unit: second) */
float max_v_sec;
......@@ -210,25 +212,29 @@ static double x_after_ticks(struct controller_block_private *priv,
return x_at_t;
}
static void calculate(struct controller_block *spg)
static void setpoint_generator_calculate(struct controller_block *spg)
{
struct controller_block_private *priv = spg->private;
double cur_x, cur_v;
bool ignore_x = false;
bool must_brake = false;
bool good_x;
bool good_v;
cur_x = priv->cur_x;
cur_v = priv->cur_v;
if (*priv->reset) {
priv->cmd_x = *priv->reset_x;
priv->cur_x = priv->cmd_x;
cur_x = priv->cmd_x;
priv->current_command.done = 1;
priv->current_command.type = BLOCK_SPG_SETPOINT;
priv->next_command.done = 1;
priv->cmd_v = 0.0;
priv->cur_v = 0.0;
cur_v = 0.0;
priv->cur_a = 0.0;
priv->cur_j = 0.0;
priv->start_x = priv->cur_x;
priv->start_x = cur_x;
priv->start_v = 0.0;
priv->start_a = 0.0;
priv->start_j = 0.0;
......@@ -305,7 +311,7 @@ static void calculate(struct controller_block *spg)
if (priv->current_command.type == BLOCK_SPG_SPEED) {
ignore_x = true;
priv->cmd_x = priv->cur_x;
priv->cmd_x = cur_x;
}
if (priv->cmd_x > priv->max_x)
......@@ -313,8 +319,8 @@ static void calculate(struct controller_block *spg)
if (priv->cmd_x < priv->min_x)
priv->cmd_x = priv->min_x;
good_x = almost_equal(priv->cur_x, priv->cmd_x);
good_v = almost_equal(priv->cur_v, priv->cmd_v);
good_x = almost_equal(cur_x, priv->cmd_x);
good_v = almost_equal(cur_v, priv->cmd_v);
if (!good_v || !good_x) {
double error_x;
......@@ -322,7 +328,7 @@ static void calculate(struct controller_block *spg)
double error_x_jpos;
double error_x_j0;
double error_x_jneg;
double error_v = priv->cmd_v - priv->cur_v;
double error_v = priv->cmd_v - 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;
......@@ -334,8 +340,8 @@ static void calculate(struct controller_block *spg)
requested speed.
*/
x = priv->cur_x;
v = priv->cur_v;
x = cur_x;
v = cur_v;
a = priv->cur_a;
req_x = priv->cmd_x;
req_v = priv->cmd_v;
......@@ -527,12 +533,12 @@ static void calculate(struct controller_block *spg)
/* When moving can we brake before the position limits? */
if (fabs(priv->cur_v) > 0.0) {
if (fabs(cur_v) > 0.0) {
double t, x, v, a, j;
bool done = false;
x = priv->cur_x;
v = priv->cur_v;
x = cur_x;
v = cur_v;
a = priv->cur_a;
/* add 1 tick, we want to know if we are still safe untill the next
......@@ -582,7 +588,7 @@ static void calculate(struct controller_block *spg)
/* Assuming we have a constant v,
check if we have to start deceleration now. */
t = ticks_to_v(priv, priv->cur_v, 0);
t = ticks_to_v(priv, cur_v, 0);
/* will we hit max a? */
if (fabs(a_after_ticks(priv, a, j, t/2)) > priv->max_a) {
......@@ -630,7 +636,7 @@ static void calculate(struct controller_block *spg)
if (fabs(priv->cur_a) > 0.0) {
double t, v, a, j;
v = priv->cur_v;
v = cur_v;
a = priv->cur_a;
/* add 1 tick, we want to know if we are still safe untill the next
......@@ -665,30 +671,30 @@ static void calculate(struct controller_block *spg)
/* Is the difference between spg and command small enough?
If so, make outputs equal to command */
if (!must_brake &&
fabs(priv->cmd_x - priv->cur_x) < priv->precision_x &&
fabs(priv->cmd_v - priv->cur_v) < priv->precision_v &&
fabs(priv->cmd_x - cur_x) < priv->precision_x &&
fabs(priv->cmd_v - cur_v) < priv->precision_v &&
fabs(priv->cur_a) < priv->precision_a) {
priv->cur_j = 0.0;
priv->cur_a = 0.0;
priv->cur_v = priv->cmd_v;
priv->cur_x = priv->cmd_x;
cur_v = priv->cmd_v;
cur_x = priv->cmd_x;
}
/* 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_v = cur_v;
priv->start_x = 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,
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,
cur_x = x_after_ticks(priv,
priv->start_x, priv->start_v, priv->start_a, priv->start_j,
priv->start_t);
......@@ -699,58 +705,60 @@ static void calculate(struct controller_block *spg)
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_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
if (fabs(priv->cur_v) >= priv->max_v) {
if (fabs(cur_v) >= priv->max_v) {
/* prevent further acceleration beyond max v */
if (signbit(priv->cur_v) == signbit(priv->cur_a)) {
if (signbit(cur_v) == signbit(priv->cur_a)) {
priv->cur_a = 0.0;
}
if (signbit(priv->cur_v) == signbit(priv->cur_j)) {
if (signbit(cur_v) == signbit(priv->cur_j)) {
priv->cur_j = 0.0;
}
priv->cur_v = copysign(priv->max_v, priv->cur_v);
cur_v = copysign(priv->max_v, 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_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
if (priv->cur_x > priv->max_x) {
priv->cur_x = priv->max_x;
priv->cur_v = 0;
if (cur_x > priv->max_x) {
cur_x = priv->max_x;
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_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
if (priv->cur_x < priv->min_x) {
priv->cur_x = priv->min_x;
priv->cur_v = 0;
if (cur_x < priv->min_x) {
cur_x = priv->min_x;
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_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
priv->cur_x_out = priv->cur_x;
priv->cur_v_out = priv->cur_v * priv->freq;
priv->cur_a_out = priv->cur_a * priv->freq * priv->freq;
priv->cur_j_out = priv->cur_j * priv->freq * priv->freq * priv->freq;
priv->cur_x_out = cur_x;
priv->cur_v_out = cur_v * priv->freq;
priv->cur_a_out = priv->cur_a * priv->freq2;
priv->cur_j_out = priv->cur_j * priv->freq3;
priv->cmd_x_out = priv->cmd_x;
priv->cur_x = cur_x;
priv->cur_v = cur_v;
}
static bool block_setpoint_generator_queue_space(struct controller_block *spg)
......@@ -895,6 +903,8 @@ static void param_set(struct controller_block *spg, int param, va_list val)
spg->private->precision_a = spg->private->precision_a_sec * spg->private->tick * spg->private->tick;
spg->private->freq = 1.0 / spg->private->tick;
spg->private->freq2 = spg->private->freq * spg->private->freq;
spg->private->freq3 = spg->private->freq2 * spg->private->freq;
spg->private->inv_max_j = 1.0 / spg->private->max_j;
spg->private->inv_max_a = 1.0 / spg->private->max_a;
}
......@@ -960,6 +970,8 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
spg->private->max_a_sec = 0.0;
spg->private->max_j_sec = 0.0;
spg->private->freq = 1.0;
spg->private->freq2 = 1.0;
spg->private->freq3 = 1.0;
spg->private->precision_x = 0.0;
spg->private->precision_v = 0.0;
spg->private->precision_a = 0.0;
......@@ -988,7 +1000,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
if (controller_block_outterm_list_init(spg, outterms))
goto err_input;
spg->calculate = calculate;
spg->calculate = setpoint_generator_calculate;
if (controller_block_param_list_init(spg, params))
goto err_output;
......
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