Francisco Santos
/
SRA_VFF
pepe
Diff: Robot.cpp
- Revision:
- 2:06b7789c7da0
- Parent:
- 1:2716ea33958b
diff -r 2716ea33958b -r 06b7789c7da0 Robot.cpp --- 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)