Construyo wrappers con la nueva interfaz que habíamos hablado para el sistema autónom...

[z.facultad/75.68/celdas.git] / trunk / src / breve / robot / Celdas-2-6.tz

diff --git a/trunk/src/breve/robot/Celdas-2-6.tz b/trunk/src/breve/robot/Celdas-2-6.tz
index e0d352c3ac30664361870dfca6b5a5a46532bfc8..1e3f9a6b8bd8d6106c8982ba5fe96fb6f4675a76 100644 (file)
--- a/trunk/src/breve/robot/Celdas-2-6.tz
+++ b/trunk/src/breve/robot/Celdas-2-6.tz
@@ -160,7 +160,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        % at higher speeds.\r
         +variables:\r
             lSensor, rSensor, fSensor, bSensor (object).\r
        % at higher speeds.\r
         +variables:\r
             lSensor, rSensor, fSensor, bSensor (object).\r

-            lWheel,rWheel (object).        \r
+            lWheel,rWheel (object).\r
+           tleft,tright (int).     \r
+        \r

         \r
        - to get-density:\r
                return 20.0.\r
         \r
        - to get-density:\r
                return 20.0.\r


@@ -178,14 +180,21 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        + to get-global-velocity:\r
                return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.\r
 \r
        + to get-global-velocity:\r
                return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.\r
 \r

-       + to turn-right with-velocity velocity (float):         \r
-               lWheel set-velocity to velocity.\r
-                rWheel set-velocity to -velocity.\r
+       + to turn-right:                \r
+               tright++.\r
+\r
+               self rotate around-axis (0,1,0) by (-1.5709/10)*tright. \r
+                       \r
+               if(tright==10): tright=0.\r
+\r
+\r
+       + to turn-left:\r
+               tleft++.\r
+\r
+               self rotate around-axis (0,1,0) by (1.5709/10)*tleft. \r
+                       \r
+               if(tleft==10): tleft=0.\r

 \r
 \r

-       + to turn-left with-velocity velocity (float):\r
-#                vehicle rotate around-axis (0,1,0) by 1. \r
-               lWheel set-velocity to -velocity.       \r
-               rWheel set-velocity to velocity.\r

 \r
         + to get-sensor-value:\r
                return (fSensor get-sensor-value).\r
 \r
         + to get-sensor-value:\r
                return (fSensor get-sensor-value).\r


@@ -214,10 +223,12 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
             rSensor set-id at 4.\r
            rSensor set-body at self.\r
 \r
             rSensor set-id at 4.\r
            rSensor set-body at self.\r
 \r

-            lWheel  = (self add-wheel at (0, 0, -1.5)).\r
-            rWheel  = (self add-wheel at (0, 0, 1.5)).\r
+            lWheel = (self add-wheel at (0, 0, -1.5)).\r
+            rWheel = (self add-wheel at (0, 0, 1.5)).\r
+\r
+           tleft=tright=1.     #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r

                         \r
                         \r

-        +to iterate:            \r
+        +to iterate:\r

         #+ to post-iterate:\r
                valuef,valueb,valuer,valuel (float).\r
                fl, fr(float).\r
         #+ to post-iterate:\r
                valuef,valueb,valuer,valuel (float).\r
                fl, fr(float).\r


@@ -226,18 +237,24 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                valueb=bSensor get-data.\r
                 valuel=lSensor get-data.\r
                 valuer=rSensor get-data.\r
                valueb=bSensor get-data.\r
                 valuel=lSensor get-data.\r
                 valuer=rSensor get-data.\r

+               \r
+               \r
+               if(tright>0):\r
+                       self turn-right.\r
+               \r

 \r
 \r

