PrologLexer::PrologLexer()
{
lexer.rules[Prolog::Spacing] = loop1(charOf(" \t\n\r"));
lexer.rules[Prolog::LParen] = str("(");
lexer.rules[Prolog::RParen] = str(")");
lexer.rules[Prolog::LBracket] = str("[");
lexer.rules[Prolog::RBracket] = str("]");
lexer.rules[Prolog::OnlyIf] = str(":-");
lexer.rules[Prolog::Eq] = str("=");
/*
lexer.rules[Prolog::Lt] = str("<");
lexer.rules[Prolog::Gt] = str(">");
lexer.rules[Prolog::Le] = str("<=");
lexer.rules[Prolog::Ge] = str(">=");
lexer.rules[Prolog::Ne] = str("<>");
*/
lexer.rules[Prolog::Dot] = str(".");
lexer.rules[Prolog::Comma] = str(",");
lexer.rules[Prolog::Semi] = str(";");
lexer.rules[Prolog::Bar] = str("|");
lexer.rules[Prolog::DomainsKw] = str("domains");
lexer.rules[Prolog::PredicatesKw] = str("predicates");
lexer.rules[Prolog::ClausesKw] = str("clauses");
lexer.rules[Prolog::AssertKw] = str("assert");
lexer.rules[Prolog::Str] = seq(str("\""),
loop(anyBut(choice(str("\""), str("\n")))),
str("\""));
shared_ptr<RegExp> digit = charIs([](QChar c) {
return c.isDigit();
},"<digit>");
shared_ptr<RegExp> letter = charIs([](QChar c) {
return c.isLetter();
}, "<letter>");
shared_ptr<RegExp> smallLetter = charIs([](QChar c) {
return c.isLower();
}, "<lowercase>");
shared_ptr<RegExp> capitalLetter = charIs([](QChar c) {
return c.isUpper();
}, "<uppercase>");
shared_ptr<RegExp> alpha = charIs([](QChar c) {
return c.isLetterOrNumber();
},"<alphanumeric>");
shared_ptr<RegExp> symbol = charOf("+-/*<>=");
shared_ptr<RegExp> digits = seq(loop1(digit), checkNo(letter));
shared_ptr<RegExp> smallLetterOrSymbol = choice(smallLetter, symbol);
shared_ptr<RegExp> alphaOrUnderscore = choice(alpha, str("_"));
shared_ptr<RegExp> alphaOrSymbol= choice(alpha, symbol);
lexer.rules[Prolog::Num] = seq(digits,
optional(seq(str("."), digits)));
lexer.rules[Prolog::Symbol] = seq(smallLetterOrSymbol, loop(choice(digit, alphaOrSymbol)));
lexer.rules[Prolog::Variable] = seq(choice(capitalLetter, str("_")),
loop(choice(digit, alphaOrUnderscore)));
}
int main(int argc, char *argv[]) {
double start1,start2,end1,end2;
int r;
init1();
start1 = omp_get_wtime();
for (r=0; r<reps; r++){
loop1();
}
end1 = omp_get_wtime();
double out1 = valid1();
printf("%f,", (float)(end1-start1));
init2();
start2 = omp_get_wtime();
for (r=0; r<reps; r++){
loop2();
}
end2 = omp_get_wtime();
double out2 = valid2();
printf("%f,guided,64,%.10f,%.10f\n", (float)(end2-start2), out1, out2);
}
void Syncdb::bigLoop ( ) {
// try to launch our sync
if ( m_doRcp ) rcpFiles();
// we got a msg0 or msg5 outstanding. wait for it to come back first.
if ( m_outstanding ) return;
if ( ! m_calledLoop1 ) { loop1(); return; }
if ( ! m_calledLoop2 ) { loop2(); return; }
if ( ! m_calledLoop3 ) { loop3(); return; }
if ( ! m_calledLoop4 ) { loop4(); return; }
if ( ! m_calledLoop5 ) { loop5(); return; }
// if done calling loop4(), reset this
loopReset();
}
int main(int argc, char *argv[])
{
loop0();
loop1();
loop2();
loop3();
loop4();
loop5();
loop6();
loop7();
loop8();
loop9();
loop10();
}
int main(int argc, char *argv[])
{
SDL_Init(SDL_INIT_EVERYTHING);
SDL_SetVideoMode(600, 400, 32, SDL_SWSURFACE);
emscripten_run_script("keydown(37);"); // left
result += loop1();
emscripten_run_script("keydown(39);"); // right
result += loop2();
emscripten_run_script("keydown(65);"); // A
result += alphakey();
REPORT_RESULT(result);
return 0;
}
int
recinput(char *arqinput, char *argv[])
{
int fdm, c;
pid_t pid;
struct termios orig_termios;
struct winsize size;
if (! isatty(0) || ! isatty(1))
{
msg("stdin and stdout must be a terminal");
return ERRO;
}
input = criarq("", arqinput, "");
if (input == NULL)
return ERRO;
if (tty_save(STDIN_FILENO) == ERRO)
return ERRO;
pid = pty_fork2(&fdm, slave_name);
if (pid < 0)
{
return ERRO;
}
else
if (pid == 0)
{ /* child */
if (execv(argv[0], argv) < 0)
{
msg("can't execv: %s");
return ERRO;
}
}
if (tty_raw(STDIN_FILENO) < 0) /* user's tty to raw mode */
{
msg("tty_raw error");
return ERRO;
}
c = loop1(fdm); /* copies stdin -> ptym, ptym -> stdout */
waitpid(pid, NULL, 0);
fclose(input);
close(fdm);
tty_copy(STDIN_FILENO);
return c;
}
task main()
{
initializeRobot();
//waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
motor[leftRear] = -50;
motor[leftFront] = -50;
motor[rightRear] = 50;
motor[rightFront] = 50;
while (true)
{
loop1();
}
}
int main(int argc, char *argv[]) {
FILE *pFile;
double start1,start2,end1,end2,time1,time2;
int r;
init1();
start1 = omp_get_wtime();
for (r=0; r<reps; r++){
loop1();
}
end1 = omp_get_wtime();
valid1();
time1 = (float)(end1-start1);
printf("Total time for %d reps of loop 1 = %f\n",reps, time1);
pFile = fopen ("myfile.txt" , "w");
if (pFile == NULL) perror ("Error opening file");
init2();
start2 = omp_get_wtime();
for (r=0; r<reps; r++){
loop2();
}
end2 = omp_get_wtime();
valid2();
time2 = (float)(end2-start2);
printf("Total time for %d reps of loop 2 = %f\n",reps, time2);
printf("This file is functional\n");
fprintf(pFile,"%lf\t%lf\n",time1, time2);
fclose(pFile);
}
int main()
{
unsigned long long i;
unsigned long long int sum = 0;
unsigned long long int count;
count = 4;
TraceOSDbgEvent2Start(1, 2);
for (i = 0; i < count; i++) {
TraceOSDbgd1A(1);
sum+=loop1();
TraceOSDbgd2A(2, 3);
sum+=loop2();
TraceOSDbgd3A(4, 5, 6);
}
TraceOSDbgEvent2End(3, 4);
return 0;
}
int main(int argc, char *argv[]) {
double start1,start2,end1,end2;
int r;
init1();
start1 = omp_get_wtime();
for (r=0; r<reps; r++){
loop1();
}
end1 = omp_get_wtime();
valid1();
printf("Total time for %d reps of loop 1 = %f\n",reps, (float)(end1-start1));
init2();
start2 = omp_get_wtime();
for (r=0; r<reps; r++){
loop2();
}
end2 = omp_get_wtime();
valid2();
printf("Total time for %d reps of loop 2 = %f\n",reps, (float)(end2-start2));
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
//SysTick_Config(SystemCoreClock/1000); //1ms interrupt capture
counter_sweep=0;
/*************Variable Initializations*****************/
/*Example 3600/2=1800-->40K, 3600/3.6=100-->72K, 3600/4=900-->80K, 3600/5=720-->100K*/
PWM_cycle =900;
MotorV=16.0f; //(V), motor voltage, now 13.0V power source
Ts1=0.001f; //sec, for loop1, 1KHz
Ts2=0.01f; //sec, for loop2, 100Hz
time1=0.0f;
time2=0.0f;
pi=3.14159265f;
two_pi=6.2831853f;
Kc2r=two_pi/2560.0f; //ENCODER RESOLUTION, floating in 32*4*10
freq=34.0f; //Hz, flapping frequency 34.0HZ
two_pi_freq=two_pi*freq;
Vin1=0;
Vin2=0;
Ddirect1=1;
Ddirect2=1;
Hall_1=0x05;
Hall_2=0x05;
Hall_State1=1;
Hall_State2=1;
Encoder1=0;
Encoder2=0;
y1=0.0f;
y1_dot=0.0f;
y1_ddot=0.0f;
y1_save=0.0f;
y2=0.0f;
y2_dot=0.0f;
y2_ddot=0.0f;
y2_save=0.0f;
yd1=0.0f;
yd1_dot=0.0f;
yd1_ddot=0.0f;
yd2=0.0f;
yd2_dot=0.0f;
yd2_ddot=0.0f;
e1=0.0f;
e1_dot=0.0f;
e1_int=0.0f;
e2=0.0f;
e2_dot=0.0f;
e2_int=0.0f;
flap1=0.0f;
d_flap1=0.0f;
sigma1=0.5f;
phase1=0.0f;
flap2=0.0f;
d_flap2=0.0f;
sigma2=0.5f;
phase2=0.0f;
/******* body stabilization *******/
flap_cycle_flag=0;
Kp_rol=0.1f; //10deg->0.5V
Kd_rol=0.0017f; //20deg/s->0.5V
Kp_pit=2.0f; //10deg->2V
Kd_pit=0.001f; //20deg/s->1V
Kp_yaw=0.0f; //10
Kd_yaw=0.0f; //100
delta_Amp=0.0f;
delta_Spl=0.0f;
delta_Bia=0.0f;
/*!!!!!!!!!!!!!! trim condition!!!!!!!!!!!!!!!!!!!*/
//left motor is motor 1, looking at the front of motor board
trim_A1 = 16.00f; //(V), value at 12V nominal and 13V power source
//right motor is motor 2, looking at the front of motor board
trim_A2 = 16.00f; //(V), value at 12V nominal and 13V power source
#if WING_KINEMATICS
flap1=0.0f*KGR; // rad 30
d_flap1=0.0f/180.0f*pi; // rad
sigma1=0.5f;
phase1=0.0f;
flap2=0.0f*KGR; // rad 30
d_flap2=0.0f/180.0f*pi; // rad
sigma2=0.5f;
phase2=0.0f;
//////////////PID Parameters//////////250/
// Kp=38.8974f;
// Kd=0.1338f;
// Ki=6028.3f;
// Ku = 9.16732471506f;
// Kp = 16.10f;
// Ki = 0.0f;//550.0f;
// Kd = 0.001f;
// Ku=0.0f;
// Kp = 0.0f;
// Ki = 0.0f;
// Kd = 0.0f;
//////////////PID Parameters//////////200/
//Kp=21.5359f;
//Kd=0.1049f;
//Ki=3086.5f;
/*******LQR Parameters********/
Kss1=0.00015;
Kss2=0.012;
Fss1=6.1690; //6.1690
Fss2=7.6461; //7.6461
// /*******LQR Parameters********/
// Kss1=0.0001;
// Kss2=0.0117;
// Fss=8.1690;
/******* body stabilization *******/
Kp_rol=3.0f/180.0f*pi; //10deg->5deg in rad
Kd_rol=0.0018f; //10deg->5deg in rad
Kp_pit=5.0f/180.0f*pi/10.0f; //10deg->5deg in rad
Kd_pit=0.0f; //10deg->5deg in rad
Kp_yaw=0.0f; //10
Kd_yaw=0.0f; //100
delta_Amp=0.0f;
delta_Spl=0.0f;
delta_Bia=0.0f;
//left motor is motor 1, looking at the front of motor board
trim_A1 = 45.0f/180.0f*pi; //(deg)
//right motor is motor 2, looking at the front of motor board
trim_A2 = 45.0f/180.0f*pi; //(deg)
#endif
//TO DO:
trim_Bia1=0.0f; ////////(pitch) trim bias
trim_Bia2=0.0f;
//TO DO:
trim_Spl1=0.0f;
trim_Spl2=0.0f;
/**********ENCODER Configuration***********/
#if QEI_DEF
Timer1_Encoder1();
Timer4_Encoder2();
/* ENCODER COUNTER # RESET */
TIM_SetCounter(TIM1,0); //TIM4->CNT = 0; Encoder1 counts reset
TIM_SetCounter(TIM4,0); //TIM4->CNT = 0; Encoder2 counts reset
#endif
#if SERIAL_COM
serial_flag=0;
tx_index = 0;
rx_index = 0;
Usart3_Configuration();
#endif
#if I2C_IMU
MPU9150_Init();
#endif
/* gpio Configuration */
GPIO_Config(); //define GPIO & PWM with timerS
/*************Loop Initializations*****************/
LOOP1_flag=0;
LOOP2_flag=0;
/*************PWM Initializations*****************/
//DISABLE DRIVE BY DIAG_EN SET 0
GPIO_WriteBit(GPIOB, GPIO_Pin_2 , Bit_RESET);//TEST PIN RESET
GPIO_WriteBit(GPIOC, GPIO_Pin_13, Bit_RESET); // Bit-SET = high, bit-reset = low
Timer3_PWM1(PWM_cycle); // PWM Driver Input 1
Timer2_PWM2(PWM_cycle); // PWM Driver Input 2
PWM1_Set_SingleEdge( 0, 0, 0);
PWM2_Set_SingleEdge( 0, 0, 0);
/*************Wait 5sec for setup****************/
delay_ms(5000); //TICKER MIGHT BE A BETTER WAY
/*************Timer Initializations****************/
Timer7_Looptimer1_Int(LOOP0_STEP_SIZE);
#if TIM_LOOP1
Timer15_Looptimer2_Int(LOOP1_STEP_SIZE);
time1=0.0;
#endif
#if TIM_LOOP2
Timer16_Looptimer3_Int(LOOP2_STEP_SIZE);
time2=0.0;
#endif
/********** Enable drive**************/
// DLAG/EN1 :: 20 :: PB2
// DLAG/EN2 :: 02 :: PC13
GPIO_WriteBit(GPIOB, GPIO_Pin_2 , Bit_SET);//TEST PIN RESET
GPIO_WriteBit(GPIOC, GPIO_Pin_13, Bit_SET); // Bit-SET = high, bit-reset = low
/* Infinite loop */
while (1)
{
//Encoder1= TIM_GetCounter(TIM1); //////////Encoder1 test, get position; TIMx->CNT;
//Encoder2= TIM_GetCounter(TIM4); //Encoder2 test, get position; TIMx->CNT; takes 10us
//PWM1_Set_SingleEdge(0,Vin1,0); //////////PWM test
//PWM2_Set_SingleEdge(0,Vin2,0); //////////PWM test
if(LOOP1_flag)
loop1();
if(LOOP2_flag)
loop2();
// PWM1_Set_SingleEdge(Vin1,Vin1,Vin1); //////////PWM test
// PWM2_Set_SingleEdge(Vin2,Vin2,Vin2); //////////PWM test
// printf("%d \r\n", Encoder1);
}
}
int main(int argc, char* argv[])
{
std::shared_ptr<Empty> blank;
std::shared_ptr<Program> prg(new Program());
std::shared_ptr<SharedDecl> shDecl1(new SharedDecl("y", blank));
prg->Add(shDecl1);
std::shared_ptr<Node> initX(new Atom("2"));
std::shared_ptr<SharedDecl> shDecl2(new SharedDecl("x", initX));
prg->Add(shDecl2);
std::shared_ptr<Sub> sub1(new Sub("Main"));
sub1->AddParam("a");
sub1->AddParam("b");
prg->Add(sub1);
std::shared_ptr<Atom> atomA(new Atom("a"));
std::shared_ptr<Atom> atomB(new Atom("b"));
std::shared_ptr<BinaryOp> initRes(new BinaryOp("add", atomA, atomB));
std::shared_ptr<VarDecl> resDecl(new VarDecl("res", initRes));
sub1->Add(resDecl);
std::shared_ptr<Atom> atom3i(new Atom("3"));
std::shared_ptr<Atom> atom5i(new Atom("5"));
std::shared_ptr<BinaryOp> newParam(new BinaryOp("add", atom3i, atom5i));
std::shared_ptr<Allocation> allocat(new Allocation(newParam));
std::shared_ptr<Atom> atomArr(new Atom("arr"));
std::shared_ptr<Assignation> asignNew3(new Assignation(atomArr, allocat));
sub1->Add(asignNew3);
std::shared_ptr<Atom> atomArrBis(new Atom("arr"));
std::shared_ptr<Deallocation> deallocat(new Deallocation(atomArrBis));
sub1->Add(deallocat);
std::shared_ptr<Atom> atomC(new Atom("res"));
std::shared_ptr<BinaryOp> subCsumAB(new BinaryOp("substract", atomC, initRes));
std::shared_ptr<Assignation> incC(new Assignation(atomC, subCsumAB));
sub1->Add(incC);
std::shared_ptr<Atom> atom0(new Atom("0"));
std::shared_ptr<BinaryOp> cond1cond(new BinaryOp("equals", atomC, atom0));
std::shared_ptr<If> cond1(new If(cond1cond, "10", "20"));
sub1->Add(cond1);
std::shared_ptr<Atom> atom1(new Atom("1"));
std::shared_ptr<Atom> atom10(new Atom("10"));
std::shared_ptr<For> for1(new For("i", atom1, atom10, atom1));
sub1->Add(for1);
std::shared_ptr<BinaryOp> addC1(new BinaryOp("add", atomC, atom1));
std::shared_ptr<Assignation> incResActually(new Assignation(atomC, addC1));
for1->Add(incResActually);
std::shared_ptr<Loop> loop1(new Loop(cond1cond));
loop1->Add(for1); // don't double reference ever in practice...
loop1->Add(addC1);
sub1->Add(loop1);
std::shared_ptr<Call> call1(new Call("testFun", ""));
call1->AddParam(atomA);
call1->AddParam(addC1);
sub1->Add(call1);
std::shared_ptr<Return> ret1(new Return(atom0));
sub1->Add(ret1);
XMLDumpVisitor v;
prg->Accept(&v);
return 0;
}
int main(int argc, char *argv[]) {
FILE *pFile2,*pFile1;
FILE *pipe_gp1;
double start1,start2,end1,end2,time1,time2;
int r,i=0,rand(void);
pFile1 = fopen ("Data/GUIDED-n-file1.dat" , "w");
pFile2 = fopen ("Data/GUIDED-n-file2.dat" , "w");
if (pFile1 == NULL || pFile2 ==NULL) perror ("Error opening file");
for(i=0;i<100;i++)
{
init1();
start1 = omp_get_wtime();
for (r=0; r<reps; r++){
loop1();
}
end1 = omp_get_wtime();
valid1();
time1 = (float)(end1-start1);
//printf("Total time for %d reps of loop 1 = %f\n",reps, time1);
init2();
start2 = omp_get_wtime();
for (r=0; r<reps; r++){
loop2();
}
end2 = omp_get_wtime();
valid2();
time2 = (float)(end2-start2);
// printf("Total time for %d reps of loop 2 = %f\n",reps, time2);
// printf("This file is functional\n");
// Write everything in the file, before plotting.
fprintf(pFile1,"%lf\t%d\t%lf\n",(i+1)*time1,i+1,time1);
fprintf(pFile2,"%lf\t%d\t%lf\n",(i+1)*time2,i+1,time2);
}
printf("\n\t\t\tWritten to 'GUIDED-n-file1.dat & GUIDED-n-file2.dat'\n\n");
fclose(pFile1);
fclose(pFile2);
// Gnu-plot script
pipe_gp1 = popen("gnuplot","w");
fputs("set terminal wxt 0\n ", pipe_gp1);
fputs("unset key\n ", pipe_gp1);
fputs("set hidden3d\n ", pipe_gp1);
fputs("set dgrid3d 50,50\n ", pipe_gp1);
fputs("set title '3DPlot1'\n ", pipe_gp1);
fputs("set xlabel'Random Axis Arrangement (for 3-d plot)'\n ", pipe_gp1);
fputs("set ylabel 'Iteration Number'\n ", pipe_gp1);
fputs("set zlabel 'Time taken for 100 iterations'\n ", pipe_gp1);
fputs("splot 'Data/GUIDED-n-file1.dat' u 1:2:3 w lines \n ", pipe_gp1);
fputs("set terminal png size 1200,800\n ", pipe_gp1);
fputs("set output 'Plots/GUIDED-n-3dplot1.png'\n ", pipe_gp1);
fputs("replot\n ", pipe_gp1);
fputs("reset\n ", pipe_gp1);
fputs("set terminal wxt 0\n ", pipe_gp1);
fputs("unset key\n ", pipe_gp1);
fputs("set title '2DPlot1'\n ", pipe_gp1);
fputs("set xlabel 'Iteration Number'\n ", pipe_gp1);
fputs("set ylabel 'Time taken for 100 iterations'\n ", pipe_gp1);
fputs("plot 'Data/GUIDED-n-file1.dat' u 2:3 w lines \n ", pipe_gp1);
fputs("set terminal png size 1200,800\n ", pipe_gp1);
fputs("set output 'Plots/GUIDED-n-2dplot1.png'\n ", pipe_gp1);
fputs("replot\n ", pipe_gp1);
fputs("reset\n ", pipe_gp1);
fputs("set terminal wxt 0\n ", pipe_gp1);
fputs("unset key\n ", pipe_gp1);
fputs("set hidden3d\n ", pipe_gp1);
fputs("set dgrid3d 50,50\n ", pipe_gp1);
fputs("set title 'Plot2'\n ", pipe_gp1);
fputs("set xlabel'Random Axis Arrangement (for 3-d plot)'\n ", pipe_gp1);
fputs("set ylabel 'Iteration Number'\n ", pipe_gp1);
fputs("set zlabel 'Time taken for 100 iterations'\n ", pipe_gp1);
fputs("splot 'Data/GUIDED-n-file2.dat' u 1:2:3 w lines \n ", pipe_gp1);
fputs("set terminal png size 1200,800\n ", pipe_gp1);
fputs("set output 'Plots/GUIDED-n-3dplot2.png'\n ", pipe_gp1);
fputs("replot\n ", pipe_gp1);
fputs("reset\n ", pipe_gp1);
fputs("set terminal wxt 0\n ", pipe_gp1);
fputs("unset key\n ", pipe_gp1);
fputs("set title '2DPlot1'\n ", pipe_gp1);
fputs("set xlabel 'Iteration Number'\n ", pipe_gp1);
fputs("set ylabel 'Time taken for 100 iterations'\n ", pipe_gp1);
fputs("plot 'Data/GUIDED-n-file2.dat' u 2:3 w lines \n ", pipe_gp1);
fputs("set terminal png size 1200,800\n ", pipe_gp1);
fputs("set output 'Plots/GUIDED-n-2dplot2.png'\n ", pipe_gp1);
fputs("replot\n ", pipe_gp1);
pclose(pipe_gp1);
printf("\n\t\t\tPlots at 'GUIDED-n-plot1.png & GUIDED-n-plot2.png' for loop1 & loop2 (100I)\n\n");
}
returnValue DiscreteTimeExport::setup( )
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
// non equidistant integration grids not implemented for NARX integrators
if( !equidistant ) return ACADOERROR( RET_INVALID_OPTION );
String fileName( "integrator.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
ExportIndex run( "run" );
ExportIndex i( "i" );
ExportIndex j( "j" );
ExportIndex k( "k" );
ExportIndex tmp_index("tmp_index");
uint diffsDim = NX*(NX+NU);
uint inputDim = NX*(NX+NU+1) + NU + NP;
// setup INTEGRATE function
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, BT_TRUE );
rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
if( equidistantControlGrid() ) {
integrate = ExportFunction( "integrate", rk_eta, reset_int );
}
else {
integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
}
integrate.setReturnValue( error_code );
integrate.addIndex( run );
integrate.addIndex( i );
integrate.addIndex( j );
integrate.addIndex( k );
integrate.addIndex( tmp_index );
rhs_in = ExportVariable( "x", inputDim-diffsDim, 1, REAL, ACADO_LOCAL );
rhs_out = ExportVariable( "f", NX, 1, REAL, ACADO_LOCAL );
fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
rk_xxx = ExportVariable( "rk_xxx", 1, inputDim-diffsDim, REAL, structWspace );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX1, NX1+NU, REAL, structWspace );
rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX2, NX1+NX2+NU, REAL, structWspace );
}
rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2, NX1+NX2+NU, REAL, structWspace );
ExportVariable numInt( "numInts", 1, 1, INT );
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
integrate.addStatement( String( "int " ) << numInt.getName() << " = " << numStepsV.getName() << "[" << rk_index.getName() << "];\n" );
}
integrate.addStatement( rk_xxx.getCols( NX,inputDim-diffsDim ) == rk_eta.getCols( NX+diffsDim,inputDim ) );
integrate.addLinebreak( );
// integrator loop:
ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
ExportStatementBlock *loop;
if( equidistantControlGrid() ) {
loop = &tmpLoop;
}
else {
loop = &integrate;
loop->addStatement( String("for(") << run.getName() << " = 0; " << run.getName() << " < " << numInt.getName() << "; " << run.getName() << "++ ) {\n" );
}
loop->addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
// Set rk_diffsPrev:
loop->addStatement( String("if( run > 0 ) {\n") );
if( NX1 > 0 ) {
ExportForLoop loopTemp1( i,0,NX1 );
loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX1 ) );
if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
loop->addStatement( loopTemp1 );
}
if( NX2 > 0 ) {
ExportForLoop loopTemp2( i,0,NX2 );
loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+NXA+NX1*NX,i*NX+NX+NXA+NX1*NX+NX1+NX2 ) );
if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX1*NU,i*NU+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
loop->addStatement( loopTemp2 );
}
loop->addStatement( String("}\n") );
}
// evaluate states:
if( NX1 > 0 ) {
loop->addFunctionCall( lin_input.getName(), rk_xxx, rk_eta.getAddress(0,0) );
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameRHS(), rk_xxx, rk_eta.getAddress(0,NX1) );
}
// evaluate sensitivities:
if( NX1 > 0 ) {
for( uint i1 = 0; i1 < NX1; i1++ ) {
for( uint i2 = 0; i2 < NX1; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,i2,i2+1) == A11(i1,i2) );
}
for( uint i2 = 0; i2 < NU; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,NX1+i2,NX1+i2+1) == B11(i1,i2) );
}
}
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameDiffsRHS(), rk_xxx, rk_diffsNew2.getAddress(0,0) );
}
// computation of the sensitivities using chain rule:
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "if( run == 0 ) {\n" ) );
}
// PART 1
updateInputSystem(loop, i, j, tmp_index);
// PART 2
updateImplicitSystem(loop, i, j, tmp_index);
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "}\n" ) );
loop->addStatement( String( "else {\n" ) );
// PART 1
propagateInputSystem(loop, i, j, k, tmp_index);
// PART 2
propagateImplicitSystem(loop, i, j, k, tmp_index);
loop->addStatement( String( "}\n" ) );
}
// end of the integrator loop.
if( !equidistantControlGrid() ) {
loop->addStatement( "}\n" );
}
else {
integrate.addStatement( *loop );
}
// PART 1
if( NX1 > 0 ) {
Matrix zeroR = zeros(1, NX2);
ExportForLoop loop1( i,0,NX1 );
loop1.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1,i*NX+NX+NXA+NX ) == zeroR );
integrate.addStatement( loop1 );
}
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}