Skip to content

GitLab

Commit 6021486a authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add deg2rad and rpm2rads functions to the controller language 

Also a small bugfix in the sample timing shell commands
parent 98ac0a8f
Hide whitespace changes
Inline Side-by-side
console/console/start_console.sh
View file @ 6021486a
......@@ -4,6 +4,8 @@ PATH=$PWD:$PATH
screen -dmS console
sleep 1
screen -S console -X screen -t trace_proxy trace_proxy
screen -S console -X screen -t spg_auth spg_auth
screen -S console -X screen -t log_proxy log_proxy
......
controller/controller/controller_load_parser.l
View file @ 6021486a
......@@ -60,6 +60,8 @@ boolcast "("[ \t]*"bool"[ \t]*")"
"{" { return BRACEOPENSYM; }
"}" { return BRACECLOSESYM; }
"(" { return PARENTHESESOPENSYM; }
")" { return PARENTHESESCLOSESYM; }
"," { return COMMASYM; }
"frequency" { return FREQUENCYSYM; }
......@@ -69,6 +71,9 @@ boolcast "("[ \t]*"bool"[ \t]*")"
"params" { return PARAMSSYM; }
"include" { return INCLUDESYM; }
"deg2rad" { return FUNC_DEG2RAD_SYM; }
"rpm2rads" { return FUNC_RPM2RADS_SYM; }
{floatcast} { return FLOATCASTSYM; }
{doublecast} { return DOUBLECASTSYM; }
{intcast} { return INTCASTSYM; }
......
controller/controller/controller_load_parser.y
View file @ 6021486a
......@@ -54,12 +54,17 @@ void yyerror(yyscan_t *scanner, char const *s);
%token LINKSSYM
%token TRACESSYM
%token PARAMSSYM
%token PARENTHESESOPENSYM
%token PARENTHESESCLOSESYM
%token FLOATCASTSYM
%token DOUBLECASTSYM
%token INTCASTSYM
%token BOOLCASTSYM
%token INCLUDESYM
%token FUNC_DEG2RAD_SYM
%token FUNC_RPM2RADS_SYM
%union
{
char *string;
......@@ -68,6 +73,14 @@ void yyerror(yyscan_t *scanner, char const *s);
unsigned long ul;
}
%type <dbl> doublevar
%type <dbl> doublevar_casteable
%type <integer> intvar
%type <ul> unsignedlongvar
%type <dbl> deg2rad
%type <dbl> rpm2rads
%%
ctrllist: ctrl
......@@ -135,25 +148,11 @@ varlist : /* varlist may be emtpy */
var : STRINGSYM
{ controller_load_var_add_str($1, scanner); }
| FLOATCASTSYM DOUBLESYM
{ controller_load_var_add_flt($2, scanner); }
| FLOATCASTSYM INTSYM
{ controller_load_var_add_flt($2, scanner); }
| DOUBLESYM
| doublevar
{ controller_load_var_add_dbl($1, scanner); }
| DOUBLECASTSYM DOUBLESYM
{ controller_load_var_add_dbl($2, scanner); }
| DOUBLECASTSYM INTSYM
{ controller_load_var_add_dbl($2, scanner); }
| BOOLSYM
{ controller_load_var_add_int($1, scanner); }
| BOOLCASTSYM INTSYM
{ controller_load_var_add_int($2, scanner); }
| INTSYM
| intvar
{ controller_load_var_add_int($1, scanner); }
| INTCASTSYM INTSYM
{ controller_load_var_add_int($2, scanner); }
| UNSIGNEDLONGSYM
| unsignedlongvar
{ controller_load_var_add_ul($1, scanner); }
| FLOATCASTSYM
{ controller_load_var_add_float_array_start(scanner); }
......@@ -161,27 +160,57 @@ var : STRINGSYM
{ controller_load_var_add_float_array_end(scanner); }
;
floatlist: floatvar
| floatvar COMMASYM floatlist
floatlist: floatlistvar
| floatlistvar COMMASYM floatlist
;
floatvar: FLOATCASTSYM DOUBLESYM
{ controller_load_var_add_flt($2, scanner); }
| FLOATCASTSYM INTSYM
{ controller_load_var_add_flt($2, scanner); }
| DOUBLESYM
{ controller_load_var_add_flt($1, scanner); }
| DOUBLECASTSYM DOUBLESYM
{ controller_load_var_add_flt($2, scanner); }
| DOUBLECASTSYM INTSYM
{ controller_load_var_add_flt($2, scanner); }
| INTSYM
{ controller_load_var_add_flt($1, scanner); }
| INTCASTSYM INTSYM
{ controller_load_var_add_flt($2, scanner); }
| UNSIGNEDLONGSYM
floatlistvar: doublevar
{ controller_load_var_add_flt($1, scanner); }
| intvar
{ controller_load_var_add_flt($1, scanner); }
| unsignedlongvar
{ controller_load_var_add_flt($1, scanner); }
;
doublevar: DOUBLESYM
{ $$ = $1; }
| DOUBLECASTSYM doublevar_casteable
{ $$ = $2; }
| FLOATCASTSYM doublevar_casteable
{ $$ = $2; }
| deg2rad
{ $$ = $1; }
| rpm2rads
{ $$ = $1; }
;
doublevar_casteable: doublevar
{ $$ = $1 }
| intvar
{ $$ = $1; }
;
deg2rad: FUNC_DEG2RAD_SYM PARENTHESESOPENSYM doublevar_casteable PARENTHESESCLOSESYM
{ $$ = ($3) * M_PI / 180.0; }
;
rpm2rads: FUNC_RPM2RADS_SYM PARENTHESESOPENSYM doublevar_casteable PARENTHESESCLOSESYM
{ $$ = ($3) * 2.0 * M_PI / 60.0; }
;
intvar: INTSYM
{ $$ = $1; }
| INTCASTSYM intvar
{ $$ = $2; }
| BOOLSYM
{ $$ = $1; }
| BOOLCASTSYM intvar
{ $$ = $2; }
;
unsignedlongvar: UNSIGNEDLONGSYM
{ $$ = $1; }
;
links : LINKSSYM BRACEOPENSYM linklist BRACECLOSESYM
......
controller/controller/controller_sample.c
View file @ 6021486a
......@@ -268,10 +268,13 @@ static int controller_sample_timing(char *args, char *out, int *outlen)
}
static struct shell_cmd sample_shell_cmd = {
"timing", "reset/print sample timing", controller_sample_timing
};
int controller_sample_shell_add(void)
{
return shell_cmd_add(&(struct shell_cmd)
{"timing", "reset/print sample timing", controller_sample_timing });
return shell_cmd_add(&sample_shell_cmd);
}
......
controller/dt_ctrl.ctrl
View file @ 6021486a
......@@ -170,76 +170,76 @@ links {
}
traces {
{ "Azimuth_Spg0", "rad", "azimuth_spg", "x" }
{ "Azimuth_Spg1", "rad/s", "azimuth_spg", "v" }
{ "Azimuth_Spg2", "rad/s/s", "azimuth_spg", "a" }
{ "Azimuth_Spg3", "rad/s/s/s", "azimuth_spg", "j" }
{ "Azimuth_Setpoint", "rad", "azimuth_spg", "setpoint" }
{ "Azimuth_Error", "rad", "azimuth_error", "difference" }
{ "Azimuth_Setpoint_Error", "rad", "azimuth_setpoint_error", "difference" }
{ "Azimuth_PID", "rad/s", "azimuth_pid", "out" }
{ "Azimuth_I", "rad/s", "azimuth_pid", "outi" }
{ "Azimuth_PID_filtered", "rad/s", "azimuth_pid_filter", "out" }
{ "Azimuth_Position", "rad", "azimuth_position_offset_sum", "out" }
{ "Azimuth_Speed", "rad/s", "dt_az", "speed" }
{ "Azimuth_Torque", "Nm", "dt_az", "torque" }
{ "Azimuth_Safe", "Boolean", "azimuth_safety", "safe_out" }
{ "Azimuth_Enabled", "Boolean", "dt_az", "enabled" }
{ "Azimuth_Drive_Safety", "Boolean", "dt_az", "be1" }
{ "Azimuth_Drive_Safety_p270", "Boolean", "dt_az", "be4" }
{ "Azimuth_Drive_Safety_m270", "Boolean", "dt_az", "be2" }
{ "Azimuth_Spg0", "rad", "azimuth_spg", "x" }
{ "Azimuth_Spg1", "rad/s", "azimuth_spg", "v" }
{ "Azimuth_Spg2", "rad/s/s", "azimuth_spg", "a" }
{ "Azimuth_Spg3", "rad/s/s/s", "azimuth_spg", "j" }
{ "Azimuth_Setpoint", "rad", "azimuth_spg", "setpoint" }
{ "Azimuth_Error", "rad", "azimuth_error", "difference" }
{ "Azimuth_Setpoint_Error", "rad", "azimuth_setpoint_error", "difference" }
{ "Azimuth_PID", "rad/s", "azimuth_pid", "out" }
{ "Azimuth_I", "rad/s", "azimuth_pid", "outi" }
{ "Azimuth_PID_filtered", "rad/s", "azimuth_pid_filter", "out" }
{ "Azimuth_Position", "rad", "azimuth_position_offset_sum", "out" }
{ "Azimuth_Speed", "rad/s", "dt_az", "speed" }
{ "Azimuth_Torque", "Nm", "dt_az", "torque" }
{ "Azimuth_Safe", "Boolean", "azimuth_safety", "safe_out" }
{ "Azimuth_Enabled", "Boolean", "dt_az", "enabled" }
{ "Azimuth_Drive_Safety", "Boolean", "dt_az", "be1" }
{ "Azimuth_Drive_Safety_p270", "Boolean", "dt_az", "be4" }
{ "Azimuth_Drive_Safety_m270", "Boolean", "dt_az", "be2" }
{ "Focusbox_Position", "Volt", "dt_az", "ae1" }
{ "Focusbox_Position", "Volt", "dt_az", "ae1" }
{ "Elevation_Position", "rad", "elevation_input_matrix", "out0" }
{ "Elevation_Torsion", "rad", "elevation_input_matrix", "out1" }
{ "Elevation_Setpoint", "rad", "elevation_spg", "setpoint" }
{ "Elevation_Spg0", "rad", "elevation_spg", "x" }
{ "Elevation_Spg1", "rad/s", "elevation_spg", "v" }
{ "Elevation_Spg2", "rad/s", "elevation_spg", "a" }
{ "Elevation_Spg3", "rad/s", "elevation_spg", "j" }
{ "Elevation_Error", "rad", "elevation_error", "difference" }
{ "Elevation_Setpoint_Error", "rad", "elevation_setpoint_error", "difference" }
{ "Elevation_PID", "rad/s", "elevation_pid", "out" }
{ "Elevation_Torsion_Error", "rad", "elevation_torsion_error", "difference" }
{ "Elevation_Torsion_Setpoint_Error", "rad", "elevation_torsion_setpoint_error", "difference" }
{ "Elevation_Torsion_PID", "rad/s", "elevation_torsion_pid", "out" }
{ "Elevation_Position_Right", "rad", "elevation_position_offset_r_sum", "out" }
{ "Elevation_Position_Left", "rad", "elevation_position_offset_l_sum", "out" }
{ "Elevation_Speed_Right", "rad/s", "dt_el_r", "speed" }
{ "Elevation_Speed_Left", "rad/s", "dt_el_l", "speed" }
{ "Elevation_Torque_Right", "Nm", "dt_el_r", "torque" }
{ "Elevation_Torque_Left", "Nm", "dt_el_l", "torque" }
{ "Elevation_Torsion_Torque", "Nm", "elevation_torsion_torque_lp", "out" }
{ "Elevation_Safe", "Boolean", "elevation_safety", "safe_out" }
{ "Elevation_Enabled", "Boolean", "dt_el_l", "enabled" }
{ "Elevation_Drive_Safety_Right", "Boolean", "dt_el_r", "be1" }
{ "Elevation_Drive_Safety_Left", "Boolean", "dt_el_l", "be1" }
{ "Elevation_Top_Safe", "Boolean", "dt_el_l", "be4" }
{ "Elevation_Bottom_Safe", "Boolean", "dt_el_l", "be2" }
{ "Elevation_Position", "rad", "elevation_input_matrix", "out0" }
{ "Elevation_Torsion", "rad", "elevation_input_matrix", "out1" }
{ "Elevation_Setpoint", "rad", "elevation_spg", "setpoint" }
{ "Elevation_Spg0", "rad", "elevation_spg", "x" }
{ "Elevation_Spg1", "rad/s", "elevation_spg", "v" }
{ "Elevation_Spg2", "rad/s", "elevation_spg", "a" }
{ "Elevation_Spg3", "rad/s", "elevation_spg", "j" }
{ "Elevation_Error", "rad", "elevation_error", "difference" }
{ "Elevation_Setpoint_Error", "rad", "elevation_setpoint_error", "difference" }
{ "Elevation_PID", "rad/s", "elevation_pid", "out" }
{ "Elevation_Torsion_Error", "rad", "elevation_torsion_error", "difference" }
{ "Elevation_Torsion_Setpoint_Error", "rad", "elevation_torsion_setpoint_error", "difference" }
{ "Elevation_Torsion_PID", "rad/s", "elevation_torsion_pid", "out" }
{ "Elevation_Position_Right", "rad", "elevation_position_offset_r_sum", "out" }
{ "Elevation_Position_Left", "rad", "elevation_position_offset_l_sum", "out" }
{ "Elevation_Speed_Right", "rad/s", "dt_el_r", "speed" }
{ "Elevation_Speed_Left", "rad/s", "dt_el_l", "speed" }
{ "Elevation_Torque_Right", "Nm", "dt_el_r", "torque" }
{ "Elevation_Torque_Left", "Nm", "dt_el_l", "torque" }
{ "Elevation_Torsion_Torque", "Nm", "elevation_torsion_torque_lp", "out" }
{ "Elevation_Safe", "Boolean", "elevation_safety", "safe_out" }
{ "Elevation_Enabled", "Boolean", "dt_el_l", "enabled" }
{ "Elevation_Drive_Safety_Right", "Boolean", "dt_el_r", "be1" }
{ "Elevation_Drive_Safety_Left", "Boolean", "dt_el_l", "be1" }
{ "Elevation_Top_Safe", "Boolean", "dt_el_l", "be4" }
{ "Elevation_Bottom_Safe", "Boolean", "dt_el_l", "be2" }
}
params {
{ "azimuth_spg", "setpoint", (float) 0.0 }
{ "azimuth_spg", "t", (float) 0.004 }
{ "azimuth_spg", "max_x", (float) 4.712388980384689858 }
{ "azimuth_spg", "min_x", (float)-4.712388980384689858 }
{ "azimuth_spg", "max_v", (float) 0.01466076571675236845 }
{ "azimuth_spg", "max_a", (float) 0.0002 }
{ "azimuth_spg", "max_j", (float) 0.00001 }
{ "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_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 }
{ "azimuth_pid", "t", (float) 0.004 }
{ "azimuth_pid", "maxw", (float) 0.0005235987755982988731 }
{ "azimuth_pid", "minw", (float)-0.0005235987755982988731 }
{ "azimuth_spg", "setpoint", (float) 0.0 }
{ "azimuth_spg", "t", (float) 0.004 }
{ "azimuth_spg", "max_x", (float) deg2rad(270) }
{ "azimuth_spg", "min_x", (float) deg2rad(-270) }
{ "azimuth_spg", "max_v", (float) rpm2rads(0.14) }
{ "azimuth_spg", "max_a", (float) 0.0002 }
{ "azimuth_spg", "max_j", (float) 0.00001 }
{ "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_x", (float) deg2rad(270) }
{ "azimuth_servo_state", "min_x", (float) deg2rad(-270) }
{ "azimuth_servo_state", "max_v", (float) rpm2rads(0.14) }
{ "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 }
{ "azimuth_pid", "t", (float) 0.004 }
{ "azimuth_pid", "maxw", (float) rpm2rads(0.005) }
{ "azimuth_pid", "minw", (float) rpm2rads(-0.005) }
{ "azimuth_pid_filter", "transfer", (int) 2,
(double) 6.343831259e+05,
......@@ -248,20 +248,20 @@ params {
}
{ "azimuth_pid_limit", "offset", (float) 0.0006981317007977318308 }
{ "azimuth_pid_limit", "gain", (float)-0.3 }
{ "azimuth_speed_limit", "min", (float)-0.02094395102393195492 }
{ "azimuth_speed_limit", "max", (float) 0.02094395102393195492 }
{ "azimuth_speed_servo", "gain", (float)-15006.75 }
{ "azimuth_torque", "value", (float) 10.0 }
{ "azimuth_pid_limit", "gain", (float)-0.3 }
{ "azimuth_speed_limit", "min", (float) rpm2rads(-0.1999) }
{ "azimuth_speed_limit", "max", (float) rpm2rads(0.1999) }
{ "azimuth_speed_servo", "gain", (float)-15006.75 }
{ "azimuth_torque", "value", (float) 10.0 }
{ "azimuth_position_gain", "gain", (float)-6.663668016059439919e-05 }
{ "azimuth_position_offset","value", (float) 0.0 }
{ "azimuth_position_offset","value", (float) 0.0 }
{ "azimuth_safety", "position_min", (float)-5.061454830783555773 }
{ "azimuth_safety", "position_max", (float) 5.061454830783555773 }
{ "azimuth_safety", "safe_zone_min", (float)-4.886921905584122815 }
{ "azimuth_safety", "safe_zone_max", (float) 4.886921905584122815 }
{ "azimuth_safety", "safe_zone_min_speed", (float)-0.0001047197551196597746 }
{ "azimuth_safety", "safe_zone_max_speed", (float) 0.0001047197551196597746 }
{ "azimuth_safety", "position_min", (float) deg2rad(-290) }
{ "azimuth_safety", "position_max", (float) deg2rad(290) }
{ "azimuth_safety", "safe_zone_min", (float) deg2rad(-280) }
{ "azimuth_safety", "safe_zone_max", (float) deg2rad(280) }
{ "azimuth_safety", "safe_zone_min_speed", (float) rpm2rads(-0.001) }
{ "azimuth_safety", "safe_zone_max_speed", (float) rpm2rads(0.001) }
{ "azimuth_safety", "emergency_torque", (float) 3.0 }
{ "azimuth_safety", "recover", (int) 0 }
......@@ -273,23 +273,23 @@ params {
{ "elevation_spg", "setpoint", (float) 0.0 }
{ "elevation_spg", "t", (float) 0.004 }
{ "elevation_spg", "max_x", (float) 1.570796326794896619 }
{ "elevation_spg", "min_x", (float)-0.0008726646259971647885 }
{ "elevation_spg", "max_v", (float) 0.004921828490624009407 }
{ "elevation_spg", "max_x", (float) deg2rad(90) }
{ "elevation_spg", "min_x", (float) deg2rad(-0.05) }
{ "elevation_spg", "max_v", (float) rpm2rads(0.047) }
{ "elevation_spg", "max_a", (float) 0.0003 }
{ "elevation_spg", "max_j", (float) 0.000016 }
{ "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_x", (float) 1.570796326794896619 }
{ "elevation_servo_state", "min_x", (float)-0.0008726646259971647885 }
{ "elevation_servo_state", "max_v", (float) 0.004921828490624009407 }
{ "elevation_servo_state", "max_x", (float) deg2rad(90) }
{ "elevation_servo_state", "min_x", (float) deg2rad(-0.05) }
{ "elevation_servo_state", "max_v", (float) rpm2rads(0.047) }
{ "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 }
{ "elevation_torsion_spg", "min_x", (float)-0.01 }
{ "elevation_torsion_spg", "max_v", (float) 0.0001047197551196597746 }
{ "elevation_torsion_spg", "max_v", (float) rpm2rads(0.001) }
{ "elevation_torsion_spg", "max_a", (float) 0.00004 }
{ "elevation_torsion_spg", "max_j", (float) 0.00001 }
{ "elevation_torsion_spg", "precision_x", (float) 0.000001 }
......@@ -299,10 +299,10 @@ params {
{ "elevation_pid", "ki", (float) 0.001 }
{ "elevation_pid", "kd", (float) 0.001 }
{ "elevation_pid", "t", (float) 0.004 }
{ "elevation_pid", "maxw", (float) 0.0002094395102393195492 }
{ "elevation_pid", "minw", (float)-0.0002094395102393195492 }
{ "elevation_speed_limit", "min", (float)-0.008377580409572781969 }
{ "elevation_speed_limit", "max", (float) 0.008377580409572781969 }
{ "elevation_pid", "maxw", (float) rpm2rads(0.002) }
{ "elevation_pid", "minw", (float) rpm2rads(-0.002) }
{ "elevation_speed_limit", "min", (float) rpm2rads(-0.08) }
{ "elevation_speed_limit", "max", (float) rpm2rads(0.08) }
{ "elevation_jerk_limit", "min", (float)-0.003 }
{ "elevation_jerk_limit", "max", (float) 0.003 }
{ "elevation_jerk_limit", "t", (float) 0.004 }
......@@ -312,8 +312,8 @@ params {
{ "elevation_torsion_pid", "kd", (float) 0.0 }
{ "elevation_torsion_pid", "maxw", (float) 0.0001 }
{ "elevation_torsion_pid", "minw", (float)-0.0001 }
{ "elevation_torsion_speed_limit", "min", (float)-0.0001047197551196597746 }
{ "elevation_torsion_speed_limit", "max", (float) 0.0001047197551196597746 }
{ "elevation_torsion_speed_limit", "min", (float) rpm2rads(-0.001) }
{ "elevation_torsion_speed_limit", "max", (float) rpm2rads(0.001) }
{ "elevation_safety", "safe_zone_min_speed", (float)-30 }
......
controller/shell/shell.c
View file @ 6021486a
......@@ -112,6 +112,10 @@ void shell_cmd(struct client *shell)
tok = strtok_r(shell->inbuf, " ", &sptr);
args = strtok_r(NULL, "", &sptr);
if (!tok) {
shell->outsize = sprintf(shell->outbuf, "\n");
return;
}
if (!strcmp(tok, "help")) {
shell->outsize = sprintf(shell->outbuf,
"%s\nCommands:\n\n"
......
Markdown is supported
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