00001 #include "WorldState.h"
00002 #include "WorldStateSerializer.h"
00003 #include "Shared/Config.h"
00004 #include "Shared/get_time.h"
00005 #include <string.h>
00006 #include "RobotInfo.h"
00007
00008 WorldStateSerializer::WorldStateSerializer() : wsJoints(NULL), wsPIDs(NULL) {
00009 wsJoints=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048);
00010 wireless->setDaemon(wsJoints);
00011 wireless->listen(wsJoints, config->main.wsjoints_port);
00012 wsPIDs=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048);
00013 wireless->setDaemon(wsPIDs);
00014 wireless->listen(wsPIDs, config->main.wspids_port);
00015 }
00016
00017 void
00018 WorldStateSerializer::serialize() {
00019 char *buf=(char*)wsPIDs->getWriteBuffer((NumPIDJoints*3+1)*4);
00020 if (buf) {
00021 encode(&buf,state->lastSensorUpdateTime);
00022 encode(&buf,(char*)state->pids,NumPIDJoints*3*4);
00023 wsPIDs->write((NumPIDJoints*3+1)*4);
00024 }
00025
00026 buf=(char*)wsJoints->getWriteBuffer((NumPIDJoints*2+15)*4);
00027 if (buf) {
00028 encode(&buf,state->lastSensorUpdateTime);
00029 encode(&buf,(char*)&state->outputs[PIDJointOffset], NumPIDJoints*4);
00030 encode(&buf,(char*)state->sensors,6*4);
00031 encode(&buf,(char*)state->buttons,8*4);
00032 encode(&buf,(char*)state->pidduties,NumPIDJoints*4);
00033 wsJoints->write((NumPIDJoints*2+15)*4);
00034 }
00035 }
