Guillaume Chauvon
l
Diff: deplacement.cpp
- Revision:
- 6:e1585b8bd07d
- Parent:
- 5:3638d7e7c5c1
- Child:
- 7:6b15a1feed2d
diff -r 3638d7e7c5c1 -r e1585b8bd07d deplacement.cpp
--- a/deplacement.cpp Mon May 20 23:08:09 2019 +0000
+++ b/deplacement.cpp Fri May 24 21:45:17 2019 +0000
@@ -8,9 +8,11 @@
#include "Ecran.h"
#include "AnalyseArcLib.h"
+
extern Serial pc;
extern bool detectionUltrasons;
bool stopCapteurs = false;
+extern bool typeEvitement;
extern int distanceUltrasonGauche;
extern int distanceUltrasonDroit;
@@ -139,6 +141,12 @@
consigne_tab[19][1]=0;*/
}
+void deplacement::arreterRobot()
+{
+ vitesse_nulle_D(0);
+ vitesse_nulle_G(0);
+}
+
void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
int sens_G=signe(vitesse_G);
@@ -269,8 +277,7 @@
}
-
-void deplacement::ligne_droite(long int distance)
+void deplacement::ligne_droite_basique(long int distance)
{
somme_y=0;
// le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
@@ -326,85 +333,120 @@
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;
- if (stopCapteurs == true)
+ if (typeEvitement == ARRET)
{
- 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))
+ if(stopCapteurs == true)
{
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);
- }
+ 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();
+ commande_vitesse(vitesse_G,vitesse_D);
+ somme_y+=y_actuel;
+ //y_local_prec = y_local;
+ 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 (compteur_asser==150){
+ compteur_asser=0;
+ //printf("%lf\n",get_y_actuel());
+ }
+ compteur_asser++;
+ //printf(" VG : %f VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise_deg);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+ }
+ //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
+ rotation_abs(angle_vise_deg);
+ //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
+}
+
+
+void deplacement::ligne_droite(long int distance, double x, double y, double cap)
+{
+ somme_y=0;
+ // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
+ motors_on();
+ actualise_position();
+ 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;
+ 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;
+
+ //long int y_local_prec = y_local;
+ float vitesse_G;
+ float vitesse_D;
+
+ angle = get_angle();
+ float Kip=0;
+ float Kpp= 0.05 ;
+ 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){
+ vitesse_G=400;
+ vitesse_D=400;
+ }
+ if (vitesse_G<-400){
+ vitesse_G=-400;
+ vitesse_D=-400;
+ }
+
+ angle = get_angle();
+
+ vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
+ 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;
+ if (typeEvitement == ARRET)
+ {
+ if(stopCapteurs == true)
+ {
+ evitementValider = true;
+ vitesse_nulle_D(0);
+ vitesse_nulle_G(0);
+ }
+ else
+ {
+ commande_vitesse(vitesse_G,vitesse_D);
+ }
+ }
+ else
+ {
+ if(stopCapteurs == true)
+ {
+ evitement(x, y, cap);
+ return;
}
else
{
- vitesse_nulle_D(0);
- vitesse_nulle_G(0);
+ commande_vitesse(vitesse_G,vitesse_D);
}
-
- }
- else
- {
- commande_vitesse(vitesse_G,vitesse_D);
}
actualise_position();
x_actuel = get_x_actuel();
@@ -613,8 +655,8 @@
srand(time(NULL));
motors_on();
}
-void deplacement::arc(Coordonnees p1, Coordonnees p2){
- motors_on();
+
+void deplacement::arc(Coordonnees p1, Coordonnees p2, int sens){
actualise_position();
float vitesse_G;
float vitesse_D;
@@ -637,38 +679,43 @@
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];
+ double angle_tan;
- double angle_tan;
- /*if (yc != 0.0){
- angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
+ double angle_a_parcourir = recup_angle_entre_trois_points_213(xc,yc,p0.x,p0.y,p2.x,p2.y);
+ //printf("angle_a_parcourirrir : %lf\n",angle_a_parcourir);
+
+ if (angle_a_parcourir >0 && sens == A_DROITE){
+ angle_a_parcourir -=360.0 ;
}
- else {
- if (p1.y > 0){
- angle_tan = 90.0;
+ if (angle_a_parcourir <0 && sens == A_GAUCHE){
+ angle_a_parcourir +=360;
+ }
+
+ if (angle_a_parcourir >= 0)
+ {
+ if (p0.y != yc){
+ angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
}
- 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;
+ else{
+ angle_tan = 90;
}
}
- /*if (angle_a_parcourir < 0){
- angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
+ else
+ {
+ if (p0.y != yc){
+ angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
+ }
+ else{
+ angle_tan = -90;
+ }
}
- 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;
+
+ //printf("angle_tan : %lf\n",angle_tan);
rotation_abs(angle_tan);
actualise_position();
x_actuel = get_x_actuel();
@@ -676,73 +723,170 @@
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);
+ //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);
+ double mon_angle_ini = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
+ double mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
+ double mon_angle_prec;
+ if (mon_angle >= 0){
+ mon_angle -=360.0;
+ mon_angle_ini -=360.0;
+ }
+ mon_angle_prec = mon_angle;
+ while (!( mon_angle_prec + mon_angle < -359.9 && abs(mon_angle - mon_angle_prec) > 100)){
+ vitesse_D = abs(15 * mon_angle);
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);
+ double correction = int_ext_cercle(x_local,y_local);
+
+ vitesse_D= vitesse_D-correction*0.025- 3*diff_angle(angle_tan,angle);
+ vitesse_G= vitesse_G+correction*0.025 + 3*diff_angle(angle_tan,angle);
+ if(detectionUltrasons)
+ {
+ detectionUltrasons = false;
+ evitementValider = false;
+ //writeCapteurs();
+ LectureI2CCarteCapteur(*this);
+ }
+ if(stopCapteurs == true)
+ {
+ evitementValider = 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();
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;
+ mon_angle_prec = mon_angle;
+ mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
+
+
+ if (mon_angle >= 0){
+ mon_angle -=360.0;
}
- else{
- angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416;
+
+ if (y_local-yc>=0){//gauche du cercle
+ if (y_local != yc){
+ if (x_local-xc>= 0){//haut gauche
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 + 180;
+ }
+ else{//haut droite
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 - 180;
+ }
+ }
+ else{
+ if (x_local-xc>= 0){//haut gauche
+ angle_tan = 90;
+ }
+ else{//haut droite
+ angle_tan = -90;
+ }
+ }
}
- //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);
+ else{//partie droite
+ if (x_local-xc>= 0){// haut droite
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
+ }
+ else{ //bas droite
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
+ }
+ }
}
}
- 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 (angle_a_parcourir>= 0){
+ double mon_angle_ini = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
+ double mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
+ double mon_angle_prec;
+ if (mon_angle <= 0){
+ mon_angle +=360.0;
+ mon_angle_ini +=360.0;
+ }
+ mon_angle_prec = mon_angle;
+ //printf(" test :: %lf - %lf >< %lf ET %lf - %lf >< %lf, boolen : %d\n",mon_angle_ini,mon_angle,angle_a_parcourir,mon_angle_ini,mon_angle_prec,angle_a_parcourir,!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) <= angle_a_parcourir)));
+ //while(!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) >= angle_a_parcourir))){
+ while (!( mon_angle_prec + mon_angle > 359.9 && abs(mon_angle - mon_angle_prec) > 100)){
+ //printf(" test :: %lf - %lf >< %lf ET %lf + %lf >< %lf, boolen : %d\n",mon_angle_ini,mon_angle,angle_a_parcourir,mon_angle_ini,mon_angle_prec,angle_a_parcourir,!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) <= angle_a_parcourir)));
+ vitesse_G = 15 * mon_angle;
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);
+
+ vitesse_D= vitesse_D+correction*0.025 - 3*diff_angle(angle_tan,angle);
+ vitesse_G= vitesse_G-correction*0.025 + 3*diff_angle(angle_tan,angle);
+ if(detectionUltrasons)
+ {
+ detectionUltrasons = false;
+ evitementValider = false;
+ //writeCapteurs();
+ LectureI2CCarteCapteur(*this);
+ }
+ if(stopCapteurs == true)
+ {
+ evitementValider = 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();
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;
+ mon_angle_prec = mon_angle;
+ mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
+ if (mon_angle <= 0){
+ mon_angle +=360.0;
}
- else{
- angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416;
+ if (y_local-yc>=0){//gauche du cercle
+ if (y_local != yc){
+ if (x_local-xc>= 0){//haut gauche
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
+ }
+ else{//haut droite
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
+ }
+ }
+ else{
+ if (x_local-xc>= 0){//haut gauche
+ angle_tan = -90;
+ }
+ else{//haut droite
+ angle_tan = 90;
+ }
+ }
}
- //printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf\n",x_local,y_local,angle,angle_tan);
+ else{//partie droite
+ if (x_local-xc>= 0){// haut droite
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 - 180.0 ;
+ }
+ else{ //bas droite
+ angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 + 180.0 ;
+ }
+ }
+ }
+ //printf(" test :: %lf - %lf >< %lf ET %lf + %lf >< %lf, boolen : %d\n",mon_angle_ini,mon_angle,angle_a_parcourir,mon_angle_ini,mon_angle_prec,angle_a_parcourir,!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) <= angle_a_parcourir)));
- }
}
+
//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);
@@ -839,7 +983,7 @@
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);
+ ligne_droite(distance, x, y, cap);
actualise_position();
angle = get_angle();
x_robot = get_x_actuel();
@@ -878,4 +1022,188 @@
return -acos(prod_scal/(norme12*norme13))*180.0/3.1416;
}
}
-
\ No newline at end of file
+
+
+void deplacement::pente(long int distance, float vitesse, double angle_a_tourner)
+{
+ Timer time;
+ time.start();
+ somme_y=0;
+ // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
+ motors_on();
+ actualise_position();
+ 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;
+ 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;
+
+ //long int y_local_prec = y_local;
+ float vitesse_G;
+ float vitesse_D;
+
+ angle = get_angle();
+ while (distance-x_local>0){
+
+ vitesse_G = vitesse+y_local*0.02;
+ vitesse_D = vitesse-y_local*0.02;
+
+
+ commande_vitesse(vitesse_G,vitesse_D);
+ actualise_position();
+ x_actuel = get_x_actuel();
+ y_actuel = get_y_actuel();
+ somme_y+=y_actuel;
+ //y_local_prec = y_local;
+ 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(" VG : %f VD : %f ; x_local : %lf, y_local : %lf, angle_vise : %f\n",vitesse_G,vitesse_D, x_local,y_local, angle_vise_deg);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+ }
+ //printf("x : %lf, y : %lf, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
+ rotation_abs_pente(angle_a_tourner);
+ //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
+}
+
+void deplacement::rotation_rel_pente(double angle_vise)
+{
+ // rotation de angle_vise
+ motors_on();
+ double vitesse=180;
+ int sens;
+ double angle = get_angle();
+ angle_vise+=angle;
+ borne_angle_d(angle_vise);
+ if (diff_angle(angle,angle_vise)<=0){
+ sens = -1;
+ //printf("negatif\n");
+ }
+ else{
+ sens = 1;
+
+ //printf("positif\n");
+ }
+ //printf("diff : %lf ",diff_angle(angle,angle_vise));
+ while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
+ {
+ actualise_position();
+ angle = get_angle();
+ vitesse=0.5*sens*abs(diff_angle(angle,angle_vise));
+
+ commande_vitesse(-vitesse,vitesse);
+ if (compteur_asser==150){
+ compteur_asser=0;
+ //printf("%lf\n",get_y_actuel());
+ }
+ compteur_asser++;
+ //printf("vitesse : %lf ", vitesse);
+ }
+ //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
+ //printf(" x et y recu : %lf, %ld. distance parcourue : %ld ", sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+ //consigne_D = 0;
+ //consigne_G = 0;
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+
+}
+
+void deplacement::rotation_abs_pente(double angle_vise)
+{
+ actualise_position();
+ //printf("bite");
+ double angle_rel = borne_angle_d(angle_vise-get_angle());
+ rotation_rel_pente(angle_rel);
+}
+
+void deplacement::pente_combo(BrasPousser brasPousserGauche, BrasPousser brasPousserDroit){
+ activerDeuxBras(brasPousserGauche, brasPousserDroit);
+ motors_on();
+ pente(10000,50,-10);
+ pente(10000,30,0);
+ pente(50000,100,0);
+ desactiverDeuxBras(brasPousserGauche, brasPousserDroit);
+ commande_vitesse(80,80);
+ wait(2);
+ commande_vitesse(-150,-150);
+ wait(4);
+ vitesse_nulle_D(0);
+ vitesse_nulle_G(0);
+}
+
+void deplacement::evitement(double x, double y, double cap)
+{
+ 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, A_GAUCHE);
+ }
+ else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
+ {
+ evitementValider = true;
+ actualise_position();
+ Coordonnees P1 = pointIntermediaire();
+ Coordonnees P2 = pointFinale();
+ arc(P1,P2, A_DROITE);
+ }
+ }
+ 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, A_DROITE);
+ }
+ else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
+ {
+ evitementValider = true;
+ actualise_position();
+ Coordonnees P1 = pointIntermediaire();
+ Coordonnees P2 = pointFinale();
+ arc(P1,P2, A_GAUCHE);
+ }
+ }
+ 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, A_GAUCHE);
+ }
+ else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
+ {
+ evitementValider = true;
+ actualise_position();
+ Coordonnees P1 = pointIntermediaire();
+ Coordonnees P2 = pointFinale();
+ arc(P1,P2, A_DROITE);
+ }
+ }
+ else
+ {
+ evitementValider = true;
+ vitesse_nulle_D(0);
+ vitesse_nulle_G(0);
+ }
+ va_au_point(x,y,cap);
+}
\ No newline at end of file