00001 #include "SIGEL_Simulation/SIG_Interpreter.h"
00002 #include "SIGEL_Tools/SIG_IO.h"
00003 #include <cmath>
00004
00005 namespace SIGEL_Simulation
00006 {
00007
00008 SIG_Interpreter::SIG_Interpreter(SIGEL_Robot::SIG_LanguageParameters const &langParams,
00009 SIGEL_Program::SIG_Program const &robotProgram,
00010 SIG_CommandInterface &commandInterface,
00011 SIG_SimulationQueries const &simulationQueries)
00012 : robotProgram(robotProgram),
00013 langParams(langParams),
00014 commandInterface(commandInterface),
00015 simulationQueries(simulationQueries),
00016 registers(),
00017 remainingLastCommandTime(0),
00018 programCounter(0),
00019 compareFlag(false)
00020 {
00021 int numberOfRegisters = langParams.getMemorySize();
00022 int registerWidth = langParams.getRegisterWidth();
00023 registers.resize( numberOfRegisters );
00024 for( int count = 0; count < numberOfRegisters; count++ )
00025 {
00026 registers.insert( count, new SIGEL_Simulation::SIG_Register(registerWidth) );
00027 }
00028 };
00029
00030 void SIG_Interpreter::interprete(double timeAccountSize)
00031 {
00032 long programLength = const_cast<SIGEL_Program::SIG_Program &>(robotProgram).getProgramLength();
00033
00034
00035 uint numberOfRegisters = registers.size();
00036
00037
00038 double maxDelayTime = static_cast<double>( langParams.getMaximalDelayTime() ) * 0.001 ;
00039
00040 if ( timeAccountSize <= remainingLastCommandTime )
00041 {
00042 remainingLastCommandTime -= timeAccountSize;
00043 #ifdef SIG_DEBUG
00044 SIGEL_Tools::SIG_IO::cerr << "Remaining Last Command Time: " << remainingLastCommandTime << endl;
00045 #endif
00046 }
00047 else
00048 {
00049
00050 timeAccountSize -= remainingLastCommandTime;
00051
00052
00053 while ( timeAccountSize > 0)
00054 {
00055
00056 SIGEL_Program::SIG_ProgramLine *theLine = const_cast< SIGEL_Program::SIG_Program &>( robotProgram ).getLine( programCounter );
00057
00058
00059
00060
00061 #ifdef SIG_DEBUG
00062 SIGEL_Tools::SIG_IO::cerr << "--------------------------------------------------------\n";
00063 SIGEL_Tools::SIG_IO::cerr << "Remaining Time:" << timeAccountSize<< endl;
00064 SIGEL_Tools::SIG_IO::cerr << "PC: " << programCounter << endl;
00065 if( compareFlag )
00066 SIGEL_Tools::SIG_IO::cerr << "CF: 1\n";
00067 else
00068 SIGEL_Tools::SIG_IO::cerr << "CF: 0\n";
00069 QString theQLine;
00070 theLine->printToString( theQLine );
00071 SIGEL_Tools::SIG_IO::cerr << theQLine;
00072 SIGEL_Tools::SIG_IO::cerr << "Registers:\n";
00073 for( int loop=0; loop < numberOfRegisters; loop++ )
00074 {
00075 SIGEL_Tools::SIG_IO::cerr << "Reg." << loop << ":" << registers[loop]->getValue() << " ";
00076 }
00077 SIGEL_Tools::SIG_IO::cerr << endl;
00078 #endif
00079
00080
00081 switch ( theLine->getRobotinstructionType() )
00082 {
00083 case SIGEL_Program::COPY:
00084
00085 if ( langParams.hasCommand( "COPY" ) )
00086 {
00087
00088 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00089 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00090 registers[reg0]->copyReg( *registers[reg1] );
00091 programCounter = (programCounter + 1) % programLength;
00092
00093
00094 timeAccountSize -= langParams.getCommand( "COPY" )->getDuration();
00095
00096
00097
00098
00099
00100 if ( timeAccountSize < 0 )
00101 remainingLastCommandTime = - timeAccountSize;
00102 }
00103
00104
00105 else
00106 {
00107 programCounter = (programCounter + 1) % programLength;
00108 }
00109 break;
00110
00111 case SIGEL_Program::LOAD:
00112
00113 if ( langParams.hasCommand( "LOAD" ) )
00114 {
00115
00116 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00117 int value = theLine->getInstructionElement(1);
00118 registers[reg0]->loadValue( value );
00119 programCounter = (programCounter + 1) % programLength;
00120
00121
00122 timeAccountSize -= langParams.getCommand( "LOAD" )->getDuration();
00123
00124
00125
00126
00127
00128 if ( timeAccountSize < 0 )
00129 remainingLastCommandTime = - timeAccountSize;
00130 }
00131 else
00132 {
00133 programCounter = (programCounter + 1) % programLength;
00134 }
00135 break;
00136
00137 case SIGEL_Program::ADD:
00138
00139 if ( langParams.hasCommand( "ADD" ) )
00140 {
00141
00142 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00143 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00144 registers[reg0]->addReg( *registers[reg1] );
00145 programCounter = (programCounter + 1) % programLength;
00146
00147
00148 timeAccountSize -= langParams.getCommand( "ADD" )->getDuration();
00149
00150
00151
00152
00153
00154 if ( timeAccountSize < 0 )
00155 remainingLastCommandTime = - timeAccountSize;
00156 }
00157 else
00158 {
00159 programCounter = (programCounter + 1) % programLength;
00160 }
00161 break;
00162
00163 case SIGEL_Program::SUB:
00164
00165 if ( langParams.hasCommand( "SUB" ) )
00166 {
00167
00168 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00169 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00170 registers[reg0]->subReg( *registers[reg1] );
00171 programCounter = (programCounter + 1) % programLength;
00172
00173
00174 timeAccountSize -= langParams.getCommand( "SUB" )->getDuration();
00175
00176
00177
00178
00179
00180 if ( timeAccountSize < 0 )
00181 remainingLastCommandTime = - timeAccountSize;
00182 }
00183 else
00184 {
00185 programCounter = (programCounter + 1) % programLength;
00186 }
00187 break;
00188
00189 case SIGEL_Program::MUL:
00190
00191 if ( langParams.hasCommand( "MUL" ) )
00192 {
00193
00194 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00195 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00196 registers[reg0]->mulReg( *registers[reg1] );
00197 programCounter = (programCounter + 1) % programLength;
00198
00199
00200 timeAccountSize -= langParams.getCommand( "MUL" )->getDuration();
00201
00202
00203
00204
00205
00206 if ( timeAccountSize < 0 )
00207 remainingLastCommandTime = - timeAccountSize;
00208 }
00209 else
00210 {
00211 programCounter = (programCounter + 1) % programLength;
00212 }
00213 break;
00214
00215 case SIGEL_Program::DIV:
00216
00217 if ( langParams.hasCommand( "DIV" ) )
00218 {
00219
00220 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00221 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00222 registers[reg0]->divReg( *registers[reg1] );
00223 programCounter = (programCounter + 1) % programLength;
00224
00225
00226 timeAccountSize -= langParams.getCommand( "DIV" )->getDuration();
00227
00228
00229
00230
00231
00232 if ( timeAccountSize < 0 )
00233 remainingLastCommandTime = - timeAccountSize;
00234 }
00235 else
00236 {
00237 programCounter = (programCounter + 1) % programLength;
00238 }
00239 break;
00240
00241 case SIGEL_Program::MOD:
00242
00243 if ( langParams.hasCommand( "MOD" ) )
00244 {
00245
00246 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00247 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00248 registers[reg0]->modReg( *registers[reg1] );
00249 programCounter = (programCounter + 1) % programLength;
00250
00251
00252 timeAccountSize -= langParams.getCommand( "MOD" )->getDuration();
00253
00254
00255
00256
00257
00258 if ( timeAccountSize < 0 )
00259 remainingLastCommandTime = - timeAccountSize;
00260 }
00261 else
00262 {
00263 programCounter = (programCounter + 1) % programLength;
00264 }
00265 break;
00266
00267 case SIGEL_Program::MIN:
00268
00269 if ( langParams.hasCommand( "MIN" ) )
00270 {
00271
00272 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00273 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00274 registers[reg0]->minReg( *registers[reg1] );
00275 programCounter = (programCounter + 1) % programLength;
00276
00277
00278 timeAccountSize -= langParams.getCommand( "MIN" )->getDuration();
00279
00280
00281
00282
00283
00284 if ( timeAccountSize < 0 )
00285 remainingLastCommandTime = - timeAccountSize;
00286 }
00287 else
00288 {
00289 programCounter = (programCounter + 1) % programLength;
00290 }
00291 break;
00292
00293 case SIGEL_Program::MAX:
00294
00295 if ( langParams.hasCommand( "MAX" ) )
00296 {
00297
00298 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00299 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00300 registers[reg0]->maxReg( *registers[reg1] );
00301 programCounter = (programCounter + 1) % programLength;
00302
00303
00304 timeAccountSize -= langParams.getCommand( "MAX" )->getDuration();
00305
00306
00307
00308
00309
00310 if ( timeAccountSize < 0 )
00311 remainingLastCommandTime = - timeAccountSize;
00312 }
00313 else
00314 {
00315 programCounter = (programCounter + 1) % programLength;
00316 }
00317 break;
00318
00319 case SIGEL_Program::CMP:
00320
00321 if ( langParams.hasCommand( "CMP" ) )
00322 {
00323
00324 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00325 int reg1 = theLine->getInstructionElement(1) % numberOfRegisters;
00326 if ( registers[reg0]->getValue() <= registers[reg1]->getValue() )
00327 compareFlag = true;
00328 else
00329 compareFlag = false;
00330 #ifdef SIG_DEBUG
00331 SIGEL_Tools::SIG_IO::cerr << "Reg0: " << reg0 << "; Reg1: " << reg1 << endl;
00332 #endif
00333 programCounter = (programCounter + 1) % programLength;
00334
00335
00336 timeAccountSize -= langParams.getCommand( "CMP" )->getDuration();
00337
00338
00339
00340
00341
00342 if ( timeAccountSize < 0 )
00343 remainingLastCommandTime = - timeAccountSize;
00344 }
00345 else
00346 {
00347 programCounter = (programCounter + 1) % programLength;
00348 }
00349 break;
00350
00351 case SIGEL_Program::JMP:
00352
00353 if ( langParams.hasCommand( "JMP" ) )
00354 {
00355
00356 int reg0 = theLine->getInstructionElement(0);
00357 if( compareFlag )
00358 programCounter = ( programCounter + 1 + reg0 ) % programLength;
00359 else
00360 programCounter = (programCounter + 1) % programLength;
00361 if( programCounter < 0 )
00362 programCounter = -programCounter;
00363
00364
00365 timeAccountSize -= langParams.getCommand( "JMP" )->getDuration();
00366
00367
00368
00369
00370
00371 if ( timeAccountSize < 0 )
00372 remainingLastCommandTime = - timeAccountSize;
00373 }
00374 else
00375 {
00376 programCounter = (programCounter + 1) % programLength;
00377 }
00378 break;
00379
00380 case SIGEL_Program::SENSE:
00381
00382 if ( langParams.hasCommand( "SENSE" ) )
00383 {
00384
00385 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00386 int numberOfSensor = registers[reg0]->getValue();
00387 simulationQueries.sense( numberOfSensor, registers );
00388 programCounter = (programCounter + 1) % programLength;
00389
00390
00391 timeAccountSize -= langParams.getCommand( "SENSE" )->getDuration();
00392
00393
00394
00395
00396
00397 if ( timeAccountSize < 0 )
00398 remainingLastCommandTime = - timeAccountSize;
00399 }
00400 else
00401 {
00402 programCounter = (programCounter + 1) % programLength;
00403 }
00404 break;
00405
00406 case SIGEL_Program::MOVE:
00407
00408 if ( langParams.hasCommand( "MOVE" ) )
00409 {
00410
00411 int reg0 = theLine->getInstructionElement(0) % numberOfRegisters;
00412 int numberOfJoint = registers[reg0]->getValue();
00413 #ifdef SIG_DEBUG
00414 SIGEL_Tools::SIG_IO::cerr << "Moving drive " << numberOfJoint << ".\n";
00415 #endif
00416 commandInterface.moveDrive( numberOfJoint, registers );
00417 programCounter = (programCounter + 1) % programLength;
00418
00419
00420 timeAccountSize -= langParams.getCommand( "MOVE" )->getDuration();
00421
00422
00423
00424
00425
00426 if ( timeAccountSize < 0 )
00427 remainingLastCommandTime = - timeAccountSize;
00428 }
00429 else
00430 {
00431 programCounter = (programCounter + 1) % programLength;
00432 }
00433 break;
00434
00435 case SIGEL_Program::DELAY:
00436
00437 if ( langParams.hasCommand( "DELAY" ) )
00438 {
00439
00440
00441
00442 int reg = theLine->getInstructionElement(0) % numberOfRegisters;
00443 int readOut = registers[reg]->getValue();
00444 double delayTime = static_cast<double>( readOut ) * 0.001;
00445 delayTime = std::abs( delayTime );
00446 if( delayTime > maxDelayTime )
00447 delayTime = maxDelayTime;
00448 timeAccountSize -= delayTime;
00449 programCounter = (programCounter + 1) % programLength;
00450
00451
00452 timeAccountSize -= langParams.getCommand( "DELAY" )->getDuration();
00453
00454
00455
00456
00457
00458 if ( timeAccountSize < 0 )
00459 remainingLastCommandTime = - timeAccountSize;
00460 }
00461 else
00462 {
00463 programCounter = (programCounter + 1) % programLength;
00464 }
00465 break;
00466
00467 case SIGEL_Program::NOP:
00468
00469 if ( langParams.hasCommand( "NOP" ) )
00470 {
00471
00472 programCounter = (programCounter + 1) % programLength;
00473
00474
00475 timeAccountSize -= langParams.getCommand( "NOP" )->getDuration();
00476
00477
00478
00479
00480
00481 if ( timeAccountSize < 0 )
00482 remainingLastCommandTime = - timeAccountSize;
00483 }
00484 else
00485 {
00486 programCounter = (programCounter + 1) % programLength;
00487 }
00488 break;
00489
00490 default:
00491 #ifdef SIG_DEBUG
00492 SIGEL_Tools::SIG_IO::cerr << "Something went horribly wrong in the interpreter! Default was called!!! Didn't recognize command. PC++\n";
00493 #endif
00494 programCounter = (programCounter + 1) % programLength;
00495 break;
00496 }
00497 }
00498 #ifdef SIG_DEBUG
00499 SIGEL_Tools::SIG_IO::cerr << "Remaining Last Command Time: " << remainingLastCommandTime << endl;
00500 #endif
00501 }
00502 };
00503
00504 }
00505
00506
00507
00508