Commit 427cf793 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add variables to the control files.

Update simulator networks to have proper quantization
Add pi constant
parent 7f43a10d
...@@ -58,6 +58,7 @@ CONTROLLERSRCS= \ ...@@ -58,6 +58,7 @@ CONTROLLERSRCS= \
controller_sample.c \ controller_sample.c \
controller_dumpdot.c \ controller_dumpdot.c \
controller_load.c \ controller_load.c \
controller_load_variable.c \
controller_load_parser.tab.c \ controller_load_parser.tab.c \
controller_load_parser.yy.c controller_load_parser.yy.c
......
...@@ -215,13 +215,15 @@ int controller_load_block_param_set(char *block, char *param, yyscan_t scanner) ...@@ -215,13 +215,15 @@ int controller_load_block_param_set(char *block, char *param, yyscan_t scanner)
return controller_block_param_set(block, param, extra->va_list); return controller_block_param_set(block, param, extra->va_list);
} }
void controller_load_include(char *file_name) int controller_load_include(char *file_name)
{ {
int r; int r;
log_send(LOG_T_DEBUG, "Include '%s'", file_name); log_send(LOG_T_DEBUG, "Include '%s'", file_name);
r = controller_load(file_name); r = controller_load(file_name);
log_send(LOG_T_DEBUG, "End of include '%s': %d", file_name, r); log_send(LOG_T_DEBUG, "End of include '%s': %d", file_name, r);
return r;
} }
......
...@@ -44,7 +44,12 @@ void controller_load_var_clear(yyscan_t scanner); ...@@ -44,7 +44,12 @@ void controller_load_var_clear(yyscan_t scanner);
void controller_load_var_add_float_array_start(yyscan_t scanner); void controller_load_var_add_float_array_start(yyscan_t scanner);
void controller_load_var_add_float_array_end(yyscan_t scanner); void controller_load_var_add_float_array_end(yyscan_t scanner);
void controller_load_frequency(double frequency); void controller_load_frequency(double frequency);
void controller_load_include(char *file_name); int controller_load_include(char *file_name);
double controller_load_variable_double_get(char *varname);
void controller_load_variable_double_set(char *varname, double val);
char * controller_load_variable_string_get(char *varname);
void controller_load_variable_string_set(char *varname, char *val);
int controller_load_block_create(char *type, char *name, yyscan_t scanner); int controller_load_block_create(char *type, char *name, yyscan_t scanner);
int controller_load_block_param_set(char *block, char *param, yyscan_t scanner); int controller_load_block_param_set(char *block, char *param, yyscan_t scanner);
......
...@@ -38,6 +38,11 @@ ...@@ -38,6 +38,11 @@
string "\""[^"]*"\"" string "\""[^"]*"\""
variablename [a-zA-Z\_][0-9a-zA-Z\-\_]*
doublevariable "\$("{variablename}")"
stringvariable "\${"{variablename}"}"
comment "/*".*"*/" comment "/*".*"*/"
number [0-9]+ number [0-9]+
...@@ -74,21 +79,39 @@ boolcast "("[ \t]*"bool"[ \t]*")" ...@@ -74,21 +79,39 @@ boolcast "("[ \t]*"bool"[ \t]*")"
"traces" { return TRACESSYM; } "traces" { return TRACESSYM; }
"params" { return PARAMSSYM; } "params" { return PARAMSSYM; }
"include" { return INCLUDESYM; } "include" { return INCLUDESYM; }
"set" { return SETSYM; }
"deg2rad" { return FUNC_DEG2RAD_SYM; } "deg2rad" { return FUNC_DEG2RAD_SYM; }
"rpm2rads" { return FUNC_RPM2RADS_SYM; } "rpm2rads" { return FUNC_RPM2RADS_SYM; }
"pi" { return CONST_PI_SYM; }
{floatcast} { return FLOATCASTSYM; } {floatcast} { return FLOATCASTSYM; }
{doublecast} { return DOUBLECASTSYM; } {doublecast} { return DOUBLECASTSYM; }
{intcast} { return INTCASTSYM; } {intcast} { return INTCASTSYM; }
{boolcast} { return BOOLCASTSYM; } {boolcast} { return BOOLCASTSYM; }
"true" { yylval->integer = 1; return BOOLSYM; }
"false" { yylval->integer = 0; return BOOLSYM; }
{string} { {string} {
yylval->string = strdup(yytext + 1); yylval->string = strdup(yytext + 1);
yylval->string[strlen(yylval->string)-1] = 0; yylval->string[strlen(yylval->string)-1] = 0;
return STRINGSYM; return STRINGSYM;
} }
{doublevariable} {
yylval->string = strdup(yytext + 2);
yylval->string[strlen(yylval->string)-1] = 0;
return DOUBLEVARIABLESYM;
}
{stringvariable} {
yylval->string = strdup(yytext + 2);
yylval->string[strlen(yylval->string)-1] = 0;
return STRINGVARIABLESYM;
}
{double} { {double} {
sscanf(yytext, "%lg", &yylval->dbl); sscanf(yytext, "%lg", &yylval->dbl);
return DOUBLESYM; return DOUBLESYM;
...@@ -104,8 +127,12 @@ boolcast "("[ \t]*"bool"[ \t]*")" ...@@ -104,8 +127,12 @@ boolcast "("[ \t]*"bool"[ \t]*")"
return INTSYM; return INTSYM;
} }
"true" { yylval->integer = 1; return BOOLSYM; } {variablename} {
"false" { yylval->integer = 0; return BOOLSYM; } yylval->string = strdup(yytext);
return VARIABLENAMESYM;
}
{comment} /* skip */ {comment} /* skip */
[ \t\r] /* skip */ [ \t\r] /* skip */
......
...@@ -45,6 +45,9 @@ void yyerror(yyscan_t *scanner, char const *s); ...@@ -45,6 +45,9 @@ void yyerror(yyscan_t *scanner, char const *s);
%token BRACECLOSESYM %token BRACECLOSESYM
%token COMMASYM %token COMMASYM
%token <string> STRINGSYM %token <string> STRINGSYM
%token <string> VARIABLENAMESYM
%token <string> DOUBLEVARIABLESYM
%token <string> STRINGVARIABLESYM
%token <integer> BOOLSYM %token <integer> BOOLSYM
%token <integer> INTSYM %token <integer> INTSYM
%token <dbl> DOUBLESYM %token <dbl> DOUBLESYM
...@@ -54,18 +57,19 @@ void yyerror(yyscan_t *scanner, char const *s); ...@@ -54,18 +57,19 @@ void yyerror(yyscan_t *scanner, char const *s);
%token LINKSSYM %token LINKSSYM
%token TRACESSYM %token TRACESSYM
%token PARAMSSYM %token PARAMSSYM
%token SETSYM
%token PARENTHESESOPENSYM %token PARENTHESESOPENSYM
%token PARENTHESESCLOSESYM %token PARENTHESESCLOSESYM
%token INTCASTSYM
%token BOOLCASTSYM
%token INCLUDESYM %token INCLUDESYM
%token FUNC_DEG2RAD_SYM %token FUNC_DEG2RAD_SYM
%token FUNC_RPM2RADS_SYM %token FUNC_RPM2RADS_SYM
%token <dbl> CONST_PI_SYM
%left DUMMY_DOUBLEVAR %left DUMMY_DOUBLEVAR
%left DUMMY_INTVAR %left DUMMY_INTVAR
%left FLOATCASTSYM DOUBLECASTSYM %left FLOATCASTSYM DOUBLECASTSYM INTCASTSYM BOOLCASTSYM
%left MINSYM %left MINSYM
%left PLUSSYM %left PLUSSYM
...@@ -81,8 +85,8 @@ void yyerror(yyscan_t *scanner, char const *s); ...@@ -81,8 +85,8 @@ void yyerror(yyscan_t *scanner, char const *s);
unsigned long ul; unsigned long ul;
} }
%type <string> stringvar
%type <dbl> doublevar %type <dbl> doublevar
%type <dbl> double_castable
%type <integer> intvar %type <integer> intvar
%type <ul> unsignedlongvar %type <ul> unsignedlongvar
...@@ -101,6 +105,7 @@ ctrl : frequency ...@@ -101,6 +105,7 @@ ctrl : frequency
| traces | traces
| params | params
| include | include
| set
; ;
frequency: FREQUENCYSYM DOUBLESYM frequency: FREQUENCYSYM DOUBLESYM
...@@ -110,7 +115,12 @@ frequency: FREQUENCYSYM DOUBLESYM ...@@ -110,7 +115,12 @@ frequency: FREQUENCYSYM DOUBLESYM
; ;
include: INCLUDESYM STRINGSYM include: INCLUDESYM STRINGSYM
{ controller_load_include($2); } {
if (controller_load_include($2)) {
yyerror(scanner, "Failed to parse include");
YYERROR;
}
}
; ;
blocks : BLOCKSSYM BRACEOPENSYM blocklist BRACECLOSESYM blocks : BLOCKSSYM BRACEOPENSYM blocklist BRACECLOSESYM
...@@ -158,7 +168,7 @@ varlist : /* varlist may be emtpy */ ...@@ -158,7 +168,7 @@ varlist : /* varlist may be emtpy */
| COMMASYM var varlist | COMMASYM var varlist
; ;
var : STRINGSYM var : stringvar
{ controller_load_var_add_str($1, scanner); } { controller_load_var_add_str($1, scanner); }
| doublevar %prec DUMMY_DOUBLEVAR | doublevar %prec DUMMY_DOUBLEVAR
{ controller_load_var_add_dbl($1, scanner); } { controller_load_var_add_dbl($1, scanner); }
...@@ -172,31 +182,34 @@ var : STRINGSYM ...@@ -172,31 +182,34 @@ var : STRINGSYM
{ controller_load_var_add_float_array_end(scanner); } { controller_load_var_add_float_array_end(scanner); }
; ;
stringvar: STRINGSYM
| STRINGVARIABLESYM
{ controller_load_variable_string_get($1); }
;
floatlist: floatlistvar floatlist: floatlistvar
| floatlistvar COMMASYM floatlist | floatlistvar COMMASYM floatlist
; ;
floatlistvar: double_castable floatlistvar: doublevar
{ controller_load_var_add_flt($1, scanner); } { controller_load_var_add_flt($1, scanner); }
; ;
double_castable: doublevar doublevar: DOUBLEVARIABLESYM
{ $$ = $1; } { $$ = controller_load_variable_double_get($1); }
| intvar | DOUBLESYM
{ $$ = $1; }
| unsignedlongvar
{ $$ = $1; }
;
doublevar: DOUBLESYM
{ $$ = $1; } { $$ = $1; }
| FLOATCASTSYM doublevar
{ $$ = $2; }
| DOUBLECASTSYM doublevar | DOUBLECASTSYM doublevar
{ $$ = $2; } { $$ = $2; }
| FLOATCASTSYM doublevar | FLOATCASTSYM intvar
{ $$ = $2; } { $$ = $2; }
| DOUBLECASTSYM intvar | DOUBLECASTSYM intvar
{ $$ = $2; } { $$ = $2; }
| FLOATCASTSYM intvar | FLOATCASTSYM unsignedlongvar
{ $$ = $2; }
| DOUBLECASTSYM unsignedlongvar
{ $$ = $2; } { $$ = $2; }
| doublevar MINSYM doublevar | doublevar MINSYM doublevar
{ $$ = ($1) - ($3); } { $$ = ($1) - ($3); }
...@@ -210,16 +223,18 @@ doublevar: DOUBLESYM ...@@ -210,16 +223,18 @@ doublevar: DOUBLESYM
{ $$ = $1; } { $$ = $1; }
| rpm2rads | rpm2rads
{ $$ = $1; } { $$ = $1; }
| CONST_PI_SYM
{ $$ = M_PI; }
| MINSYM doublevar %prec UMINUS | MINSYM doublevar %prec UMINUS
{ $$ = - $2 } { $$ = - $2 }
; ;
deg2rad: FUNC_DEG2RAD_SYM PARENTHESESOPENSYM double_castable PARENTHESESCLOSESYM deg2rad: FUNC_DEG2RAD_SYM PARENTHESESOPENSYM doublevar PARENTHESESCLOSESYM
{ $$ = ($3) * M_PI / 180.0; } { $$ = ($3) * M_PI / 180.0; }
; ;
rpm2rads: FUNC_RPM2RADS_SYM PARENTHESESOPENSYM double_castable PARENTHESESCLOSESYM rpm2rads: FUNC_RPM2RADS_SYM PARENTHESESOPENSYM doublevar PARENTHESESCLOSESYM
{ $$ = ($3) * 2.0 * M_PI / 60.0; } { $$ = ($3) * 2.0 * M_PI / 60.0; }
; ;
...@@ -227,6 +242,8 @@ intvar: INTSYM ...@@ -227,6 +242,8 @@ intvar: INTSYM
{ $$ = $1; } { $$ = $1; }
| INTCASTSYM intvar | INTCASTSYM intvar
{ $$ = $2; } { $$ = $2; }
| INTCASTSYM doublevar
{ $$ = $2; }
| BOOLSYM | BOOLSYM
{ $$ = $1; } { $$ = $1; }
| BOOLCASTSYM intvar | BOOLCASTSYM intvar
...@@ -239,6 +256,12 @@ unsignedlongvar: UNSIGNEDLONGSYM ...@@ -239,6 +256,12 @@ unsignedlongvar: UNSIGNEDLONGSYM
{ $$ = $1; } { $$ = $1; }
; ;
set: SETSYM VARIABLENAMESYM doublevar
{ controller_load_variable_double_set($2, $3) }
| SETSYM VARIABLENAMESYM stringvar
{ controller_load_variable_string_set($2, $3) }
;
links : LINKSSYM BRACEOPENSYM linklist BRACECLOSESYM links : LINKSSYM BRACEOPENSYM linklist BRACECLOSESYM
linklist: link linklist: link
......
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009
Copyright Stichting C.A. Muller Radioastronomiestation, 2009
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <string.h>
#include "controller_load.h"
enum variable_type {
VARIABLE_STRING,
VARIABLE_DOUBLE,
};
struct variable {
enum variable_type type;
char *name;
double doubleval;
char *stringval;
struct variable *next;
};
static struct variable *variables = NULL;
double controller_load_variable_double_get(char *varname)
{
struct variable *entry;
for (entry = variables; entry; entry = entry->next) {
if (!strcmp(entry->name, varname) &&
entry->type == VARIABLE_DOUBLE)
return entry->doubleval;
}
return 0.0;
}
void controller_load_variable_double_set(char *varname, double val)
{
struct variable *entry;
for (entry = variables; entry; entry = entry->next) {
if (!strcmp(entry->name, varname)) {
if (entry->type == VARIABLE_STRING) {
free(entry->stringval);
}
break;
}
}
if (!entry) {
entry = calloc(sizeof(struct variable), 1);
entry->name = strdup(varname);
entry->next = variables;
variables = entry;
}
entry->type = VARIABLE_DOUBLE;
entry->doubleval = val;
}
char * controller_load_variable_string_get(char *varname)
{
struct variable *entry;
for (entry = variables; entry; entry = entry->next) {
if (!strcmp(entry->name, varname) &&
entry->type == VARIABLE_STRING)
return entry->stringval;
}
return "";
}
void controller_load_variable_string_set(char *varname, char *val)
{
struct variable *entry;
for (entry = variables; entry; entry = entry->next) {
if (!strcmp(entry->name, varname)) {
if (entry->type == VARIABLE_STRING) {
free(entry->stringval);
}
break;
}
}
if (!entry) {
entry = calloc(sizeof(struct variable), 1);
entry->name = strdup(varname);
entry->next = variables;
variables = entry;
}
entry->type = VARIABLE_STRING;
entry->stringval = strdup(val);
}
...@@ -219,13 +219,16 @@ traces { ...@@ -219,13 +219,16 @@ traces {
{ "Elevation_Bottom_Safe", "Boolean", "dt_el_l", "be2" } { "Elevation_Bottom_Safe", "Boolean", "dt_el_l", "be2" }
} }
set azimuth_gear 15006.75
set elevation_gear 44803.125
params { params {
{ "azimuth_spg", "setpoint", (float) 0.0 } { "azimuth_spg", "setpoint", (float) 0.0 }
{ "azimuth_spg", "t", (float) 0.004 } { "azimuth_spg", "t", (float) 0.004 }
# maximum speed and position clients input is checked against # maximum speed and position clients input is checked against
{ "azimuth_spg", "max_x", (float) deg2rad(270) } { "azimuth_spg", "max_x", (float) deg2rad(270.0) }
{ "azimuth_spg", "min_x", (float) deg2rad(-270) } { "azimuth_spg", "min_x", (float) deg2rad(-270.0) }
{ "azimuth_spg", "max_v", (float) rpm2rads(2100)/15006.75 } { "azimuth_spg", "max_v", (float) rpm2rads(2100.0)/$(azimuth_gear) }
# acceleration and jerk we use to generate a profile # acceleration and jerk we use to generate a profile
{ "azimuth_spg", "max_a", (float) 0.0002 } { "azimuth_spg", "max_a", (float) 0.0002 }
{ "azimuth_spg", "max_j", (float) 0.00001 } { "azimuth_spg", "max_j", (float) 0.00001 }
...@@ -234,9 +237,9 @@ params { ...@@ -234,9 +237,9 @@ params {
{ "azimuth_spg", "precision_a", (float) 0.000001 } { "azimuth_spg", "precision_a", (float) 0.000001 }
{ "azimuth_spg", "precision_v", (float) 0.000001 } { "azimuth_spg", "precision_v", (float) 0.000001 }
# maximum values within we allow normal operation # maximum values within we allow normal operation
{ "azimuth_servo_state", "max_x", (float) deg2rad(270) } { "azimuth_servo_state", "max_x", (float) deg2rad(270.0) }
{ "azimuth_servo_state", "min_x", (float) deg2rad(-270) } { "azimuth_servo_state", "min_x", (float) deg2rad(-270.0) }
{ "azimuth_servo_state", "max_v", (float) rpm2rads(2100)/15006.75 } { "azimuth_servo_state", "max_v", (float) rpm2rads(2100.0)/$(azimuth_gear) }
{ "azimuth_servo_state", "max_a", (float) 0.0002 } { "azimuth_servo_state", "max_a", (float) 0.0002 }
# controller factors # controller factors
{ "azimuth_pid", "kp", (float) 0.20 } { "azimuth_pid", "kp", (float) 0.20 }
...@@ -251,7 +254,7 @@ params { ...@@ -251,7 +254,7 @@ params {
{ "azimuth_pid_filter", "transfer", (int) 2, { "azimuth_pid_filter", "transfer", (int) 2,
(double) 6.343831259e+05, (double) 6.343831259e+05,
(float) { -0.9964520027, 1.9964456974 }, (float) { -0.9964520027, 1.9964456974 },
(float) { 1, 2 } (float) { 1.0, 2.0 }
} }
# Due to high vibrations observed at high speed it was decided to limit # Due to high vibrations observed at high speed it was decided to limit
...@@ -261,50 +264,50 @@ params { ...@@ -261,50 +264,50 @@ params {
{ "azimuth_pid_limit", "offset", (float) 0.0006981317007977318308 } { "azimuth_pid_limit", "offset", (float) 0.0006981317007977318308 }
{ "azimuth_pid_limit", "gain", (float)-0.3 } { "azimuth_pid_limit", "gain", (float)-0.3 }
# speed limit on spg + pid just before we send it to the drive # speed limit on spg + pid just before we send it to the drive
{ "azimuth_speed_limit", "min", (float) rpm2rads(-3000)/15006.75 } { "azimuth_speed_limit", "min", (float) rpm2rads(-3000.0)/$(azimuth_gear) }
{ "azimuth_speed_limit", "max", (float) rpm2rads(3000)/15006.75 } { "azimuth_speed_limit", "max", (float) rpm2rads(3000.0)/$(azimuth_gear) }
# gear ratio between DT and servodrive # gear ratio between DT and servodrive
{ "azimuth_speed_servo", "gain", (float)-15006.75 } { "azimuth_speed_servo", "gain", (float)-$(azimuth_gear) }
# torque we allow the servodrive to actuate on the DT # torque we allow the servodrive to actuate on the DT
{ "azimuth_torque", "value", (float) 10.0 } { "azimuth_torque", "value", (float) 10.0 }
# inverse gear ratio between DT and servodrive # inverse gear ratio between DT and servodrive
{ "azimuth_position_gain", "gain", (float)-1/15006.75 } { "azimuth_position_gain", "gain", (float)-1/$(azimuth_gear) }
# callibration value for position # callibration value for position
{ "azimuth_position_offset","value", (float) 0.0 } { "azimuth_position_offset","value", (float) 0.0 }
# positions were we go into safe behavior # positions were we go into safe behavior
# these must be outside the normal operating range # these must be outside the normal operating range
{ "azimuth_safety", "position_min", (float) deg2rad(-290) } { "azimuth_safety", "position_min", (float) deg2rad(-290.0) }
{ "azimuth_safety", "position_max", (float) deg2rad(290) } { "azimuth_safety", "position_max", (float) deg2rad(290.0) }
# 'safe zone' is between the operating range and the absolute safety # 'safe zone' is between the operating range and the absolute safety
# value above # value above
{ "azimuth_safety", "safe_zone_min", (float) deg2rad(-280) } { "azimuth_safety", "safe_zone_min", (float) deg2rad(-280.0) }
{ "azimuth_safety", "safe_zone_max", (float) deg2rad(280) } { "azimuth_safety", "safe_zone_max", (float) deg2rad(280.0) }
{ "azimuth_safety", "safe_zone_min_speed", (float) rpm2rads(-0.001) } { "azimuth_safety", "safe_zone_min_speed", (float) rpm2rads(-0.001) }
{ "azimuth_safety", "safe_zone_max_speed", (float) rpm2rads(0.001) } { "azimuth_safety", "safe_zone_max_speed", (float) rpm2rads(0.001) }
{ "azimuth_safety", "emergency_torque", (float) 3.0 } { "azimuth_safety", "emergency_torque", (float) 3.0 }
# we do not start in recovery mode # we do not start in recovery mode
{ "azimuth_safety", "recover", (int) 0 } { "azimuth_safety", "recover", (int) 0 }
{ "elevation_input_matrix", "constants", (float) { 0.5/44803.125, -0.5/44803.125 }, { "elevation_input_matrix", "constants", (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
(float) { 1.0/44803.125, 1.0/44803.125 } } (float) { 1.0/$(elevation_gear), 1.0/$(elevation_gear) } }
{ "elevation_output_matrix", "constants", (float) { 44803.125, 22401.5625 }, { "elevation_output_matrix", "constants", (float) { $(elevation_gear), $(elevation_gear)/2.0 },
(float) { -44803.125, 22401.5625 } } (float) { -$(elevation_gear), $(elevation_gear)/2.0 } }
{ "elevation_spg", "setpoint", (float) 0.0 } { "elevation_spg", "setpoint", (float) 0.0 }
{ "elevation_spg", "t", (float) 0.004 } { "elevation_spg", "t", (float) 0.004 }
{ "elevation_spg", "max_x", (float) deg2rad(90) } { "elevation_spg", "max_x", (float) deg2rad(90.0) }
{ "elevation_spg", "min_x", (float) deg2rad(-0.05) } { "elevation_spg", "min_x", (float) deg2rad(-0.05) }
{ "elevation_spg", "max_v", (float) rpm2rads(2100)/44803.125 } { "elevation_spg", "max_v", (float) rpm2rads(2100.0)/$(elevation_gear) }
{ "elevation_spg", "max_a", (float) 0.0003 } { "elevation_spg", "max_a", (float) 0.0003 }