Diff: fonction.cpp

Revision:

0:c2a70ac44fd7

Child:

1:cfb7e4122e21

diff -r 000000000000 -r c2a70ac44fd7 fonction.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fonction.cpp	Mon Jun 06 07:20:16 2022 +0000
@@ -0,0 +1,217 @@
+#include "mbed.h"
+#include <string>
+#include <iostream>
+#include "header.h"
+
+Serial pc (PA_2, PA_3);
+
+Serial husky (PA_0,PA_1);
+
+unsigned char reception_bidule[7]={0,0,0,0,0,0,'\n'};
+
+unsigned char data_recept[50];
+unsigned char cpt=0,flag_printf=0,flag_tram=0; 
+unsigned char data_taille=0,cpt_taille=0,chksum=0;
+unsigned char tableau_COMMAND_REQUEST[16];
+
+unsigned short pos_x, pos_y;
+
+bool flag_renvoi_pos = false;
+
+unsigned char COMMAND_REQUEST_KNOCK[6]={0x55,0xAA,0x11,0x00,0x2C,0x3C};
+unsigned char COMMAND_REQUEST_BLOCKS[6]={0x55,0xAA,0x11,0x00,0x21,0x31};
+unsigned char COMMAND_REQUEST[6]={0x55,0xAA,0x11,0x00,0x20,0x30};
+unsigned char COMMAND_REQUEST_ALGORITHM_BLOCK[8]={0x55,0xAA,0x11,0x02,0x2D,0x01,0x00,0x40};
+unsigned char COMMAND_REQUEST_ALGORITHM_COLOR[8]={0x55,0xAA,0x11,0x02,0x2D,0x04,0x00,0x43};
+
+typedef enum 
+{
+    HEADER1,
+    HEADER2,
+    ADRESSE,
+    TAILLE,
+    CHECKSUM,
+    DATA,
+    CMD
+}T_etat;
+
+static T_etat etat = HEADER1;
+
+short xg_c,xd_c;
+short xg_f,xd_f,xg_trig, xd_trig;
+short angl;
+
+float calcul_angle( void )
+{
+    xg_c = main_recept_droit();
+    xd_c = 150; //main_recept_gauche();
+    xg_f = xg_c;
+    xd_f = xd_c;
+    
+    xg_trig = 0.185 * xg_f;
+    xd_trig = 60 - 0.185 * xd_f;
+    
+    if ( xd_trig > xd_trig )
+    {
+        angl = (xd_trig - 30)%360;
+    }
+    else
+    {
+        angl = 210 - 0.185* xd_f;
+    }
+    return angl;
+}
+
+short main_recept_droit()
+
+    {
+    husky.attach(&reception,Serial::RxIrq);         //La fct réception est donc activé a chaque fois qu'une donnée passe dans le Rx
+    husky.baud(9600);
+    pc.printf("Debut du programme\n");
+    
+    pc.printf("Envoie command_request_knock\n");
+
+    envoi_commande(6,COMMAND_REQUEST_KNOCK);              //Test d'aqcuisiton avec commande_bidule
+    
+    bool flag_end = false;
+    
+    while(flag_end==false)
+    {
+        pc.printf("Check:%02X\n",chksum);      
+        if(flag_printf)
+        {   
+            affichage_buffer_reception();           //renvoie de données 
+            flag_printf=0;
+            flag_end=true;
+        }
+    }
+    
+    flag_end=false;
+
+    pc.printf("Reception de data, envoie Command_request_blocks\n");
+    
+    flag_renvoi_pos = false;
+    while(1)
+    {
+        
+    envoi_commande(6,COMMAND_REQUEST_BLOCKS);
+  
+    for(int i=0;i<data_taille+6;i++) //header + header2 + adress+ datataille + comande + data + checksum
+        {
+            tableau_COMMAND_REQUEST[i] = data_recept[i];
+        }
+    
+    flag_end=false;
+    
+
+        for(int j=0;j<16;j++) //header + header2 + adress+ datataille + comande + data + checksum
+        {
+            pc.printf("%02x:",tableau_COMMAND_REQUEST[j]);
+ 
+        }
+        pc.printf("\n");
+        reception_position();
+        
+        if(flag_renvoi_pos)
+            {
+                return pos_x;
+            }
+        flag_renvoi_pos = true;  
+    }
+}
+void reception_position(void)
+{
+    if(tableau_COMMAND_REQUEST[4] == 0x2A) //0x2A de [4] pour vérifier qu'ils s'agisse d'un block return_blocks
+    {
+        unsigned short high_x, low_x, high_y, low_y;
+        
+        high_x = tableau_COMMAND_REQUEST[6];
+        low_x = tableau_COMMAND_REQUEST[5];
+        
+        high_y = tableau_COMMAND_REQUEST[8];
+        low_y = tableau_COMMAND_REQUEST[7];
+        
+        pos_x = (high_x<<8) + low_x;
+        pos_y = (high_y<<8) + low_y;   
+    }
+    else
+    {
+        pos_x = 0x0000;
+        pos_y = 0x0000;
+    }
+    pc.printf("%04d:",pos_x);
+    pc.printf("%04d:",pos_y);
+    pc.printf("\n");
+}
+
+void envoi_commande(unsigned char taille,unsigned char buffer[])        //Envoie la commande a la carte
+{
+        for(int i=0;i<taille;i++) 
+        {
+            husky.putc(buffer[i]);
+        }    
+}
+
+void affichage_buffer_reception()                                       //Acquiere la donnée revoyé par la carte ex : [0x55]
+{
+    for(int i=0;i<data_taille+6;i++) //header + header2 + adress+ datataille + comande + data + checksum
+        {
+            pc.printf("%02x:",data_recept[i]);
+        }
+        pc.printf("\n");
+}
+
+void reception(void)                           //Automate de réception des données 
+{       
+        data_recept[cpt] = husky.getc();
+        //flag_recept=1;
+        switch(etat)
+        {
+            case HEADER1:chksum=0;  
+                        cpt=0;
+                        if(data_recept[cpt]==0x55){
+                        etat=HEADER2;
+                        }
+                        break;
+                        
+            case HEADER2 : if(data_recept[cpt]==0xAA)
+                                etat=ADRESSE;
+                            else
+                                etat=HEADER1;
+                        break;
+                        
+            case ADRESSE : if(data_recept[cpt]==0x11)
+                            etat=TAILLE;
+                            else
+                            etat=HEADER1;
+                        break;
+            case TAILLE : data_taille=data_recept[cpt];
+                        etat=CMD;
+                        break;
+            
+            case CMD:   // utilisation de cmd???
+                        etat=DATA;
+                        if(data_taille==0)
+                            etat=CHECKSUM;               
+                        break;
+                            
+            case DATA:  cpt_taille++;
+                        if(cpt_taille==data_taille)
+                            etat=CHECKSUM;
+                        break;
+            
+            case CHECKSUM :      etat=HEADER1;
+                        
+                        if(data_recept[cpt]==chksum)
+                        {
+                        flag_tram=flag_tram+1;
+                        flag_printf=1;
+                        cpt_taille=0;
+                        cpt=0;
+                        }   
+                        break;
+        }
+        chksum=chksum+data_recept[cpt];
+    cpt++;
+
+}