Revision 1:0690cf83f060, committed 2018-12-12

Comitter:

Coconara

Date:

Wed Dec 12 20:03:07 2018 +0000

Parent:

0:6ca63d45f0ee

Child:

2:977799d72329

Commit message:

v1.01

Changed in this revision

--- a/main.cpp	Tue Dec 11 19:12:55 2018 +0000
+++ b/main.cpp	Wed Dec 12 20:03:07 2018 +0000
@@ -11,9 +11,31 @@
     init_odometrie();
     init_hardware();
     
-    test_ligne_droite(1000000, 900);
+    
+    wait(3);
+    
+    test_ligne_droite(162000, 600);
+    test_rotation_rel(90,100);
+    test_ligne_droite(74000,300);
+    
+    /*while(1)
+    {
+        for (int i =0; i<4; i++)
+        {
+            test_ligne_droite(30000,500);    
         
-    
+            test_rotation_rel(90,100);
+        }
+        test_rotation_abs(0,100);
+        
+        for (int i =0; i<4; i++)
+        {
+            test_ligne_droite(150000, 600);
+            test_rotation_rel(180,100);
+        }
+        test_rotation_abs(0,100);
+    }*/
+
 
     return 0;
 }

--- a/math_precalc.cpp	Tue Dec 11 19:12:55 2018 +0000
+++ b/math_precalc.cpp	Wed Dec 12 20:03:07 2018 +0000
@@ -28,6 +28,52 @@
 }
 
 double pow(long int a, long int b)
-{
+{	
 	return pow((double) a, (double) b);
-}
\ No newline at end of file
+}
+
+double abs(double a)
+{
+	if (a<0) {return -a;}
+	return a;
+}
+
+double diff_angle(double angle1, double angle2)
+{
+	// en degré
+	if (abs(angle2 - angle1) <= 180)
+	{
+		return angle2-angle1;
+	}
+	else if (angle2-angle1>0)
+	{
+		return (angle2-angle1)-360;
+	}
+	else
+	{
+		return 360+(angle2-angle1);
+	}
+}
+
+double borne_angle_r(double angle)
+{
+	if (angle > PI) {
+		angle -= 2*PI;
+	}
+	else if (angle <= -PI) {
+		angle += 2*PI;
+	}
+	return angle;
+}
+
+double borne_angle_d(double angle)
+{
+	if (angle > 180) {
+		angle -= 2*180;
+	}
+	else if (angle <= -180) {
+		angle += 2*180;
+	}
+	return angle;
+}
+

--- a/math_precalc.h	Tue Dec 11 19:12:55 2018 +0000
+++ b/math_precalc.h	Wed Dec 12 20:03:07 2018 +0000
@@ -1,11 +1,21 @@
 #ifndef MATH_PRECALC_H
 #define MATH_PRECALC_H
 
+
+#define PI 3.1416
+
 float cos_precalc(int angle);
 float sin_precalc(int angle);
 
 double racine(long int a);
 double pow(long int a, long int b);
+double abs(double a, double b);
+
+double diff_angle(double angle1, double angle2);
+
+
+double borne_angle_d(double angle);
+double borne_angle_r(double angle);
 
 
 #endif
\ No newline at end of file

--- a/odometrie.cpp	Tue Dec 11 19:12:55 2018 +0000
+++ b/odometrie.cpp	Wed Dec 12 20:03:07 2018 +0000
@@ -66,7 +66,7 @@
             angle += dep_roue_D * 2 / ECART_ROUE;
         }
         
-    	angle = borne_angle(angle);
+    	angle = borne_angle_r(angle);
 
         x_actuel = (int) (cx + R * sin(angle) + 0.5);
         y_actuel = (int) (cy - R * cos(angle) + 0.5);
@@ -76,20 +76,9 @@
         y_actuel += (int) (dep_roue_D * sin(angle) + 0.5);
     }
         
-    printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/3.14);
+    printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
 } 
 
