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= \
controller_sample.c \
controller_dumpdot.c \
controller_load.c \
controller_load_variable.c \
controller_load_parser.tab.c \
controller_load_parser.yy.c
......
......@@ -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);
}
void controller_load_include(char *file_name)
int controller_load_include(char *file_name)
{
int r;
log_send(LOG_T_DEBUG, "Include '%s'", file_name);
r = controller_load(file_name);
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);
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_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_param_set(char *block, char *param, yyscan_t scanner);
......
......@@ -38,6 +38,11 @@
string "\""[^"]*"\""
variablename [a-zA-Z\_][0-9a-zA-Z\-\_]*
doublevariable "\$("{variablename}")"
stringvariable "\${"{variablename}"}"
comment "/*".*"*/"
number [0-9]+
......@@ -74,21 +79,39 @@ boolcast "("[ \t]*"bool"[ \t]*")"
"traces" { return TRACESSYM; }
"params" { return PARAMSSYM; }
"include" { return INCLUDESYM; }
"set" { return SETSYM; }
"deg2rad" { return FUNC_DEG2RAD_SYM; }
"rpm2rads" { return FUNC_RPM2RADS_SYM; }
"pi" { return CONST_PI_SYM; }
{floatcast} { return FLOATCASTSYM; }
{doublecast} { return DOUBLECASTSYM; }
{intcast} { return INTCASTSYM; }
{boolcast} { return BOOLCASTSYM; }
"true" { yylval->integer = 1; return BOOLSYM; }
"false" { yylval->integer = 0; return BOOLSYM; }
{string} {
yylval->string = strdup(yytext + 1);
yylval->string[strlen(yylval->string)-1] = 0;
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} {
sscanf(yytext, "%lg", &yylval->dbl);
return DOUBLESYM;
......@@ -104,8 +127,12 @@ boolcast "("[ \t]*"bool"[ \t]*")"
return INTSYM;
}
"true" { yylval->integer = 1; return BOOLSYM; }
"false" { yylval->integer = 0; return BOOLSYM; }
{variablename} {
yylval->string = strdup(yytext);
return VARIABLENAMESYM;
}
{comment} /* skip */
[ \t\r] /* skip */
......
......@@ -45,6 +45,9 @@ void yyerror(yyscan_t *scanner, char const *s);
%token BRACECLOSESYM
%token COMMASYM
%token <string> STRINGSYM
%token <string> VARIABLENAMESYM
%token <string> DOUBLEVARIABLESYM
%token <string> STRINGVARIABLESYM
%token <integer> BOOLSYM
%token <integer> INTSYM
%token <dbl> DOUBLESYM
......@@ -54,18 +57,19 @@ void yyerror(yyscan_t *scanner, char const *s);
%token LINKSSYM
%token TRACESSYM
%token PARAMSSYM
%token SETSYM
%token PARENTHESESOPENSYM
%token PARENTHESESCLOSESYM
%token INTCASTSYM
%token BOOLCASTSYM
%token INCLUDESYM
%token FUNC_DEG2RAD_SYM
%token FUNC_RPM2RADS_SYM
%token <dbl> CONST_PI_SYM
%left DUMMY_DOUBLEVAR
%left DUMMY_INTVAR
%left FLOATCASTSYM DOUBLECASTSYM
%left FLOATCASTSYM DOUBLECASTSYM INTCASTSYM BOOLCASTSYM
%left MINSYM
%left PLUSSYM
......@@ -81,8 +85,8 @@ void yyerror(yyscan_t *scanner, char const *s);
unsigned long ul;
}
%type <string> stringvar
%type <dbl> doublevar
%type <dbl> double_castable
%type <integer> intvar
%type <ul> unsignedlongvar
......@@ -101,6 +105,7 @@ ctrl : frequency
| traces
| params
| include
| set
;
frequency: FREQUENCYSYM DOUBLESYM
......@@ -110,7 +115,12 @@ frequency: FREQUENCYSYM DOUBLESYM
;
include: INCLUDESYM STRINGSYM
{ controller_load_include($2); }
{
if (controller_load_include($2)) {
yyerror(scanner, "Failed to parse include");
YYERROR;
}
}
;
blocks : BLOCKSSYM BRACEOPENSYM blocklist BRACECLOSESYM
......@@ -158,7 +168,7 @@ varlist : /* varlist may be emtpy */
| COMMASYM var varlist
;
var : STRINGSYM
var : stringvar
{ controller_load_var_add_str($1, scanner); }
| doublevar %prec DUMMY_DOUBLEVAR
{ controller_load_var_add_dbl($1, scanner); }
......@@ -172,31 +182,34 @@ var : STRINGSYM
{ controller_load_var_add_float_array_end(scanner); }
;
stringvar: STRINGSYM
| STRINGVARIABLESYM
{ controller_load_variable_string_get($1); }
;
floatlist: floatlistvar
| floatlistvar COMMASYM floatlist
;
floatlistvar: double_castable
floatlistvar: doublevar
{ controller_load_var_add_flt($1, scanner); }
;
double_castable: doublevar
{ $$ = $1; }
| intvar
{ $$ = $1; }
| unsignedlongvar
{ $$ = $1; }
;
doublevar: DOUBLESYM
doublevar: DOUBLEVARIABLESYM
{ $$ = controller_load_variable_double_get($1); }
| DOUBLESYM
{ $$ = $1; }
| FLOATCASTSYM doublevar
{ $$ = $2; }
| DOUBLECASTSYM doublevar
{ $$ = $2; }
| FLOATCASTSYM doublevar
| FLOATCASTSYM intvar
{ $$ = $2; }
| DOUBLECASTSYM intvar
{ $$ = $2; }
| FLOATCASTSYM intvar
| FLOATCASTSYM unsignedlongvar
{ $$ = $2; }
| DOUBLECASTSYM unsignedlongvar
{ $$ = $2; }
| doublevar MINSYM doublevar
{ $$ = ($1) - ($3); }
......@@ -210,16 +223,18 @@ doublevar: DOUBLESYM
{ $$ = $1; }
| rpm2rads
{ $$ = $1; }
| CONST_PI_SYM
{ $$ = M_PI; }
| MINSYM doublevar %prec UMINUS
{ $$ = - $2 }
;
deg2rad: FUNC_DEG2RAD_SYM PARENTHESESOPENSYM double_castable PARENTHESESCLOSESYM
deg2rad: FUNC_DEG2RAD_SYM PARENTHESESOPENSYM doublevar PARENTHESESCLOSESYM
{ $$ = ($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; }
;
......@@ -227,6 +242,8 @@ intvar: INTSYM
{ $$ = $1; }
| INTCASTSYM intvar
{ $$ = $2; }
| INTCASTSYM doublevar
{ $$ = $2; }
| BOOLSYM
{ $$ = $1; }
| BOOLCASTSYM intvar
......@@ -239,6 +256,12 @@ unsignedlongvar: UNSIGNEDLONGSYM
{ $$ = $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
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 {
{ "Elevation_Bottom_Safe", "Boolean", "dt_el_l", "be2" }
}
set azimuth_gear 15006.75
set elevation_gear 44803.125
params {
{ "azimuth_spg", "setpoint", (float) 0.0 }
{ "azimuth_spg", "t", (float) 0.004 }
# maximum speed and position clients input is checked against
{ "azimuth_spg", "max_x", (float) deg2rad(270) }
{ "azimuth_spg", "min_x", (float) deg2rad(-270) }
{ "azimuth_spg", "max_v", (float) rpm2rads(2100)/15006.75 }
{ "azimuth_spg", "max_x", (float) deg2rad(270.0) }
{ "azimuth_spg", "min_x", (float) deg2rad(-270.0) }
{ "azimuth_spg", "max_v", (float) rpm2rads(2100.0)/$(azimuth_gear) }
# acceleration and jerk we use to generate a profile
{ "azimuth_spg", "max_a", (float) 0.0002 }
{ "azimuth_spg", "max_j", (float) 0.00001 }
......@@ -234,9 +237,9 @@ params {
{ "azimuth_spg", "precision_a", (float) 0.000001 }
{ "azimuth_spg", "precision_v", (float) 0.000001 }
# maximum values within we allow normal operation
{ "azimuth_servo_state", "max_x", (float) deg2rad(270) }
{ "azimuth_servo_state", "min_x", (float) deg2rad(-270) }
{ "azimuth_servo_state", "max_v", (float) rpm2rads(2100)/15006.75 }
{ "azimuth_servo_state", "max_x", (float) deg2rad(270.0) }
{ "azimuth_servo_state", "min_x", (float) deg2rad(-270.0) }
{ "azimuth_servo_state", "max_v", (float) rpm2rads(2100.0)/$(azimuth_gear) }
{ "azimuth_servo_state", "max_a", (float) 0.0002 }
# controller factors
{ "azimuth_pid", "kp", (float) 0.20 }
......@@ -251,7 +254,7 @@ params {
{ "azimuth_pid_filter", "transfer", (int) 2,
(double) 6.343831259e+05,
(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
......@@ -261,50 +264,50 @@ params {
{ "azimuth_pid_limit", "offset", (float) 0.0006981317007977318308 }
{ "azimuth_pid_limit", "gain", (float)-0.3 }
# 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", "max", (float) rpm2rads(3000)/15006.75 }
{ "azimuth_speed_limit", "min", (float) rpm2rads(-3000.0)/$(azimuth_gear) }
{ "azimuth_speed_limit", "max", (float) rpm2rads(3000.0)/$(azimuth_gear) }
# 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
{ "azimuth_torque", "value", (float) 10.0 }
# 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
{ "azimuth_position_offset","value", (float) 0.0 }
# positions were we go into safe behavior
# these must be outside the normal operating range
{ "azimuth_safety", "position_min", (float) deg2rad(-290) }
{ "azimuth_safety", "position_max", (float) deg2rad(290) }
{ "azimuth_safety", "position_min", (float) deg2rad(-290.0) }
{ "azimuth_safety", "position_max", (float) deg2rad(290.0) }
# 'safe zone' is between the operating range and the absolute safety
# value above
{ "azimuth_safety", "safe_zone_min", (float) deg2rad(-280) }
{ "azimuth_safety", "safe_zone_max", (float) deg2rad(280) }
{ "azimuth_safety", "safe_zone_min", (float) deg2rad(-280.0) }
{ "azimuth_safety", "safe_zone_max", (float) deg2rad(280.0) }
{ "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 }
# we do not start in recovery mode
{ "azimuth_safety", "recover", (int) 0 }
{ "elevation_input_matrix", "constants", (float) { 0.5/44803.125, -0.5/44803.125 },
(float) { 1.0/44803.125, 1.0/44803.125 } }
{ "elevation_input_matrix", "constants", (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
(float) { 1.0/$(elevation_gear), 1.0/$(elevation_gear) } }
{ "elevation_output_matrix", "constants", (float) { 44803.125, 22401.5625 },
(float) { -44803.125, 22401.5625 } }
{ "elevation_output_matrix", "constants", (float) { $(elevation_gear), $(elevation_gear)/2.0 },
(float) { -$(elevation_gear), $(elevation_gear)/2.0 } }
{ "elevation_spg", "setpoint", (float) 0.0 }
{ "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", "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_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) deg2rad(90) }
{ "elevation_servo_state", "max_x", (float) deg2rad(90.0) }
{ "elevation_servo_state", "min_x", (float) deg2rad(-0.05) }
{ "elevation_servo_state", "max_v", (float) rpm2rads(2100)/44803.125 }
{ "elevation_servo_state", "max_v", (float) rpm2rads(2100.0)/$(elevation_gear) }
{ "elevation_servo_state", "max_a", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
......
......@@ -50,9 +50,9 @@ links {
}
params {
{ "dt_az_speed_limit", "max", (float)314.159265359 }
{ "dt_az_speed_limit", "min", (float)-314.159265359 }
{ "dt_az_speed_quantize", "quantum", (float)0.00479368996214 }
{ "dt_az_speed_limit", "max", (float) rpm2rads(3000.0) }
{ "dt_az_speed_limit", "min", (float)-rpm2rads(3000.0) }
{ "dt_az_speed_quantize", "quantum", (float) rpm2rads(3000.0)/16384.0 }
{ "dt_az_driver", "kp", (float)1.0 }
{ "dt_az_driver", "ki", (float)100.0 }
{ "dt_az_driver", "kd", (float)0.01 }
......@@ -69,7 +69,7 @@ params {
{ "dt_az_speed2pos", "ki", (float)1.0 }
{ "dt_az_speed2pos", "kd", (float)0.0 }
{ "dt_az_speed2pos", "t", (float)0.004 }
{ "dt_az_pos_quantize", "quantum", (float)8.72664625997e-07 }
{ "dt_az_pos_quantize", "quantum", (float)2.0 * pi / 16384.0 }
{ "dt_az_torque_noise_gain", "gain", (float)0.0 }
{ "dt_az_torque_noise_0", "value", (float)0.0 }
{ "dt_az_torque_noise_sine", "f", (float)0.0 }
......
......@@ -45,9 +45,9 @@ blocks {
params {
{ "dt_el_torque_gain_r", "gain", (float)1.0 }
{ "dt_el_speed_limit_r", "max", (float)314.159265359 }
{ "dt_el_speed_limit_r", "min", (float)-314.159265359 }
{ "dt_el_speed_quantize_r", "quantum", (float)0.00479368996214 }
{ "dt_el_speed_limit_r", "max", (float) rpm2rads(3000.0)}
{ "dt_el_speed_limit_r", "min", (float)-rpm2rads(3000.0)}
{ "dt_el_speed_quantize_r", "quantum", (float) rpm2rads(3000.0)/16384.0 }
{ "dt_el_driver_r", "kp", (float)5.0 }
{ "dt_el_driver_r", "ki", (float)10.0 }
{ "dt_el_driver_r", "kd", (float)0.01 }
......@@ -65,12 +65,12 @@ params {
{ "dt_el_speed2pos_r", "kd", (float)0.0 }
{ "dt_el_speed2pos_r", "t", (float)0.004 }
{ "dt_el_offset_r", "value", (float)-40680.0833715 }
{ "dt_el_pos_quantize_r", "quantum", (float)8.72664625997e-07 }
{ "dt_el_pos_quantize_r", "quantum", (float)2.0 * pi / 16384.0 }
{ "dt_el_torque_gain_l", "gain", (float)1.0 }
{ "dt_el_speed_limit_l", "max", (float)314.159265359 }
{ "dt_el_speed_limit_l", "min", (float)-314.159265359 }
{ "dt_el_speed_quantize_l", "quantum", (float)0.00479368996214 }
{ "dt_el_speed_limit_l", "max", (float) rpm2rads(3000.0)}
{ "dt_el_speed_limit_l", "min", (float)-rpm2rads(3000.0)}
{ "dt_el_speed_quantize_l", "quantum", (float) rpm2rads(3000.0)/16384.0 }
{ "dt_el_driver_l", "kp", (float)5.0 }
{ "dt_el_driver_l", "ki", (float)10.0 }
{ "dt_el_driver_l", "kd", (float)0.01 }
......@@ -88,7 +88,7 @@ params {
{ "dt_el_speed2pos_l", "kd", (float)0.0 }
{ "dt_el_speed2pos_l", "t", (float)0.004 }
{ "dt_el_offset_l", "value", (float)-10783.776612094 }
{ "dt_el_pos_quantize_l", "quantum", (float)8.72664625997e-07 }
{ "dt_el_pos_quantize_l", "quantum", (float)2.0 * pi / 16384.0 }
{ "dt_el_cross_tension_r", "gain", (float)-50.0 }
{ "dt_el_cross_tension_l", "gain", (float)-50.0 }
......
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