Guillaume Chauvon
asser1
Diff: deplacement.cpp
- Revision:
- 4:deef042e9c02
- Parent:
- 3:1dba6eca01ad
diff -r 1dba6eca01ad -r deef042e9c02 deplacement.cpp
--- a/deplacement.cpp Mon May 06 13:48:45 2019 +0000
+++ b/deplacement.cpp Wed May 08 20:46:46 2019 +0000
@@ -13,6 +13,8 @@
somme_erreur_G = 0;
erreur_precedente_D = 0;
erreur_precedente_G = 0;
+ compteur_asser =0;
+ somme_y=0;
for (int k =0; k<5;k++){
erreur_glissee_D[k] = 0;
@@ -110,14 +112,14 @@
if(abs(vitesse_G) > 900){
vitesse_local_G=900;
}
- if(abs(vitesse_G)<10){
- vitesse_local_G=0;
+ if(abs(vitesse_G)<5){
+ vitesse_local_G=2;
}
if(abs(vitesse_D) > 900){
vitesse_local_D=900;
}
- if(abs(vitesse_D)< 10){
- vitesse_local_D=0;
+ if(abs(vitesse_D)< 5){
+ vitesse_local_D=2;
}
;
@@ -147,86 +149,38 @@
set_PWM_moteur_D(0);
}
}
-void deplacement::reculer_un_peu(int distance){
+void deplacement::reculer_un_peu(int distance)
+{
+ somme_y=0;
+ // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
motors_on();
- long int x_ini = get_x_actuel();
- long int y_ini = get_y_actuel();
+ 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.142/180;
+ double angle_vise=angle_vise_deg*3.1416/180;
double angle = get_angle();
- long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
- long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+ 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();
- long int x_actuel = get_x_actuel();
- long int y_actuel = get_y_actuel();
- long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
- long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+ 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();
-
- //printf("YOOOO\n\n ");
- while (distance+x_local>0){
-
- vitesse_G = (distance+x_local)/70;
- vitesse_D = vitesse_G;
- if(vitesse_G >150){
- vitesse_G=150;
- vitesse_D=150;
- }
- if (vitesse_G<-150){
- vitesse_G=-150;
- vitesse_D=-150;
- }
+ float Kip=0;
+ float Kpp= 0.05 ;
+ float Kdp= 10;
+ while (distance-x_local<0){
- angle = get_angle();
- vitesse_G = vitesse_G - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local; // petit asser en angle
- vitesse_D = vitesse_D + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local;
-
- commande_vitesse(-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(" VG : %f VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// 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));
-
- }
- test_rotation_abs(angle_vise_deg);
- vitesse_nulle_G(0);
- vitesse_nulle_D(0);
- wait(0.3);
- motors_stop();
-}
-
-void deplacement::ligne_droite(long int distance)
-{
- // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
- motors_on();
- long int x_ini = get_x_actuel();
- long int y_ini = get_y_actuel();
- double angle_vise_deg = get_angle();
- double angle_vise=angle_vise_deg*3.142/180;
- double angle = get_angle();
-
- long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
- long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
-
- long int x_actuel = get_x_actuel();
- long int y_actuel = get_y_actuel();
- long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
- long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
-
- float vitesse_G;
- float vitesse_D;
-
- angle = get_angle();
-
- while (distance-x_local>0){
-
vitesse_G = (distance-x_local)/70;
vitesse_D = vitesse_G;
if(vitesse_G >400){
@@ -239,54 +193,65 @@
}
angle = get_angle();
- vitesse_G = vitesse_G + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local; // petit asser en angle
- vitesse_D = vitesse_D - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local;
-
+
+ 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;
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 : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// 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));
-
+ if (compteur_asser==150){
+ compteur_asser=0;
+ //printf("%lf\n",get_y_actuel());
+ }
+ compteur_asser++;
+ printf(" VG : %f VD : %f ; x_local : %lf, y_local : %lf, angle_vise : %f\n",vitesse_G,vitesse_D, x_local,y_local, angle);// 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());
test_rotation_abs(angle_vise_deg);
- vitesse_nulle_G(0);
- vitesse_nulle_D(0);
- wait(0.3);
- motors_stop();
+ //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
}
+
+
+
void deplacement::ligne_droite_v2(long int distance)
{
+ somme_y=0;
// le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
motors_on();
- long int x_ini = get_x_actuel();
- long int y_ini = get_y_actuel();
+ 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.142/180;
+ double angle_vise=angle_vise_deg*3.1416/180;
double angle = get_angle();
- long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
- long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+ 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);
- long int x_actuel = get_x_actuel();
- long int y_actuel = get_y_actuel();
+ double x_actuel = get_x_actuel();
+ double y_actuel = get_y_actuel();
- long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
- long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+
+ 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 Ki2= 0.000015;
- float Kp2= 0.04;
+ float Kip=0;
+ float Kpp= 0.05 ;
+ float Kdp= 10;
while (distance-x_local>0){
-
+
vitesse_G = (distance-x_local)/70;
vitesse_D = vitesse_G;
if(vitesse_G >400){
@@ -299,52 +264,65 @@
}
angle = get_angle();
- //vitesse_G = vitesse_G + (y_local * 0.02) + (y_local - y_local_prec)*2;
- //vitesse_D = vitesse_D - (y_local * 0.02) - (y_local - y_local_prec)*2;
- vitesse_G = vitesse_G * (1 + Ki2*y_local + Kp2 * diff_angle(angle_vise_deg, angle));
- vitesse_D = vitesse_D * (1 - Ki2*y_local - Kp2 * diff_angle(angle_vise_deg, 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;
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 : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// 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));
+ 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());
test_rotation_abs(angle_vise_deg);
- vitesse_nulle_G(0);
- vitesse_nulle_D(0);
- wait(0.3);
- motors_stop();
+ //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
}
void deplacement::test_rotation_rel(double angle_vise)
{
// rotation de angle_vise
motors_on();
- float vitesse;
+ 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");
}
- while (sens*diff_angle(angle,angle_vise)>0)
+ //printf("diff : %lf ",diff_angle(angle,angle_vise));
+ while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
{
- vitesse=diff_angle(angle,angle_vise);
+ actualise_position();
+ angle = get_angle();
+ vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
commande_vitesse(-vitesse,vitesse);
- actualise_position();
- angle = get_angle();
- //printf("vitesse : %f", 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;
@@ -358,7 +336,7 @@
void deplacement::test_rotation_abs(double angle_vise)
{
actualise_position();
- printf("bite");
+ //printf("bite");
double angle_rel = borne_angle_d(angle_vise-get_angle());
test_rotation_rel(angle_rel);
}
@@ -486,4 +464,13 @@
void deplacement::bouton(){
DigitalIn depart(USER_BUTTON);
while (depart){}
+}
+
+void deplacement::poussette(){
+ motors_on();
+ commande_vitesse(150,150);
+ wait(2);
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+ motors_stop();
}
\ No newline at end of file