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->listen(wsJoints, config->main.wsjoints_port);
00011 wsPIDs=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048);
00012 wireless->listen(wsPIDs, config->main.wspids_port);
00013 }
00014
00015 void
00016 WorldStateSerializer::serialize() {
00017 char *buf=(char*)wsPIDs->getWriteBuffer((NumPIDJoints*3+1)*4);
00018 if (buf) {
00019 encode(&buf,state->lastSensorUpdateTime);
00020 encode(&buf,(char*)state->pids,NumPIDJoints*3*4);
00021 wsPIDs->write((NumPIDJoints*3+1)*4);
00022 }
00023
00024 buf=(char*)wsJoints->getWriteBuffer((NumPIDJoints*2+15)*4);
00025 if (buf) {
00026 encode(&buf,state->lastSensorUpdateTime);
00027 encode(&buf,(char*)&state->outputs[PIDJointOffset], NumPIDJoints*4);
00028 encode(&buf,(char*)state->sensors,6*4);
00029 encode(&buf,(char*)state->buttons,8*4);
00030 encode(&buf,(char*)state->pidduties,NumPIDJoints*4);
00031 wsJoints->write((NumPIDJoints*2+15)*4);
00032 }
00033 }