bool CNearbySession::readRequest(SOCKET sessionSocket, std::vector<uint8_t>& buffer, uint32_t& sequence,
std::string& remoteDevice, std::string& remoteEndpoint) {
bool sessionLoop = true;
struct sockaddr_in cliAddr;
socklen_t cliLen = sizeof(cliAddr);
size_t dataSize = buffer.size();
buffer.resize(dataSize + chunkSize);
if (int read = recvfrom(sessionSocket, (char *) &buffer[dataSize], chunkSize, 0,
(struct sockaddr *) &cliAddr, &cliLen)) {
buffer.resize(dataSize + read);
CNearbyMessage message;
while (auto readMsg = message.parse(buffer)) {
buffer.erase(std::begin(buffer), std::next(std::begin(buffer), readMsg)); //remove message from buffer
if (message.wantAck()) {
sendAnswer(sessionSocket, message.buildAck());
}
if (message.isRequest()) {
std::string local;
//TODO check if local is correct
std::vector<uint8_t> requestPayload;
message.getRequestPayload(remoteDevice, remoteEndpoint, local, requestPayload);
std::vector<uint8_t> acceptPayload;
if(nearbyServer->sessionRequest(sessionSocket, remoteDevice, remoteEndpoint, requestPayload, acceptPayload)) {
sendAnswer(sessionSocket, message.buildAccept(sequence++, acceptPayload));
}
} else if (message.isMessage()) {
std::vector<uint8_t> payload;
message.getMessagePayload(payload);
nearbyServer->sessionMessage(sessionSocket, payload, message.isReliable());
} else if (message.isQuit()) {
nearbyServer->sessionDisconnect(sessionSocket);
sessionLoop = false;
}
}
}
return sessionLoop;
}
void TWorker::work(int socket)
{
std::string request;
std::string fileName;
// setNonBlock(socket);
if( readRequest(socket, request) ) {
#if defined(DEBUG)
std::cout << "\n\nReqest:\n" << request;
#endif
std::string answer;
if( parseRequest(request, fileName) ) {
std::string context;
if( getContext(fileName, context) ) {
answer = createHeader(200, context.size() ) + context;
}
else {
answer = createHeader(404);
}
}
else {
answer = createHeader(500);
}
#if defined(DEBUG)
std::cout << "\n\nAnswer:\n" << answer;
#endif
sendAnswer(socket, answer);
}
disconnect(socket);
}
int solver(int *map, int x1, int y1, int x2, int y2, int *original_stones, int n)
{
int i;
dump(map, x1, y1, x2, y2, original_stones, n);
// Prepare
Stone stones[256];
stoneEncode(stones, original_stones, n);
for (i=0; i<1024; i++) {
int tmp = map[i];
map[i] = (tmp == 0) ? -1 : -2;
}
// Search
Score best;
best.score = 1024;
for (i=0; i<10; i++) {
putWrapper(&best, stones, n, map, x1, y1, x2, y2, original_stones);
}
// Best
printf("Best Score: %d, Zk: %d\n", best.score, best.zk);
dumpMap2(best.map);
// Send
sendMsg("S");
sendAnswer(best.map, stones, original_stones, n);
if (sendMsg("E") == EXIT_FAILURE) return EXIT_SUCCESS;
return EXIT_FAILURE;
}
void attendClient(int sock)
{
MESSAGE msg;
while(-1 != waitMessage(sock, &msg))
{
switch(msg.type)
{
case GCD: sendAnswer(sock,gcd(msg.param1,msg.param2));
break;
case LCM: sendAnswer(sock,lcm(msg.param1,msg.param2));
break;
}
}
close(sock);
}
void getValuesFromGyroscope(void)
{
writeFloatToBuffer(gyroscope_getX(), 0);
writeFloatToBuffer(gyroscope_getY(), 4);
writeFloatToBuffer(gyroscope_getZ(), 8);
sendAnswer(12);
}
void getValuesFromMagnetometer(void)
{
writeFloatToBuffer(compass_getX(), 0);
writeFloatToBuffer(compass_getY(), 4);
writeFloatToBuffer(compass_getZ(), 8);
sendAnswer(12);
}
void getValuesFromAccelerometer(void)
{
writeFloatToBuffer(accelerometer_getX(), 0);
writeFloatToBuffer(accelerometer_getY(), 4);
writeFloatToBuffer(accelerometer_getZ(), 8);
sendAnswer(12);
}
void * consumerFunction(void * arg) {
uint32_t value;
PMESSAGE pMsg;
PFIFO pFifo = (PFIFO) arg;
while(!get(pFifo, &pMsg)) {
switch(pMsg->type) {
case GCD: value = gcd(pMsg->param1, pMsg->param2);
break;
case LCM: value = lcm(pMsg->param1, pMsg->param2);
break;
}
if (-1 == sendAnswer(sock,to,value)) return -1;
}
return NULL;
}
void inicializar(nodoNivel*raiz,global*general){
int estado;
answer tempo;
FD_SET(raiz->nid,general->original->original);
if(raiz->nid>*(general->maxfd))*(general->maxfd)=raiz->nid;
do{
if(!mpantalla)puts("\nPidiendo algoritmo.");
sendAnswer(6,0,' ',' ',(short)raiz->nid);
enviarLog(raiz->nid,*general,6,0,' ',' ');
estado=recvAnswer(&tempo,(short)raiz->nid);
recibirLog(*general,raiz->nid,tempo);
if(estado!=6) puts("El nivel flasheo cualquiera!");
}while(estado!=6);
modificarAlgoritmo(tempo,*general);
do{
if(!mpantalla)puts("\nPidiendo retardo entre turnos.");
sendAnswer(4,0,' ',' ',(short)raiz->nid);
enviarLog(raiz->nid,*general,4,0,' ',' ');
estado=recvAnswer(&tempo,(short)raiz->nid);
recibirLog(*general,raiz->nid,tempo);
if(estado!=4) puts("El nivel flasheo cualquiera!");
}while(estado!=4);
modificarRetardo(tempo,*general);
}
bool CNearbySession::doWriteQueue(SOCKET sessionSocket, uint32_t& sequence) {
CNearbyMessage message;
std::vector<uint8_t> payload;
{
std::lock_guard<std::mutex> lock(messageMutex);
if(!messageQueue.empty()){
payload = std::move(messageQueue.front());
messageQueue.erase(messageQueue.begin());
}
}
if(!payload.empty()) {
sendAnswer(sessionSocket, message.buildMessage(sequence++, payload));
}
}
void UDPServer::processPendingDatagrams()
{
pQuery pq;
QHostAddress sFromAddress;
DEBUG_MSG_INFO(QString("has datagrams..."), CMsg::eMT_Info1);
while (m_pobjUDPInSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(m_pobjUDPInSocket->pendingDatagramSize());
m_pobjUDPInSocket->readDatagram(datagram.data(), datagram.size(), &sFromAddress);
pq.setDatagram(datagram.data());
QStringList lstr;
if(pq.verifyChecksum()) {
lstr.clear();
lstr << QString("query | %1 | %2").arg(sFromAddress.toString(), 15).arg(pq.print());
DEBUG_MSG_INFO(lstr, CMsg::eMT_Info1);
pAnswer pa;
pa.setID(pq.getID());
pa.setDateTime(QDateTime::currentDateTime());
pa.setSID( m_usSId );
if( !getAnswer(pa, pq.getType(), pq.getIDC(), sFromAddress.toString() ) ) {
sendAnswer( pa, sFromAddress, pq.getIDC() );
lstr.erase(lstr.begin(), lstr.end());
lstr << QString("answer | %1 | %2").arg(sFromAddress.toString(), 15).arg(pa.print());
DEBUG_MSG_INFO(lstr, CMsg::eMT_Info1);
if(pa.getVariableCount())
DEBUG_MSG_INFO(pa.print_vars(), CMsg::eMT_Info1);
}
}
else {
DEBUG_MSG_INFO(QString().sprintf("received corrupted datagram from %s\n", sFromAddress.toString().toAscii().data()), CMsg::eMT_Info1);
}
}
}
void putWrapper(Score *best, Stone *stones, int n, const int *map, int x1, int y1, int x2, int y2, int *original_stones)
{
// Score
int i;
Stone sorted[256];
static int counter = 0;
switch(counter++) {
case 0:
for (i=0; i<n; i++) stones[i].score = ((double)(n - i) / (double)n);
break;
case 1:
for (i=0; i<n; i++) stones[i].score = (((double)(n - i) / (double)n) * (double)stones[i].len) * 0.0625;
break;
case 2:
for (i=0; i<n; i++) stones[i].score = ((double)(n - i) / (double)n) * -1;
break;
case 3:
for (i=0; i<n; i++) stones[i].score = (((double)(n - i) / (double)n) * (double)stones[i].len) * -0.0625;
break;
default:
srand((unsigned)time(NULL));
for (i=0; i<n; i++) stones[i].score = rand();
}
memcpy(sorted, stones, sizeof(Stone) * 256);
qsort(sorted, n, sizeof(Stone), sortByScore);
// Put
int tmpmap[1024];
for (i=0; i<n; i++) {
memcpy(tmpmap, map, sizeof(int) * 1024);
putStone(sorted, sorted[i].id, n, tmpmap, x1, y1, x2, y2);
if (bestScore(best, tmpmap)) {
printf("Update best score: (%d, %d)\n", best->score, best->zk);
sendMsg("S");
sendAnswer(best->map, stones, original_stones, n);
if (sendMsg("E") == EXIT_FAILURE) return;
}
}
}
void sendUnsignedShort(unsigned short value)
{
answerBuffer[0] = (unsigned char)(value / 256);
answerBuffer[1] = (unsigned char)(value % 256);
sendAnswer(2);
}
void sendOK(void)
{
answerBuffer[0] = 0;
sendAnswer(1);
}
char execCommand(InPackStruct* cmd) //обработать входящую команду
{
switch(cmd->command)
{
case 0x01: //Эхо
{
char *key= cmd->param;
if ((key[0] =='E')&&(key[1] =='C')&&(key[2] =='H')&&(key[3] =='O') )
{
char * str ="mobile robot V1.0";
sendAnswer(cmd->command, str, strlen(str)+1);
}
}
break;
case 0x02: //Установить текущие координаты
{
float *(temp) ={(float*)cmd->param};
robotCoord[0]= temp[0];
robotCoord[1]= temp[1];
robotCoord[2]= temp[2];
points[0].center[0]= temp[0];
points[0].center[1]= temp[1];
points[0].center[2]= temp[2];
CreatePath(&points[0], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x03: //установить скважность шим
{
char ch = *cmd->param;
float temp =*((float*)(cmd->param + 1));
setPWM( ch - 1, temp);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x04: //Установить бит направления
{
char * ch = cmd->param;
set_pin(PWM_DIR[(*ch)-1]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x05: //Снять бит направления
{
char * ch = cmd->param;
reset_pin(PWM_DIR[(*ch)-1]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x06: //Установить напряжение на двигателе
{
char ch = *cmd->param;
float duty = *((float*)(cmd->param + 1));
uint16_t dir = *((uint16_t*)(cmd->param + 2));
setVoltageMaxon( ch-1, dir, duty);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x08: //Установить параметры регулятора
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i<=3; i++)
{
wheelsPidStruct[i].p_k = temp[0];
wheelsPidStruct[i].i_k = temp[1];
wheelsPidStruct[i].d_k = temp[2];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x09: //Установить требуюему скорость двигателей
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i <= 3; i++)
{
regulatorOut[i] = temp[i];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0B: //Включить рассчет кинематики
{
curState.kinemEn=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0C: //Выключить рассчет кинематики
{
curState.kinemEn = 0;
char * str = "Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0D: //Задать скорости движения
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i<=2; i++)
{
vTargetGlob[i] = temp[i];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0E: //Включить траекторный регулятор
{
curState.trackEn=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0F: //Выключить траекторный регулятор
{
curState.trackEn=0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x10: //Очистить очередь точек
{
while(lastPoint>0) removePoint(&points[0],&lastPoint);
points[0].center[0]= robotCoord[0];
points[0].center[1]= robotCoord[1];
points[0].center[2]= robotCoord[2];
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x11: //Добавить точку в очередь
{
float *(temp) ={(float*)(cmd->param)};
char * ch = cmd->param + 12;
lastPoint++;
points[lastPoint].center[0] = temp[0];
points[lastPoint].center[1] = temp[1];
points[lastPoint].center[2] = temp[2];
points[lastPoint].speedVelTipe = speedType[*ch];
points[lastPoint].speedRotTipe = rotType[*ch];
points[lastPoint].endTask = NULL;
points[lastPoint].movTask = NULL;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x12: //Состояние очереди точек
{
char outdata[15];
float * temp =(float*)(&outdata[3]);
char * cntPoint = (&outdata[0]);
uint16_t * curPoint = (uint16_t *)(&outdata[1]);
*cntPoint= lastPoint;
*curPoint = totalPointComplite;
temp[0]= points[0].center[0];
temp[1]= points[0].center[1];
temp[2]= points[0].center[2];
//char * str ="Ok";
sendAnswer(cmd->command,outdata, 15);
}
break;
case 0x13: //отправить текущие координаты
{
sendAnswer(cmd->command,(char *)robotCoord, sizeof(robotCoord));
}
break;
case 0x14: //отправить текущую скорость
{
sendAnswer(cmd->command,(char *)robotSpeed, sizeof(robotCoord));
}
break;
case 0x15: //Задать скорость движения
{
float *(temp) = {(float*)(cmd->param)};
char i;
for (i = 0; i<=4; i++)
normalVelFast[i]= temp[i];
for (i = 0; i<=4; i++)
stopVelFast[i]= temp[i];
stopVelFast[2]=-0.2;
char * str = "Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x16: //Установить режим ножки
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
pinType[ch] = *((char *)(cmd->param +1));
if (pinType[ch] == ADC_ANALOG_PIN )
conf_pin(GENERAL_PIN[ch], ANALOG, PUSH_PULL, FAST_S, NO_PULL_UP);
if (pinType[ch] == ADC_DIG_INPUT )
conf_pin(GENERAL_PIN[ch], INPUT, PUSH_PULL, FAST_S, NO_PULL_UP);
if (pinType[ch] == ADC_DIG_OUTPUT )
conf_pin(GENERAL_PIN[ch], GENERAL, PUSH_PULL, FAST_S, NO_PULL_UP);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x17: //отправить состояние выбранного входа АЦП
{
char ch = (*((char *)(cmd->param))) - 1;
if (ch < 10)
sendAnswer(cmd->command,(char *)&(adcData[ch]), sizeof(uint16_t));
}
break;
case 0x18: //отправить состояние всех АЦП
{
sendAnswer(cmd->command,(char *)adcData, sizeof(adcData));
}
break;
case 0x19: //отправить состояние входа
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
char temp = (pin_val(GENERAL_PIN[ch])!=0);
sendAnswer(cmd->command,&temp, sizeof(temp));
}
}
break;
case 0x1A: //отправить состояние всех входов
{
char temp[10];
char i ;
for ( i = 0; i<10; i++) temp[i] = (pin_val(GENERAL_PIN[i])!=0);
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x1B: //установить состояние выхода
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
if (*(cmd->param+1)==0) reset_pin(GENERAL_PIN[ch]); else
set_pin(GENERAL_PIN[ch]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x1C: //отправить текущий режим ножки
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
sendAnswer(cmd->command,(char *) &(pinType[ch]), sizeof(uint8_t));
}
break;
case 0x1D: //установить режим ножки EXTI
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
extiType[ch] = (*((char *)(cmd->param +1)));
if (extiType[ch] == EXTI_BOTH )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_BOTH_EDGES);
}
if (extiType[ch] == EXTI_RISE )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_RISING_EDGE);
}
if (extiType[ch] == EXTI_FALL )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_FALLING_EDGE) ;
}
if (extiType[ch] == EXTI_DIG_INPUT )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, NO_PULL_UP);
clear_ext_interrupt(EXTI_PIN[ch]) ;
}
if (extiType[ch] == EXTI_DIG_OUTPUT )
{
conf_pin(EXTI_PIN[ch], GENERAL, PUSH_PULL, FAST_S, NO_PULL_UP);
clear_ext_interrupt(EXTI_PIN[ch]) ;
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x1E: //отправить состояние входа
{
char ch = *((char *)(cmd->param))-1;
if ((ch)<10)
{
char temp = ch;
if (pin_val(EXTI_PIN[ch])) temp |=0x80;
sendAnswer(cmd->command,&temp, sizeof(temp));
}
}
break;
case 0x1F: //отправить состояние всех входов
{
char temp[10];
char i ;
for ( i = 0; i<10; i++) temp[i] = (pin_val(EXTI_PIN[i])!=0);
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x20: //установить состояние выхода
{
char ch = *((char *)(cmd->param))-1;
if (ch<10)
if (*(cmd->param+1)==0) reset_pin(EXTI_PIN[ch]); else
set_pin(EXTI_PIN[ch]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x21: //отправить текущий режим ножки
{
char ch = *((char *)(cmd->param))-1;
if (ch<10)
sendAnswer(cmd->command,(char *) &(extiType[ch]), sizeof(uint8_t));
}
break;
case 0x22: //установить состояние выхода +12В
{
char ch = (*((char *)(cmd->param)))-1;
if (ch<6)
{
if (*(cmd->param+1)==0)
reset_pin(V12_PIN[ch]); else
set_pin(V12_PIN[ch]);
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x23: //Выключить ПИД регуляторы приводов
{
curState.pidEnabled=0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x24: //Включить ПИД регуляторы приводов
{
curState.pidEnabled=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x25: //set current coordinate
{
float *(temp) ={(float*)cmd->param};
robotCoord[0]= temp[0];
robotCoord[1]= temp[1];
robotCoord[2]= temp[2];
addPointInFrontOfQueue(&points[0], &temp[0], (char) 4, &lastPoint);
CreatePath(&points[1], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
case 0x26: //set dynamixel angle
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(angle) ={(uint16_t*)(cmd->param + 1)};
if (setServoMovingSpeed(ID, angle))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x27: //set CW angle limit
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(limit) ={(uint16_t*)(cmd->param + 1)};
if (setServoCWAngleLimit(ID, limit))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x28: //set CCW angle limit
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(limit) ={(uint16_t*)(cmd->param + 1)};
if (setServoCCWAngleLimit(ID, limit))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x29: //set servo moving speed
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(speed) ={(uint16_t*)(cmd->param + 1)};
uint16_t *(direction) ={(uint16_t*)(cmd->param + 3)};
if (setServoMovingSpeed(ID, speed, direction))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x2A: //add a point to the beginning of Queue
{
float *(temp) = (float*)(cmd->param);
char * ch = cmd->param + 12;
addPointInFrontOfQueue(&points[0], &temp[0], &ch, &lastPoint);
CreatePath(&points[1], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x2B: //Open Cubes Catcher
{
openCubesCatcher();
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x2C: //Close Cubes Catcher
{
uint8_t numberOfCubesCatched;
closeCubesCatcher(&numberOfCubesCatched);
sendAnswer(cmd->command, (char *)&numberOfCubesCatched, sizeof(uint8_t));
}
break;
case 0x2D: // Open wall
{
openWall();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x2E: // Close wall
{
closeWall();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x2F: // Switch On the vibration
{
uint8_t temp ={*(uint8_t*)(cmd->param)};
switchOnVibration(temp);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x30: // Switch Off the vibration
{
switchOffVibration();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x31: // Set angle of cubes catcher
{
float *(temp) = (float*)(cmd->param);
cubesCatcherPID.target = *temp;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x32: // Flag of reached point
{
sendAnswer(cmd->command, (char *)&traceFlag, sizeof(traceFlag));
}
break;
case 0x33: // Switch on collision avoidance
{
curState.collisionAvoidance = 1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x34: // Switch off collision avoidance
{
curState.collisionAvoidance = 0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x35: // Get Cubes Catcher angle
{
sendAnswer(cmd->command, (char *)&cubesCatcherPID.current, sizeof(cubesCatcherPID.current));
}
break;
case 0x36: // Get IR Distances
{
float temp[4];
int i = 0;
for(; i < 5; i++)
{
temp[i] = distanceFromIR[i][0];
}
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x37: // Get Sonar Distances
{
float temp[5];
int i = 0;
for(; i < 6; i++)
{
temp[i] = distanceFromSonars[i][0];
}
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x38: // Stop command
{
closeWall();
openCubesCatcher();
curState.pidEnabled = 0;
switchOffVibration();
char i;
for (i = 0; i < 4; i++)
{
setVoltageMaxon(WHEELS[i], (uint8_t) 1, (float) 0);
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x39: // Generate new trajectory with correction
{
float *(temp) ={(float*)cmd->param};
char * ch = cmd->param + 24;
robotCoord[0] = temp[0];
robotCoord[1] = temp[1];
robotCoord[2] = temp[2];
lastPoint++;
points[lastPoint].center[0] = temp[3];
points[lastPoint].center[1] = temp[4];
points[lastPoint].center[2] = temp[5];
points[lastPoint].speedVelTipe = speedType[*ch];
points[lastPoint].speedRotTipe = rotType[*ch];
points[lastPoint].endTask = NULL;
points[lastPoint].movTask = NULL;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3A: // Open cubes catcher widely
{
openCubesCatcherWidely();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3B : // Kick the cone on a purple side
{
moveCone();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3C: // Close the cone kicker cone on a purple side
{
closeCone();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
default:
break;
}
return 0;
}
int leerNovedad(global*tanda){
if(mtexto)printf("NOVEDAD.I--%s\n",tanda->cabecera->name);
if (tanda->cabecera->tandaRaiz->pid==0){
if(mtexto)printf("NOVEDAD.F.SIN-NOVEDAD--%s\n",tanda->cabecera->name);
return 0;
}else{
int respuesta;
t_player*temp;
if(!mpantalla)puts("Se ha conectado un jugador!!");
if(!mpantalla)puts("Avisandole al nivel..");
sendAnswer(7,0,' ',tanda->cabecera->tandaRaiz->sym,(short)tanda->cabecera->nid); //Le aviso al nivel que hay un nuevo jugador.
enviarLog(tanda->cabecera->nid,*tanda,7,0,' ',tanda->cabecera->tandaRaiz->sym);
char mensaje[128],numero[16];
strcpy(mensaje,"Salida");
strcat(mensaje,"\t--Msg:7--Cont:0--Data:' '--Sym:");
numero[0]=tanda->cabecera->tandaRaiz->sym;
numero[1]='\0';
strcat(mensaje,numero);
strcat(mensaje,"--\t --Cliente:");
strcat(mensaje,tanda->cabecera->name);
log_info(tanda->logging.info,mensaje,"INFO");
if(mtexto)printf("NOVEDAD.MID.A.I--%s\n",tanda->cabecera->name);
respuesta=selectear(NULL,1,tanda->cabecera->nid,*tanda); //Selecteo hasta que el nivel me responda 1 (-1 Siempre es una opcion de respuesta).
if(mtexto)printf("NOVEDAD.MID.A.F--%s\n",tanda->cabecera->name);
switch (respuesta){
case 1:if(!mpantalla){puts("--El nivel ha dado el ok.--");
puts("Cargando jugador a la base de datos..");}
temp=malloc(sizeof(t_player));
crearStruct(tanda->cabecera,temp,tanda->algo->remainDist);
cargarAlFinal(temp,tanda->ready,tanda->algo->algo); //Carga al final y reordena si es necesario.
borrarNodo(tanda->cabecera);
FD_SET(temp->pid,tanda->original->original);
if(temp->pid>*(tanda->maxfd))*(tanda->maxfd)=temp->pid;
tanda->cabecera->cantJugadores++; //Si el nivel da el ok, entonces aumento la cantidad de jugadores activos.
sendAnswer(1,0,' ',' ',temp->pid);
if(!mpantalla){puts("\nLa lista hasta ahora a quedado asi:");
int i=0;
void _each_Personaje(void*jug){
t_player* jugador;
jugador=(t_player*)jug;
printf("El jugador Nº %d ",i);
printf("es: %c\n",jugador->sym);
i++;
}
list_iterate(tanda->ready,_each_Personaje); //Muestra por pantalla como esta la lista.
puts("");}
char mensaje[64],numero[16];
itoa(temp->pid,numero,10);
strcpy(mensaje,"Nuevo Jugador--Socket Nº:");
strcat(mensaje,numero);
strcat(mensaje,"--Simbolo:");
numero[0]=temp->sym;
numero[1]='\0';
strcat(mensaje,numero);
strcat(mensaje,".");
log_info(tanda->logging.info,mensaje,"INFO");
break;
case -1:if(!mpantalla)puts("--ERROR: El nivel comenta que hubo un error.--");
sendAnswer(-1,0,' ',' ',tanda->cabecera->tandaRaiz->pid);
borrarNodo(tanda->cabecera);
strcpy(mensaje,"Jugador rechazado:");
numero[0]=tanda->cabecera->tandaRaiz->sym;
numero[1]='\0';
strcat(mensaje,numero);
itoa(tanda->cabecera->tandaRaiz->pid,numero,10);
strcat(mensaje,"--Socekt Nº:");
strcat(mensaje,numero);
strcat(mensaje,".");
log_info(tanda->logging.info,mensaje,"WARNING");
break;
case -2:return -2;
break;
case -3:return 1;
break;
}
void CNearbySession::doSession(SOCKET sessionSocket, SOCKET cancelSocket) {
this->sessionSocket = sessionSocket;
std::cout << "CNearby::startup - accepted connection!" << std::endl;
uint32_t sequence = 1;
fd_set readFds;
struct timeval tv;
tv.tv_sec = 5;
tv.tv_usec = 0;
std::string remoteDevice, remoteEndpoint;
SOCKET selectSocket = std::max<SOCKET>(sessionSocket, std::max<SOCKET>(queueReadySocket, cancelSocket)) + 1;
std::vector<uint8_t> buffer;
buffer.reserve(chunkSize*2);
bool sessionLoop = true;
while(sessionLoop) {
FD_ZERO(&readFds);
FD_SET(sessionSocket, &readFds);
FD_SET(queueReadySocket, &readFds);
if (cancelSocket != SOCKET_ERROR) {
FD_SET(cancelSocket, &readFds);
}
tv.tv_sec = 5;
tv.tv_usec = 0;
if(int activity = select(selectSocket, &readFds, nullptr, nullptr, &tv)) {
//check for read
if (FD_ISSET(cancelSocket, &readFds)) {
//data on cancelSocket means we should quit
std::cerr << "CNearby::doSession - sessionSocket error set!" << std::endl;
break;
}
if (FD_ISSET(queueReadySocket, &readFds)) {
//data on messageQueue means we should send it
std::cerr << "CNearby::doSession - messageQueue notification set!" << std::endl;
NotificationSocket::Clear(queueReadySocket);
doWriteQueue(sessionSocket, sequence);
}
if (FD_ISSET(sessionSocket, &readFds)) {
sessionLoop = readRequest(sessionSocket, buffer, sequence, remoteDevice, remoteEndpoint);
}
}
else
{
std::cerr << "CNearby::doSession - send Ping!" << std::endl;
CNearbyMessage message;
sendAnswer(sessionSocket, message.buildPing(sequence));
}
}
closesocket(sessionSocket);
this->sessionSocket = SOCKET_ERROR;
std::cout << "CNearby::doSession - complete!" << std::endl;
}
int main(){
//подключенные автомобили
int num = 1;
char* ip[] = {"0.0.0.0"};
//получение сокетов
WSADATA wsaData;
WSAStartup(MAKEWORD(2, 2), &wsaData);
int port_in = 8888;
int port_out = 4444;
sockaddr_in addr;
addr.sin_family = AF_INET;
SOCKET *sct_in = new SOCKET[num];
SOCKET *sct_out = new SOCKET[num];
for(int i = 0; i<num; i++){
addr.sin_addr.s_addr = inet_addr(ip[i]);
addr.sin_port = htons(port_in);
sct_in[i] = socket(AF_INET, SOCK_STREAM, 0);
connect(sct_in[i],(SOCKADDR *) &addr, sizeof(addr));
addr.sin_port = htons(port_out);
sct_out[i] = socket(AF_INET, SOCK_STREAM, 0);
connect(sct_out[i], (SOCKADDR *) &addr, sizeof(addr));
}
char *buf = new char[11];
std::string s = "keepalive";
int n = 0;
//получение изображения
CvCapture *capture = cvCreateFileCapture("http://192.168.77.1:8080/?action=stream.mjpg");
assert(capture);
cvNamedWindow("capture", CV_WINDOW_AUTOSIZE);
IplImage *frame = 0;
IplImage *image = 0;
while(true){
frame = cvQueryFrame(capture);
image = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
cvConvertImage(frame, image, CV_YUV2BGR);
cvConvertImage(image, image, CV_BGR2GRAY);
cvSmooth(image, image, CV_GAUSSIAN, 3);
cvThreshold(image, image, 120, 255, CV_THRESH_BINARY);
cvErode(image, image);
cvShowImage("capture", image);
/* проверка на новые сообщения */
for(int i = 0; i<num; i++){
send(sct_in[i], &s[0], s.size(), 0);
n = recv(sct_in[i], buf, 11, 0);
if(n <= 0){
std::cout << "error" << std::endl;
}
if(buf != "9:keepalive"){
std::cout << buf << std::endl;
n = getBestPlace(getAvailablePlaces(image));
std::cout << n << std::endl;
sendAnswer(sct_out[i], n);
}
}
/* проверка на новые сообщения */
if(cvWaitKey(50) == 32){
break;
}
}
cvReleaseCapture(&capture);
cvDestroyWindow("capture");
//закрытие сокетов
for(int i = 0; i<num; i++){
closesocket(sct_in[i]);
closesocket(sct_out[i]);
}
delete sct_in;
delete sct_out;
WSACleanup();
return 0;
}
void getBatteryVoltage(void)
{
writeFloatToBuffer(charger_getBatteryVoltage(), 0);
sendAnswer(4);
}
void backTracking(int id, int *map, int nowscore, int first_flg)
{
int len;
if (id >= global_n) return;
if (bestScore2(&global_best, map, &len)) {
printf("Update best score: (%d, %d)\n", global_best.score, global_best.zk);
sendMsg("S");
sendAnswer(global_best.map, global_stones, global_original_stones, global_n);
if (sendMsg("E") == EXIT_FAILURE) return;
}
if ((nowscore - global_sumlen[id]) > global_best.score) return;
//if (isTraveled(map, id, global_x1, global_y1, global_x2, global_y2, len)) return;
if (len == 0) return;
if (len >= global_stones[id].len) {
int x, y, i, j;
for (y=global_y1; y<global_y2; y++) {
for (x=global_x1; x<global_x2; x++) {
int bidx = (y << 5) + x;
if (map[bidx] == -2 || map[bidx] >= 0) continue;
int *p = &global_operation[id << 8];
len = *p;
for (i=0; i<len; i++) {
int xy[32];
int *pp = &p[i << 5];
int flg = first_flg || (map[bidx] < -2);
for (j=1; j<global_stones[id].len; j++) {
int xx = x + pp[j << 1];
int yy = y + pp[(j << 1) + 1];
if (isInValid(xx, yy, global_x1, global_y1, global_x2, global_y2)) goto DAMEDESU;
int idx = (yy << 5) + xx;
if (map[idx] == -2 || map[idx] >= 0) goto DAMEDESU;
xy[j << 1] = xx;
xy[(j << 1) + 1] = yy;
flg |= (map[idx] < -2);
}
if (!flg) continue;
// Put
xy[0] = x;
xy[1] = y;
int org[16];
putStone(map, xy, global_stones[id].len, id, org, global_x1, global_y1, global_x2, global_y2);
// Recursive
backTracking(id+1, map, nowscore - global_stones[id].len, 0);
// Restore
restoreStone(map, xy, global_stones[id].len, id, org, global_x1, global_y1, global_x2, global_y2);
DAMEDESU:
continue; // noop
}
}
}
}
backTracking(id+1, map, nowscore, first_flg);
}
