Revision 14:5374abce265d, committed 2020-06-04

Comitter:

jlesoudeer

Date:

Thu Jun 04 09:31:38 2020 +0000

Parent:

13:a2c52e2a8104

Commit message:

test lecture des positions

Changed in this revision

diff -r a2c52e2a8104 -r 5374abce265d MX12_ENS.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MX12_ENS.lib	Thu Jun 04 09:31:38 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ajuton/code/MX12_ENS/#6bd931ef8d58

diff -r a2c52e2a8104 -r 5374abce265d main.cpp
--- a/main.cpp	Fri May 15 09:52:46 2020 +0000
+++ b/main.cpp	Thu Jun 04 09:31:38 2020 +0000
@@ -1,14 +1,15 @@
 #include "mbed.h"
 
 //AX12 myax12 (PC_4, PC_5, 1,115200);
-Serial myserial_ax12(PC_4,PC_5,1000000);                                         // Modifier le baud rate en fonction de votre utilisation des moteurs
+Serial myserial_ax12(PC_4,PC_5,115200);                                         // Modifier le baud rate en fonction de votre utilisation des moteurs
 Serial pc(USBTX,USBRX,115200);
-//Serial zigBee(PC_1,PC_0,115200); 
+//Serial zigBee(PC_1,PC_0,115200);
 
 
 DigitalOut my_led1(LED1);
 DigitalOut my_led2(LED2);
 DigitalOut my_led3(LED3);
+DigitalIn bp(USER_BUTTON);
 
 void SetCRSpeed(int ID, float speed);
 void SetMode(int ID, int mode);
@@ -23,179 +24,242 @@
 int num_moteur = 1;
 int vitesse;
 char sens;
-
-Ticker ticker_10ms;
+int temps_ms = 0;
+//pour connaitre le nombre de tours
+int tour_1 = 0;
+int tour_2 = 0;
+int tour_3 = 0;
 
