Guillaume Chauvon
l
Diff: deplacement.cpp
- Revision:
- 7:6b15a1feed2d
- Parent:
- 6:e1585b8bd07d
- Child:
- 8:f2425e4302fc
diff -r e1585b8bd07d -r 6b15a1feed2d deplacement.cpp
--- a/deplacement.cpp Fri May 24 21:45:17 2019 +0000
+++ b/deplacement.cpp Sun May 26 14:57:54 2019 +0000
@@ -656,6 +656,7 @@
motors_on();
}
+
void deplacement::arc(Coordonnees p1, Coordonnees p2, int sens){
actualise_position();
float vitesse_G;
@@ -675,10 +676,18 @@
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;
+ /*double p1x = p1.x*cos(angle_vise) + p1.y*sin(angle_vise)-x_local_ini;
+ double p2x = p2.x*cos(angle_vise) + p2.y*sin(angle_vise)-x_local_ini;
+ double p1y = p1.y*cos(angle_vise) - p1.x*sin(angle_vise)-y_local_ini;
+ double p2y = p2.y*cos(angle_vise) - p2.x*sin(angle_vise)-y_local_ini;*/
Coordonnees p0;
p0.x = x_local;
p0.y = y_local;
+ /*p1.x = p1x;
+ p2.x = p2x;
+ p1.y = p1y;
+ p2.y = p2y;*/
cercle(p0,p1,p2);
double xc = point[0];
@@ -687,7 +696,7 @@
double angle_tan;
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);
+ printf("angle_a_parcourirrir : %lf\n",angle_a_parcourir);
if (angle_a_parcourir >0 && sens == A_DROITE){
angle_a_parcourir -=360.0 ;
@@ -700,23 +709,36 @@
{
if (p0.y != yc){
angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
+ if (xc<0){
+ angle_tan -=180.0;
+ }
}
else{
angle_tan = 90;
+ if (xc<0){
+ angle_tan -=180.0;
+ }
}
}
else
{
if (p0.y != yc){
angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
+ if (xc<0){
+ angle_tan+=180.0;
+ }
}
else{
angle_tan = -90;
+ if (xc<0){
+ angle_tan+=180.0;
+ }
}
+
}
//printf("angle_tan : %lf\n",angle_tan);
- rotation_abs(angle_tan);
+ rotation_rel(angle_tan);
actualise_position();
x_actuel = get_x_actuel();
y_actuel = get_y_actuel();
@@ -744,9 +766,18 @@
}
vitesse_G = vitesse_D*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
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);
+ double correction_en_cours = correction*0.035 - 3*diff_angle(borne_angle_d(angle_tan-angle_vise_deg),angle);
+ if (correction_en_cours>50){
+ correction_en_cours = 50;
+ }
+ if (correction_en_cours < -50){
+ correction_en_cours = -50;
+ }
+ vitesse_D= vitesse_D-correction_en_cours;
+ vitesse_G= vitesse_G+correction_en_cours;
+
+ //printf("angle _tan : %lf, angle : %lf , diff_angle : %lf , borne_angle : %lf\n",angle_tan, angle, diff_angle(borne_angle_d(angle_tan-angle_vise_deg),angle),borne_angle_d(angle_tan-angle_vise_deg));
+
if(detectionUltrasons)
{
detectionUltrasons = false;
@@ -804,6 +835,9 @@
angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
}
}
+ if (xc<0){
+ angle_tan+=180.0;
+ }
}
}
if (angle_a_parcourir>= 0){
@@ -818,6 +852,7 @@
//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){
@@ -825,9 +860,16 @@
}
vitesse_D = vitesse_G*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
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);
+ double correction_en_cours = correction*0.035 - 3*diff_angle(borne_angle_d(angle_tan+angle_vise_deg),angle);
+ if (correction_en_cours>50){
+ correction_en_cours = 50;
+ }
+ if (correction_en_cours < -50){
+ correction_en_cours = -50;
+ }
+ vitesse_D= vitesse_D+correction_en_cours;
+ vitesse_G= vitesse_G-correction_en_cours;
+ //printf("%lf , %lf ,%lf ,%lf, correction : %lf, vitesse_D : %f, vitesse_G : %f\n",angle_tan , angle,borne_angle_d(angle_tan+angle_vise_deg) ,diff_angle(borne_angle_d(angle_tan+angle_vise_deg),angle),correction,vitesse_D,vitesse_G);
if(detectionUltrasons)
{
detectionUltrasons = false;
@@ -882,6 +924,8 @@
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)));
@@ -893,7 +937,6 @@
vitesse_nulle_G(0);
wait(0.2);
motors_stop();
-
}
@@ -1125,16 +1168,21 @@
rotation_rel_pente(angle_rel);
}
-void deplacement::pente_combo(BrasPousser brasPousserGauche, BrasPousser brasPousserDroit){
+void deplacement::pente_combo(BrasPousser brasPousserGauche, BrasPousser brasPousserDroit, Pompe pompe){
activerDeuxBras(brasPousserGauche, brasPousserDroit);
motors_on();
- pente(10000,50,-10);
+ pente(15000,50,0);
pente(10000,30,0);
- pente(50000,100,0);
+ pente(52500,100,0);
desactiverDeuxBras(brasPousserGauche, brasPousserDroit);
commande_vitesse(80,80);
- wait(2);
- commande_vitesse(-150,-150);
+ wait(1.5);
+ vitesse_nulle_D(0);
+ vitesse_nulle_G(0);
+ //hardHiz();
+ pompe.desactiver();
+ wait(3);
+ rotation_rel(-90);
wait(4);
vitesse_nulle_D(0);
vitesse_nulle_G(0);
@@ -1144,7 +1192,7 @@
{
if((distanceUltrasonGauche == 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
{
- if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
+ if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
{
evitementValider = true;
actualise_position();
@@ -1152,7 +1200,7 @@
Coordonnees P2 = pointFinale();
arc(P1,P2, A_GAUCHE);
}
- else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
+ else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
{
evitementValider = true;
actualise_position();
@@ -1163,7 +1211,7 @@
}
if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit == 1100) && (evitementValider == false))
{
- if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
+ if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
{
evitementValider = true;
actualise_position();
@@ -1171,7 +1219,7 @@
Coordonnees P2 = pointFinale();
arc(P1,P2, A_DROITE);
}
- else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
+ else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
{
evitementValider = true;
actualise_position();
@@ -1182,7 +1230,7 @@
}
if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
{
- if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
+ if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
{
evitementValider = true;
actualise_position();
@@ -1190,7 +1238,7 @@
Coordonnees P2 = pointFinale();
arc(P1,P2, A_GAUCHE);
}
- else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
+ else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
{
evitementValider = true;
actualise_position();