Skip to content
GitLab
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
3b378c24
Commit
3b378c24
authored
Sep 02, 2018
by
Jeroen Vreeken
Browse files
Working after test at corso
parent
8b90f674
Changes
6
Hide whitespace changes
Inline
Side-by-side
controller/atsamx70/block_atsamx70_gmac.c
View file @
3b378c24
...
...
@@ -53,7 +53,7 @@ static void atsamx70_gmac_tx_calculate(struct controller_block *packet_tx)
{
struct
controller_block_private
*
priv
=
packet_tx
->
private
;
((
char
*
)
priv
->
tx_packet
)[
0
]
++
;
((
char
*
)
priv
->
tx_packet
)[
11
]
++
;
gmac_tx
(
&
priv
->
gmac
,
priv
->
tx_packet
,
priv
->
tx_size
);
}
...
...
controller/atsamx70/block_atsamx70_pwm.c
View file @
3b378c24
...
...
@@ -49,8 +49,9 @@ static void pwm_calculate(struct controller_block *pwm)
uint32_t
dty
=
abs
(
duty
);
/* Make sure we generate negative pulses for capacitor loading */
// TODO make this a parameter (in seconds off)
if
(
dty
>=
period
)
{
dty
-
-
;
dty
-
=
1
+
period
/
1000
;
}
if
(
duty
>
0
)
{
...
...
controller/atsamx70/block_atsamx70_tc.c
View file @
3b378c24
...
...
@@ -25,6 +25,11 @@
#include
<controller/controller_time.h>
#include
<log/log.h>
enum
tc_mode
{
TC_MODE_POSITION
=
0
,
TC_MODE_SPEED
=
1
,
};
struct
controller_block_private
{
float
position
;
...
...
@@ -41,45 +46,81 @@ struct controller_block_private {
uint32_t
tcn
;
};
#include
<stdio.h>
static
void
tc_calculate
(
struct
controller_block
*
tc
)
{
struct
controller_block_private
*
priv
=
tc
->
private
;
uint16_t
cnt0
;
int16_t
cnt0p
;
int16_t
cnt1
;
//
int16_t cnt1;
uint16_t
prevcnt0
=
priv
->
prevcnt0
;
float
speed
;
float
position
;
int
rev
=
priv
->
rev
;
cnt0
=
tc_get_cv
(
priv
->
tcn
,
0
);
cnt1
=
tc_get_cv
(
priv
->
tcn
,
1
);
//
cnt1 = tc_get_cv(priv->tcn, 1);
uint32_t
qisr
=
tc_get_qisr
(
priv
->
tcn
);
priv
->
homed
|=
qisr
&
TC_QISR_IDX
;
priv
->
homed
&=
!
(
*
priv
->
reset
);
cnt0p
=
cnt0
;
// if (cnt0p > rev)
// cnt0p += rev;
int16_t
diff
=
cnt0
-
prevcnt0
;
if
(
diff
>
rev
/
2
)
diff
-=
rev
;
if
(
diff
<
-
rev
/
2
)
diff
+=
rev
;
bool
dir
=
qisr
&
TC_QISR_DIR
;
speed
=
diff
*
priv
->
speedfac
;
bool
setspeed
=
true
;
cnt0p
=
cnt0
;
if
(
cnt0p
>
rev
)
cnt0p
+=
rev
;
if
(
dir
&&
cnt0
==
0
)
cnt0
=
-
rev
;
int
diff
;
// int16_t diff = cnt0 - prevcnt0;
if
(
!
dir
)
{
diff
=
(
int
)
cnt0
-
(
int
)
prevcnt0
;
/* positive */
if
(
diff
<
0
)
{
setspeed
=
false
;
}
}
else
{
diff
=
((
int
)
cnt0
-
65536
)
-
((
int
)
prevcnt0
-
65536
);
if
(
diff
>
0
)
{
setspeed
=
false
;
}
}
if
(
setspeed
)
priv
->
speed
=
diff
*
priv
->
speedfac
;
// priv->speed = diff;
position
=
(
/*cnt1 * rev + */
cnt0p
)
*
priv
->
posfac
;
priv
->
prevcnt0
=
cnt0
;
priv
->
speed
=
speed
;
priv
->
position
=
position
;
}
static
void
tc_calculate_speed
(
struct
controller_block
*
tc
)
{
struct
controller_block_private
*
priv
=
tc
->
private
;
uint16_t
cnt0
;
uint16_t
prevcnt0
=
priv
->
prevcnt0
;
float
speed
;
float
position
=
priv
->
position
;
cnt0
=
tc_get_cv
(
priv
->
tcn
,
0
);
int16_t
diff
=
cnt0
-
prevcnt0
;
speed
=
diff
*
priv
->
speedfac
;
position
+=
diff
*
priv
->
posfac
;
if
(
*
priv
->
reset
)
position
=
0
;
priv
->
prevcnt0
=
cnt0
;
priv
->
position
=
position
;
priv
->
speed
=
speed
;
}
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"position"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
position
)
},
...
...
@@ -100,6 +141,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
int
tc_nr
;
int
rev
;
int
inverted
;
enum
tc_mode
tc_mode
=
TC_MODE_POSITION
;
tc_nr
=
va_arg
(
ap
,
int
);
if
(
tc_nr
<
0
||
tc_nr
>
3
)
{
...
...
@@ -109,6 +151,11 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
}
rev
=
va_arg
(
ap
,
int
);
inverted
=
va_arg
(
ap
,
int
);
if
(
argc
>
3
)
{
tc_mode
=
va_arg
(
ap
,
int
);
}
log_send
(
LOG_T_DEBUG
,
"Mode: %d"
,
tc_mode
);
if
(
!
(
tc
=
controller_block_alloc
(
"atsamx70_tc"
,
name
,
sizeof
(
struct
controller_block_private
))))
goto
err_alloc
;
...
...
@@ -119,7 +166,10 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
if
(
controller_block_interm_list_init
(
tc
,
interms
))
goto
err_interm
;
tc
->
calculate
=
tc_calculate
;
if
(
tc_mode
==
TC_MODE_POSITION
)
tc
->
calculate
=
tc_calculate
;
else
tc
->
calculate
=
tc_calculate_speed
;
uint32_t
pa
,
pb
,
pi
;
uint32_t
fa
,
fb
,
fi
;
...
...
@@ -207,10 +257,13 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
The direction status is reported on TC_QISR.
*/
tc_init
(
tc
->
private
->
tcn
,
TC_CH0
,
TC_CMR_ETRGEDG_RISING
|
TC_CMR_ABETRG_A
|
TC_CMR_TCCLKS_XC0
|
((
rev
*
4
<
32768
)
?
TC_CMR_CPCTRG
:
0
),
0
);
uint32_t
cmr
=
TC_CMR_ABETRG_A
|
TC_CMR_TCCLKS_XC0
;
if
(
tc_mode
==
TC_MODE_POSITION
)
{
cmr
|=
TC_CMR_ETRGEDG_RISING
;
if
(
rev
*
4
<
32768
)
cmr
|=
TC_CMR_CPCTRG
;
}
tc_init
(
tc
->
private
->
tcn
,
TC_CH0
,
cmr
,
0
);
tc_write_rc
(
tc
->
private
->
tcn
,
TC_CH0
,
rev
*
4
);
tc_init
(
tc
->
private
->
tcn
,
TC_CH1
,
...
...
@@ -225,7 +278,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
tc_start
(
tc
->
private
->
tcn
,
TC_CH0
);
tc_start
(
tc
->
private
->
tcn
,
TC_CH1
);
tc
->
private
->
speedfac
=
(
controller_time_frequency_get
(
tc
->
time
)
*
M_PI
*
2
)
/
(
rev
*
4
);
tc
->
private
->
speedfac
=
(
controller_time_frequency_get
(
tc
->
time
)
*
M_PI
*
2
.
0
)
/
(
double
)
(
rev
*
4
);
tc
->
private
->
posfac
=
(
M_PI
*
2
)
/
(
rev
*
4
);
if
(
controller_block_add
(
tc
))
...
...
@@ -242,5 +295,5 @@ err_alloc:
BLOCK_CREATE
(
atsamx70_tc
)
=
{
.
create
=
block_atsamx70_tc_create
,
.
args
=
{
"int,int,int"
,
NULL
},
.
args
=
{
"int,int,int"
,
"int,int,int,int"
,
NULL
},
};
controller/block/block_setpoint_generator_3d.c
View file @
3b378c24
...
...
@@ -109,6 +109,7 @@ struct controller_block_private {
float
freq
;
/* ticks per second */
float
freq2
;
float
freq3
;
float
tick
;
double
t_max_a
;
double
v_delta_from_max_a
;
controller_trigger_time
period
;
...
...
@@ -141,6 +142,8 @@ struct controller_block_private {
uint32_t
id
;
bool
softreset
;
struct
controller_command_entry
cur_command
;
bool
cur_done
;
bool
cur_start
;
...
...
@@ -248,21 +251,22 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv
->
cmd_x
=
*
priv
->
reset_x
;
cur_x
=
priv
->
cmd_x
;
priv
->
cur_done
=
true
;
priv
->
cur_command
.
type
=
COMMAND_PTYPE_SETPOINT
;
priv
->
cmd_v
=
0
.
0
;
cur_v
=
0
.
0
;
priv
->
cur_a
=
0
.
0
;
priv
->
cur_j
=
0
.
0
;
priv
->
start_x
=
cur_x
;
priv
->
start_v
=
0
.
0
;
priv
->
start_a
=
0
.
0
;
priv
->
start_j
=
0
.
0
;
priv
->
start_t
=
0
;
priv
->
id
=
COMMAND_ID_NONE
;
if
(
!
priv
->
softreset
)
{
priv
->
cur_command
.
type
=
COMMAND_PTYPE_SETPOINT
;
priv
->
cmd_v
=
0
.
0
;
cur_v
=
0
.
0
;
priv
->
cur_a
=
0
.
0
;
priv
->
cur_j
=
0
.
0
;
priv
->
start_x
=
cur_x
;
priv
->
start_v
=
0
.
0
;
priv
->
start_a
=
0
.
0
;
priv
->
start_j
=
0
.
0
;
priv
->
start_t
=
0
;
priv
->
id
=
COMMAND_ID_NONE
;
goto
set_output
;
}
controller_command_queue_read
(
priv
->
command
,
&
entry
);
goto
set_output
;
}
if
(
*
priv
->
track_x_cmd
)
{
priv
->
cur_done
=
false
;
...
...
@@ -297,7 +301,6 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv
->
cur_done
=
true
;
break
;
case
COMMAND_PTYPE_SETPOINT_TRACK
:
priv
->
cmd_v
=
0
.
0
;
priv
->
cur_done
=
true
;
break
;
case
COMMAND_PTYPE_SPEED
:
...
...
@@ -349,13 +352,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv
->
cmd_x
=
cur_x
+
priv
->
cmd_v
;
}
if
(
priv
->
cur_command
.
type
==
COMMAND_PTYPE_SPEED_TRACK
)
{
float
track_v
=
*
priv
->
track_v
;
float
track_v
=
*
priv
->
track_v
*
priv
->
tick
;
ignore_x
=
true
;
priv
->
cmd_v
=
track_v
;
priv
->
cmd_x
=
cur_x
+
track_v
*
controller_time_period_get
(
spg
->
time
);
}
if
(
priv
->
cur_command
.
type
==
COMMAND_PTYPE_SETPOINT_TRACK
)
{
priv
->
cmd_x
=
*
priv
->
track_x
;
priv
->
cmd_v
=
0
.
0
;
}
if
(
priv
->
cmd_x
>
priv
->
max_x
)
...
...
@@ -550,7 +554,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if
(
fabs
(
error_x_at_v
)
<
fabs
(
error_x
)
||
(
signbit
(
error_x
)
!=
signbit
(
error_x_at_v
)
&&
!
state_to_max_a
&&
!
state_at_max_a
)
||
(
signbit
(
error_x
)
!=
signbit
(
error_x_at_v
)
/*
&& !state_to_max_a
*/
&&
!
state_at_max_a
)
||
ignore_x
)
{
if
(
!
ignore_x
&&
signbit
(
error_x_at_v
)
==
signbit
(
error_x
)
)
{
...
...
@@ -712,9 +716,9 @@ static void setpoint_generator_3d_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
-
cur_x
)
<
priv
->
precision_x
&&
fabs
(
priv
->
cmd_v
-
cur_v
)
<
priv
->
precision_v
&&
fabs
(
priv
->
cur_a
)
<
priv
->
precision_a
)
{
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
;
cur_v
=
priv
->
cmd_v
;
...
...
@@ -857,6 +861,7 @@ static void scale(struct controller_block *spg)
spg
->
private
->
precision_v
=
spg
->
private
->
precision_v_sec
*
tick
;
spg
->
private
->
precision_a
=
spg
->
private
->
precision_a_sec
*
tick
*
tick
;
spg
->
private
->
tick
=
tick
;
spg
->
private
->
freq
=
1
.
0
/
tick
;
spg
->
private
->
freq2
=
spg
->
private
->
freq
*
spg
->
private
->
freq
;
spg
->
private
->
freq3
=
spg
->
private
->
freq2
*
spg
->
private
->
freq
;
...
...
@@ -952,6 +957,14 @@ static int param_set_precision_a(struct controller_block *spg, char *param,
return
0
;
}
static
int
param_set_softreset
(
struct
controller_block
*
spg
,
char
*
param
,
int
argc
,
va_list
val
)
{
spg
->
private
->
softreset
=
va_arg
(
val
,
int
);
return
0
;
}
static
struct
controller_block_param_list
params
[]
=
{
{
"setpoint"
,
true
,
param_set_setpoint
,
.
args
=
{
"double"
,
NULL
}
},
{
"max_x"
,
true
,
param_set_max_x
,
.
args
=
{
"double"
,
NULL
}
},
...
...
@@ -962,6 +975,7 @@ static struct controller_block_param_list params[] = {
{
"precision_x"
,
true
,
param_set_precision_x
,
.
args
=
{
"double"
,
NULL
}
},
{
"precision_v"
,
true
,
param_set_precision_v
,
.
args
=
{
"double"
,
NULL
}
},
{
"precision_a"
,
true
,
param_set_precision_a
,
.
args
=
{
"double"
,
NULL
}
},
{
"softreset"
,
false
,
param_set_softreset
,
.
args
=
{
"int"
,
NULL
}
},
{
NULL
},
};
...
...
@@ -1001,6 +1015,7 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name,
if
(
!
spg
)
return
NULL
;
spg
->
private
->
softreset
=
false
;
spg
->
private
->
cmd_x
=
0
.
0
;
spg
->
private
->
cmd_v
=
0
.
0
;
spg
->
private
->
max_x
=
0
.
0
;
...
...
controller/ctrl_embedded.ctrl
View file @
3b378c24
...
...
@@ -20,7 +20,7 @@ blocks ($(frequency), $(delay)) {
#{ "controller_profile", "profile" }
{ "atsamx70_tc", "counter0", 0, 2500, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0
, 1
}
{ "atsamx70_afec", "afec0", 0 }
{ "atsamx70_pwm", "pwm0", 0, 0, 10000.0 }
...
...
@@ -37,17 +37,21 @@ blocks ($(frequency), $(delay)) {
{ "gain", "w2V" }
{ "gain", "V2pwm" }
{ "deadzone", "joystick_deadzone" }
{ "gain", "joystick2w" }
{ "gain", "joystick2v" }
{ "gain", "joystick2p" }
{ "motor_model_dc_v", "motor_model" }
{ "i2t", "motor_i2t" }
{ "gain", "Ilim2Vlim" }
{ "limit", "Vlim_supply" }
{ "limit_dyn", "Vlim" }
{ "not", "button
0n
" }
{ "not", "button
1n
" }
{ "not", "button
_v
" }
{ "not", "button
_x
" }
{ "add", "position_offset" }
{ "and2", "track_x_cmd" }
{ "delay", "spg_reset" }
{ "not", "spg_reset" }
{ "delay_bool", "spg_enable" }
{ "setpoint_generator_3d", "spg", "spg", "rad" }
{ "subtract", "error" }
{ "deadzone", "error_deadzone" }
...
...
@@ -55,27 +59,31 @@ blocks ($(frequency), $(delay)) {
{ "add", "speed_ff" }
{ "value_bool", "false" }
{ "value
_float
",
"zero"
}
{ "value",
"position_calibrate"
}
{ "atsamx70_gmac", "packet" }
{ "packet_out_float_be", "out_position", "packet", 0 }
{ "packet_out_float_be", "out_stick", "packet", 4 }
{ "packet_out_float_be", "out_speed", "packet", 8 }
{ "packet_out_bool_byte", "out_homed", "packet", 9 }
{ "packet_out_float_be", "out_speedff", "packet", 12 }
{ "packet_out_float_be", "out_positionff", "packet", 16 }
{ "packet_out_float_be", "out_pid", "packet", 20 }
{ "packet_out_float_be", "out_motor_I", "packet", 24 }
{ "packet_out_bool_byte", "out_homed", "packet", 28 }
}
alias {
{ "position", "
counter0", "position
" }
{ "position", "
position_offset", "out
" }
{ "motor_speed", "counter1", "speed" }
{ "error", "error", "difference" }
{ "setpoint", "spg", "x" }
}
set w_nom 1800.0
set w_nom
rpm2rads(
1800.0
)
set V_nom 24.0
set I_nom 28.0
set I_peak 95.0
set t_peak 0.
5
#???
set t_peak 0.
1
#???
set R_peak $(V_nom)/$(I_peak)
set V_supply 24.0 #27.6?
...
...
@@ -83,44 +91,49 @@ set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33
set max_speed $(w_nom)/$(gear_ratio)
set max_accel $(max_speed)
set max_jerk $(max_
jerk)
set max_accel $(max_speed)
*2.0
set max_jerk $(max_
accel)*2.0
links {
{ "button0", "value", "button
0n
", "input", true }
{ "button1", "value", "button
1n
", "input", true }
{ "button0", "value", "button
_v
", "input", true }
{ "button1", "value", "button
_x
", "input", true }
{ "button
1n
", "output", "track_x_cmd", "a", true }
{ "button
_x
", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true }
{ "counter0", "position", "position_offset", "in0", true }
{ "position_calibrate", "value", "position_offset", "in1", true }
{ "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true }
{ "afec0", "value", "joystick_deadzone", "in", true }
{ "joystick_deadzone", "out", "joystick2
w
", "in",
true }
{ "joystick_deadzone", "out", "joystick2
v
", "in", true }
{ "joystick_deadzone", "out", "joystick2p", "in", true }
{ $<position>, "spg", "reset_x", true }
{ "
zero", "value",
"spg", "track_
x
", true }
{ "joystick2
w
", "out", "spg", "track_
v
", true }
{ "
joystick2v", "out",
"spg", "track_
v
", true }
{ "joystick2
p
", "out", "spg", "track_
x
", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "button0n", "output", "spg", "track_v_cmd", true }
{ "counter0", "homed", "spg_reset", "in", true }
{ "spg_reset", "out", "spg", "reset", true }
{ "button_v", "output", "spg", "track_v_cmd", true }
{ "spg_enable", "out", "spg_reset", "input", true }
{ "spg_reset", "output", "spg", "reset", true }
{ "counter0", "homed", "spg_enable", "in", true }
{ "spg", "v", "speed_ff", "in1", true }
{ $<setpoint>, "error", "positive", true }
{ $<position>, "error", "negative", true }
{ $<error>, "error_deadzone", "in", true }
{ "error_deadzone", "out", "pid", "in", true }
{ "spg_enable", "out", "pid", "enable", true }
{ "pid", "out", "speed_ff", "in0", true }
{ "speed_ff", "out", "w2V", "in", true }
{ "
w2V
", "out", "motor_model", "V", false }
{ $<motor_peed>,
"motor_model", "w", true }
{ "
Vlim
", "out", "motor_model", "V", false }
{ $<motor_
s
peed>, "motor_model", "w", true }
{ "motor_model", "I", "motor_i2t", "I", true }
{ "motor_i2t", "Ilim", "Ilim2Vlim", "in", true }
{ "Ilim2Vlim", "out", "Vlim", "limit", true }
{ "Ilim2Vlim", "out", "Vlim_supply", "in", true }
{ "Vlim_supply", "out", "Vlim", "limit", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
...
...
@@ -135,27 +148,35 @@ links {
{ "blinky", "output", "blinky_LED", "value", true }
{ "blinky", "output", "status0_LED", "value", true }
{ "button
0
", "
value",
"status3_LED", "value", true }
{ "button
1
", "
value",
"status4_LED", "value", true }
{ "button
_v
", "
output",
"status3_LED", "value", true }
{ "button
_x
", "
output",
"status4_LED", "value", true }
{ "button2", "value", "status5_LED", "value", true }
# Packet output
{ $<position>, "out_position", "value", true }
{ "afec0", "value", "out_stick", "value", true }
{ $<motor_speed>, "out_speed", "value", true }
{ "counter0", "homed", "out_homed", "value", true }
{ $<position>, "out_position", "value", true }
{ "joystick_deadzone", "out", "out_stick", "value", true }
{ $<motor_speed>, "out_speed", "value", true }
{ "counter0", "homed", "out_homed", "value", true }
{ "spg", "v", "out_speedff", "value", true }
{ "spg", "x", "out_positionff", "value", true }
{ "pid", "out", "out_pid", "value", true }
{ "motor_model", "I", "out_motor_I", "value", true }
}
params {
{ "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
{ "V2pwm", "gain", (float)1.0/$(V_supply) }
{ "joystick2w", "gain", (float)$(max_speed) }
{ "joystick2v", "gain", (float)$(max_speed) }
{ "joystick2p", "gain", (float)deg2rad(45.0) }
{ "position_calibrate", "value", 0.0 }
# GNM8055/4
# 24V 28A 490W
# zul. Spitzenstrom 95A
# S3 25% I.CI F
#
# 24/95 --> R=0.25263158 (max R)
# 672W - 490W = 182W --> R=0.23214286 (6.5V) (17.5V emf)
# 490W @1800rpm --> T = 2.6Nm
...
...
@@ -168,22 +189,28 @@ params {
{ "motor_i2t", "peak", (float)$(I_peak) }
{ "motor_i2t", "duration", (float)$(t_peak) }
{ "Vlim_supply", "min", (float)-$(V_supply) }
{ "Vlim_supply", "max", (float) $(V_supply) }
{ "Ilim2Vlim", "gain", (float)$(R_peak) }
{ "joystick_deadzone", "deadzone", (float)0.1 }
{ "joystick_deadzone", "intercept", (float)1.0 }
{ "spg", "max_x", (float)deg2rad( 4
5
.0) }
{ "spg", "min_x", (float)deg2rad(-
30
.0) }
{ "spg", "max_x", (float)deg2rad( 4
3
.0) }
{ "spg", "min_x", (float)deg2rad(-
28
.0) }
{ "spg", "max_v", (float)$(max_speed) }
{ "spg", "max_a", (float)$(max_accel) }
{ "spg", "max_j", (float)$(max_jerk) }
{ "spg", "softreset", true }
{ "spg", "precision_x", (float)deg2rad(0.1) }
{ "spg", "precision_v", 0.0005 }
{ "spg", "precision_a", $(max_jerk) }
{ "error_deadzone", "deadzone", (float)deg2rad(
1.0
) }
{ "error_deadzone", "intercept
s
", (float)deg2rad(45.0) }
{ "error_deadzone", "deadzone", (float)deg2rad(
0.1
) }
#
{ "error_deadzone", "intercept",
(float)deg2rad(45.0) }
{ "pid", "kp", (float)$(max_speed) }
{ "pid", "ki", (float)$(max_speed)
/1
0.0 }
{ "pid", "kp", (float)$(max_speed)
* 20.0
}
{ "pid", "ki", (float)$(max_speed)
*
0.0 }
{ "pid", "kd", (float)0.0 }
{ "pid", "maxw", (float)$(max_speed)/10.0 }
{ "pid", "minw", (float)$(max_speed)/-10.0 }
...
...
controller/dt_ctrl.c
View file @
3b378c24
...
...
@@ -52,7 +52,10 @@
extern
char
_binary_controller_ctrl_embedded_ctrl_start
;
extern
char
_binary_controller_ctrl_embedded_ctrl_end
;
#endif
#ifdef HAVE_SYSTICK
#include
<samx70.h>
#include
<wdt.h>
#endif
int
main
(
int
argc
,
char
**
argv
)
{
...
...
@@ -93,11 +96,13 @@ int main(int argc, char **argv)
}
#else
log_send
(
LOG_T_DEBUG
,
"Going to load embedded controller file"
);
controller_load_mem
(
"ctrl_embedded"
,
if
(
controller_load_mem
(
"ctrl_embedded"
,
&
_binary_controller_ctrl_embedded_ctrl_start
,
&
_binary_controller_ctrl_embedded_ctrl_end
-
&
_binary_controller_ctrl_embedded_ctrl_start
);
&
_binary_controller_ctrl_embedded_ctrl_end
-
&
_binary_controller_ctrl_embedded_ctrl_start
))
goto
err_init
;
controller_block_link
();
if
(
controller_block_link
())
goto
err_init
;
#endif
if
(
controller_time_process
()
!=
0
)
{
...
...
@@ -183,6 +188,9 @@ int main(int argc, char **argv)
controller_bus_recover
();
}
#ifdef HAVE_SYSTICK
wdt_restart
();
#endif
sleep
(
1
);
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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