-double borne_angle(double angle)
-{
-	if (angle > 3.14) {
-		angle -= 2*3.14;
-	}
-	else if (angle <= -3.14) {
-		angle += 2*3.14;
-	}
-	return angle;
-}
-
 long int get_x_actuel()
 {
 	return x_actuel;
@@ -102,5 +91,5 @@
 
 double get_angle()
 {
-	return angle*180/3.14;
+	return angle*180/PI;
 }
\ No newline at end of file

--- a/odometrie.h	Tue Dec 11 19:12:55 2018 +0000
+++ b/odometrie.h	Wed Dec 12 20:03:07 2018 +0000
@@ -1,13 +1,9 @@
 #ifndef ODOMETRIE_H
 #define ODOMETRIE_H
 
-#define PI_MILLI 3142
-#define DEUX_PI_MILLI 6283
-#define PI 3.1415
 
 void init_odometrie(void);
 void actualise_position(void);
-double borne_angle(double angle);
 long int get_x_actuel(void);
 long int get_y_actuel(void);
 double get_angle(void);

--- a/odometrie1.0.txt	Tue Dec 11 19:12:55 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,173 +0,0 @@
-#include "mbed.h"
-
-int AencoderPin1 = 18;
-int AencoderPin2 = 19;
-int BencoderPin1 = 20;
-int BencoderPin2 = 21;
- 
-volatile int AlastEncoded = 0;
-volatile int AencoderValue = 0;
-volatile int AencoderValueb = 0;
-volatile int BlastEncoded = 0;
-volatile int BencoderValue = 0;
-volatile int BencoderValueb = 0;
- 
-long AlastencoderValue = 0;
-long BlastencoderValue = 0;
-
-int sumA = 0;
-int sumB = 0;
-
-const long e = 11750; // ecart entre les 2 roues
-const float D = 8.5;
-
-long pos[2] = {0, 0}; // position en (x, y)
-short angle = 0; // angle du robot
-long DA = 0;
-long DB = 0;
-
-int dr[2]; // vitesse des roues droite et gauche (vg, vd), en millieme de cm / millisec
-    
- 
-void setup() {
-  Serial.begin (115200);
- 
-  pinMode(AencoderPin1, INPUT); 
-  pinMode(AencoderPin2, INPUT);
-  pinMode(BencoderPin1, INPUT); 
-  pinMode(BencoderPin2, INPUT);
-
- 
-  digitalWrite(AencoderPin1, HIGH); //turn pullup resistor on
-  digitalWrite(AencoderPin2, HIGH); //turn pullup resistor on
-  digitalWrite(BencoderPin1, HIGH); //turn pullup resistor on
-  digitalWrite(BencoderPin2, HIGH); //turn pullup resistor on
- 
-  //call updateEncoder() when any high/low changed seen
-  //on interrupt 0 (pin 2), or interrupt 1 (pin 3) 
-  attachInterrupt(digitalPinToInterrupt(AencoderPin1), AupdateEncoder, CHANGE); 
-  attachInterrupt(digitalPinToInterrupt(AencoderPin2), AupdateEncoder, CHANGE);
-  attachInterrupt(digitalPinToInterrupt(BencoderPin1), BupdateEncoder, CHANGE); 
-  attachInterrupt(digitalPinToInterrupt(BencoderPin2), BupdateEncoder, CHANGE);
- 
-}
-
-void getParametres(int dr[])
-{
-    dr[0] = (int) (AencoderValue * 8.41413233 + 0.5);
-    sumA += AencoderValue;
-    AencoderValue = 0;
-    /*DA += dr[0];
-    Serial.print("drA : ");
-    Serial.print(DA);*/
-    dr[1] = (int) (BencoderValue * 8.4034010 + 0.5);
-    sumB+= BencoderValue;
-    BencoderValue = 0;
-    /*DB += dr[1];
-    Serial.print("  drB : ");
-    Serial.print(DB);
-    Serial.print("  ");*/
-}
-
-void updatePos(long pos[], short* pangle, int dr[])
-{
-    /*
-    on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
-    */
-
-    long R = 0; // rayon du cercle décrit par la trajectoire
-    int d = 0; // vitesse du robot
-    long c[2] = {0, 0}; // position du centre du cercle décrit par la trajectoire
-
-    // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
-    if (dr[0] != dr[1]){
-        R = e / 2 * (dr[1] + dr[0]) / (dr[1] - dr[0]);
-        c[0] = pos[0] - (int) (R * sin(*pangle * PI / 32768) + 0.5);
-        c[1] = pos[1] + (int) (R * cos(*pangle * PI / 32768) + 0.5);
-        d = (dr[0] + dr[1]) / 2;
-
-        // mise à jour des coordonnées du robot
-        if (dr[0] + dr[1] != 0){
-            *pangle += (int) (d * 32768.0 / R / PI);
-        }
-        else{
-            *pangle += (int) (dr[1] * 2 * 32768.0 / e / PI);
-        }
-
-        pos[0] = c[0] + (int) (R * sin(*pangle * PI / 32768) + 0.5);
-        pos[1] = c[1] - (int) (R * cos(*pangle * PI / 32768) + 0.5);
-    }
-    else if (dr[0] == dr[1]){ // cas où la trajectoire est une parfaite ligne droite
-        pos[0] += (int) (dr[0] * cos(*pangle * PI / 32768));
-        pos[1] += (int) (dr[0] * sin(*pangle * PI / 32768));
-    }
-
-  Serial.print("dr : ");
-  Serial.print(dr[0]);
-  Serial.print(", ");
-  Serial.print(dr[1]);
-  Serial.print("R : ");
-  Serial.println(R);
-}
-
-
-void loop(){ 
-  //Do stuff here
-  
-  Serial.print("tics : ");
-  Serial.print(AencoderValue);
-  Serial.print(", ");
-  Serial.println(BencoderValue);
-  
-  Serial.print("tics totaux : ");
-  Serial.print(AencoderValueb);
-  Serial.print(", ");
-  Serial.println(BencoderValueb);
-  
-  Serial.print("tics comptés : ");
-  Serial.print(sumA);
-  Serial.print(", ");
-  Serial.println(sumB);
-  
-  getParametres(dr);
-  updatePos(pos, &angle, dr);
-
- 
-  Serial.print("x : ");
-  Serial.print(pos[0]);
-  Serial.print(", y : ");
-  Serial.print(pos[1]);
-  Serial.print(" angle : ");
-  Serial.println((float) (angle) * 180 / 32768);
-  
-  Serial.println("");
-  
-  delay(50); //just here to slow down the output, and show it will work  even during a delay
-}
- 
- 
-void AupdateEncoder(){
-  int AMSB = digitalRead(AencoderPin1); //MSB = most significant bit
-  int ALSB = digitalRead(AencoderPin2); //LSB = least significant bit
- 
-  int Aencoded = (AMSB << 1) |ALSB; //converting the 2 pin value to single number
-  int Asum  = (AlastEncoded << 2) | Aencoded; //adding it to the previous encoded value
- 
-  if(Asum == 0b1101 || Asum == 0b0100 || Asum == 0b0010 || Asum == 0b1011) {AencoderValue ++; AencoderValueb++;}
-  if(Asum == 0b1110 || Asum == 0b0111 || Asum == 0b0001 || Asum == 0b1000) {AencoderValue --; AencoderValueb--;}
- 
-  AlastEncoded = Aencoded; 
-}
-
-void BupdateEncoder(){
-  int BMSB = digitalRead(BencoderPin1); //MSB = most significant bit
-  int BLSB = digitalRead(BencoderPin2); //LSB = least significant bit
- 
-  int Bencoded = (BMSB << 1) |BLSB; //converting the 2 pin value to single number
-  int Bsum  = (BlastEncoded << 2) | Bencoded; //adding it to the previous encoded value
- 
-  if(Bsum == 0b1101 || Bsum == 0b0100 || Bsum == 0b0010 || Bsum == 0b1011) {BencoderValue ++; BencoderValueb++;}
-  if(Bsum == 0b1110 || Bsum == 0b0111 || Bsum == 0b0001 || Bsum == 0b1000) {BencoderValue --; BencoderValueb--;}
- 
-  BlastEncoded = Bencoded; 
-}
\ No newline at end of file

--- a/tests_moteurs.cpp	Tue Dec 11 19:12:55 2018 +0000
+++ b/tests_moteurs.cpp	Wed Dec 12 20:03:07 2018 +0000
@@ -8,8 +8,9 @@
 
 #include "odometrie.h"
 
-void test_ligne_droite(long int distance, int vitesse, int angle_vise)
+void test_ligne_droite(long int distance, int vitesse)
 {
+    // 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();
@@ -21,16 +22,15 @@
     int vitesse_G = vitesse;
     
     double angle = get_angle();
+    double angle_vise = get_angle();
     
     
-    while (pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5) < distance)//x_actuel < distance)
+    while (pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5) < distance) // a revoir : utilisation de sqrt
     {
-        //float a = racine((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel));
-        printf(" SQRT : %lf", pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5));
         angle = get_angle();
