Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
controller/controller/Makefile
View file @ 427cf793
......@@ -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
......
controller/controller/controller_load.c
View file @ 427cf793
......@@ -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;
}
......
controller/controller/controller_load.h
View file @ 427cf793
......@@ -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);
......
controller/controller/controller_load_parser.l
View file @ 427cf793
......@@ -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 */
......
controller/controller/controller_load_parser.y
View file @ 427cf793
......@@ -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
......
controller/controller/controller_load_variable.c 0 → 100644
View file @ 427cf793
/*
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);
}
controller/dt_ctrl.ctrl
View file @ 427cf793
......@@ -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 }
......
controller/dt_ctrl_az_sim.ctrl
View file @ 427cf793
......@@ -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 }
......
controller/dt_ctrl_el_sim.ctrl
View file @ 427cf793
......@@ -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 }
......
Supports Markdown
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