/** * @brief Sends the given \a message over the socket as a text message and returns the number of bytes actually sent. * @param message The message to be sent * @return The number of bytes actually sent. */ qint64 WebSocket::send(const QString &message) { return doWriteData(message.toUtf8(), false); }
/** * @brief Sends the given \a data over the socket as a binary message and returns the number of bytes actually sent. * @param data The binary data to be sent. * @return The number of bytes actually sent. */ qint64 WebSocket::send(const QByteArray &data) { return doWriteData(data, true); }
byte doInterpolation(DynamixelComm *dc, struct RobotData &robotData, struct InterpolationData &ipoData, struct SCurveParameters ¶ms) { byte failure = AX12_NOERROR; unsigned long tmpTime = gettickcount(); if (tmpTime >= robotData.readTime + ipoData.ipoPause) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); robotData.readTimeDiff = tmpTime - robotData.readTime; robotData.readTime = tmpTime; //printf("past the time - %ld, pause = %ld\n", tmpTime, ipoData.ipoPause); // read or write every ipoData.ipoPause Milliseconds if (ipoData.ipoCounter <= ipoData.ipoMax) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); // if at least two points are available if ((robotData.writeBufferLength > ipoData.ipoParam) && ipoData.preparationDone) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); /* if (!ipoData.preparationDone) { setWriteStartBuffer(robotData, ipoData, params); // preprocessing doPreparation(robotData, ipoData, params); } */ // write (interpolated) data to servos doWriteData(dc, robotData, ipoData, params); // remove data from robotData.writeBuffer at end of interpolation cycle if (ipoData.ipoCounter >= ipoData.ipoMax) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); while(gettickcount() < tmpTime + ipoData.ipoPause) ; ipoData.preparationDone = false; robotData.writeBufferLength--; byte oldWriteBufferIndex = robotData.writeBufferIndex; robotData.writeBufferIndex = (robotData.writeBufferIndex + 1) % WRITEBUFFERMAX; // copy processed write target to last buffer // modified: //memcpy(robotData.writeLastBuffer, &robotData.writeBuffer[oldWriteBufferIndex][0], AX12_COUNT*AX12_DATA_WRITE*sizeof(byte)); robotData.writeStartBuffer = &robotData.writeBuffer[oldWriteBufferIndex][0]; robotData.writeLastBufferIsValid = true; if (robotData.writeBufferLength > 0) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); // modified: //robotData.writeStartBuffer = robotData.writeLastBuffer; // preprocessing printf("doPreparation inner\n"); doPreparation(robotData, ipoData, params); } // stuff read op between this and the next write op // modified: robotData.readTime -= ipoData.ipoPause / 2; //robotData.readTime -= (ipoData.ipoPause * 3)/ 4; // calc average length of one write cycle tmpTime = robotData.writeTime; robotData.writeTime = gettickcount(); if ((robotData.writeTime - tmpTime) < WRITEBUFFERMAXPAUSE) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); robotData.writeTimeDiffAverage = (unsigned int) (0.7 * (float)robotData.writeTimeDiffAverage + 0.3 * (float) (robotData.writeTime - tmpTime)); if (ipoData.ipoPauseAutoSet) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); float tmp = ((float)ipoData.ipoTotalTime * (float)ipoData.ipoTotalTime)/ ((float)ipoData.ipoMax * (float)robotData.writeTimeDiffAverage); if (robotData.writeBufferLength > 1) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); float tmp2 = (float) (robotData.writeBufferLength-1); tmp *= 1.0 - ipoData.ipoAutoAdjustParameter + ipoData.ipoAutoAdjustParameter*(1-(tmp2 * tmp2)/64.0); } ipoData.ipoPause = (unsigned long) (tmp + 0.5); } } /* if ((robotData.writeTime - tmpTime) < WRITEBUFFERMAXPAUSE) { robotData.writeTimeDiffs[robotData.writeTimeDiffCounter] = (robotData.writeTime - tmpTime); if (robotData.writeTimeDiffs[robotData.writeTimeDiffCounter] > WRITETIMEDIFFMAX) robotData.writeTimeDiffs[robotData.writeTimeDiffCounter] = WRITETIMEDIFFMAX; robotData.writeTimeDiffCounter++; if (robotData.writeTimeDiffCounter == WRITETIMEDIFFLEN) { robotData.writeTimeDiffCounter = 0; robotData.writeTimeDiffAverage = (unsigned int) (getTrimmedAverage(robotData.writeTimeDiffs, WRITETIMEDIFFLEN, WRITETIMEDIFFREMOVE)); } if (ipoData.ipoPauseAutoSet) { float tmp = ((float)ipoData.ipoTotalTime * (float)ipoData.ipoTotalTime)/ ((float)ipoData.ipoMax * (float)robotData.writeTimeDiffAverage); if (robotData.writeBufferLength > 1) { float tmp2 = (float) (robotData.writeBufferLength-1); tmp *= 1.0 - ipoData.ipoAutoAdjustParameter + ipoData.ipoAutoAdjustParameter*(1-(tmp2 * tmp2)/64.0); } ipoData.ipoPause = (unsigned int) (tmp + 0.5); } robotData.writeTimeDiffAverage = ((unsigned int)((float)robotData.writeTimeDiffAverage / (float)ipoData.ipoPause + 0.5)) * ipoData.ipoPause; } */ } } } // modified: else // read ax12 data every (ipoData.ipoMax + 1) +ipoData.ipoPause Milliseconds //if (ipoData.ipoCounter >= ipoData.ipoMax) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); failure = AX12_NOERROR; if (!ipoData.preparationDone) // comment this line in order to produce position feedback during write actions { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); readPositionData(dc, robotData.readBuffer, failure); } if (failure != AX12_NOERROR) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); robotData.readTime = gettickcount() - ipoData.ipoPause + AX12_READERROR_TIMEOUT; return failure; } ipoData.ipoCounter = 0; if (failure == AX12_NOERROR) if (!ipoData.preparationDone) { printf("%s:%d\n",__func__,__LINE__); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); setWriteStartBuffer(robotData, ipoData, params); // preprocessing printf("doPreparation outer\n"); testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); doPreparation(robotData, ipoData, params); } // stuff read op between two write ops // modified: robotData.readTime -= ipoData.ipoPause / 2; //robotData.readTime -= ipoData.ipoPause / 4; } ipoData.ipoCounter++; } testWB(robotData.writeBuffer[robotData.writeBufferIndex],__LINE__); return failure; }