Guillaume Chauvon
asser1
Diff: deplacement.cpp
- Revision:
- 3:1dba6eca01ad
- Child:
- 4:deef042e9c02
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/deplacement.cpp Mon May 06 13:48:45 2019 +0000
@@ -0,0 +1,489 @@
+#include "deplacement.h"
+#include "mbed.h"
+#include "odometrie.h"
+#include "hardware.h"
+#include "math_precalc.h"
+#include "reglages.h"
+
+
+deplacement::deplacement(){
+ consigne_D = 0;
+ consigne_G = 0;
+ somme_erreur_D = 0;
+ somme_erreur_G = 0;
+ erreur_precedente_D = 0;
+ erreur_precedente_G = 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;*/
+}
+
+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);
+ int sens_D=signe(vitesse_D);
+ double vitesse_local_G=abs(vitesse_G);
+ double vitesse_local_D=abs(vitesse_D);
+
+ if(abs(vitesse_G) > 900){
+ vitesse_local_G=900;
+ }
+ if(abs(vitesse_G)<10){
+ vitesse_local_G=0;
+ }
+ if(abs(vitesse_D) > 900){
+ vitesse_local_D=900;
+ }
+ if(abs(vitesse_D)< 10){
+ vitesse_local_D=0;
+
+ }
+ ;
+ int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
+ int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
+ float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
+ float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
+ float centieme_D = (VD_f-VD_int)*1000;
+ float centieme_G = (VG_f-VG_int)*1000;
+ if ((rand()%1000) < centieme_G){
+ VG_int+=1;
+ }
+ if ((rand()%1000) < centieme_D){
+ VD_int+=1;
+ }
+ //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
+ set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
+ set_PWM_moteur_D(VG_int);//
+}
+void deplacement::vitesse_nulle_G(int zero){
+ if(zero == 0){
+ set_PWM_moteur_G(0);
+ }
+}
+void deplacement::vitesse_nulle_D(int zero){
+ if(zero == 0){
+ set_PWM_moteur_D(0);
+ }
+}
+void deplacement::reculer_un_peu(int distance){
+ 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();
+
+ //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;
+ }
+
+ 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){
+ vitesse_G=400;
+ vitesse_D=400;
+ }
+ if (vitesse_G<-400){
+ vitesse_G=-400;
+ vitesse_D=-400;
+ }
+
+ 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_v2(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;
+
+ //long int y_local_prec = y_local;
+
+ float vitesse_G;
+ float vitesse_D;
+
+ angle = get_angle();
+
+ float Ki2= 0.000015;
+ float Kp2= 0.04;
+ while (distance-x_local>0){
+
+ 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 + (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));
+ commande_vitesse(vitesse_G,vitesse_D);
+ actualise_position();
+ x_actuel = get_x_actuel();
+ y_actuel = get_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));
+ }
+ test_rotation_abs(angle_vise_deg);
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+ wait(0.3);
+ motors_stop();
+}
+
+void deplacement::test_rotation_rel(double angle_vise)
+{
+ // rotation de angle_vise
+ motors_on();
+ float vitesse;
+ int sens;
+ double angle = get_angle();
+ angle_vise+=angle;
+ borne_angle_d(angle_vise);
+ if (diff_angle(angle,angle_vise)<=0){
+ sens = -1;
+ }
+ else{
+ sens = 1;
+ }
+ while (sens*diff_angle(angle,angle_vise)>0)
+ {
+ vitesse=diff_angle(angle,angle_vise);
+
+ commande_vitesse(-vitesse,vitesse);
+ actualise_position();
+ angle = get_angle();
+ //printf("vitesse : %f", vitesse);
+ }
+
+ //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);
+ wait(0.2);
+ motors_stop();
+}
+
+
+void deplacement::test_rotation_abs(double angle_vise)
+{
+ actualise_position();
+ printf("bite");
+ double angle_rel = borne_angle_d(angle_vise-get_angle());
+ test_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.0;
+ erreur_G = erreur_G/5.0; // 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.5){
+ //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::bouton(){
+ DigitalIn depart(USER_BUTTON);
+ while (depart){}
+}
\ No newline at end of file