Revision 2:06b7789c7da0, committed 2019-11-24

Comitter:

FJMS

Date:

Sun Nov 24 18:37:46 2019 +0000

Parent:

1:2716ea33958b

Commit message:

pepe;

Changed in this revision

--- a/Robot.cpp	Wed May 22 09:51:53 2019 +0000
+++ b/Robot.cpp	Sun Nov 24 18:37:46 2019 +0000
@@ -10,23 +10,24 @@
 int16_t countsRight = 0;
 int i, j;
 
-float De=0, Dd=0, pi=3.141529, Ldis=14.2, r=6.95/2, theta=0, teta=0, teta_=0, x=25, y=25, D=0, Wee=0, Wdd=0, v=0, w=0;
+float De=0, Dd=0, pi=3.141529, Ldis=14.2, r=6.95/2, theta=0, teta=0, teta_=0, x=20, y=20, D=0, Wee=0, Wdd=0, v=0, w=0;
 float xc, yc; // coordenadas da célula
 float x_obj = 0, y_obj = 0;
-float xf = 200, yf = 200; // coordenadas finais
+float xf = 100, yf = 20; // coordenadas finais
 
-float fcr = 1000; // Força Constante Repulsiva
-float fca = 14; // Força Constante Atrativa
+float fcr = 500; // Força Constante Repulsiva
+float fca = 30; // Força Constante Atrativa
 float fre_x = 0, fre_y = 0, fa_x = 0, fa_y = 0, Fr_x = 0, Fr_y = 0;
 
 
 float d = 0, dt = 0;
 float P = 0, I = 0;
 float phi1=0,aux=0;
-float Kv=50, Ks=20, Ki = 0.001;
+float Kv=5, Ks=10, Ki = 0.01;
 float erro = 0;
 int aux_x = 0, aux_y = 0;
 
+
 //PC.baud(9600);
 ////////////////////////////////////////////////////
 //            Seguimento de um percurso           //
@@ -39,12 +40,9 @@
         f_atrativa();
         Fr_x = fre_x + fa_x;
         Fr_y = fre_y + fa_y;
-        if(Fr_x>15) Fr_x = 15;
-        else if(Fr_x<-15) Fr_x = -15;
-        
-        if(Fr_y>15) Fr_y = 15;
-        else if(Fr_y<-15) Fr_y = -15;
-        
+        //PC.printf("Fr_x=%f                      Fr_y=%f\n",Fr_x,Fr_y);
+        //if(Fr_x<8) Fr_x=8;
+        //if(Fr_y<8) Fr_y=8;
         x_obj = x + Fr_x;
         y_obj = y + Fr_y;
 
@@ -53,8 +51,8 @@
         P = Kv*erro;
         I += erro;
         v = P + Ki*I;
-        if(v>60) v = 60;
-        else if(v < -60) v=-60;
+        if(v>25) v = 25;
+        else if(v < -25) v=-25;
 
         phi1=atan2((y_obj-y),(x_obj-x)); // Ângulo entre a posição final e a posição atual
         aux = phi1-teta;
@@ -69,10 +67,10 @@
         w=Ks*(aux);
         if(w>5) w = 5;
         else if(w<-5) w = -5;
-        //motion();
+        motion();
 
         //PC.printf("fre_x=%f fre_y=%f fa_x=%f fa_y=%f erro=%f P=%f I=%f v=%f w=%f x=%f y=%f\n", fre_x,fre_y,fa_x,fa_y,erro,P,I,v,w,x,y);
-        PC.printf("%f %f %f\n",erro, x,y);
+        //PC.printf("%f %f %f\n",erro, x,y);
         setSpeeds(Wee+33*abs(v)/v,Wdd+30*abs(v)/v);
     }
     setSpeeds(0,0);
@@ -119,7 +117,7 @@
     //map.add(8, 16, 4);
     for(i=1; i<=x3; i++) {
         for(j=1; j<=y3; j++) {
-            PC.printf("%d ", int(map.getNumber(i,j)));
+            //PC.printf("%d ", int(map.getNumber(i,j)));
         }
         printf("\n");
     }