-               self turn-left with-velocity(20).\r
-                self set-global-velocity to (15).\r
-               if valuef >7:           \r
-                    self set-global-velocity to (15).\r
-                else if (valuef <=7) && (valuef > 0):\r
-                {   \r
+\r
+               #self turn-left with-velocity(20).\r
+                #self set-global-velocity to (15).\r
+               #if valuef >7:          \r
+                #    self set-global-velocity to (15).\r
+                #else if (valuef <=7) && (valuef > 0):\r
+                #{   \r

                     self set-global-velocity to (0).\r
                     #self turn-left with-velocity(2).\r
                     #self turn-right with-velocity(2).\r
                     #self set-global-velocity to (0).\r
                     self set-global-velocity to (0).\r
                     #self turn-left with-velocity(2).\r
                     #self turn-right with-velocity(2).\r
                     #self set-global-velocity to (0).\r

-               }\r
+               #}\r

                 #print "sensor valuef: $valuef  valueb: $valueb".\r
                 \r
                 #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
                 #print "sensor valuef: $valuef  valueb: $valueb".\r
                 \r
                 #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r


@@ -374,16 +391,15 @@ Link : CeldasSensor (aka CeldasSensors) {
                 v,obs(vector).\r
                aux(float).\r
                j (int).\r
                 v,obs(vector).\r
                aux(float).\r
                j (int).\r

-               des1,des2,des3(int).\r
+               des2,des3(int).\r

                wallBegin,wallEnd,wallCenter (float).\r
                 \r
                wallBegin,wallEnd,wallCenter (float).\r
                 \r

-               aux1,aux2,aux3,aux4 (float).\r
-               yo,toObstacle, transDir (vector).\r
+               toObstacle(vector).\r

                 largeWall (float).\r
                 largeWall (float).\r

-                source,destiny (vector).\r
+                \r

                 obsLoc (vector).                \r
                 location (vector).\r
                 obsLoc (vector).                \r
                 location (vector).\r

-                posObstacle,posSensor (vector).\r
+                posObstacle,destiny,yo(vector).\r

                                              \r
                draw clear.\r
                value = 0.0.\r
                                              \r
                draw clear.\r
                value = 0.0.\r


@@ -445,15 +461,13 @@ Link : CeldasSensor (aka CeldasSensors) {
                                 \r
                        }                               \r
 \r
                                 \r
                        }                               \r
 \r

-                       aux1=dot((self get-location),(i get-direction)).\r
+                       #aux1=dot((self get-location),(i get-direction)).\r

 \r
                        #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
 \r
                        #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r

-                       #print "dist: $aux1 begin: $wallBegin end: $wallEnd ".\r
+                       #print "begin: $wallBegin end: $wallEnd ".\r

                        #print "begin: $wallBegin end: $wallEnd".\r
 \r
                        \r
                        #print "begin: $wallBegin end: $wallEnd".\r
 \r
                        \r

-                       if ((i get-location)::z==20):\r
-                               print "------>sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo  des2: $des2 des1: $des1".  \r

 \r
                        #print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
                        if ((des2) && (des3)):\r
 \r
                        #print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
                        if ((des2) && (des3)):\r


@@ -461,20 +475,17 @@ Link : CeldasSensor (aka CeldasSensors) {
                                draw clear.                             \r
                                #print " posObstacle: $posObstacle".                                                    \r
                                \r
                                draw clear.                             \r
                                #print " posObstacle: $posObstacle".                                                    \r
                                \r

-                       #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
-                       #print "dist: $aux1 begin: $wallBegin end: $wallEnd ".\r
-\r
-\r
+                       print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
+                       print "begin: $wallBegin end: $wallEnd ".\r

                                        \r
 \r
                                        \r
 \r

-\r

                                dist=|obsLoc - (self get-location)|.\r
                                if( (j==0) || (min>dist) ):\r
                                 {\r
                                        min=dist.\r
                                        obs=obsLoc.\r
                                        j++.\r
                                dist=|obsLoc - (self get-location)|.\r
                                if( (j==0) || (min>dist) ):\r
                                 {\r
                                        min=dist.\r
                                        obs=obsLoc.\r
                                        j++.\r

-                                       print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". \r
+                                       #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".        \r

                                 }\r
 \r
                         }                                              \r
                                 }\r
 \r
                         }                                              \r