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);
00029 sfMoveRobot ((float)YPosInic,-(float)XPosInic,(float)Heading);
00030
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);
00049 sfOnStartupFn(myStartupFn);
00050 sfStartup(hInst, nCmdShow, 1);
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);
00063 }
00064
00065 void myConnectFn(void)
00066 {
00067 sfInitBasicProcs();
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
00120 sfSetRVelocity(0);
00121
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,
00184 (*Parametros).ConstantVelocityParams.Prioridad,
00185 (*Parametros).ConstantVelocityParams.Suspendido,
00186 (*Parametros).ConstantVelocityParams.Velocidad);
00187
00188 sfStartBehavior(sfKeepOff,
00189 "Mantener Distancias",
00190 (*Parametros).KeepOffParams.TimeOut,
00191 (*Parametros).KeepOffParams.Prioridad,
00192 (*Parametros).KeepOffParams.Suspendido,
00193 (*Parametros).KeepOffParams.VelocidadCauta,
00194 (*Parametros).KeepOffParams.Sensibilidad);
00195
00196 sfStartBehavior(sfAvoidCollision,
00197 "Evita Colision",
00198 (*Parametros).AvoidCollisionParams.TimeOut,
00199 (*Parametros).AvoidCollisionParams.Prioridad,
00200 (*Parametros).AvoidCollisionParams.Suspendido,
00201 (*Parametros).AvoidCollisionParams.SensibilidadFrontal,
00202 (*Parametros).AvoidCollisionParams.SensibilidadLateral,
00203 (*Parametros).AvoidCollisionParams.GananciaGiro,
00204 (*Parametros).AvoidCollisionParams.RadioSeguridad);
00205
00206
00207 sfStartBehavior(sfStop,
00208 "Detener Robot",
00209 (*Parametros).StopParams.TimeOut,
00210 (*Parametros).StopParams.Prioridad,
00211 (*Parametros).StopParams.Suspendido);
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
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
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
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
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,
00355 2,
00356 0,
00357 (double)(*Val).Velocidad,
00358 goal,
00359 (double)(*Val).RadioExito);
00360 while (sfGetProcessState(p1)!=sfFAILURE && Succes==false)
00361 {
00362 sfPause(1);
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 }