Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_Interpreter.cpp

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     // save how many registers are available as it will be needed very often...
00035     uint numberOfRegisters = registers.size();
00036 
00037     // save the maximalDelayTime
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         // use the rest of remaining and interprete on! :)
00050         timeAccountSize -= remainingLastCommandTime;
00051 
00052         // we still got time to do something
00053         while ( timeAccountSize > 0)
00054           {
00055             // fetch the next command
00056             SIGEL_Program::SIG_ProgramLine *theLine = const_cast< SIGEL_Program::SIG_Program &>( robotProgram ).getLine( programCounter );
00057             
00058             /*
00059              * ONLY FOR DEBUGGING-PURPOSES!
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             // what command was fetched?
00081             switch ( theLine->getRobotinstructionType() )
00082               {
00083               case SIGEL_Program::COPY:
00084                 // is the command allowed?
00085                 if ( langParams.hasCommand( "COPY" ) )
00086                   {
00087                     // do it!
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                     // subtract the needed time for the command
00094                     timeAccountSize -= langParams.getCommand( "COPY" )->getDuration();
00095 
00096                     /*
00097                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00098                      * by which we have exeeded it and do nothing (will be done the next time).
00099                      */
00100                     if ( timeAccountSize < 0 )
00101                       remainingLastCommandTime = - timeAccountSize;
00102                   }
00103                 // the command is not allowed but we have to increase the PC so we don't get the
00104                 // same line again
00105                 else
00106                   {
00107                     programCounter = (programCounter + 1) % programLength;
00108                   }
00109                 break;
00110 
00111               case SIGEL_Program::LOAD:
00112                 // is the command allowed?
00113                 if ( langParams.hasCommand( "LOAD" ) )
00114                   {
00115                     // do it!
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                     // subtract the needed time for the command
00122                     timeAccountSize -= langParams.getCommand( "LOAD" )->getDuration();
00123                     
00124                     /*
00125                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00126                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00139                 if ( langParams.hasCommand( "ADD" ) )
00140                   {
00141                     // do it!
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                     // subtract the needed time for the command
00148                     timeAccountSize -= langParams.getCommand( "ADD" )->getDuration();
00149                     
00150                     /*
00151                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00152                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00165                 if ( langParams.hasCommand( "SUB" ) )
00166                   {
00167                     // do it!
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                     // subtract the needed time for the command
00174                     timeAccountSize -= langParams.getCommand( "SUB" )->getDuration();
00175                     
00176                     /*
00177                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00178                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00191                 if ( langParams.hasCommand( "MUL" ) )
00192                   {
00193                     // do it!
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                     // subtract the needed time for the command
00200                     timeAccountSize -= langParams.getCommand( "MUL" )->getDuration();
00201                     
00202                     /*
00203                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00204                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00217                 if ( langParams.hasCommand( "DIV" ) )
00218                   {
00219                     // do it!
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                     // subtract the needed time for the command
00226                     timeAccountSize -= langParams.getCommand( "DIV" )->getDuration();
00227                     
00228                     /*
00229                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00230                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00243                 if ( langParams.hasCommand( "MOD" ) )
00244                   {
00245                     // do it!
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                     // subtract the needed time for the command
00252                     timeAccountSize -= langParams.getCommand( "MOD" )->getDuration();
00253                     
00254                     /*
00255                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00256                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00269                 if ( langParams.hasCommand( "MIN" ) )
00270                   {
00271                     // do it!
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                     // subtract the needed time for the command
00278                     timeAccountSize -= langParams.getCommand( "MIN" )->getDuration();
00279                     
00280                     /*
00281                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00282                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00295                 if ( langParams.hasCommand( "MAX" ) )
00296                   {
00297                     // do it!
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                     // subtract the needed time for the command
00304                     timeAccountSize -= langParams.getCommand( "MAX" )->getDuration();
00305                     
00306                     /*
00307                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00308                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00321                 if ( langParams.hasCommand( "CMP" ) )
00322                   {
00323                     // do it!
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                     // subtract the needed time for the command
00336                     timeAccountSize -= langParams.getCommand( "CMP" )->getDuration();
00337                     
00338                     /*
00339                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00340                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00353                 if ( langParams.hasCommand( "JMP" ) )
00354                   {
00355                     // do it!
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                     // subtract the needed time for the command
00364 
00365                     timeAccountSize -= langParams.getCommand( "JMP" )->getDuration();
00366                     
00367                     /*
00368                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00369                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00382                 if ( langParams.hasCommand( "SENSE" ) )
00383                   {
00384                     // do it!
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                     // subtract the needed time for the command
00391                     timeAccountSize -= langParams.getCommand( "SENSE" )->getDuration();
00392                     
00393                     /*
00394                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00395                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00408                 if ( langParams.hasCommand( "MOVE" ) )
00409                   {
00410                     // do it!
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                     // subtract the needed time for the command
00420                     timeAccountSize -= langParams.getCommand( "MOVE" )->getDuration();
00421                     
00422                     /*
00423                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00424                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00437                 if ( langParams.hasCommand( "DELAY" ) )
00438                   {
00439                     // do it!
00440                     // int readOut = theLine->getInstructionElement(0);
00441                     // get the register
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                     // subtract the needed time for the command
00452                     timeAccountSize -= langParams.getCommand( "DELAY" )->getDuration();
00453                     
00454                     /*
00455                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00456                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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                 // is the command allowed?
00469                 if ( langParams.hasCommand( "NOP" ) )
00470                   {
00471                     // do it!
00472                     programCounter = (programCounter + 1) % programLength;
00473 
00474                     // subtract the needed time for the command
00475                     timeAccountSize -= langParams.getCommand( "NOP" )->getDuration();
00476                     
00477                     /*
00478                      * have we exeeded the allowed time? if so, set remainingLastCommandTime to the amout
00479                      * by which we have exeeded it and do nothing (will be done the next time). else do it!
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               } // close switch
00497           } // close while
00498 #ifdef SIG_DEBUG
00499         SIGEL_Tools::SIG_IO::cerr << "Remaining Last Command Time: " << remainingLastCommandTime << endl; // debug! delete!
00500 #endif
00501       } // close else
00502   };
00503 
00504 }
00505 
00506 
00507 
00508 

Generated at Mon Sep 3 01:32:26 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000