Guillaume Chauvon
asser1
Diff: commande_moteurs.cpp
- Revision:
- 2:5764f89a27f6
- Child:
- 3:1dba6eca01ad
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/commande_moteurs.cpp Wed Apr 17 18:55:22 2019 +0000
@@ -0,0 +1,202 @@
+
+#include "mbed.h"
+
+#include "commande_moteurs.h"
+#include "hardware.h"
+#include "reglages.h"
+#include "math_precalc.h"
+
+#include "odometrie.h"
+
+
+void 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=10;
+ }
+ if(abs(vitesse_D) > 900){
+ vitesse_local_D=900;
+ }
+ if(abs(vitesse_D)< 10){
+ vitesse_local_D=10;
+
+ }
+ ;
+ 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 vitesse_nulle_G(int zero){
+ if(zero == 0){
+ set_PWM_moteur_G(0);
+ }
+}
+void vitesse_nulle_D(int zero){
+ if(zero == 0){
+ set_PWM_moteur_D(0);
+ }
+}
+void 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 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();
+
+ printf("YOOOO\n\n ");
+ 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 test_rotation_rel(double angle_vise)
+{
+ // rotation de angle_vise
+ motors_on();
+ float vitesse;
+ double angle = get_angle();
+ angle_vise+=angle;
+ borne_angle_d(angle_vise);
+
+ while (abs(diff_angle(angle,angle_vise))>0.08)
+ {
+ vitesse=1.3*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));
+
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+ wait(0.2);
+ motors_stop();
+}
+
+
+void test_rotation_abs(double angle_vise)
+{
+ double angle_rel = borne_angle_d(angle_vise-get_angle());
+ test_rotation_rel(angle_rel);
+}
\ No newline at end of file