//------------------------------------------------------------------------------
void* pic_main2(void *Args)
{
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
PIC_ARG *Simul=(PIC_ARG*)Args;
int comm_id=-1; // pour les communications avec le pc
char *_msg;
while(1)
{
//Communication avec le pc
if(comm_id<0)
connect_to_pc(Simul->my_id,Simul->my_nbr,&comm_id);
else
{
#ifdef SIMULATION
if(query_webcam != -1)
{
int size = Simul->robot->webcams[query_webcam].getPicture(&pixels);
if(size != -1)
{
char f[100];
sprintf(f, "webcam%d.dat", query_webcam);
FILE *file = fopen(f, "wb+");
fwrite(&Simul->robot->webcams[query_webcam].W, sizeof(int), 1, file);
fwrite(&Simul->robot->webcams[query_webcam].H, sizeof(int), 1, file);
fwrite(pixels, size, 1, file);
fclose(file);
free(pixels);
int len = sizeof(MSG_INT1_t) + strlen(f) + 1;
char *buffer = (char*) malloc(len);
MSG_INT1_t *msg = (MSG_INT1_t*)buffer;
char *data = &buffer[sizeof(MSG_INT1_t)];
msg->type = MSG_WEBCAM;
msg->msg_id = msg_webcam;
msg->value = strlen(f) + 1;
strcpy(data, f);
if(write_usb(comm_id,buffer,len)<0)
close_connection(&comm_id);
else
query_webcam=-1;
free(buffer);
}
}
#endif
int type = read_usb(SIMULARG2(comm_id, &_msg));
if(type<0)
{
close_connection(&comm_id);
}
else switch(type)
{
case MSG_EMPTY: break;
#ifdef SIMULATION
case MSG_POS_INFO:
{
picInfo_t *msg = (picInfo_t*)_msg;
Simul->robot->position.x=msg->posX;
Simul->robot->position.y=msg->posY;
Simul->robot->angle=msg->posA;
}
break;
case MSG_INFO:
{
picInfo_t *msg = (picInfo_t*)_msg;
Simul->robot->data[Simul->my_nbr].destX=msg->destX;
Simul->robot->data[Simul->my_nbr].destY=msg->destY;
Simul->robot->data[Simul->my_nbr].destA=msg->destA;
Simul->robot->data[Simul->my_nbr].posX=msg->posX;
Simul->robot->data[Simul->my_nbr].posY=msg->posY;
Simul->robot->data[Simul->my_nbr].angle=msg->posA;
}
break;
case MSG_WEBCAM_QUERY:
{
MSG_INT1_t *msg = (MSG_INT1_t *)_msg;
query_webcam = msg->value;
msg_webcam = msg->msg_id;
}
break;
#endif
case MSG_QUERY:
{
MSG_INT1_t *msg = (MSG_INT1_t *)_msg;
switch(msg->value)
{
case MSG_DIGIT:
{
MSG_INT1_t answer;
answer.type = MSG_DIGIT;
answer.msg_id = msg->msg_id;
answer.value = get_input2(SIMULARG3(Simul,MSG_DIGIT,0));
if(write_usb(comm_id,&answer,sizeof(MSG_INT1_t))<0)
close_connection(&comm_id);
}
break;
case MSG_ANALOG:
{
MSG_INT2_t *msg = (MSG_INT2_t *)_msg;
MSG_INT1_t answer;
answer.type = MSG_ANALOG;
answer.msg_id = msg->msg_id;
answer.value = get_input2(SIMULARG3(Simul,MSG_ANALOG,msg->value2));
if(write_usb(comm_id,&answer,sizeof(MSG_INT1_t))<0)
close_connection(&comm_id);
}
break;
/* case DIST:
{
MSG_INTn_t *msg2;
int *values;
char *buff = (char*) malloc(sizeof(MSG_INTn_t) + sizeof(int)*4);
msg2 = (MSG_INTn_t*) buff;
values = (int*) &buff[sizeof(MSG_INTn_t)];
for(int i=0;i<4;i++)
values[i] = get_input2(SIMULARG3(Simul,DIST,(i==1?2:(i==2?1:i)) ));
msg2->type = DIST;
msg2->msg_id = msg_id--;
msg2->n = 4;
if(write_usb(SIMULARG3(comm_id,buff,sizeof(MSG_INTn_t) + sizeof(int)*4))<0)
close_connection(&comm_id);
free(buff);
}
break;*/
default:
printf("<pic_main2.c> PIC2 Unknown MSG_QUERY type %d.\n",type);
fflush(stdout);
break;
}
}
break;
case MSG_DCMOTOR:
case MSG_SERVOMOTOR:
{
MSG_INT2_t *msg = (MSG_INT2_t *)_msg;
set_output2(SIMULARG4(Simul,type,msg->value1,msg->value2));
}
break;
default:
printf("<pic_main2.c> Unknown message type %d.\n",type);
fflush(stdout);
break;
}
if(type>0)
free(_msg);
}
pthread_barrier_wait(Simul->barrier);
pthread_barrier_wait(Simul->barrier);
if(*Simul->STOP)
{
*Simul->alive_thread = *Simul->alive_thread-1;
break;
}
}
free(Args);
return NULL;
}
/*----------------------------------------------------------------------------*/
u8 io_read_usb(u8 _xdata *buf, u16 lbah, u16 lbal)
{
return read_usb(lbal, lbah, buf);
}
//------------------------------------------------------------------------------
void* pic2_handle_msg(void *)
{
if(!WANT_PIC2)
while(true) usleep(100000);
char *_msg;
while(true)
{
if(connect_usb(PIC2)<0)
{
fprintf(stderr,"Connection failed PIC2, retrying...\n");
sleep(1);
continue;
}
picBegin();
if(callbackOnReconnectPIC2)
callbackOnReconnectPIC2();
fprintf(stderr,"Listen to PIC2...\n"); fflush(stdout);
while((_msg=read_usb(PIC2)))
{
if(_msg==NULL)
break;
//printf("Recv PIC2: %d\n",get_msg_type(_msg));
switch(get_msg_type(_msg))
{
case EMPTY_MSG:
#ifdef SIMULATION
fprintf(stderr,"<picInterface.cpp> PIC2: Connexion réinitialisée.\n");
fflush(stdout);
if(callbackRecvReset)
callbackRecvReset();
#endif
break;
#ifdef SIMULATION
case WEBCAM:
{
MSG_INT1_t *msg = (MSG_INT1_t *)_msg;
int size = msg->value;
int *ptr = &msg->value;
ptr++;
*((char**)webcam_data) = new char[size];
memcpy(*((char**)webcam_data),ptr,size);
pthread_mutex_unlock(mut_webcam);
mut_webcam = NULL;
}
break;
#endif
case BEGIN:
break;
case JACK:
{
callbackOnJack();
#ifdef SIMULATION
int buf = JACK;
if(write_usb(PIC2, &buf, sizeof(int))<0) break;
#else
unsigned char buf = JACK;
if(write_usb(PIC2, &buf, sizeof(buf))<0) break;
#endif
}
break;
case PRINTF:
{
MSG_INT1_t *msg = (MSG_INT1_t *)_msg;
printf("from pic %d: %d\n",2,msg->value);
}
break;
case VPOS:
if(mut_vpos)
{
if(true /*msg->msg_id == msg_id_vpos*/)
{
msg_id_vpos = 0;
pthread_mutex_unlock(mut_vpos);
mut_vpos = NULL;
}
}
else
fprintf(stderr,"<picInterface.cpp> PIC2: Unmatched ACK VPOS.\n");
fflush(stdout);
break;
case HPOS:
if(mut_hpos)
{
if(true /*msg->msg_id == msg_id_hpos*/)
{
msg_id_hpos = 0;
pthread_mutex_unlock(mut_hpos);
mut_hpos = NULL;
}
}
else
fprintf(stderr,"<picInterface.cpp> PIC2: Unmatched ACK HPOS.\n");
fflush(stdout);
break;
case CLAMP:
/*if(mut_clamp)
{
if(msg->msg_id == msg_id_clamp)
{
msg_id_clamp = 0;
pthread_mutex_unlock(mut_clamp);
mut_clamp = NULL;
}
}
else
printf("<picInterface.cpp> PIC2: Unmatched ACK CLAMP.\n");
fflush(stdout); */
break;
case DIST:
if(callbackRecvDistCaptors)
{
MSG_INTn_t *msg = (MSG_INTn_t *)_msg;
float *tab;
if(msg->n == 0)
tab = NULL;
else
tab = new float[msg->n];
int *values = &msg->n;
values++;
for(int i=0; i<msg->n; i++)
{
tab[i] = COEFF_A_DIST * pow(COEFF_B_DIST, ((double)(values[i]>>6)));
if(tab[i]<0.) tab[i] = 0.;
}
#ifdef SIMULATION
float tmp = tab[1];
values[1] = tab[2];
values[2] = tmp;
#endif
callbackRecvDistCaptors(msg->n, tab);
if(tab != NULL)
delete tab;
}
break;
case COUL:
if(callbackRecvCoulCaptors)
{
MSG_INTn_t *msg = (MSG_INTn_t *)_msg;
float *tab = new float[msg->n];
int *values = &msg->n;
values++;
for(int i=0; i<msg->n; i++)
tab[i] = ((float)values[i]);
callbackRecvCoulCaptors(msg->n, tab);
delete tab;
}
break;
default:
fprintf(stderr,"<picInterface.cpp> PIC2: Unknown message type %d.\n",get_msg_type(_msg));
fflush(stdout);
break;
}
delete _msg;
}
fprintf(stderr,"Connection closed PIC2...\n");
fflush(stdout);
usleep(10000);
break;
}
return NULL;
}
//------------------------------------------------------------------------------
void* pic1_handle_msg(void *)
{
if(!WANT_PIC1)
while(true) usleep(100000);
char *_msg;
while(true)
{
if(connect_usb(PIC1)<0)
{
fprintf(stderr,"Connection failed PIC1, retrying...\n");
sleep(1);
continue;
}
if(callbackOnReconnectPIC1)
callbackOnReconnectPIC1();
fprintf(stderr,"Listen to PIC1...\n"); fflush(stdout);
while((_msg=read_usb(PIC1)))
{
if(_msg==NULL)
break;
//printf("Recv PIC1: %d\n",get_msg_type(_msg));
switch(get_msg_type(_msg))
{
case EMPTY_MSG:
#ifdef SIMULATION
fprintf(stderr,"<picInterface.cpp> PIC1: Connexion réinitialisée.\n");
fflush(stdout);
if(callbackRecvReset)
callbackRecvReset();
#endif
break;
case CODER:
#ifdef SIMULATION
picSendInfo();
#endif
if(callbackRecvCoder)
{
MSG_INT2_t *msg = (MSG_INT2_t *)_msg;
#ifdef SIMULATION
callbackRecvCoder(msg->value1, msg->value2);
#else
callbackRecvCoder(msg->value2, -msg->value1);
#endif
}
break;
case PRINTF:
{
MSG_INT1_t *msg = (MSG_INT1_t *)_msg;
printf("from pic %d: %d\n",1,msg->value);
}
break;
default:
fprintf(stderr,"<picInterface.cpp> PIC1: Unknown message type %d.\n",get_msg_type(_msg));
fflush(stdout);
break;
}
delete _msg;
}
fprintf(stderr,"Connection closed PIC1...\n");
fflush(stdout);
usleep(10000);
break;
}
return NULL;
}
int main(int argc, char **argv)/*the main function*/
{
/** Initialize ROS node.
* It must be declared in the begining of the program because of TF Listener
*This specifies the name of the node **/
ros::init(argc, argv, "telosb_node");
//create a node handle
ros::NodeHandle nh ("~");
std::string port;//"/dev/telosb";
nh.getParam("port",port);
printf ("Starting TelosB Node ... \n");
//char* device = "/dev/ttyUSB0";
//char* device = "/dev/telosb";
char *device = (char *) malloc(100);;
printf ("set device name ... \n");
strcpy(device,port.c_str());
//printf ("cout ... \n");
std::cout <<device<<std::endl;
if (device == NULL )
device = "/dev/ttyUSB0";
printf("attempting to open %s", device);
int baudrate = 115200;
printf ("open USB port ... \n");
int b = open_usb(device, baudrate);
//first, discard any previous data
flush_usb();
if (b==1)
printf ("USB Port Open \n");
if (b==-1)
printf ("USB Port NOT Open \n");
int r, i, j;
BYTE buffer[BUFSIZE];
BYTE read_buffer[BUFSIZE];
int received_bytes;
int count =0;
//must be defined after opening the USB port!!
ros::Publisher telosb_publisher= nh.advertise<telosb::TelosBMsg>("telosb_topic",1000);
while (ros::ok())
{
//fflush(stdout);
//printf ("before read_usd \n");
read_usb(read_buffer, BUFSIZE, &r);
//printf ("after read_usd \n");
//printf("r=%d\n ",r);
printf("count=%d\n ",count);
//printf("%s\n",buffer);
//printf("current index = %d \n ", current_index);
if(r!=0){
//fflush(stdout);
int offset=count;
count = count+r;
int i=0;
//printf ("before for ( i = 0; i<r; i++) %d, offset = %d\n", r, offset);
for ( i = 0; i<r; i++){
buffer[i+offset]=read_buffer[i];
}
//printf ("after for ( i = 0; i<r; i++) \n");
//printf ("before if (count==12) \n");
if (count==12){
count = 0;
received_bytes=0;//re-init received_bytes
//fflush(stdout);
printf ("before id = \n");
int id = (buffer[2]-'0')+((buffer[1]-'0')*10)+(buffer[0]-'0')*100;
int light = (buffer[5]-'0')+((buffer[4]-'0')*10)+(buffer[3]-'0')*100;
int temp = (buffer[8]-'0')+((buffer[7]-'0')*10)+(buffer[6]-'0')*100;
int humidity = (buffer[11]-'0')+((buffer[10]-'0')*10)+(buffer[9]-'0')*100;
//printf ("r=%d \n", r);
//printf ("Sensor reading \n");
printf ("before Sensor ID = \n");
printf ("Sensor ID : %d \n", id);
printf ("Light : %d Lux\n", light);
printf ("Temperature: %d °C\n", temp);
printf ("Humidity : %d %%\n\n", humidity);
//printf("%d\n ",i);
telosb::TelosBMsg telosb_msg;
telosb_msg.sensorID=id;
telosb_msg.temperature=temp;
telosb_msg.humidity=humidity;
telosb_msg.light=light;
ROS_INFO("\n[ID = %d]: temperature: %d C, light: %d Lux, Humidity: %d \n",
telosb_msg.sensorID, telosb_msg.temperature, telosb_msg.light, telosb_msg.humidity);
telosb_publisher.publish(telosb_msg);
ros::spinOnce();
//loop_rate.sleep();
}
}
}
printf (" read usb completed. \n");
return 1;
}