Guillaume Chauvon
l
Diff: deplacement.cpp
- Revision:
- 5:3638d7e7c5c1
- Parent:
- 4:eac6746544fb
- Child:
- 6:e1585b8bd07d
--- 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