-//Définition des trames de requête pour le moteur 1
-char trame_demande_pos_1[8]={0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
-char trame_demande_pos_2[8]={0xFF,0xFF,0x02,0x4,0x2,0x24,2,0xD1};
-char trame_demande_pos_3[8]={0xFF,0xFF,0x03,0x4,0x2,0x24,2,0xD0};
-char trame_demande_vitesse[8]={0xFF,0xFF,0x01,0x4,0x2,0x28,2,0xCE};
+Ticker ticker_1ms;
+
+//Définition des trames de requête
+char trame_demande_pos_1[8]= {0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
+char trame_demande_pos_2[8]= {0xFF,0xFF,0x02,0x4,0x2,0x24,2,0xD1};
+char trame_demande_pos_3[8]= {0xFF,0xFF,0x03,0x4,0x2,0x24,2,0xD0};
 
 
-#define acq_vitesse 0
-#define acq_position 1
-
-//Fonction d'interruption sur réception de charactère sur liaison série
+//Fonction d'interruption sur réception de caractère sur liaison série
 void Rx_Irq_fonction(void)
 {
     my_led1 = !my_led1;
-    
+
     static int i_rx=0;
     tata[i_rx]=myserial_ax12.getc();
-    
+
     if (tata[0]!=0xFF)                                                          // On vérifie que le premier et le deuxième octet sont biens 0xFF
         i_rx=0;
     else i_rx++;
     if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0;
-    
+
 //Une fois qu'on sait que la trame est "juste" on récupère le reste
-    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7)
-    {
+    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7) {
         my_led3 = !my_led3;
         i_rx=0;
-        
-        if(tata[4] == 0){ //si erreur  est nulle
+
+        if(tata[4] == 0) { //si erreur  est nulle
             //On transforme les données en fonction de ce qu'on lit depuis les moteurs
-            if (acq_position == 1) position_1 = tata[5]+((uint16_t)tata[6]<<8);
-            /*if (acq_vitesse == 1){
-                sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
-                vitesse = tata[5]+((uint16_t)tata[6]<<8);
-            }*/
+            position_1 = tata[5]+((uint16_t)tata[6]<<8);
         }
-        tata[0]=0;
+        for(int i=0;i<10;i++)
+            {tata[i]=0;}
     }
-    
-    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7)
-    {
+
+    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7) {
         my_led3 = !my_led3;
         i_rx=0;
-        
-        if(tata[4]==00){ //si erreur nulle
+
+        if(tata[4]==00) { //si erreur nulle
             //On transforme les données en fonction de ce qu'on lit depuis les moteurs
-            if (acq_position == 1) position_2 = tata[5]+((uint16_t)tata[6]<<8);
-            /*if (acq_vitesse == 1){
-                sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
-                vitesse = tata[5]+((uint16_t)tata[6]<<8);
-            }*/
+            position_2 = tata[5]+((uint16_t)tata[6]<<8);
+
         }
-        tata[0]=0;
+        for(int i=0;i<10;i++)
+            {tata[i]=0;}
     }
-    
-    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7)
-    {
+
+    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7) {
         my_led3 = !my_led3;
         i_rx=0;
-        
-        if(tata[4]==00){ //si erreur nulle
+
+        if(tata[4]==00) { //si erreur nulle
             //On transforme les données en fonction de ce qu'on lit depuis les moteurs
-            if (acq_position == 1) position_3 = tata[5]+((uint16_t)tata[6]<<8);
-            /*if (acq_vitesse == 1){
-                sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
-                vitesse = tata[5]+((uint16_t)tata[6]<<8);
-            }*/
+            position_3 = tata[5]+((uint16_t)tata[6]<<8);
+
         }
-        tata[0]=0;
+        for(int i=0;i<10;i++)
+            {tata[i]=0;}
     }
-    
-    if (i_rx>9)
-    {
-        /*for(int i=0;i<10;i++)
-            tata[i]=0;*/
+
+    if (i_rx>9) {
+        for(int i=0;i<10;i++)
+            tata[i]=0;
         i_rx=0;
     }
 }
 
-void fonction_10ms(void)
+void fonction_1ms(void)
 {
-    /*for(int i=0;i<=19;i++)
-    {
-        pc.printf("%X ",tata[i]);
-    }
-    pc.printf("\n\r");*/
-    
-    for(int i=0;i<=7;i++)
-    {
-       //On envoi la trame que l'on souhaite
-        if(num_moteur == 1)myserial_ax12.putc(trame_demande_pos_1[i]);
-        if(num_moteur == 2)myserial_ax12.putc(trame_demande_pos_2[i]);
-        if(num_moteur == 3)myserial_ax12.putc(trame_demande_pos_3[i]);
-        /*if(acq_vitesse){
-            myserial_ax12.putc(trame_demande_vitesse[i]);
-            //pc.printf("\n\rvitesse = %d\n\rsens = %d",vitesse,sens);
-        }*/
-    }
-    num_moteur++;
-    if(num_moteur > 3)num_moteur = 1;
+    temps_ms++;
 }
 
-int main(){
+
+int main()
+{
     //myax12.SetMode(1); //passage en Continuous rotation
 
     //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
     //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5};
     //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4};
-    float Angle;
-    short speed;
-    char data[10];
-    char s=0;
-    float temp;
-    int position_old_1=0,i,a;
-    int position_old_2=0;
-    int position_old_3=0;
+
+    //pour qu'il s'arrete
+    uint8_t car_recu=0;
+
+    uint8_t bp_lu;
     
+    //pour regarder les 3 moteurs les uns à la suite des autres
+    int etat=1;
+    int fm_etat=1;
+    int position_1_old;
+    int position_2_old;
+    int position_3_old;
     pc.printf("test lecture AX12\n\r");
-    
+
     SetMode(1, 1);
     wait(0.1);
     SetMode(2, 1);
     wait(0.1);
     SetMode(3, 1);
-    
+
     wait(0.1);
-    
+
     SetCRSpeed(1,0.05);
     wait(0.1);
     SetCRSpeed(2,0.05);
     wait(0.1);
     SetCRSpeed(3,0.05);
-    
+
     myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
-    ticker_10ms.attach(&fonction_10ms,1);
-
+    ticker_1ms.attach(&fonction_1ms,0.001);
+    etat=2;
     while(1) {
-       
-
-        if (position_1 != position_old_1)
-        {   
-            pc.printf("\n\rposition moteur 1  = %d :", position_1);
-            //pc.printf("\n\rposition moteur 1  = %d\n\r mot recu :",position_1);    
+        
+        //commande des moteurs (pour les tests)
+        time_t seconds = time(NULL);
+        bp_lu = bp.read();
+        if(pc.readable()) {
+            car_recu=pc.getc();
+            switch(car_recu) {
+            case 's' :
+                SetCRSpeed(1,0);
+                SetCRSpeed(2,0);
+                SetCRSpeed(3,0);
+                break;
+            case 'a' :
+                SetCRSpeed(1,0.05);
+                wait(0.1);
+                SetCRSpeed(2,0.05);
+                wait(0.1);
+                SetCRSpeed(3,0.05);
             
-            /*for(i=0;i<=7;i++)
-            {
-                pc.printf("%X ",tata[i]);
-            }*/
-            position_old_1 = position_1;
+        }
         }
-        if (position_2 != position_old_2)
-        {   
-            pc.printf("\n\rposition moteur 2  = %d :", position_2);
 
-            position_old_2 = position_2;
-        }
-        if (position_3 != position_old_3)
-        {   
-            pc.printf("\n\rposition moteur 3  = %d :", position_3);
-            
-            position_old_3 = position_3;
-        }
         
 
-    }       
-    
+
+
+        //on regarde la position des moteurs
+        switch(etat) {
+            case 2 :
+                if(fm_etat) {
+                    position_1_old = position_1;
+                    for (int i=0; i<=9; i++){
+                        pc.printf("%x ", tata[i]);
+                        }
+                    for(int i=0; i<=7; i++) {
+                        //On envoi la trame que l'on souhaite
+                        
+                        myserial_ax12.putc(trame_demande_pos_1[i]);
+                        //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
+                    }
+                    if(position_1_old> position_1) {
+                            tour_1++;
+                        }
+                        
+                    fm_etat=0;
+                }
+
+                if(temps_ms>=1000) {
+                    etat=3;
+                    fm_etat=1;
+                }
+                break;
+
+            case 3 :
+            
+                if(fm_etat) {
+                    position_2_old = position_2;
+                    for(int i=0; i<=7; i++) {
+                        //On envoi la trame que l'on souhaite
+                        myserial_ax12.putc(trame_demande_pos_2[i]);
+                        //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
+                        
+                    }
+                    if(position_2_old > position_2) {
+                            tour_2++;
+                        }
+                    fm_etat=0;
+                }
+
+                if(temps_ms>=1000) {
+                    etat=4;
+                    fm_etat=1;
+                }
+                break;
+
+            case 4 :
+                if(fm_etat) {
+                    position_3_old = position_3;
+                    for(int i=0; i<=7; i++) {
+                        //On envoi la trame que l'on souhaite
+                        myserial_ax12.putc(trame_demande_pos_3[i]);
+                        
+                    }
+                    if(position_3_old > position_3) {
+                            tour_3++;
+                        }
+                    fm_etat=0;
+                }
+
+                if(temps_ms>=1000) {
+                    etat=5;
+                    fm_etat=1;
+                }
+                break;
+
+            case 5 :
+                
+                pc.printf("\n\rposition moteur 1  = %d :", position_1);
+                pc.printf("\n\rtour moteur 1  = %d :", tour_1);
+                pc.printf("\n\rposition moteur 2  = %d :", position_2);
+                pc.printf("\n\rtour moteur 2  = %d :", tour_2);
+                pc.printf("\n\rposition moteur 3  = %d :", position_3);
+                pc.printf("\n\rtour moteur 3  = %d :", tour_3);
+                etat = 2;
+                fm_etat =1;
+                temps_ms = 0;
+                break;
+        }
+
+
+
+
+    }
+
 }
 
-void SetCRSpeed(int ID, float speed) {
+void SetCRSpeed(int ID, float speed)
+{
 
     // bit 10     = direction, 0 = CCW, 1=CW
     // bits 9-0   = Speed
@@ -215,7 +279,8 @@
     Write(ID, 0x20, 2, data);
 }
 
-void SetMode(int ID, int mode) {
+void SetMode(int ID, int mode)
+{
 
     if (mode == 1) { // set CR
         SetCWLimit(ID,0);
@@ -228,7 +293,8 @@
     }
 }
 
-void SetCWLimit (int ID, int degrees) {
+void SetCWLimit (int ID, int degrees)
+{
 
     char data[2];
     short limit = (4095 * degrees) / 360;
@@ -239,18 +305,20 @@
     Write(ID, 0x06, 2, data);
 }
 
-void SetCCWLimit (int ID, int degrees) {
-    
+void SetCCWLimit (int ID, int degrees)
+{
+
     char data[2];
     short limit = (4095 * degrees) / 360;
-    
+
     data[0] = limit & 0xff; // bottom 8 bits
     data[1] = limit >> 8;   // top 8 bits
 
     Write(ID, 0x08, 2, data);
 }
 
-void Write(int ID, int start, int bytes, char* data){
+void Write(int ID, int start, int bytes, char* data)
+{
     char TxBuf[16];
     char sum = 0;
 
@@ -288,88 +356,8 @@
     for (int i = 0; i < (7 + bytes) ; i++) {
         myserial_ax12.putc(TxBuf[i]);
     }
-    
+
     wait(0.00002);
 
 }
 
-//void set_speed(ID,speed)
-//void demande_lecture_pos(ID)
-
-
-/*
-#include "mbed.h"
-
-//AX12 myax12 (PC_4, PC_5, 1,115200);
-Serial myserial_ax12(PC_4,PC_5,115200);
-char tata[10];
-int position;
-
-void Rx_Irq_fonction(void)
-{
-    static int i_rx;
-    tata[i_rx]=myserial_ax12.getc();
-    if (tata[0]!=0xFF) 
-        i_rx=0;
-    else i_rx++;
-    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7)
-    {
-        i_rx=0;
-        if(tata[4]==00) //si erreur  est nulle
-            position = tata[5]+((uint16_t)tata[6]<<8);
-        tata[0]=0;
-    }
-    if (i_rx>9)
-    {
-        for(int i=0;i<10;i++)
-            tata[i]=0;
-        i_rx=0;
-    }
-}
-
-int main(){
-    //myax12.SetMode(1); //passage en Continuous rotation
-    char trame_demande_pos[8]={0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
-    char trame_cmd_speed[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
-    float Angle;
-    short speed;
-    char data[10];
-    char s=0;
-    float temp;
-    int position_old=0,i,a;
-    //myax12.SetCRSpeed(0.05);
-    
-    for(i=0;i<=8;i++)
-    {
-        myserial_ax12.putc(trame_cmd_speed[i]);
-    }
-    
-    for(i=0;i<=7;i++)
-    {
-        myserial_ax12.putc(trame_demande_pos[i]);
-    }
-    myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
-        
-
-    while(1) {
-        if (position != position_old)
-        {   
-            printf("\n\rposition = %d\n\r mot recu :",position);    
-            for(i=0;i<=7;i++)
-            {
-                printf("%X ",tata[i]);
-            }
-            position_old=position;
-        }
-        wait(0.01);
-        a++;
-        if(!(a % 20))
-            for(i=0;i<=7;i++)
-            {
-                 myserial_ax12.putc(trame_demande_pos[i]);
-            }
-    }
-    
-}
-
-void set_speed(ID,speed)*/
\ No newline at end of file