@@ -132,25 +130,20 @@
 {
     fre_x = 0;
     fre_y = 0;
-    aux_x = x/5;
-    aux_y = y/5;
+    aux_x = x/5 + 1;
+    aux_y = y/5 + 1;
     for(i=aux_x-5; i<=aux_x+5; i++) {
         for(j=aux_y-5; j<=aux_y+5; j++) {
             if(i<=0 || i>80 || j<=0 || j>80 || (i == aux_x && j == aux_y)) {
                 // do nothing
             } else {
-                xc = (i - 1)*5 + 2.5;
-                yc = (j - 1)*5 + 2.5;
-                d = map.getNumber(i,j)*sqrt((x-xc)*(x-xc)+(y-yc)*(y-yc));
-                if(d < 20 ) fcr = fcr * 1.5;
-                else fcr = 80;
-                if(d == 0){
-                    
-                }
-                else {
-                    fre_x = fre_x + ((fcr*map.getNumber(i,j)*(x-xc))/(d*d*d));
-                    fre_y = fre_y + ((fcr*map.getNumber(i,j)*(y-yc))/(d*d*d));
-                }
+                xc = i*5 - 2.5;
+                yc = j*5 - 2.5;
+                d = sqrt((x-xc)*(x-xc)+(y-yc)*(y-yc));
+                
+                fre_x = fre_x + ((fcr*map.getNumber(i,j)*(x-xc))/(d*d*d));
+                fre_y = fre_y + ((fcr*map.getNumber(i,j)*(y-yc))/(d*d*d));
+                //PC.printf("fre_x=%f                        fre_y=%f \n", fre_x,fre_y);
             }
         }
     }
@@ -188,17 +181,18 @@
         teta=teta+theta;
     }
 
+    teta=atan2(sin(teta),cos(teta));
     //Limite entre [-2pi; 2pi]
-    if (teta>=(pi)) {
-        teta=teta-2*pi;
-    }
-    if (teta<=(-pi)) {
-        teta=teta+2*pi;
-    }
+    //if (teta>=(pi)) {
+      //  teta=teta-2*pi;
+    //}
+    //if (teta<=(-pi)) {
+      //  teta=teta+2*pi;
+    //}
 
     Wee=(v-(Ldis/2)*w);
     Wdd=(v+(Ldis/2)*w);
-    //PC.printf("phi1=%f D=%f x=%f y=%f teta=%f\n", w,theta,x,y,teta);
+    PC.printf("De=%f Dd=%f x=%f y=%f teta=%f\n", De,Dd,x,y,teta);
 }
 
 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)

--- a/main.cpp	Wed May 22 09:51:53 2019 +0000
+++ b/main.cpp	Sun Nov 24 18:37:46 2019 +0000
@@ -9,10 +9,12 @@
 float x1=20,y1=20;          // Posição inicial do robo
 float a1=0,b1=-1,c1=65;       // Reta ax + by + c = 0
 float x2=65,y2=60,phi2=-pi/2; // Pose arbitrária
-int p[2] = {100,100};
-int p1[2] = {140, 80};
-int p2[2] = {140, 35};
-int p3[2] = {140, 130};
+
+int p[2] = {59, 43};
+int p1[2] = {70, 17};
+int p2[2] = {20, 46};
+int p3[2] = {48, 66};
+//int p4[2] = {200,200};
 
 void pressed()
 {
@@ -26,10 +28,10 @@
     setSpeeds(0,0);
     //wait(0.2);
     mapa(80,80);
-    cria_obj(60,40,p);
-    //cria_obj(20,10,p1);
-    //cria_obj(20,60,p2);
-    //cria_obj(20,40,p3);
+    //cria_obj(117,84,p);
+    //cria_obj(20,30,p1);
+   // cria_obj(28,13,p2);
+    //cria_obj(25,12,p3);
 
     //int_map(80, 80);
     while(1) {