Guillaume Chauvon
l
Diff: deplacement.cpp
- Revision:
- 8:f2425e4302fc
- Parent:
- 7:6b15a1feed2d
--- a/deplacement.cpp Sun May 26 14:57:54 2019 +0000
+++ b/deplacement.cpp Wed May 29 06:34:17 2019 +0000
@@ -46,99 +46,9 @@
}
deplacement::deplacement(){
- consigne_D = 0;
- consigne_G = 0;
- somme_erreur_D = 0;
- 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;
- erreur_glissee_G[k] = 0;
- }
- compteur_glisse = 0;
-
- Kp_D = 1.5;//1
- Ki_D = 0.12;//0.15
- Kd_D = 0.5;//1
-
- Kp_G = 1;//1
- Ki_G = 0.13;//0.15
- Kd_G = 1.2;//1
-
- tick_prec_D=0;
- tick_prec_G = 0;
- dix_ms = 0;
- for (int k =0; k<TAILLE_TAB;k++){
- tab_cmd_G[k]=0;
- tab_cmd_D[k]=0;
- vtab_G[k]=0;
- vtab_D[k]=0;
- c_D[k]=0;
- c_G[k]=0;
- }
- consigne_tab[0][0]=0;
- consigne_tab[0][1]=0;
-
- consigne_tab[1][0]=10;
- consigne_tab[1][1]=10;
-
- consigne_tab[2][0]=20;
- consigne_tab[2][1]=20;
-
- consigne_tab[3][0]=30;
- consigne_tab[3][1]=30;
-
- consigne_tab[4][0]=40;
- consigne_tab[4][1]=40;
-
- /* consigne_tab[5][0]=3*5;
- consigne_tab[5][1]=3*5;
-
- consigne_tab[6][0]=3*6;
- consigne_tab[6][1]=3*6;
-
- consigne_tab[7][0]=3*7;
- consigne_tab[7][1]=3*7;
-
- consigne_tab[8][0]=3*8;
- consigne_tab[8][1]=3*8;
-
- consigne_tab[9][0]=3*9;
- consigne_tab[9][1]=3*9;
-
- consigne_tab[10][0]=3*10;
- consigne_tab[10][1]=3*10;
-
- consigne_tab[11][0]=3*11;
- consigne_tab[11][1]=3*11;
-
- consigne_tab[12][0]=3*12;
- consigne_tab[12][1]=3*12;
-
- consigne_tab[13][0]=3*13;
- consigne_tab[13][1]=3*13;
-
- consigne_tab[14][0]=3*14;
- consigne_tab[14][1]=3*14;
-
- consigne_tab[15][0]=0;
- consigne_tab[15][1]=0;
-
- consigne_tab[16][0]=0;
- consigne_tab[16][1]=0;
-
- consigne_tab[17][0]=0;
- consigne_tab[17][1]=0;
-
- consigne_tab[18][0]=0;
- consigne_tab[18][1]=0;
-
- consigne_tab[19][0]=0;
- consigne_tab[19][1]=0;*/
+ point[0]=0;
+ point[1]=0;
+ point[2]=0;
}
void deplacement::arreterRobot()
@@ -196,7 +106,7 @@
}
void deplacement::marche_arriere(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();
actualise_position();
@@ -221,7 +131,6 @@
float vitesse_D;
angle = get_angle();
- float Kip=0;
float Kpp= 0.05 ;
float Kdp= 10;
while (distance-x_local<0){
@@ -244,8 +153,8 @@
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;
+ vitesse_G = vitesse_G - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle);
+ vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle);
//consigne_D = vitesse_D;
//consigne_G = vitesse_G;
if (stopCapteurs == true)
@@ -260,17 +169,11 @@
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;
- 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());
rotation_abs(angle_vise_deg);
//printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
@@ -279,7 +182,6 @@
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)
motors_on();
actualise_position();
@@ -304,7 +206,6 @@
float vitesse_D;
angle = get_angle();
- float Kip=0;
float Kpp= 0.05 ;
float Kdp= 10;
while (distance-x_local>0){
@@ -329,8 +230,8 @@
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;
+ vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle);
+ vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle);
//consigne_D = vitesse_D;
//consigne_G = vitesse_G;
if (typeEvitement == ARRET)
@@ -350,26 +251,18 @@
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;
+void deplacement::ligne_droite(long int distance, double x, double y, double cap){
// le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
motors_on();
actualise_position();
@@ -394,7 +287,6 @@
float vitesse_D;
angle = get_angle();
- float Kip=0;
float Kpp= 0.05 ;
float Kdp= 10;
while (distance-x_local>0){
@@ -419,8 +311,8 @@
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;
+ vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle);
+ vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle);
//consigne_D = vitesse_D;
//consigne_G = vitesse_G;
if (typeEvitement == ARRET)
@@ -451,17 +343,10 @@
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;
- 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());
@@ -490,14 +375,17 @@
{
actualise_position();
angle = get_angle();
- vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
+ vitesse=3*sens*abs(diff_angle(angle,angle_vise));
+ if (vitesse > 90){
+ vitesse = 90;
+ }
+ if (vitesse < -90){
+ vitesse = -90;
+ }
+
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));
@@ -519,125 +407,7 @@
rotation_rel(angle_rel);
}
-void deplacement::asservissement(){
- long int tick_D = get_nbr_tick_D();
- long int tick_G = get_nbr_tick_G();
-
- long int tick_D_passe = tick_D-tick_prec_D;
- long int tick_G_passe = tick_G-tick_prec_G;
-
- tick_prec_D=tick_D;
- tick_prec_G=tick_G;
-
- float vitesse_codeuse_D = tick_D_passe;
- float vitesse_codeuse_G = tick_G_passe;
-
- float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
- float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
-
- if (compteur_glisse == 5)
- compteur_glisse = 0;
-
- if (compteur_glisse == -1)
- {
- compteur_glisse = 0;
- for (int i = 0; i<5; i++){
- erreur_glissee_D[compteur_glisse] = erreur_D;
- erreur_glissee_G[compteur_glisse] = erreur_G;
- }
- }
-
- erreur_glissee_D[compteur_glisse] = erreur_D;
- erreur_glissee_G[compteur_glisse] = erreur_G;
- compteur_glisse++;
-
- erreur_D = erreur_glissee_D[0];
- erreur_G = erreur_glissee_G[0];
- for (int i=1; i<5; i++)
- {
- erreur_D += erreur_glissee_D[i];
- erreur_G += erreur_glissee_G[i];
- }
-
- erreur_D = erreur_D/5.0f;
- erreur_G = erreur_G/5.0f; // erreur est maintenant la moyenne des 5 erreurs prec
-
- somme_erreur_D += erreur_D;
- somme_erreur_G += erreur_G;
-
- float delta_erreur_D = erreur_D-erreur_precedente_D;
- float delta_erreur_G = erreur_G-erreur_precedente_G;
-
- erreur_precedente_G = erreur_G;
- erreur_precedente_D = erreur_D;
-
- float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
- float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
-
- if (cmd_G <0){
- cmd_G = 0;
- }
- if (cmd_G > 500){
- cmd_G = 500;
- }
- if (cmd_D <0){
- cmd_D = 0;
- }
- if (cmd_D > 500){
- cmd_D = 500;
- }
- c_D[dix_ms]=consigne_D;
- c_G[dix_ms]=consigne_G;
- //printf("%d\n",c[i]);
- tab_cmd_D[dix_ms] = cmd_D;
- tab_cmd_G[dix_ms] = cmd_G;
- vtab_D[dix_ms] = vitesse_codeuse_D;
- vtab_G[dix_ms] = vitesse_codeuse_G;
- commande_vitesse(cmd_G,cmd_D);
- dix_ms++;
- //printf("%d\n",i);
- //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
- //printf("%f,%f\n",cmd_G,cmd_D);
- //printf("oui");
-}
-void deplacement::printftab(){
-
- for (int j =0;j<TAILLE_TAB;j++){
- if(j==500)
- bouton();
- printf("%f,%f,%f,%f,%f,%f\n",tab_cmd_G[j],10*vtab_G[j],10*c_D[j],tab_cmd_D[j],10*vtab_D[j],10*c_G[j]);
- }
- /*if (j<5)
- printf("%f,%f,%f,%f,%f\n",tab_cmd_G[j],10*vtab_G[j],10*c[j],tab_cmd_D[j],10*vtab_D[j]);
- else
- printf("%f,%f,%f,%f,%f\n",tab_cmd_G[j],2*(vtab_G[j]+vtab_G[j-1]+vtab_G[j-2]+vtab_G[j-3]+vtab_G[j-4]),10*c[j],tab_cmd_D[j],2*(vtab_D[j]+vtab_D[j-1]+vtab_D[j-2]+vtab_D[j-3]+vtab_D[j-4]));
- }*/
-
- /*for (int j =0;j<TAILLE_TAB;j++){
- printf("%f,%f,%d\n",2*(vtab_G[j]+vtab_G[j-1]+vtab_G[j-2]+vtab_G[j-3]+vtab_G[j-4]), 2*(vtab_D[j]+vtab_D[j-1]+vtab_D[j-2]+vtab_D[j-3]+vtab_D[j-4]), j);
- }*/
-}
-
-void deplacement::test(){
- Timer t;
- t.start();
- for (int i =0;i<5 ;i++){
- changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
- while(t.read()<0.5f){
- //actualise_positio n();
- }
- //printf("t.read() : %f\n",t.read());
- //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
- t.reset();
- }
-}
-
-void deplacement::changement_consigne(int cons_D, int cons_G){
- consigne_D = cons_D;
- consigne_G = cons_G;
- compteur_glisse = -1;
-}
void deplacement::poussette(float temps){
@@ -684,6 +454,7 @@
Coordonnees p0;
p0.x = x_local;
p0.y = y_local;
+ //printf("xo: %lf yo:%lf \n", p0.x,p0.y);
/*p1.x = p1x;
p2.x = p2x;
p1.y = p1y;
@@ -696,7 +467,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 ;
@@ -767,11 +538,11 @@
vitesse_G = vitesse_D*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
double correction = int_ext_cercle(x_local,y_local);
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>30){
+ correction_en_cours = 30;
}
- if (correction_en_cours < -50){
- correction_en_cours = -50;
+ if (correction_en_cours < -30){
+ correction_en_cours = -30;
}
vitesse_D= vitesse_D-correction_en_cours;
vitesse_G= vitesse_G+correction_en_cours;
@@ -861,11 +632,11 @@
vitesse_D = vitesse_G*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
double correction = int_ext_cercle(x_local,y_local);
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>30){
+ correction_en_cours = 30;
}
- if (correction_en_cours < -50){
- correction_en_cours = -50;
+ if (correction_en_cours < -30){
+ correction_en_cours = -30;
}
vitesse_D= vitesse_D+correction_en_cours;
vitesse_G= vitesse_G-correction_en_cours;
@@ -1071,7 +842,6 @@
{
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();
@@ -1098,15 +868,14 @@
angle = get_angle();
while (distance-x_local>0){
- vitesse_G = vitesse+y_local*0.02;
- vitesse_D = vitesse-y_local*0.02;
+ vitesse_G = vitesse+y_local*0.02 + 5 * diff_angle(angle_vise_deg, angle);;
+ vitesse_D = vitesse-y_local*0.02 - 5 * diff_angle(angle_vise_deg, angle);;
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;
@@ -1144,13 +913,7 @@
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;
@@ -1168,12 +931,12 @@
rotation_rel_pente(angle_rel);
}
-void deplacement::pente_combo(BrasPousser brasPousserGauche, BrasPousser brasPousserDroit, Pompe pompe){
+void deplacement::pente_combo(double angle_pente, BrasPousser brasPousserGauche, BrasPousser brasPousserDroit, Pompe pompe){
activerDeuxBras(brasPousserGauche, brasPousserDroit);
motors_on();
- pente(15000,50,0);
- pente(10000,30,0);
- pente(52500,100,0);
+ pente(15000,50,angle_pente);
+ pente(10000,20,angle_pente);
+ pente(54500,80,angle_pente);
desactiverDeuxBras(brasPousserGauche, brasPousserDroit);
commande_vitesse(80,80);
wait(1.5);