Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
Diff: deplacement.cpp
- Revision:
- 4:eac6746544fb
- Parent:
- 3:d38aa400d5e7
- Child:
- 5:3638d7e7c5c1
diff -r d38aa400d5e7 -r eac6746544fb deplacement.cpp --- 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