Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
Diff: deplacement.cpp
- Revision:
- 5:3638d7e7c5c1
- Parent:
- 4:eac6746544fb
- Child:
- 6:e1585b8bd07d
diff -r eac6746544fb -r 3638d7e7c5c1 deplacement.cpp --- a/deplacement.cpp Thu May 16 09:10:16 2019 +0000 +++ b/deplacement.cpp Mon May 20 23:08:09 2019 +0000 @@ -1,10 +1,47 @@ #include "deplacement.h" +#include "Strategie.h" #include "mbed.h" #include "odometrie.h" #include "hardware.h" #include "math_precalc.h" #include "reglages.h" +#include "Ecran.h" +#include "AnalyseArcLib.h" +extern Serial pc; +extern bool detectionUltrasons; +bool stopCapteurs = false; + +extern int distanceUltrasonGauche; +extern int distanceUltrasonDroit; +extern int distanceUltrasonArriere; + +extern double posFinalRobotX; +extern double posFinalRobotY; + +extern double posInterRobotX; +extern double posInterRobotY; + +bool evitementValider = false; +Coordonnees pointIntermediaire() +{ + Coordonnees P1; + double x_init = get_x_actuel(); + double y_init = get_y_actuel(); + P1.x = posInterRobotX *100 - x_init; + P1.y = posInterRobotY *100 - y_init; + return P1; +} + +Coordonnees pointFinale() +{ + Coordonnees P1; + double x_init = get_x_actuel(); + double y_init = get_y_actuel(); + P1.x = posFinalRobotX *100 - x_init; + P1.y = posFinalRobotY *100 - y_init; + return P1; +} deplacement::deplacement(){ consigne_D = 0; @@ -180,7 +217,12 @@ float Kpp= 0.05 ; float Kdp= 10; while (distance-x_local<0){ - + if(detectionUltrasons) + { + detectionUltrasons = false; + //writeCapteurs(); + LectureI2CCarteCapteur(*this); + } vitesse_G = (distance-x_local)/70; vitesse_D = vitesse_G; if(vitesse_G >400){ @@ -198,7 +240,15 @@ vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y; //consigne_D = vitesse_D; //consigne_G = vitesse_G; - commande_vitesse(vitesse_G,vitesse_D); + if (stopCapteurs == true) + { + vitesse_nulle_D(0); + vitesse_nulle_G(0); + } + else + { + commande_vitesse(vitesse_G,vitesse_D); + } actualise_position(); x_actuel = get_x_actuel(); y_actuel = get_y_actuel(); @@ -252,6 +302,13 @@ float Kdp= 10; while (distance-x_local>0){ + if(detectionUltrasons) + { + detectionUltrasons = false; + evitementValider = false; + //writeCapteurs(); + LectureI2CCarteCapteur(*this); + } vitesse_G = (distance-x_local)/70; vitesse_D = vitesse_G; if(vitesse_G >400){ @@ -269,7 +326,86 @@ vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y; //consigne_D = vitesse_D; //consigne_G = vitesse_G; - commande_vitesse(vitesse_G,vitesse_D); + if (stopCapteurs == true) + { + if((distanceUltrasonGauche == 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false)) + { + + if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE) + { + evitementValider = true; + actualise_position(); + Coordonnees P1 = pointIntermediaire(); + Coordonnees P2 = pointFinale(); + arc(P1,P2); + wait(2); + } + else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE) + { + evitementValider = true; + actualise_position(); + Coordonnees P1 = pointIntermediaire(); + Coordonnees P2 = pointFinale(); + arc(P1,P2); + wait(2); + } + } + if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit == 1100) && (evitementValider == false)) + { + + if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE) + { + evitementValider = true; + actualise_position(); + Coordonnees P1 = pointIntermediaire(); + Coordonnees P2 = pointFinale(); + arc(P1,P2); + wait(2); + } + else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE) + { + evitementValider = true; + actualise_position(); + Coordonnees P1 = pointIntermediaire(); + Coordonnees P2 = pointFinale(); + arc(P1,P2); + wait(2); + } + + } + if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false)) + { + evitementValider = true; + if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE) + { + evitementValider = true; + actualise_position(); + Coordonnees P1 = pointIntermediaire(); + Coordonnees P2 = pointFinale(); + arc(P1,P2); + wait(2); + } + else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE) + { + evitementValider = true; + actualise_position(); + Coordonnees P1 = pointIntermediaire(); + Coordonnees P2 = pointFinale(); + arc(P1,P2); + wait(2); + } + } + else + { + vitesse_nulle_D(0); + vitesse_nulle_G(0); + } + + } + else + { + commande_vitesse(vitesse_G,vitesse_D); + } actualise_position(); x_actuel = get_x_actuel(); y_actuel = get_y_actuel(); @@ -462,22 +598,21 @@ } - -void deplacement::poussette(){ +void deplacement::poussette(float temps){ motors_on(); - commande_vitesse(150,150); - wait(1.5); + commande_vitesse(100,100); + wait_ms(temps); vitesse_nulle_G(0); vitesse_nulle_D(0); motors_stop(); } + void deplacement::initialisation(){ init_odometrie(); init_hardware(); srand(time(NULL)); motors_on(); } - void deplacement::arc(Coordonnees p1, Coordonnees p2){ motors_on(); actualise_position(); @@ -502,49 +637,114 @@ Coordonnees p0; p0.x = x_local; p0.y = y_local; - printf("x_local : %lf, y_local : %lf\n",x_local,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)); + double xc = point[0]; + double yc = point[1]; + double Rc = point[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); + double angle_tan; + /*if (yc != 0.0){ + angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416; + } + else { + if (p1.y > 0){ + angle_tan = 90.0; + } + else if (p1.y <0){ + angle_tan = -90.0; + } + }*/ + printf("x : %lf, y : %lf bite ",p0.x,p0.y); + double angle_a_parcourir = recup_angle_entre_trois_points_213(xc,yc,p0.x,p0.y,p2.x,p2.y); + if (angle_a_parcourir >179.999){ + if (recup_angle_entre_trois_points_213(xc,yc,p1.x,p1.y,p2.x,p2.y)<0){ + angle_a_parcourir = -179.999; } } - 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); + /*if (angle_a_parcourir < 0){ + angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416; + } + else{ + angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416; + }*/ + angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416; + rotation_abs(angle_tan); + actualise_position(); + x_actuel = get_x_actuel(); + y_actuel = get_y_actuel(); + angle = get_angle(); + 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("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)); + printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf,angle_a_parcourir : %lf\n",x_local,y_local,angle,angle_tan,angle_a_parcourir); + motors_on(); + if (angle_a_parcourir < 0){ + while(!(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)>0 && abs(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y))<100)){ + //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)); + vitesse_D = -15*recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y); + if (vitesse_D>300){ + vitesse_D = 300; + } + vitesse_G = vitesse_D*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0); + double correction = int_ext_cercle(x_actuel,y_actuel); + //printf("correction : %lf",correction); + //printf("diff_angle : %lf ",diff_angle(angle_tan,angle)); + vitesse_G= vitesse_G+correction*0.02 + 0.8*diff_angle(angle_tan,angle); + vitesse_D= vitesse_D-correction*0.02 - 0.8*diff_angle(angle_tan,angle); + 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(); + angle = get_angle(); + 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; + if (y_local >=0){ + angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416; + } + else{ + angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416; + } + //angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416; + //printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf\n",x_local,y_local,angle,angle_tan); + } + } + if (angle_a_parcourir>= 0){//arc vers la droite + //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)); + while(!(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)<0 && abs(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y))<100)){ + printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)); + vitesse_G = 15*(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)); + if (abs(vitesse_G)>300){ + vitesse_G = 300; + } + vitesse_D = vitesse_G*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0); + double correction = int_ext_cercle(x_local,y_local); + //printf("correction : %lf",correction); + //printf("diff_angle : %lf",diff_angle(angle_tan,angle)); + vitesse_D= vitesse_D+correction*0.02 - 0.8*diff_angle(angle_tan,angle); + vitesse_G= vitesse_G-correction*0.02 + 0.8*diff_angle(angle_tan,angle); + 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(); + angle = get_angle(); + 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; + + if (y_local >=0){ + angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416; + } + else{ + angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416; + } + //printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf\n",x_local,y_local,angle,angle_tan); } } - printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local); + //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)); + //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local); vitesse_nulle_D(0); vitesse_nulle_G(0); wait(0.2); @@ -561,9 +761,9 @@ double by2 = pow(b.y,2); double cx2 = pow(c.x,2); double cy2 = pow(c.y,2); - double pente_a; + double pente_a=0; double ord_b; - double pente_A; + double pente_A=0; double ord_B; double xc; double yc; @@ -580,7 +780,8 @@ } else{ - cercle(c,b,a); + cercle(c,a,b); + return 0; } if (c.y-a.y !=0){ @@ -591,7 +792,8 @@ } else{ - cercle(b,a,c); + cercle(b,c,a); + return 0; } @@ -628,8 +830,8 @@ 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("angle ::: %lf\n",recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete)); + rotation_rel(recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete)); //printf("oui\n"); actualise_position(); angle = get_angle(); @@ -648,14 +850,32 @@ } double deplacement::recup_angle_entre_trois_points_213(double x1,double y1,double x2,double y2,double x3,double y3){ + double x13 = x3-x1; + double y13 = y3-y1; 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 ux = x13/norme13; + double uy = y13/norme13; + double wx = x12/norme12; + double wy = y12/norme12; + //printf("u : %lf,%lf ,v : %lf,%lf ,w : %lf,%lf\n",ux,uy,-uy,ux,wx,wy); 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; + double cos_angle = prod_scal/(norme12*norme13); + double sin_angle; + if (uy!=0){ + sin_angle = -1*(wx - cos_angle*ux)/uy; + } + else{ + sin_angle = (wy - cos_angle*uy)/ux; + } + //printf("cos : %lf sin : %lf\n",cos_angle,sin_angle); + if (sin_angle >=0){ + return acos(prod_scal/(norme12*norme13))*180.0/3.1416; + } + else{ + return -acos(prod_scal/(norme12*norme13))*180.0/3.1416; + } } - \ No newline at end of file + \ No newline at end of file