-        vitesse_G = (int) (vitesse * (1 + 0.05*(angle_vise-angle)));
-        vitesse_D = (int) (vitesse * (1 - 0.05*(angle_vise-angle)));
-        
+        vitesse_G = (int) (vitesse * (1 - 0.05*diff_angle(angle_vise,angle))); // petit asser en angle
+        vitesse_D = (int) (vitesse * (1 + 0.05*diff_angle(angle_vise,angle)));
+         
         set_PWM_moteur_D(vitesse_D);
         set_PWM_moteur_G(vitesse_G);
         actualise_position();
@@ -40,6 +40,46 @@
     
     }
     
+    set_PWM_moteur_D(0);
+    set_PWM_moteur_G(0);
+    wait(0.5);
+    motors_stop();
+}
+
+
+void test_rotation_rel(double angle_vise, int vitesse, int nombreTours)
+{
+    // rotation de angle_vise
+    motors_on();
+    
+    int compteur = 0;
+    
+    int sensRotation = 1;
+    if(angle_vise>0)
+    {
+        sensRotation = -1;
+    }
+    
+    int vitesse_D = vitesse*sensRotation;
+    int vitesse_G = -vitesse*sensRotation;
+    
+    double angle = get_angle();
+    angle_vise+=angle;
+    borne_angle_d(angle_vise);
+        
+    while (abs(diff_angle(angle,angle_vise))>1 || (compteur != nombreTours))
+    {
+        set_PWM_moteur_D(vitesse_D);
+        set_PWM_moteur_G(vitesse_G);
+        actualise_position();
+        if (angle*get_angle() < 0 && angle > 100 && compteur<nombreTours) // angle>100 pour que compteur ne s'incremente qu'au passage de 180 a -180, pas de -1 a 1
+        {
+            compteur++;
+        }
+        angle = get_angle();
+        //printf("angle recu  : %lf", angle);
+    }
+    
     //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));
     
     set_PWM_moteur_D(0);
@@ -49,40 +89,8 @@
 }
 
 
