Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Tammo Jan Dijkema
dt_ctrl
Commits
509f9d68
Commit
509f9d68
authored
Nov 28, 2013
by
Jeroen Vreeken
Browse files
Some small improvements.
parent
4863fafa
Changes
1
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
->
freq
2
;
priv
->
cur_j_out
=
priv
->
cur_j
*
priv
->
freq
3
;
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
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment