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.
main.cpp
- Committer:
- dylancachan
- Date:
- 2015-04-15
- Revision:
- 0:6ce9c65992e5
File content as of revision 0:6ce9c65992e5:
#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;
}
}