void LinearMotorControlTask::setLateralAngle(double beta)
{
/*bool currDir;
if(beta>lastBeta)
currDir = true;
else
currDir = false;
if(dirBeta!=currDir)
{
lastBeta = beta;
filterBeta = true;
}
dirBeta = currDir;
int32_t sidePosOff;
//std::cerr << lastBeta-beta << std::endl;
if(filterBeta && (fabs(lastBeta-beta)>1e-5))
filterBeta=false;
if(filterBeta)
sidePosOff = (int32_t)(((double)sideMotDist)*tan(lastBeta));
else
sidePosOff = (int32_t)(((double)sideMotDist)*tan(beta));*/
int32_t sidePosOff = (int32_t)(((double)sideMotDist) * tan(beta));
int32_t positionOne = posMiddle - sidePosOff;
int32_t positionTwo = posMiddle + sidePosOff;
/*setPositionOne((positionOne>0)?positionOne:0);
setPositionTwo((positionTwo>0)?positionTwo:0);*/
setPositionOne(positionOne);
setPositionTwo(positionTwo);
}
void AMBeamConfiguration::alignPositionTwo()
{
QVector3D centerOne = findCenter(positionOne_);
QVector3D centerTwo = findCenter(positionTwo_);
QVector3D ray = centerTwo - centerOne;
QVector<QVector3D> newPosition;
for(int i = 0; i < positionOne_.count(); i++)
{
newPosition<<(positionOne_.at(i)+ray);
}
setPositionTwo(newPosition);
}
LinearMotorControlTask::LinearMotorControlTask(XenomaiSocketCan &setCan)
: XenomaiTask("LinearMotorControlTask")
, can(&setCan)
, lastAlpha(0.0)
, lastBeta(0.0)
{
can->setRecvTimeout(1000);
pdoOneFrame.can_id = pdoOneId;
pdoOneFrame.can_dlc = 8;
pdoTwoFrame.can_id = pdoTwoId;
pdoTwoFrame.can_dlc = 8;
pdoThreeFrame.can_id = pdoThreeId;
pdoThreeFrame.can_dlc = 8;
stateOneFrame.can_id = statusOneId;
stateOneFrame.can_dlc = 2;
*(uint16_t *)(stateOneFrame.data) = 0;
stateTwoFrame.can_id = statusTwoId;
stateTwoFrame.can_dlc = 2;
*(uint16_t *)(stateTwoFrame.data) = 0;
stateThreeFrame.can_id = statusThreeId;
stateThreeFrame.can_dlc = 2;
*(uint16_t *)(stateThreeFrame.data) = 0;
setPositionOne(posLowerBound);
setPositionTwo(posLowerBound);
setPositionThree(posLowerBound);
setVelocityOne(velLowerBound);
setVelocityTwo(velLowerBound);
setVelocityThree(velLowerBound);
setAccelerationOne(accLowerBound);
setAccelerationTwo(accLowerBound);
setAccelerationThree(accLowerBound);
sendStates = false;
}
