Dylan Saada
/
telemetre2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 0:6ce9c65992e5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Apr 15 06:23:18 2015 +0000
@@ -0,0 +1,731 @@
+#include "mbed.h"
+#include "all_includes.h"
+#include "USBHostSerial.h"
+#include <stdio.h>
+#define RVISION 1000
+#define TETABALISE 0.785398
+
+// LED
+DigitalOut myled[4]={LED1,LED2,LED3,LED4};
+// end LED
+
+Serial pc(USBTX, USBRX); // tx, rx
+CAN can(p30,p29);
+DigitalIn pot2(p20);
+
+CANMessage msgTx; // EMISSION
+CANMessage msgRx;
+CANMessage can_MsgRx[SIZE_FIFO]; // RECEPTION
+
+Timer t;
+Timeout flipper;
+
+unsigned char flag_timeout=1; // quand 1 pas de timeout si 0 timeout
+
+void fonction_timeout()
+{
+ flag_timeout=0;
+}
+
+typedef struct{
+ short X;
+ short Y;
+ double teta;
+ short Xabs;
+ short Yabs;
+ double teta_abs;
+
+
+
+ }T_pass;
+
+T_pass passage[20];
+
+unsigned short d1=0,d2=0;
+char buffer[66],com_echo_command[16],com_status_command[4],com_echo[16],com_status[4], time_stamp_and_sum[6],vide[1],final_LF[1];
+unsigned short status=0, distance=0,old_distance=0,d=0;
+unsigned short discont_dist[VAL][2]={{0},{0}};
+signed short discont_check[VAL][2]={{0},{0}};
+double x1=0,y1=0,x2=0,y2=0;
+char variable_interupt_ecran = 0;
+char variable_interupt_strat = 0;
+char compteur_passage=0;
+unsigned short DIST=0,Centre_Y_pass=0;
+short coeff_directeur=0,old_coeff_directeur=0, Centre_X_pass=0, Gobelet_found_X=0;
+
+/*************************** coo demande pour la cartographie **************************/
+short cooX_demande=0,cooY_demande=0; // coo du point a mesure dans le repere absolue de la table en mm
+unsigned short angle_cartographie = 0; // angle demande centre sur le point demande en degre
+/***************************************************************************************/
+
+/*************************** coo de debut de match du robot **************************/
+short cooXdepart=0,cooYdepart=0; // coo debut du match dans le repere absolue de la table
+
+/***************************************************************************************/
+
+/*************************** coo actuelle du robot **************************/
+short cooX_actuelle=965, cooY_actuelle=0,teta_actuelle = 90; // coo du robot dans le repere absolue de la table
+
+/***************************************************************************************/
+
+/********************* coo des points mesure par le telemetre **************************/
+short x_telemetre=0, y_telemetre=0; // coo dans le repere du telemetre
+short x_absolu=0, y_absolu=0;
+double teta = 0,teta_actuelle_rad=90; //90 // angle
+/***************************************************************************************/
+
+
+/*
+Sensor Commands
+MDMS-Command max 32 bytes min 16 bytes
+GDGS-Command max 14 bytes min 13 bytes
+BM-Command max 4 bytes min 3 bytes
+QT-Command max 4 bytes min 3 bytes
+RS-Command max 4 bytes min 3 bytes
+TM-Command max 5 bytes min 4 bytes
+SS-Command max 10 bytes min 9 bytes
+CR-Command max 6 bytes min 5 bytes
+HS-Command max 5 bytes min 4 bytes
+DB-Command max 6 bytes min 5 bytes
+VV-Command max 4 bytes min 3 bytes
+PP-Command max 4 bytes min 3 bytes
+II Command max 4 bytes min 3 bytes
+*/
+
+T_cmd MS_all = {{"MS"},{},16,44,725,1,1,0}; // vision max du telemetre avec la resolution la plus forte
+T_cmd MS_centre_avant = {{"MS"},{},16,55,215,1,1,1}; // vision angle avant autorise
+T_cmd MS_centre_arriere = {{"MS"},{},16,55,215,1,1,1}; // vision angle arriere autorise
+
+T_cmd MS_avant_moyenne = {{"MS"},{},16,508,724,27,1,1}; // vision angle avant avec un gros cluster pour faire la detection des dangers
+T_cmd MS_arriere_moyenne = {{"MS"},{},16,508,724,27,1,1}; // vision angle avant avec un gros cluster pour faire la detection des dangers
+
+T_cmd MS_one = {{"MS"},{},16,240,240,1,1,0}; // vision cluster de 1
+T_cmd MS_one_shot = {{"MS"},{},16,384,384,1,1,0};
+
+T_cmd MS_center = {{"MS"},{},16,44,725,1,1,1};
+
+//T_cmd MS_center={{"MS"},{},3,436 ,724,1,0,1}; // 90� face avant
+
+
+ETAT etat_connection=BEGIN_CONNECTION;
+
+void create_command(T_cmd *Pcmd)
+{
+ switch(Pcmd->command[0])
+ {
+ case 'M': // manquue string characters pour l'instant
+ sprintf(Pcmd->created_command,"%s%04d%04d%02d%01d%02d\n"
+ ,Pcmd->command,Pcmd->debut,Pcmd->fin,Pcmd->count,Pcmd->scan_interval,Pcmd->number_of_scan);
+ Pcmd->number_of_full_frame=(((Pcmd->fin)-(Pcmd->debut)+1)/32);
+ Pcmd->size_of_incomplete_frame=(((Pcmd->fin)-(Pcmd->debut)+1)%32);
+ Pcmd->length_reponse=((Pcmd->length_command)+1)*2+5+10+Pcmd->number_of_full_frame*66+Pcmd->size_of_incomplete_frame+3;
+ break;
+
+ default: sprintf(Pcmd->created_command,"%s%s\n",Pcmd->command,Pcmd->string_character);
+ break;
+
+ }
+}
+
+
+
+
+
+
+int main()
+{
+ unsigned char i=0,j=0,sum;
+ unsigned long temps=0,diff_temps,old_temps;
+ short cooX=0,cooY=0,compteur_global=0;
+ double Angle=0;
+ unsigned char swap_distance[66]; //66
+ char status_buff[5],number_of_scan_remaining_buff[3],c;
+
+// cration des commande
+create_command(&MS_all);
+create_command(&MS_centre_avant);
+create_command(&MS_centre_arriere);
+create_command(&MS_avant_moyenne);
+create_command(&MS_arriere_moyenne);
+create_command(&MS_one);
+create_command(&MS_center);
+
+// wait(2);
+ USBHostSerial serial;
+ pc.baud(921600);
+
+
+// initialisation du CAN
+ can.frequency(1000000); // vitesse du bus can
+ can.attach(&can_ISR_Reader); // Fonction appeler lors de l'interruption CAN en reception
+
+// message CAN autorise a declenche l'interruption
+ CAN2_wrFilter(RECEIVE_ODOMETRIE); // message odometrie pour pouvoir mettre a jour la position du robot au moment du tir telemetre
+ CAN2_wrFilter(TELEMETRE_RTR_CARTO); // message de demande de cartographie
+ CAN2_wrFilter(RESET_TELEMETRE); // message de reset de la carte telemetre
+
+
+ DEBUG_PRINTF("Debut du programme de Bruno LARNAUDIE\r\n");
+ myled[2] = 1;
+
+ while(1)
+ {
+ myled[1] = 1;
+
+
+ switch(etat_connection)
+ {
+ case BEGIN_CONNECTION: DEBUG_PRINTF("host serial launch\n\r");
+ etat_connection=WAIT_CONNECTION;
+ break;
+
+ case WAIT_CONNECTION: DEBUG_PRINTF("wait for connection\n\r");
+ while(!serial.connect())
+ wait(1);
+ DEBUG_PRINTF("Host device connected\n\r");
+ //serial.printf("SS750000\n"); // envoi la commande au laser
+ //wait_ms(10);
+ serial.printf("RS\n");
+ while(serial.available())
+ serial.getc();
+ wait_ms(100);
+ while(serial.available())
+ serial.getc();
+ wait_ms(100);
+ while(serial.available())
+ serial.getc();
+ etat_connection=CONNECTION_OK;
+ break;
+
+ case CONNECTION_OK : DEBUG_PRINTF("state connection ok\n\r");
+ wait(1);
+ if(!serial.connected())
+ etat_connection=WAIT_CONNECTION;
+ else
+ {
+ if(serial.available()!=0)
+ {
+ DEBUG_PRINTF("Vidage de la memoire usb serial\n\r");
+ etat_connection=CONNECTION_OK;
+ while (serial.available()!=0)
+ {
+ DEBUG_PRINTF("%c",serial.getc());
+ }
+ }
+ else
+ {
+ DEBUG_PRINTF("Buffer est vide\n");
+ etat_connection=TRANSITION;
+ }
+ }
+ break;
+
+ case TRANSITION: // preparation de la commande a envoyer et calcul de la longeur de la reponse
+ DEBUG_PRINTF("Transition\n\r");
+ wait_ms(100);
+ MS_center=MS_all;
+ etat_connection=SEND_COMMAND;
+ break;
+
+ case SEND_COMMAND: // envoi de la commande
+ DEBUG_PRINTF("MS\n");
+ if(!serial.connected())
+ etat_connection=WAIT_CONNECTION;
+ else
+ {
+ DEBUG_PRINTF("command_send\n");
+ MS_center.created_command[15]=10;
+ serial.printf(MS_center.created_command); // envoi la commande au laser
+ MS_center.created_command[15]=0;
+ DEBUG_PRINTF("%s",MS_center.created_command);
+ /*flag_timeout=1;
+ flipper.attach(&fonction_timeout, 0.1); // setup flipper to call flip after 2 seconds
+ */etat_connection=WAIT_ECHO_COMMAND;
+ }
+ break;
+
+ case WAIT_ECHO_COMMAND:// attente de la reponse
+ if(!serial.connected())
+ etat_connection=WAIT_CONNECTION;
+ else
+ {
+ if(serial.available()>=(MS_center.length_command+5)) // Attend d'avoir recu les caracteres de status
+ {
+ MS_center.created_command[15]=10;
+ for(i=0;i<(MS_center.length_command);i++)
+ {
+ c=serial.getc();
+ if(MS_center.created_command[i]==c)
+ {
+ }
+ else
+ {
+ DEBUG_PRINTF("retour incorrect\n");
+ etat_connection=RESET;
+ }
+ }
+ serial.readBuf(status_buff,5);
+ if((status_buff[0]=='0')&&(status_buff[1]=='0')&&(status_buff[2]=='P')&&(status_buff[3]==10)&&(status_buff[4]==10))
+ {
+ etat_connection=WAIT_ANSWER_BEGIN;
+ status_buff[3]=0;
+ }
+ else
+ {
+ etat_connection=RESET;
+ DEBUG_PRINTF("status incorrect\n");
+ }
+ }
+ else if(flag_timeout==0)
+ {
+ flag_timeout=1;
+ DEBUG_PRINTF("TIMEOUT\n");
+ etat_connection=BEFORE_END;
+ }
+ }
+ break;
+
+
+ case WAIT_ANSWER_BEGIN:// attente de l'encapsulage des datas
+ if(!serial.connected())
+ etat_connection=WAIT_CONNECTION;
+ else
+ {
+ if(serial.available()>=(MS_center.length_command+4)) // Attend d'avoir recu les caractères de status
+ {
+ // DEBUG_PRINTF("command_echo\n");
+ for(i=0;i<(MS_center.length_command)-3;i++)
+ {
+ c=serial.getc();
+ if(MS_center.created_command[i]==c)
+ {
+ // DEBUG_PRINTF("%c",c);
+
+ }
+ else
+ {
+ etat_connection=CONNECTION_OK;
+ DEBUG_PRINTF("retour incorrect=%c\n",c);
+ }
+ }
+ serial.readBuf(number_of_scan_remaining_buff,3);
+
+ number_of_scan_remaining_buff[2]=0;
+ // DEBUG_PRINTF("scan_remaining=%s\n",number_of_scan_remaining_buff);
+ // DEBUG_PRINTF("99b et timestamp\n");
+ if(serial.available()>=4)
+ serial.readBuf(status_buff,4);
+ if(serial.available()>=6)
+ serial.readBuf(time_stamp_and_sum,6);
+ temps= (((((unsigned long)(time_stamp_and_sum[0]-0x30))<<18)&0xFC0000)
+ |((((unsigned long)(time_stamp_and_sum[1]-0x30))<<12)&0x03F000)
+ |((((unsigned long)(time_stamp_and_sum[2]-0x30))<<06)&0x000FC0)
+ |((((unsigned long)(time_stamp_and_sum[3]-0x30)))&0x00003F));
+
+ /*flipper.detach();
+ flag_timeout=1;
+ flipper.attach(&fonction_timeout, 0.2); // setup flipper to call flip after 2 seconds
+ */etat_connection=WAIT_FULL_FRAME;
+
+ }
+ else if(flag_timeout==0)
+ {
+ flag_timeout=1;
+ DEBUG_PRINTF("TIMEOUT\n");
+ etat_connection=CONNECTION_OK;
+ }
+ }
+ break;
+
+
+ case WAIT_FULL_FRAME: // attente d'une trame pleine
+
+ msgTx.id=0x315;
+ msgTx.len=8;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ compteur_global=0;
+
+ for(i=0;i<MS_center.number_of_full_frame;i++)
+ {
+ //DEBUG_PRINTF("sa=%02d ",serial.available());
+ sum=0;
+ for(j=0;j<66;j=j+2)
+ {
+ while(serial.available()<2);
+ //DEBUG_PRINTF("%02d ",serial.available());
+ swap_distance[j]=serial.getc();
+ swap_distance[j+1]=serial.getc();
+
+ if(j==64)
+ {
+ if((((sum&0x3F)+0x30)==(swap_distance[j]))&&(swap_distance[j+1]==10))
+ {
+ etat_connection=WAIT_INCOMPLETE_FRAME;
+ }
+ else
+ {
+ DEBUG_PRINTF("error");
+ etat_connection=BEFORE_END;
+ }
+ if((MS_center.size_of_incomplete_frame==0)&&(i==MS_center.number_of_full_frame-1)) // envoi du dernier point si que des trames completes
+ {
+ COO_PRINTF("%+05d%+05d\n",cooX,cooY);
+ }
+ }
+ else
+ {
+ sum=sum+swap_distance[j]+swap_distance[j+1];
+ distance=(((unsigned short)(swap_distance[j]-0x30)<<6)&0xFC0)|(((unsigned short)(swap_distance[j+1]-0x30))&0x3F);
+ Angle=M_PI/2+(COEF_TETE_EN_BAS*(MS_center.debut+(j/2+i*32)*MS_center.count)-384)*DELTA_ANGLE;
+ /////////////////////////
+ if(distance<=20 || distance>=4000)
+ distance=4000;
+ /////////////////////////
+
+ cooX=(short)(cos(Angle)*distance);
+ cooY=(short)(sin(Angle)*distance);
+
+ /*msgTx.data[(compteur_global*2)%8]=(char)(cooX/10); // 40
+ msgTx.data[(compteur_global*2+1)%8]=(char)(cooY/10);// 40
+
+ compteur_global++;
+ if((((compteur_global)%4)==0)&&(compteur_global>=3))
+ {
+ can.write(msgTx);
+ //wait_ms(1);
+ }*/
+
+ if((old_distance > distance+DIST_DISCONT || old_distance+DIST_DISCONT<distance)&&(old_distance!=0))
+ {
+
+ if((old_distance+DIST_DISCONT<distance)&&(old_distance<RVISION) &&(d1==0))
+ {
+ d1=old_distance;
+ x1=(short)(old_distance*cos(Angle));
+ y1=(unsigned short)(old_distance*sin(Angle));
+ }
+
+ else if( (old_distance > (distance+DIST_DISCONT)) &&(distance<RVISION) &&(d2==0)&&(d1!=0) )
+ {
+ d2=distance;
+ x2=(short)(distance*cos(Angle));
+ y2=(unsigned short)(distance*sin(Angle));
+ }
+
+ if((d1!=0)&&(d2!=0))
+ {
+ // calcul de la distance entre 2 points avec la discontinuit� de distance et pyhtagore
+ DIST= sqrt( (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) ) ; //1.220
+
+
+ if(DIST >= 400)
+ {
+ compteur_passage++;
+
+ DEBUG_PRINTF("\n\nPASSAGE numero %d:",compteur_passage);
+ DEBUG_PRINTF("\nLargeur: %d mm",DIST);
+ DEBUG_PRINTF("\nDistance: %d mm",(d1+d2)/2);
+ DEBUG_PRINTF("\nAngle: %lf ",Angle);
+ DEBUG_PRINTF("\nx2=%lf\tx1=%lf\ty2=%lf\ty1=%lf",x2,x1,y2,y1);
+
+ teta_actuelle_rad=teta_actuelle*M_PI/180;
+
+ Centre_X_pass = (x2+x1)/2;
+ Centre_Y_pass = (y2+y1)/2;
+
+ teta = atan2((double)Centre_X_pass,(double)Centre_Y_pass);
+
+
+ DEBUG_PRINTF("\nCentre passage en X = %d\t Centre passage en Y = %d , teta = %lf\n",Centre_X_pass,Centre_Y_pass,teta);
+
+
+ DEBUG_PRINTF("\nd1= %d , d2= %d , teta= %lf, teta_actuelle= %lf",d1,d2,teta,teta_actuelle_rad);
+ x_absolu = cooX_actuelle + ((d1+d2)/2)*cos(teta+teta_actuelle_rad);
+ y_absolu = cooY_actuelle + ((d1+d2)/2)*sin(teta+teta_actuelle_rad);
+
+ passage[compteur_passage-1].X=Centre_X_pass;
+ passage[compteur_passage-1].Y=Centre_Y_pass;
+ passage[compteur_passage-1].teta=teta;
+ passage[compteur_passage-1].Xabs=x_absolu;
+ passage[compteur_passage-1].Yabs=y_absolu;
+ passage[compteur_passage-1].teta_abs=teta+teta_actuelle_rad;
+
+
+
+
+ DEBUG_PRINTF("\n xabs= %d yabs= %d\n",x_absolu,y_absolu);
+
+ }
+
+ //wait(3);
+ d1=0;
+ d2=0;
+ }
+
+ coeff_directeur = (y2-y1)/(x2-x1);
+ //DEBUG_PRINTF("\nCOEFF DIRECTEUR = %d", coeff_directeur);
+ //if((old_coeff_directeur && coeff_directeur) == 1)
+ //DEBUG_PRINTF("\rBORD\r");
+
+ }
+
+ old_coeff_directeur = coeff_directeur;
+ old_distance = distance;
+
+
+ }
+ }
+
+
+ } //DEBUG_PRINTF(status_buff);
+
+ //flipper.detach(); // setup flipper to call flip after 2 seconds
+ break;
+
+ case WAIT_INCOMPLETE_FRAME: DEBUG_PRINTF("\nWAIT_INCOMPLETE_FRAME\n");
+ // attente d'une trame non pleine
+ sum=0;
+
+ for(j=0;j<(MS_center.size_of_incomplete_frame*2)+1;j=j+2)
+ {
+ while(serial.available()<2);
+ //DEBUG_PRINTF("%02d ",serial.available());
+ swap_distance[j]=serial.getc();
+ swap_distance[j+1]=serial.getc();
+ //DEBUG_PRINTF("%02d ",j);
+ if(j==(MS_center.size_of_incomplete_frame*2))
+ {
+ while(serial.available()<1);
+ //DEBUG_PRINTF("%02d ",j);
+ swap_distance[j+2]=serial.getc();
+ //DEBUG_PRINTF("sum=%02d calcul=%02d\n",(sum&0x3F)+0x30,(((swap_distance[j]))));
+ if((((sum&0x3F)+0x30)==(swap_distance[j]))&&(swap_distance[j+1]==10)&&(swap_distance[j+2]==10))
+ {
+ //DEBUG_PRINTF("i=%02d Good",i);
+ etat_connection=WAIT_INCOMPLETE_FRAME;
+ }
+ else
+ {
+ DEBUG_PRINTF("error\n");
+ etat_connection=BEFORE_END;
+ }
+ }
+ else
+ {
+ sum=sum+swap_distance[j]+swap_distance[j+1];
+ distance=(((unsigned short)(swap_distance[j]-0x30)<<6)&0xFC0)|(((unsigned short)(swap_distance[j+1]-0x30))&0x3F);
+ Angle=M_PI/2+(COEF_TETE_EN_BAS*(MS_center.debut+(j/2+i*32)*MS_center.count)-384)*DELTA_ANGLE;
+ /////////////////////////
+ if(distance<=20 || distance>=4000)
+ distance=4000;
+ /////////////////////////
+
+ cooX=(short)(cos(Angle)*distance);
+ cooY=(short)(sin(Angle)*distance);
+
+
+ /*msgTx.data[(compteur_global*2)%8]=(char)(cooX/10); // 40
+ msgTx.data[(compteur_global*2+1)%8]=(char)(cooY/10);// 40
+
+ compteur_global++;
+ if((((compteur_global)%4)==0)&&(compteur_global>=3))
+ {
+ can.write(msgTx);
+ //wait_ms(1);
+ }
+
+ */
+
+ if((old_distance > distance+DIST_DISCONT || old_distance+DIST_DISCONT<distance)&&(old_distance!=0))
+ {
+
+ if((old_distance+DIST_DISCONT<distance)&&(old_distance<RVISION) &&(d1==0))
+ {
+ d1=old_distance;
+ x1=(short)(old_distance*cos(Angle));
+ y1=(unsigned short)(old_distance*sin(Angle));
+ }
+
+ else if( (old_distance > (distance+DIST_DISCONT)) &&(distance<RVISION) &&(d2==0)&&(d1!=0) )
+ {
+ d2=distance;
+ x2=(short)(distance*cos(Angle));
+ y2=(unsigned short)(distance*sin(Angle));
+ }
+
+ if((d1!=0)&&(d2!=0))
+ {
+ // calcul de la distance entre 2 points avec la discontinuite de distance et pyhtagore
+ DIST= sqrt( (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) ) ; //1.220
+
+
+ if(DIST >= 400)
+ {
+ compteur_passage++;
+
+ DEBUG_PRINTF("\n\nPASSAGE numero %d:",compteur_passage);
+ DEBUG_PRINTF("\nLargeur: %d mm",DIST);
+ DEBUG_PRINTF("\nDistance: %d mm",(d1+d2)/2);
+ DEBUG_PRINTF("\nAngle: %lf ",Angle);
+ DEBUG_PRINTF("\nx2=%lf\tx1=%lf\ty2=%lf\ty1=%lf",x2,x1,y2,y1);
+
+ teta_actuelle_rad=teta_actuelle*M_PI/180;
+
+ Centre_X_pass = (x2+x1)/2;
+ Centre_Y_pass = (y2+y1)/2;
+
+ teta = atan2((double)Centre_X_pass,(double)Centre_Y_pass);
+
+
+ DEBUG_PRINTF("\nCentre passage en X = %d\t Centre passage en Y = %d , teta = %lf",Centre_X_pass,Centre_Y_pass,teta);
+
+ DEBUG_PRINTF("\nd1= %d , d2= %d , teta= %lf, teta_actuelle= %lf",d1,d2,teta,teta_actuelle_rad);
+ x_absolu = cooX_actuelle + ((d1+d2)/2)*cos(teta+teta_actuelle_rad);
+ y_absolu = cooY_actuelle + ((d1+d2)/2)*sin(teta+teta_actuelle_rad);
+ DEBUG_PRINTF("\n xabs= %d yabs= %d\n",x_absolu,y_absolu);
+
+ passage[compteur_passage-1].X=Centre_X_pass;
+ passage[compteur_passage-1].Y=Centre_Y_pass;
+ passage[compteur_passage-1].teta=teta;
+ passage[compteur_passage-1].Xabs=x_absolu;
+ passage[compteur_passage-1].Yabs=y_absolu;
+ passage[compteur_passage-1].teta_abs=teta+teta_actuelle_rad;
+
+
+ }
+
+ //wait(3);
+ d1=0;
+ d2=0;
+ }
+
+ coeff_directeur = (y2-y1)/(x2-x1);
+
+
+ }
+
+ old_coeff_directeur = coeff_directeur;
+ old_distance = distance;
+
+ }
+ }
+ DEBUG_PRINTF("Fin de tir\n");
+ myled[2] = !myled[2];
+
+ etat_connection=ENVOI_CAN_STRAT;
+ break;
+
+ /* case SEND_STATE:
+ compteur_passage =0;
+
+ diff_temps=temps-old_temps;
+ old_temps=temps;
+ msgTx.id=0x400;
+ msgTx.len=2;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.data[0]=(unsigned char)(diff_temps>>8); // x0
+ msgTx.data[1]=(unsigned char)(diff_temps); // suite de x0
+ can.write(msgTx);
+ if(flag_mbed_reset==1)
+ {
+ etat_connection=RESET;
+ }
+ else
+ {
+ if(MS_center.number_of_scan==0)
+ etat_connection=WAIT_ANSWER_BEGIN;
+ else
+ etat_connection=SEND_COMMAND;
+ }
+ break;
+ */
+
+ case DETECTION:
+
+ break;
+
+
+ case ENVOI_CAN_STRAT: for(i=0;i<9;i++){
+ if((abs(TETABALISE-passage[i].teta_abs))<(abs(TETABALISE-passage[i+1].teta_abs))){
+ x_absolu=passage[i].Xabs;
+ y_absolu=passage[i].Yabs;
+ }
+ }
+
+ msgTx.id=0x315;
+ msgTx.len=4;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.data[1] = (x_absolu);
+ msgTx.data[0] = (x_absolu>>8);
+ msgTx.data[3] = (y_absolu);
+ msgTx.data[2] = (y_absolu>>8);
+
+ compteur_passage =0;
+
+ can.write(msgTx);
+ if((can.read(msgRx))&&(msgRx.id==0x130)){
+ cooX_actuelle= msgRx.data[0]||msgRx.data[1]<<8;
+ cooY_actuelle= msgRx.data[2]||msgRx.data[3]<<8;
+ }
+ variable_interupt_strat = 0;
+ msgTx.id=0x999;
+ if(flag_mbed_reset==1)
+ {
+ etat_connection=RESET;
+ }
+ else
+ {
+ if(MS_center.number_of_scan==0)
+ etat_connection=WAIT_ANSWER_BEGIN;
+ else
+ etat_connection=SEND_COMMAND;
+ }
+ break;
+
+
+
+ case MS_ECHO: DEBUG_PRINTF("MS_echo\n");
+ while(serial.available()==0);
+ while (serial.available())
+ {
+
+ DEBUG_PRINTF("%c", serial.getc());
+
+ }
+ etat_connection=BEFORE_END;
+ break;
+
+ case BEFORE_END :
+ DEBUG_PRINTF("end\n");
+ etat_connection=END;
+ break;
+
+ case END : break;
+
+ case END2 : break;
+
+ case END3 :
+ break;
+
+ case RESET: serial.printf("RS\n");
+ while(serial.available())
+ serial.getc();
+ wait_ms(100);
+ while(serial.available())
+ serial.getc();
+ mbed_reset();
+
+ break;
+
+ default:
+ DEBUG_PRINTF("default\n\r");
+ break;
+
+ }
+ myled[1] = 0;
+ }
+
+}