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
f3e1ff7a
Commit
f3e1ff7a
authored
Mar 06, 2013
by
Jeroen Vreeken
Browse files
Change terminal names of servo state block
parent
e1fbd0f7
Changes
2
Hide whitespace changes
Inline
Side-by-side
controller/controller/block_servo_state.c
View file @
f3e1ff7a
...
...
@@ -32,11 +32,11 @@
---------------------------
| |
---| 0 spg
0
0 out
0
|----
---| 0 spg
_x
0 out
_x
|----
| |
---| 1 spg
1
1 out
1
|----
---| 1 spg
_v
1 out
_v
|----
| |
---| 2 spg
2
2 out
2
|----
---| 2 spg
_a
2 out
_a
|----
| |
---| 3 safe 3 spg_rst |----
| |
...
...
@@ -47,21 +47,21 @@
*/
struct
controller_block_private
{
float
*
spg
0
;
float
*
spg
1
;
float
*
spg
2
;
float
*
spg
_x
;
float
*
spg
_v
;
float
*
spg
_a
;
bool
*
safe
;
float
out
0
;
float
out
1
;
float
out
2
;
float
out
_x
;
float
out
_v
;
float
out
_a
;
bool
reset
;
bool
enable
;
bool
emergency
;
float
min
0
;
float
max
0
;
float
max
1
;
float
max
2
;
float
min
_x
;
float
max
_x
;
float
max
_v
;
float
max
_a
;
enum
{
SERVO_STATE_DISABLED
,
SERVO_STATE_ENABLED
,
...
...
@@ -86,9 +86,9 @@ static void calculate(struct controller_block *servo_state)
}
if
(
servo_state
->
private
->
override
)
{
servo_state
->
private
->
out
0
=
*
servo_state
->
private
->
spg
0
;
servo_state
->
private
->
out
1
=
*
servo_state
->
private
->
spg
1
;
servo_state
->
private
->
out
2
=
*
servo_state
->
private
->
spg
2
;
servo_state
->
private
->
out
_x
=
*
servo_state
->
private
->
spg
_x
;
servo_state
->
private
->
out
_v
=
*
servo_state
->
private
->
spg
_v
;
servo_state
->
private
->
out
_a
=
*
servo_state
->
private
->
spg
_a
;
servo_state
->
private
->
enable
=
true
;
servo_state
->
private
->
reset
=
false
;
return
;
...
...
@@ -96,12 +96,12 @@ static void calculate(struct controller_block *servo_state)
switch
(
servo_state
->
private
->
state
)
{
case
SERVO_STATE_ENABLED
:
servo_state
->
private
->
out
0
=
*
servo_state
->
private
->
spg
0
;
servo_state
->
private
->
out
1
=
*
servo_state
->
private
->
spg
1
;
servo_state
->
private
->
out
2
=
*
servo_state
->
private
->
spg
2
;
servo_state
->
private
->
out
_x
=
*
servo_state
->
private
->
spg
_x
;
servo_state
->
private
->
out
_v
=
*
servo_state
->
private
->
spg
_v
;
servo_state
->
private
->
out
_a
=
*
servo_state
->
private
->
spg
_a
;
if
(
!
safe
)
{
servo_state
->
private
->
enable
=
false
;
...
...
@@ -128,9 +128,9 @@ static void calculate(struct controller_block *servo_state)
}
/* Fallthrough if disabling */
case
SERVO_STATE_DISABLING
:
{
float
out
0
=
servo_state
->
private
->
out
0
;
float
out
1
=
servo_state
->
private
->
out
1
;
float
out
2
=
0
.
0
;
float
out
_x
=
servo_state
->
private
->
out
_x
;
float
out
_v
=
servo_state
->
private
->
out
_v
;
float
out
_a
=
0
.
0
;
float
t
=
controller_sample_period
();
...
...
@@ -145,23 +145,23 @@ static void calculate(struct controller_block *servo_state)
break
;
}
if
(
out
1
>
0
.
0
)
{
out
2
=
-
servo_state
->
private
->
max
2
;
out
1
+=
out
2
*
t
;
if
(
out
1
<
0
.
0
)
{
out
2
+=
out
1
;
out
1
=
0
.
0
;
if
(
out
_v
>
0
.
0
)
{
out
_a
=
-
servo_state
->
private
->
max
_a
;
out
_v
+=
out
_a
*
t
;
if
(
out
_v
<
0
.
0
)
{
out
_a
+=
out
_v
;
out
_v
=
0
.
0
;
}
}
else
if
(
out
1
<
0
.
0
)
{
out
2
=
servo_state
->
private
->
max
2
;
out
1
+=
out
2
*
t
;
if
(
out
1
>
0
.
0
)
{
out
2
-=
out
1
;
out
1
=
0
.
0
;
}
else
if
(
out
_v
<
0
.
0
)
{
out
_a
=
servo_state
->
private
->
max
_a
;
out
_v
+=
out
_a
*
t
;
if
(
out
_v
>
0
.
0
)
{
out
_a
-=
out
_v
;
out
_v
=
0
.
0
;
}
}
if
(
out
1
==
0
.
0
)
{
if
(
out
_v
==
0
.
0
)
{
servo_state
->
private
->
state
=
SERVO_STATE_DISABLED
;
log_send
(
LOG_T_INFO
,
...
...
@@ -169,16 +169,16 @@ static void calculate(struct controller_block *servo_state)
servo_state
->
name
);
}
out
0
+=
out
1
*
t
;
out
_x
+=
out
_v
*
t
;
if
(
out
0
>
servo_state
->
private
->
max
0
)
out
0
=
servo_state
->
private
->
max
0
;
if
(
out
0
<
servo_state
->
private
->
min
0
)
out
0
=
servo_state
->
private
->
min
0
;
if
(
out
_x
>
servo_state
->
private
->
max
_x
)
out
_x
=
servo_state
->
private
->
max
_x
;
if
(
out
_x
<
servo_state
->
private
->
min
_x
)
out
_x
=
servo_state
->
private
->
min
_x
;
servo_state
->
private
->
out
0
=
out
0
;
servo_state
->
private
->
out
1
=
out
1
;
servo_state
->
private
->
out
2
=
out
2
;
servo_state
->
private
->
out
_x
=
out
_x
;
servo_state
->
private
->
out
_v
=
out
_v
;
servo_state
->
private
->
out
_a
=
out
_a
;
servo_state
->
private
->
enable
=
true
;
servo_state
->
private
->
reset
=
true
;
...
...
@@ -186,10 +186,10 @@ static void calculate(struct controller_block *servo_state)
break
;
}
case
SERVO_STATE_DISABLED
:
servo_state
->
private
->
out
0
=
*
servo_state
->
private
->
spg
0
;
servo_state
->
private
->
out
1
=
0
.
0
;
servo_state
->
private
->
out
2
=
0
.
0
;
servo_state
->
private
->
out
_x
=
*
servo_state
->
private
->
spg
_x
;
servo_state
->
private
->
out
_v
=
0
.
0
;
servo_state
->
private
->
out
_a
=
0
.
0
;
servo_state
->
private
->
enable
=
false
;
servo_state
->
private
->
reset
=
true
;
...
...
@@ -206,11 +206,11 @@ static void calculate(struct controller_block *servo_state)
}
static
struct
controller_block_param_list
params
[]
=
{
{
"min
0
"
,
true
},
{
"max
0
"
,
true
},
{
"max
1
"
,
true
},
{
"max
2
"
,
true
},
{
"enabled"
,
true
},
{
"min
_x
"
,
true
},
{
"max
_x
"
,
true
},
{
"max
_v
"
,
true
},
{
"max
_a
"
,
true
},
{
"enabled"
,
true
},
{
"emergency"
,
true
},
{
"override"
,
true
},
{
NULL
},
...
...
@@ -220,17 +220,17 @@ static void param_get(struct controller_block *servo_state, int param, void *val
{
switch
(
param
)
{
case
0
:
*
(
float
*
)
val
=
servo_state
->
private
->
min
0
;
*
(
float
*
)
val
=
servo_state
->
private
->
min
_x
;
break
;
case
1
:
*
(
float
*
)
val
=
servo_state
->
private
->
max
0
;
*
(
float
*
)
val
=
servo_state
->
private
->
max
_x
;
break
;
case
2
:
*
(
float
*
)
val
=
servo_state
->
private
->
max
1
;
*
(
float
*
)
val
=
servo_state
->
private
->
max
_v
;
break
;
case
3
:
*
(
float
*
)
val
=
servo_state
->
private
->
max
2
;
*
(
float
*
)
val
=
servo_state
->
private
->
max
_a
;
break
;
case
4
:
*
(
bool
*
)
val
=
servo_state
->
private
->
enable_param
;
...
...
@@ -248,16 +248,16 @@ static void param_set(struct controller_block *servo_state, int param, va_list v
{
switch
(
param
)
{
case
0
:
servo_state
->
private
->
min
0
=
va_arg
(
val
,
double
);
servo_state
->
private
->
min
_x
=
va_arg
(
val
,
double
);
break
;
case
1
:
servo_state
->
private
->
max
0
=
va_arg
(
val
,
double
);
servo_state
->
private
->
max
_x
=
va_arg
(
val
,
double
);
break
;
case
2
:
servo_state
->
private
->
max
1
=
va_arg
(
val
,
double
);
servo_state
->
private
->
max
_v
=
va_arg
(
val
,
double
);
break
;
case
3
:
servo_state
->
private
->
max
2
=
va_arg
(
val
,
double
);
servo_state
->
private
->
max
_a
=
va_arg
(
val
,
double
);
break
;
case
4
:
servo_state
->
private
->
enable_param
=
va_arg
(
val
,
int
);
...
...
@@ -299,9 +299,9 @@ struct controller_block * block_servo_state_create(char *name)
servo_state
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
servo_state
->
private
)
goto
err_name
;
servo_state
->
private
->
out
0
=
0
.
0
;
servo_state
->
private
->
out
1
=
0
.
0
;
servo_state
->
private
->
out
2
=
0
.
0
;
servo_state
->
private
->
out
_x
=
0
.
0
;
servo_state
->
private
->
out
_v
=
0
.
0
;
servo_state
->
private
->
out
_a
=
0
.
0
;
servo_state
->
private
->
reset
=
true
;
servo_state
->
private
->
enable
=
false
;
servo_state
->
private
->
enable_param
=
false
;
...
...
@@ -313,17 +313,17 @@ struct controller_block * block_servo_state_create(char *name)
servo_state
->
input
=
malloc
(
sizeof
(
struct
controller_block_interm
)
*
4
);
if
(
!
servo_state
->
input
)
goto
err_private
;
servo_state
->
input
[
0
].
name
=
"spg
0
"
;
servo_state
->
input
[
0
].
name
=
"spg
_x
"
;
servo_state
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
servo_state
->
input
[
0
].
value
.
f
=
&
servo_state
->
private
->
spg
0
;
servo_state
->
input
[
0
].
value
.
f
=
&
servo_state
->
private
->
spg
_x
;
servo_state
->
input
[
0
].
ghostof
=
NULL
;
servo_state
->
input
[
1
].
name
=
"spg
1
"
;
servo_state
->
input
[
1
].
name
=
"spg
_v
"
;
servo_state
->
input
[
1
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
servo_state
->
input
[
1
].
value
.
f
=
&
servo_state
->
private
->
spg
1
;
servo_state
->
input
[
1
].
value
.
f
=
&
servo_state
->
private
->
spg
_v
;
servo_state
->
input
[
1
].
ghostof
=
NULL
;
servo_state
->
input
[
2
].
name
=
"spg
2
"
;
servo_state
->
input
[
2
].
name
=
"spg
_a
"
;
servo_state
->
input
[
2
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
servo_state
->
input
[
2
].
value
.
f
=
&
servo_state
->
private
->
spg
2
;
servo_state
->
input
[
2
].
value
.
f
=
&
servo_state
->
private
->
spg
_a
;
servo_state
->
input
[
2
].
ghostof
=
NULL
;
servo_state
->
input
[
3
].
name
=
"safe"
;
servo_state
->
input
[
3
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
...
...
@@ -334,17 +334,17 @@ struct controller_block * block_servo_state_create(char *name)
servo_state
->
output
=
malloc
(
sizeof
(
struct
controller_block_outterm
)
*
5
);
if
(
!
servo_state
->
output
)
goto
err_input
;
servo_state
->
output
[
0
].
name
=
"out
0
"
;
servo_state
->
output
[
0
].
name
=
"out
_x
"
;
servo_state
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
servo_state
->
output
[
0
].
value
.
f
=
&
servo_state
->
private
->
out
0
;
servo_state
->
output
[
0
].
value
.
f
=
&
servo_state
->
private
->
out
_x
;
servo_state
->
output
[
0
].
source
=
servo_state
;
servo_state
->
output
[
1
].
name
=
"out
1
"
;
servo_state
->
output
[
1
].
name
=
"out
_v
"
;
servo_state
->
output
[
1
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
servo_state
->
output
[
1
].
value
.
f
=
&
servo_state
->
private
->
out
1
;
servo_state
->
output
[
1
].
value
.
f
=
&
servo_state
->
private
->
out
_v
;
servo_state
->
output
[
1
].
source
=
servo_state
;
servo_state
->
output
[
2
].
name
=
"out
2
"
;
servo_state
->
output
[
2
].
name
=
"out
_a
"
;
servo_state
->
output
[
2
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
servo_state
->
output
[
2
].
value
.
f
=
&
servo_state
->
private
->
out
2
;
servo_state
->
output
[
2
].
value
.
f
=
&
servo_state
->
private
->
out
_a
;
servo_state
->
output
[
2
].
source
=
servo_state
;
servo_state
->
output
[
3
].
name
=
"reset"
;
servo_state
->
output
[
3
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
...
...
controller/dt_ctrl.ctrl
View file @
f3e1ff7a
...
...
@@ -53,18 +53,18 @@ links {
{ "azimuth_position_offset_sum", "out", "azimuth_error", "negative" , true }
{ "azimuth_servo_state", "reset", "azimuth_spg", "reset" , false }
{ "azimuth_position_offset_sum", "out", "azimuth_spg", "reset_x" , true }
{ "azimuth_spg", "x", "azimuth_servo_state", "spg
0"
, true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg
1"
, true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg
2"
, true }
{ "azimuth_spg", "x", "azimuth_servo_state", "spg
_x"
, true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg
_v"
, true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg
_a"
, true }
{ "azimuth_safety", "safe_out", "azimuth_servo_state", "safe" , false }
{ "azimuth_servo_state", "out
0
",
"azimuth_error", "positive" , true }
{ "azimuth_servo_state", "out
_x
", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
{ "azimuth_servo_state", "out
1
",
"azimuth_speed_ff", "in0" , true }
{ "azimuth_servo_state", "out
_v
", "azimuth_speed_ff", "in0" , true }
{ "azimuth_error", "difference", "azimuth_pid", "in" , true }
{ "azimuth_servo_state", "enable", "azimuth_pid", "enable" , true }
{ "azimuth_pid", "out", "azimuth_pid_limit", "in" , true }
{ "azimuth_servo_state", "out
1
",
"azimuth_pid_limit", "limit" , true }
{ "azimuth_servo_state", "out
_v
", "azimuth_pid_limit", "limit" , true }
{ "azimuth_pid_limit", "out", "azimuth_pid_filter", "in" , true }
{ "azimuth_pid_filter", "out", "azimuth_speed_ff", "in1" , true }
{ "azimuth_speed_ff", "out", "azimuth_speed_limit", "in" , true }
...
...
@@ -84,12 +84,12 @@ links {
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ "elevation_input_matrix", "out0", "elevation_spg", "reset_x" , true }
{ "elevation_spg", "x", "elevation_servo_state", "spg
0"
, true }
{ "elevation_spg", "v", "elevation_servo_state", "spg
1"
, true }
{ "elevation_spg", "a", "elevation_servo_state", "spg
2"
, true }
{ "elevation_spg", "x", "elevation_servo_state", "spg
_x"
, true }
{ "elevation_spg", "v", "elevation_servo_state", "spg
_v"
, true }
{ "elevation_spg", "a", "elevation_servo_state", "spg
_a"
, true }
{ "elevation_safety", "safe_out", "elevation_servo_state", "safe" , false }
{ "elevation_servo_state", "out
0
",
"elevation_error", "positive" , true }
{ "elevation_servo_state", "out
1
",
"elevation_speed_ff", "in0" , true }
{ "elevation_servo_state", "out
_x
", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out
_v
", "elevation_speed_ff", "in0" , true }
{ "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true }
{ "dt_el_l", "position", "elevation_position_offset_l_sum", "in0" , true }
{ "elevation_position_offset_r", "value", "elevation_position_offset_r_sum", "in1" , true }
...
...
@@ -208,10 +208,10 @@ params {
{ "azimuth_spg", "precision_x", (float) 0.000001 }
{ "azimuth_spg", "precision_a", (float) 0.000001 }
{ "azimuth_spg", "precision_v", (float) 0.000001 }
{ "azimuth_servo_state", "max
0
",
(float) 4.712388980384689858 }
{ "azimuth_servo_state", "min
0
",
(float)-4.712388980384689858 }
{ "azimuth_servo_state", "max
1
",
(float) 0.01466076571675236845 }
{ "azimuth_servo_state", "max
2
",
(float) 0.0002 }
{ "azimuth_servo_state", "max
_x
", (float) 4.712388980384689858 }
{ "azimuth_servo_state", "min
_x
", (float)-4.712388980384689858 }
{ "azimuth_servo_state", "max
_v
", (float) 0.01466076571675236845 }
{ "azimuth_servo_state", "max
_a
", (float) 0.0002 }
{ "azimuth_pid", "kp", (float) 0.20 }
{ "azimuth_pid", "ki", (float) 0.00 }
{ "azimuth_pid", "kd", (float) 0.0 }
...
...
@@ -259,10 +259,10 @@ params {
{ "elevation_spg", "precision_x", (float) 0.000001 }
{ "elevation_spg", "precision_a", (float) 0.000001 }
{ "elevation_spg", "precision_v", (float) 0.000001 }
{ "elevation_servo_state", "max
0
",
(float) 1.570796326794896619 }
{ "elevation_servo_state", "min
0
",
(float)-0.0008726646259971647885 }
{ "elevation_servo_state", "max
1
",
(float) 0.004921828490624009407 }
{ "elevation_servo_state", "max
2
",
(float) 0.0003 }
{ "elevation_servo_state", "max
_x
", (float) 1.570796326794896619 }
{ "elevation_servo_state", "min
_x
", (float)-0.0008726646259971647885 }
{ "elevation_servo_state", "max
_v
", (float) 0.004921828490624009407 }
{ "elevation_servo_state", "max
_a
", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
{ "elevation_torsion_spg", "max_x", (float) 0.01 }
...
...
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