Página principal   Lista alfabética   Lista de componentes   Lista de archivos   Miembros de las clases   Archivos de los miembros  

robot.cpp

Ir a la documentación de este archivo.
00001 #include "robot.h"
00002 
00014 DLL_EXPORT long STD ConexionConRobot (long TipoConexion, double XPosInic ,
00015                                                                           double YPosInic, double Heading, char *DireccionIP)
00016 {
00017         long i;
00018         switch (TipoConexion)
00019         {
00020         case 0:
00021                 i=paiRobotStartup (sfLOCALPORT,sfCOMLOCAL,NULL,NULL);
00022                 break;
00023         case 1:
00024                 i=paiRobotStartup (TCP,DireccionIP,NULL,NULL);
00025                 break;
00026         }
00027         if (i==1) {
00028                 sfRobotComInt(4,1);     //Conecta los motores del robot si hay conexion con él
00029                 sfMoveRobot ((float)YPosInic,-(float)XPosInic,(float)Heading);  //Comunica a Saphira la posición que  
00030                                                                                                                    //ocupa el robot en el momento de conectarlo
00031         }
00032         return i;
00033 }
00034 
00040 DLL_EXPORT void STD DesconexionRobot (void)
00041 {
00042         sfDisconnectFromRobot();
00043 }
00044 
00045 int paiRobotStartup(int channel, char *name, HANDLE hInst, int nCmdShow)
00046 {
00047         int i;
00048         sfOnConnectFn(myConnectFn);     /* register a connection function */
00049         sfOnStartupFn(myStartupFn);     /* register a startup function */
00050         sfStartup(hInst, nCmdShow, 1);  /* start up Saphira, and  keep going */
00051         i = 50;
00052         while (!sfIsConnected && i--) {
00053                 sfPause(50);
00054         }
00055         sfInitControlProcs();
00056         sfRunEvaluator();
00057         return sfConnectToRobot(channel, name); 
00058 }
00059 
00060 void myStartupFn(void)
00061 {
00062         sfSetDisplayState(sfGLOBAL, TRUE);      /* use the global view */
00063 }
00064 
00065 void myConnectFn(void)
00066 {
00067         sfInitBasicProcs();             /* start up comm processes */
00068 }       
00069 
00079 DLL_EXPORT void STD GirarRobotDerecha (long velocidad)
00080 {
00081         sfSetRVelocity((int)(-velocidad));
00082 }
00083 
00093 DLL_EXPORT void STD GirarRobotIzquierda (long velocidad)
00094 {
00095         sfSetRVelocity((int)(velocidad));
00096 }
00105 DLL_EXPORT void STD GirarRobotHastaAnguloN (double Angulo)
00106 {
00107         sfSetHeading ((int)(Angulo));
00108 }
00109 
00117 DLL_EXPORT void STD DetenerGiroRobot (void)
00118 {
00119 //      do {
00120                 sfSetRVelocity(0);
00121 //      }while (sfRobot.rv!=0);
00122 }
00123 
00131 DLL_EXPORT void STD AvanzarRobot (long velocidad)
00132 {
00133         sfSetVelocity(velocidad);
00134 }
00135 
00143 DLL_EXPORT void STD RetrocederRobot (long velocidad)
00144 {
00145         sfSetVelocity(-velocidad);
00146 }
00147 
00154 DLL_EXPORT void STD DetenerTraslacionRobot (void)
00155 {
00156         sfSetVelocity(0);
00157 }
00158 
00166 DLL_EXPORT void STD GetRobotPosition (double *PosX,double *PosY,double *Heading)
00167 {
00168         *PosX=-sfRobot.ay;
00169         *PosY=sfRobot.ax;
00170         *Heading=sfRobot.ath;
00171 }
00172 
00179 DLL_EXPORT void STD CargarBehaviors (BehaviorsParams *Parametros)
00180 {
00181         sfStartBehavior(sfConstantVelocity,
00182                                         "Velocidad Constante",
00183                                         (*Parametros).ConstantVelocityParams.TimeOut,           //Timeout
00184                                         (*Parametros).ConstantVelocityParams.Prioridad,         //Prioridad
00185                                         (*Parametros).ConstantVelocityParams.Suspendido,        //Suspension
00186                                         (*Parametros).ConstantVelocityParams.Velocidad);        //Velocidad de traslación
00187 
00188         sfStartBehavior(sfKeepOff,
00189                                         "Mantener Distancias",
00190                                         (*Parametros).KeepOffParams.TimeOut,            //Timeout
00191                                         (*Parametros).KeepOffParams.Prioridad,          //Prioridad
00192                                         (*Parametros).KeepOffParams.Suspendido,         //Suspension
00193                                         (*Parametros).KeepOffParams.VelocidadCauta,     //Velocidad de precaucion
00194                                         (*Parametros).KeepOffParams.Sensibilidad);      //Sensibilidad a los obstáculos (de 0.2 a 2.0)
00195 
00196         sfStartBehavior(sfAvoidCollision,
00197                                         "Evita Colision",
00198                                         (*Parametros).AvoidCollisionParams.TimeOut,             //Timeout
00199                                         (*Parametros).AvoidCollisionParams.Prioridad,   //Prioriudad
00200                                         (*Parametros).AvoidCollisionParams.Suspendido,  //suspension
00201                                         (*Parametros).AvoidCollisionParams.SensibilidadFrontal, //Sensibilidad a obstáculos frontales (de 0.5 a 3.0)
00202                                         (*Parametros).AvoidCollisionParams.SensibilidadLateral, //Sensibilidad a obstáculos laterales (de 0.5 a 3.0)
00203                                         (*Parametros).AvoidCollisionParams.GananciaGiro,                //Ganancia del giro (de 4.0 a 10.0)
00204                                         (*Parametros).AvoidCollisionParams.RadioSeguridad);             //Define la burbuja de seguridad del 
00205                                                                                                                                                                         // robot en milímetros
00206 
00207         sfStartBehavior(sfStop,
00208                                         "Detener Robot",
00209                                         (*Parametros).StopParams.TimeOut,       //Timeout
00210                                         (*Parametros).StopParams.Prioridad,     //Prioriudad
00211                                         (*Parametros).StopParams.Suspendido);   //suspension
00212 }
00213 
00219 DLL_EXPORT void STD DescargarBehaviors ()
00220 {
00221         sfRemoveTask ("Velocidad Constante");
00222         sfRemoveTask ("Mantener Distancias");
00223         sfRemoveTask ("Evita Colision");
00224         sfRemoveTask ("Detener Robot");
00225 }
00226 
00227 /*
00228 DLL_EXPORT void STD     Behav_ConstVelocity (char *NombreInstancia,long TimeOut, 
00229                                                                                 long Prioridad,long Suspension)
00230 {
00231         ComportamientoVel=sfStartBehavior(sfConstantVelocity,
00232                                         NombreInstancia,
00233                                         (int)TimeOut,
00234                                         (int)Prioridad,
00235                                         (int)Suspension,
00236                                         100.0);
00237         sfLoadEvalFile("c:\\sfTrackingObject.dll");
00238         sfLoadEvalFile("c:\\sfTracking.act");
00239         i=sfTaskSuspended (NombreInstancia);
00240 }
00241 */
00242 
00251 DLL_EXPORT void STD Behav_Loaded (char *NombreFichero, char *NombreInstancia)
00252 {
00253         sfLoadEvalFile (NombreFichero);
00254 }
00255 
00264 DLL_EXPORT void STD Behav_Resume (char *NombreInstancia)
00265 {
00266         sfResumeTask (NombreInstancia);
00267 }
00268 
00277 DLL_EXPORT void STD Behav_Kill (char *NombreInstancia)
00278 {
00279         sfRemoveTask (NombreInstancia);
00280 }
00281 
00290 DLL_EXPORT void STD Behav_Pause (char *NombreInstancia)
00291 {
00292         sfInterruptTask (NombreInstancia);
00293 }
00294 
00295 /*DLL_EXPORT void STD splinetemp ()
00296 {
00297 //      sfLoadEvalFile("h:\\sfDoTrajectory.dll");
00298 
00299         sfDTAddGlobalPoint(0,0);
00300 
00301         sfDTAddGlobalPoint(0,0);
00302         sfDTAddGlobalPoint(1000,0);
00303         sfDTAddGlobalPoint(1500,0);
00304         sfDTAddGlobalPoint(2000,-500);
00305         sfDTAddGlobalPoint(2500,500);
00306         sfDTAddGlobalPoint(3000,0);
00307         sfDTAddGlobalPoint(3000,0);
00308         
00309         sfDTDrawTrajectory();
00310         sfDTStartTrajectory();
00311 }
00312 */
00323  DLL_EXPORT void STD SeguirSpline (long x,long y,long th,double vel,double radio)
00324 {
00325         ThreadFinished=false;
00326         Valores.x=y;
00327         Valores.y=-x;
00328         Valores.th=th;
00329         Valores.Velocidad=vel;
00330         Valores.RadioExito=radio;
00331         _beginthread (ThreadSeguirSpline,0,(void*)&Valores);
00332 }
00333 
00342 void ThreadSeguirSpline (void *Datos)
00343 {
00344         static sfprocess *p1=NULL;
00345         point *goal;
00346         bool Succes=false;
00347         PassToThreadType* Val = (PassToThreadType*)Datos;
00348 
00349         goal=sfCreateGlobalPoint ((float)(*Val).x,(float)(*Val).y,0);
00350         sfSetGlobalCoords(goal);        
00351         sfAddPointCheck(goal);
00352         p1=sfStartBehavior (sfAttendAtPos,
00353                                                 "sfGotoPos",
00354                                                 0,                                              //Timeout 
00355                                                 2,                                              //Prioridad
00356                                                 0,                                              //Suspensión
00357                                                 (double)(*Val).Velocidad,               //Velocidad de traslación
00358                                                 goal,                                   //Punto de destino
00359                                                 (double)(*Val).RadioExito);             //Radio de exito
00360    while (sfGetProcessState(p1)!=sfFAILURE && Succes==false) 
00361    {
00362                 sfPause(1);     //Las cosas van mejor con una pausa aqui ;)
00363                 switch(sfGetProcessState(p1)) 
00364                 {
00365                         case sfSUCCESS:
00366                                 if (sfFinished(p1)) 
00367                                 {
00368                                         p1=NULL;
00369                                         sfRemPoint(goal);
00370                                         Succes=true;
00371                                         break;
00372                                 }
00373             case sfTIMEOUT:
00374                 p1=NULL;
00375                 sfRemPoint(goal);
00376                 break;
00377                 }
00378         }
00379    ThreadFinished=true;
00380    _endthread();
00381 }
00382 
00390 DLL_EXPORT int STD ThreadState (void)
00391 {
00392         if (ThreadFinished==true) {
00393                 return 1;
00394         }
00395         return 0;
00396 }

Generado el Tue Apr 24 06:55:49 2001 para Dllcontrol por doxygen1.2.6 escrito por Dimitri van Heesch, © 1997-2001