-void test_rotation(double angle_vise, int vitesse, int nombre)
+void test_rotation_abs(double angle_vise, int vitesse, int nombre)
 {
-    motors_on();
-    /*long int x_ini = get_x_actuel();
-    long int y_ini = get_y_actuel();
-    
-    long int x_actuel = get_x_actuel();
-    long int y_actuel = get_y_actuel();*/
-    
-    int compteur = 0;
-    
-    int vitesse_D = -vitesse;
-    int vitesse_G = vitesse;
-    
-    double angle = get_angle();
-    
-    while ((angle < angle_vise - 1) || (angle > angle_vise + 1) || (compteur != nombre) || ((angle < -180 + 1) && (angle_vise == 180)))
-    {
-        set_PWM_moteur_D(vitesse_D);
-        set_PWM_moteur_G(vitesse_G);
-        actualise_position();
-        if (angle*get_angle() < 0 && angle > 20)
-        {
-            compteur++;
-        }
-        angle = get_angle();
-        printf("angle recu  : %lf", 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 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));
-    
-    set_PWM_moteur_D(0);
-    set_PWM_moteur_G(0);
-    wait(0.5);
-    motors_stop();
+    double angle_rel = borne_angle_d(angle_vise-get_angle());
+    test_rotation_rel(angle_rel, vitesse, nombre);
 }
\ No newline at end of file

--- a/tests_moteurs.h	Tue Dec 11 19:12:55 2018 +0000
+++ b/tests_moteurs.h	Wed Dec 12 20:03:07 2018 +0000
@@ -4,9 +4,9 @@
 //fonctions de débug et de calibrage du robot
 
 
-void test_ligne_droite(long int distance, int vitesse, int angle_vise = 0);
+void test_ligne_droite(long int distance, int vitesse);
 
-void test_rotation(double angle_vise, int vitesse, int nombre = 0);
-
+void test_rotation_rel(double angle_vise, int vitesse, int nombre = 0);
+void test_rotation_abs(double angle_vise, int vitesse, int nombre = 0);
 
 #endif
\ No newline at end of file