Guillaume Chauvon
l
Diff: deplacement.cpp
- Revision:
- 4:eac6746544fb
- Parent:
- 3:d38aa400d5e7
- Child:
- 5:3638d7e7c5c1
--- a/deplacement.cpp Thu May 09 07:09:54 2019 +0000
+++ b/deplacement.cpp Thu May 16 09:10:16 2019 +0000
@@ -476,4 +476,186 @@
init_hardware();
srand(time(NULL));
motors_on();
-}
\ No newline at end of file
+}
+
+void deplacement::arc(Coordonnees p1, Coordonnees p2){
+ motors_on();
+ actualise_position();
+ float vitesse_G;
+ float vitesse_D;
+ double x_ini = get_x_actuel();
+ double y_ini = get_y_actuel();
+ double angle_vise_deg = get_angle();
+ double angle_vise=angle_vise_deg*3.1416/180.0;
+ double angle = get_angle();
+
+ double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
+ double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+
+ double x_actuel = get_x_actuel();
+ double y_actuel = get_y_actuel();
+
+
+ double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+
+ Coordonnees p0;
+ p0.x = x_local;
+ p0.y = y_local;
+ printf("x_local : %lf, y_local : %lf\n",x_local,y_local);
+ cercle(p0,p1,p2);
+ double xc= point[0];
+ double yc= point[1];
+ double Rc= point[2];
+ printf("xc: %lf, yc : %lf, Rc: %lf,vg: %lf,vd : %lf\n",xc,yc,Rc,150.0,150.0*(Rc + ECART_ROUE/2.0)/(Rc-ECART_ROUE/2));
+
+ if (p2.y > 0){
+ while(p2.y-y_local>0){
+ vitesse_D = 150*(Rc + ECART_ROUE/2.0)/(Rc-ECART_ROUE/2.0);
+ vitesse_G = 150;
+ double correction = int_ext_cercle(x_actuel,y_actuel);
+ vitesse_G= vitesse_G+correction*0.02;
+ vitesse_D= vitesse_D-correction*0.02;
+ commande_vitesse(vitesse_G,vitesse_D);
+ //printf("vitesse_G : %f, vitesse_D : %f",vitesse_G,vitesse_D);
+ actualise_position();
+ x_actuel = get_x_actuel();
+ y_actuel = get_y_actuel();
+ x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+ //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
+ }
+ }
+ if (p2.y < 0){
+ while(p2.y-y_local<0){
+ vitesse_G = 150.0*(Rc + ECART_ROUE/2.0)/(Rc-ECART_ROUE/2);
+ vitesse_D = 150.0;
+ double correction = int_ext_cercle(x_local,y_local);
+ vitesse_G= vitesse_G+correction*0.001;
+ vitesse_D= vitesse_D-correction*0.001;
+ commande_vitesse(vitesse_G,vitesse_D);
+ //printf("vitesse_G : %f, vitesse_D : %f",vitesse_G,vitesse_D);
+ actualise_position();
+ x_actuel = get_x_actuel();
+ y_actuel = get_y_actuel();
+ x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+ //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
+
+ }
+ }
+ printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
+ vitesse_nulle_D(0);
+ vitesse_nulle_G(0);
+ wait(0.2);
+ motors_stop();
+
+
+}
+
+
+int deplacement::cercle(Coordonnees a,Coordonnees b, Coordonnees c){
+ double ax2 = pow(a.x,2);
+ double ay2 = pow(a.y,2);
+ double bx2 = pow(b.x,2);
+ double by2 = pow(b.y,2);
+ double cx2 = pow(c.x,2);
+ double cy2 = pow(c.y,2);
+ double pente_a;
+ double ord_b;
+ double pente_A;
+ double ord_B;
+ double xc;
+ double yc;
+ double Rc;
+ if (a.y == b.y && b.y == c.y){
+ printf("Les points sont alignes1\n");
+ return 0;
+ }
+ if (b.y-a.y !=0){
+ pente_a = -1*(b.x-a.x)/(b.y-a.y);
+ printf("a : %lf ",pente_a);
+ ord_b = (by2-ay2+bx2-ax2)/(2*(b.y-a.y));
+ printf("b : %lf ",ord_B);
+
+ }
+ else{
+ cercle(c,b,a);
+
+ }
+ if (c.y-a.y !=0){
+ pente_A = -1*(c.x-a.x)/(c.y-a.y);
+ printf("A : %lf ",pente_A);
+ ord_B = (cy2-ay2+cx2-ax2)/(2*(c.y-a.y));
+ printf("B : %lf ",ord_B);
+
+ }
+ else{
+ cercle(b,a,c);
+ }
+
+
+ if (pente_a-pente_A!=0){
+ xc = (ord_B-ord_b)/(pente_a-pente_A);
+ yc = pente_a*xc+ord_b;
+ Rc = pow((pow(xc-a.x,2)+pow(yc-a.y,2)),0.5);
+ point[0] = xc;
+ point[1] = yc;
+ point[2] = Rc;
+ printf("xc : %f, yc : %f, Rc : %f\n",xc,yc,Rc);
+ }
+ else{
+ printf("Les points sont alignes2\n");
+ return 0;
+ }
+
+
+ return 0;
+}
+double deplacement::int_ext_cercle(double x, double y){
+ double xc= point[0];
+ double yc= point[1];
+ double Rc= point[2];
+ double rayon = pow((pow(xc-x,2)+pow(yc-y,2)),0.5);
+ return Rc-rayon;
+
+}
+
+void deplacement::va_au_point(double x,double y, double cap){
+ actualise_position();
+ double angle = get_angle();
+ double x_robot = get_x_actuel();
+ double y_robot = get_y_actuel();
+ double x_projete = 10000.0*cos(angle*3.1416/180.0)+x_robot;
+ double y_projete = 10000.0*sin(angle*3.1416/180.0)+y_robot;
+ printf("angle ::: %lf\n",recup_angle_entre_trois_points_213(x_robot,y_robot,x_projete,y_projete,x,y));
+ rotation_rel(recup_angle_entre_trois_points_213(x_robot,y_robot,x_projete,y_projete,x,y));
+ //printf("oui\n");
+ actualise_position();
+ angle = get_angle();
+ x_robot = get_x_actuel();
+ y_robot = get_y_actuel();
+ double distance = pow(pow(x-x_robot,2)+pow(y-y_robot,2),0.5);
+ printf("distance : %lf\n",distance);
+ ligne_droite(distance);
+ actualise_position();
+ angle = get_angle();
+ x_robot = get_x_actuel();
+ y_robot = get_y_actuel();
+ rotation_abs(cap);
+
+
+}
+
+double deplacement::recup_angle_entre_trois_points_213(double x1,double y1,double x2,double y2,double x3,double y3){
+ double x12 = x2-x1;
+ double y12 = y2-y1;
+ double x13 = x3-x1;
+ double y13 = y3-y1;
+ double norme12 = pow(x12*x12+y12*y12,0.5);
+ double norme13 = pow(x13*x13+y13*y13,0.5);
+ double prod_scal = x12*x13+y12*y13;
+ //printf("%lf\n",acos(prod_scal/(norme12*norme13))*180.0/3.1416);
+ return acos(prod_scal/(norme12*norme13))*180.0/3.1416;
+}
+
\ No newline at end of file