PreMotorComplex.C

00001 #ifndef __PREMOTOR_COMPLEX_C__
00002 #define __PREMOTOR_COMPLEX_C__
00003 
00004 #include "Util/log.H"
00005 #include "BeoSub/BeeBrain/PreMotorComplex.H"
00006 #include "Raster/Raster.H"
00007 
00008 
00009 // ######################################################################
00010 PreMotorComplex::PreMotorComplex(nub::soft_ref<SubController> motor, /*rutz::shared_ptr<PreFrontalCortexAgent pfc,*/ std::string name)
00011 //, rutz::shared_ptr<AgentManagerA> managerPtr)
00012   :
00013 itsCurrentMove(),
00014 itsNextMove(),
00015 itsPrimitiveMotor(motor)
00016   //itsCortex(pfc)
00017 {
00018   itsName = name;
00019   //managerA = NULL;//managerPtr;
00020   //cortex = NULL; //managerA->getPreFrontalCortexAgent();
00021   itsStop = itsInterrupt = false;
00022 }
00023 
00024 // ######################################################################
00025 PreMotorComplex::PreMotorComplex(std::string name)
00026 //, rutz::shared_ptr<AgentManagerA> managerPtr)
00027   :
00028 itsCurrentMove(),
00029 itsNextMove()
00030 {
00031   itsName = name;
00032   //managerA = NULL;//managerPtr;
00033   //cortex = NULL; //managerA->getPreFrontalCortexAgent();
00034   itsStop = itsInterrupt = false;
00035 }
00036 
00037 // ######################################################################
00038 PreMotorComplex::~PreMotorComplex() {
00039 }
00040 
00041 // ######################################################################
00042 void PreMotorComplex::start() {
00043 
00044   while(!itsStop) {
00045 
00046     while(itsCurrentMove.is_valid() && !itsInterrupt) {
00047       LINFO("RUNNING MOVE");
00048       run();
00049     }
00050 
00051     //    itsPrimitiveMotor->setLevel();
00052 
00053     if(itsInterrupt) {
00054       LINFO("INTERRUPT MOVE SET");
00055 
00056       if(itsNextMove.is_valid()) {
00057         LINFO("NEW MOVE SET TO RUN");
00058         itsCurrentMove = itsNextMove;
00059         itsNextMove.reset();
00060         itsInterrupt = false;
00061       }
00062       else
00063         LINFO("NEW MOVE IS NOT VALID. NO MOVE");
00064     }
00065     else
00066       LINFO("NO MOVE");
00067   }
00068 
00069 
00070   LINFO("STOP EVERYTHING");
00071 
00072   itsPrimitiveMotor->setDepth(0);
00073   while(itsPrimitiveMotor->getDepth() > 0) {}
00074   itsPrimitiveMotor->killMotors();
00075 }
00076 
00077 
00078 // ######################################################################
00079 void PreMotorComplex::run(rutz::shared_ptr<ComplexMovement> c) {
00080   //itsNextMove = c;
00081   //itsInterrupt = false;
00082   itsCurrentMove = c;
00083   //  msgHalt();
00084   run();
00085 }
00086 
00087 
00088 // ######################################################################
00089 void PreMotorComplex::run() {
00090 
00091   uint i = 0;
00092 
00093   while(!itsInterrupt && itsCurrentMove.is_valid()) {
00094 
00095 
00096     ////////// once function calls ///////
00097     ////////// world/sensor moves /////////
00098     i = 0;
00099     while(!itsInterrupt && i < itsCurrentMove->W_onceFuncs.size()) {
00100       (*this.*(itsCurrentMove->W_onceFuncs[i]))(itsCurrentMove->W_onceParams[i]);
00101       i++;
00102     }
00103 
00104     ////////// vision moves ///////////
00105     i = 0;
00106     while(!itsInterrupt && i < itsCurrentMove->Vis_onceFuncs.size()) {
00107       (*this.*(itsCurrentMove->Vis_onceFuncs[i]))(itsCurrentMove->Vis_onceParams[i]);
00108       i++;
00109     }
00110 
00111 
00112 
00113     /////////// repeat function calls ////////////
00114     /////////// world/sensor moves ////////////
00115     i = 0;
00116     while(!itsInterrupt && i < itsCurrentMove->W_reFuncs.size()) {
00117       (*this.*(itsCurrentMove->W_reFuncs[i]))(itsCurrentMove->W_reParams[i]);
00118       i++;
00119     }
00120 
00121     //////////// vision moves //////////
00122     i = 0;
00123     while(!itsInterrupt && i < itsCurrentMove->Vis_reFuncs.size()) {
00124       (*this.*(itsCurrentMove->Vis_reFuncs[i]))(itsCurrentMove->Vis_reParams[i]);
00125       i++;
00126     }
00127 
00128     //////// clear the once function lists //////
00129     itsCurrentMove->W_onceFuncs.clear();
00130     itsCurrentMove->W_onceParams.clear();
00131     itsCurrentMove->Vis_onceFuncs.clear();
00132     itsCurrentMove->Vis_onceParams.clear();
00133 
00134     if(itsCurrentMove->Vis_reFuncs.size() == 0 && itsCurrentMove->W_reFuncs.size() == 0)
00135       break;
00136 
00137   }
00138 
00139   // if(!itsInterrupt)
00140   //  itsCortex->msgMovementComplete();
00141 
00142 }
00143 
00144 
00145 
00146 
00147 // #####  world/sensor moves #####
00148 
00149 // ######################################################################
00150 void PreMotorComplex::turn(const SensorInput goal) {
00151 
00152   if(goal.angle.getVal() != INVALID) {
00153     LINFO("turn to Aboslute Angle: %f", goal.angle.getVal());
00154     LINFO("current Heading: %d\n", itsPrimitiveMotor->getHeading());
00155     itsPrimitiveMotor->setHeading((int)goal.angle.getVal());
00156   }
00157 
00158 }
00159 
00160 
00161 // ######################################################################
00162 void PreMotorComplex::forward(const SensorInput goal) {
00163 
00164   if(INVALID != goal.data) {
00165     LINFO("move forward/reverse to dist(m): %f", goal.data);
00166     //    int speed;
00167     //    itsPrimitiveMotor->setSpeed(100);
00168     sleep(1);
00169     //    itsPrimitiveMotor->setSpeed(100);
00170     //printf("get current heading: %d\n", itsPrimitiveMotor->getHeading());
00171   }
00172 
00173 
00174 }
00175 
00176 
00177 // ######################################################################
00178 void PreMotorComplex::dive(const SensorInput goal) {
00179 
00180   if(INVALID != goal.data) {
00181     LINFO("dive/surface to depth(m): %f", goal.data);
00182     LINFO("curent depth: %d", itsPrimitiveMotor->getDepth());
00183     itsPrimitiveMotor->setDepth((int)goal.data);
00184   }
00185 
00186 
00187 }
00188 
00189 
00190 // #####  vision moves  #####
00191 
00192 // ######################################################################
00193 void PreMotorComplex::vis_turn(const VisionInput& goal) {
00194 
00195   if(goal.position.is_valid())
00196     printf("turn to pixel coordinate: %d, %d, %d\n",
00197            goal.position->x, goal.position->y, goal.position->z);
00198   else if(goal.angle.is_valid())
00199     printf("turn to camera angle: %f\n", goal.angle->getVal());
00200 }
00201 
00202 
00203 // ######################################################################
00204 void PreMotorComplex::vis_forward(const VisionInput& goal) {
00205   if(goal.position.is_valid())
00206     printf("move forward to pixel coordinate: %d, %d, %d\n",
00207            goal.position->x, goal.position->y, goal.position->z);
00208 }
00209 
00210 
00211 // ######################################################################
00212 void PreMotorComplex::vis_dive(const VisionInput& goal) {
00213   if(goal.position.is_valid())
00214     printf("dive to pixel coordinate: %d, %d, %d\n",
00215            goal.position->x, goal.position->y, goal.position->z);
00216 
00217 }
00218 
00219 
00220 // ######################################################################
00221 void PreMotorComplex::vis_center(const VisionInput& goal) {
00222   if(goal.position.is_valid()) {
00223     LINFO("center on image coordinate: %d, %d, %d\n",
00224           goal.position->x, goal.position->y, goal.position->z);
00225     vis_turn(goal);
00226     if(goal.position->z != INVALID)
00227       vis_dive(goal);
00228     else
00229       vis_forward(goal);
00230   }
00231 }
00232 
00233 
00234 // ######################################################################
00235 void PreMotorComplex::vis_lost(const VisionInput& goal) {
00236   if(goal.position.is_valid()) {
00237 
00238     LINFO("retract: find lost object, was last seen: %d, %d, %d\n",
00239           goal.position->x, goal.position->y, goal.position->z);
00240 
00241     //vis_turn(-1*goal);
00242     //if(goal.position->z != INVALID)
00243     //  vis_dive(-1*goal);
00244     //else
00245     //  vis_forward(-1*goal);
00246   }
00247 }
00248 
00249 #endif
Generated on Sun May 8 08:40:19 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3