Commit fb1d67f0 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Use the correct time when staring execution of a timed command

Do not override a brake.
parent 64fbd94c
......@@ -207,6 +207,7 @@ static void calculate(struct controller_block *spg)
{
struct controller_block_private *priv = spg->private;
bool ignore_x = false;
bool must_brake = false;
if (*priv->reset) {
priv->cmd_x = *priv->reset_x;
......@@ -251,7 +252,7 @@ static void calculate(struct controller_block *spg)
if (!priv->current_command.start) {
t = (double)(priv->current_command.time
-controller_time_seconds) * priv->freq -
-(double)controller_time_samplenr;
(double)controller_time_samplenr;
if (t < 1.0 ||
priv->current_command.time <
controller_time_seconds) {
......@@ -579,6 +580,7 @@ static void calculate(struct controller_block *spg)
/* once we are still, would we still be within limits? */
if ((x > priv->max_x || x < priv->min_x) && priv->cur_j != j) {
priv->cur_j = j;
must_brake = true;
}
}
}
......@@ -609,19 +611,20 @@ static void calculate(struct controller_block *spg)
} else {
priv->cur_j = j;
}
must_brake = true;
}
}
/* Is the difference between spg and command small enough?
If so, make outputs equal to command */
if (fabs(priv->cmd_x - priv->cur_x) < priv->precision_x) {
if (fabs(priv->cmd_v - priv->cur_v) < priv->precision_v) {
if (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;
}
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->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;
}
priv->cur_a += priv->cur_j;
......
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