Commit 898f22c4 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Fix comment

parent 509f9d68
...@@ -99,6 +99,8 @@ struct controller_block_private { ...@@ -99,6 +99,8 @@ struct controller_block_private {
float freq; /* ticks per second */ float freq; /* ticks per second */
float freq2; float freq2;
float freq3; float freq3;
double t_max_a;
double v_delta_from_max_a;
/* parameters in real world format (time unit: second) */ /* parameters in real world format (time unit: second) */
float max_v_sec; float max_v_sec;
...@@ -160,8 +162,8 @@ static double ticks_to_v(struct controller_block_private *priv, ...@@ -160,8 +162,8 @@ static double ticks_to_v(struct controller_block_private *priv,
double t; double t;
/* From a constant speed to a constant speed is done in two halves: /* From a constant speed to a constant speed is done in two halves:
First halve (untill half the speed difference is reached is done First halve (untill half the speed difference is reached) is
at max jerk, second half is done at -max jerk. done at max jerk, second half is done at -max jerk.
1/2 v = 1/2 j t^2 -> v = j t^2 -> t = sqrt(v/j) 1/2 v = 1/2 j t^2 -> v = j t^2 -> t = sqrt(v/j)
...@@ -363,10 +365,8 @@ static void setpoint_generator_calculate(struct controller_block *spg) ...@@ -363,10 +365,8 @@ static void setpoint_generator_calculate(struct controller_block *spg)
j_from_pos = 0; j_from_pos = 0;
} }
t_max_a = priv->t_max_a;
/* Calculate delta v when comming from max_a */ v_delta_from_max_a = priv->v_delta_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); j = copysign(priv->max_j, error_v);
...@@ -596,7 +596,7 @@ static void setpoint_generator_calculate(struct controller_block *spg) ...@@ -596,7 +596,7 @@ static void setpoint_generator_calculate(struct controller_block *spg)
double v_start_3, t_2; double v_start_3, t_2;
t = ticks_to_a(priv, 0, priv->max_a); t = t_max_a;
x = x_after_ticks(priv, x, v, a, j, t); x = x_after_ticks(priv, x, v, a, j, t);
v = v_after_ticks(priv, v, a, j, t); v = v_after_ticks(priv, v, a, j, t);
...@@ -855,6 +855,8 @@ static void param_get(struct controller_block *spg, int param, void *val) ...@@ -855,6 +855,8 @@ static void param_get(struct controller_block *spg, int param, void *val)
static void param_set(struct controller_block *spg, int param, va_list val) static void param_set(struct controller_block *spg, int param, va_list val)
{ {
double t_max_a;
switch (param) { switch (param) {
case 0: case 0:
spg->private->cmd_x = va_arg(val, double); spg->private->cmd_x = va_arg(val, double);
...@@ -907,6 +909,12 @@ static void param_set(struct controller_block *spg, int param, va_list val) ...@@ -907,6 +909,12 @@ static void param_set(struct controller_block *spg, int param, va_list val)
spg->private->freq3 = spg->private->freq2 * 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_j = 1.0 / spg->private->max_j;
spg->private->inv_max_a = 1.0 / spg->private->max_a; spg->private->inv_max_a = 1.0 / spg->private->max_a;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
spg->private->v_delta_from_max_a =
v_after_ticks(spg->private, 0, 0, spg->private->max_j, t_max_a);
spg->private->t_max_a = t_max_a;
} }
static struct controller_block_interm_list interms[] = { static struct controller_block_interm_list interms[] = {
......
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