/**
* @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;
}
