void doubleTouchThread::threadRelease()
{
printMessage(0,"Returning to position mode..\n");
if (!dontgoback)
{
steerArmsHome();
imodeL -> setInteractionMode(2,VOCAB_IM_STIFF);
imodeL -> setInteractionMode(3,VOCAB_IM_STIFF);
imodeR -> setInteractionMode(2,VOCAB_IM_STIFF);
imodeR -> setInteractionMode(3,VOCAB_IM_STIFF);
steerArmsHome();
}
delete encsR; encsR = NULL;
delete armR; armR = NULL;
delete encsL; encsL = NULL;
delete armL; armL = NULL;
printMessage(0,"Closing ports..\n");
closePort(skinPort);
printMessage(1,"skin port successfully closed!\n");
closePort(outPort);
printMessage(1,"output port successfully closed!\n");
printMessage(0,"Closing controllers..\n");
ddR.close();
ddL.close();
printMessage(0,"Closing solver..\n");
clearTask();
}
void TimerWidget::clearProject() {
selectedProjectId = 0;
if (guid.isEmpty()) {
ui->project->clear();
ui->projectFrame->setVisible(false);
}
// else branch API call happens in clearTask
clearTask();
}
void TimerWidget::descriptionProjectSelected(const QString &projectName, uint64_t projectId, const QString &color, const QString &taskName, uint64_t taskId) {
selectedProjectId = projectId;
selectedTaskId = taskId;
if (projectId && !projectName.isEmpty()) {
ui->projectFrame->setVisible(true);
ui->project->setText(QString("<font color=\"%1\">%2</font>").arg(color).arg(projectName));
clearTask();
if (taskId && !taskName.isEmpty()) {
ui->taskFrame->setVisible(true);
ui->task->setText(QString("<font color=\"gray\">%1</font>").arg(taskName));
}
}
}
DWORD Thread::proc() {
task = nullptr;
//Akin to a game loop, if the thread is inside this loop, it's still alive.
while (running) {
Sleep(1);
//Thread pauses when a job is finished and is resumed when it gets another job.
if (task != nullptr) {
tLock.lock();
{
TaskJob job = task->getJob();
TaskData* data = task->getData();
//Work it!
if (job != nullptr && data != nullptr) {
job(data);
}
//Done.
clearTask();
}
tLock.unlock();
}
//If no task is assigned, get the next one.
if (task == nullptr && pool->isProcessing() && pool->hasWaitingTasks()) {
task = pool->taskList.pop();
noWork = false;
}
//Check if there is work at all.
if (!pool->hasWaitingTasks()) {
noWork = true;
}
}
return 0;
}
LuaTimeline::~LuaTimeline()
{
clearTask();
}
void doubleTouchThread::run()
{
skinContactList *skinContacts = skinPort -> read(false);
sendOutput();
if (checkMotionDone())
{
switch (step)
{
case 0:
steerArmsHome();
yInfo("[doubleTouch] WAITING FOR CONTACT...\n");
step++;
break;
case 1:
if(skinContacts)
{
printMessage(2,"Waiting for contact..\n");
if (detectContact(skinContacts))
{
yInfo("[doubleTouch] CONTACT!!! skinPart: %s Link: %i Position: %s NormDir: %s",
SkinPart_s[cntctSkinPart].c_str(), cntctLinkNum,cntctPosLink.toString(3,3).c_str(),
cntctNormDir.toString(3,3).c_str());
printMessage(1,"Switching to impedance position mode..\n");
imodeS -> setInteractionMode(2,VOCAB_IM_COMPLIANT);
imodeS -> setInteractionMode(3,VOCAB_IM_COMPLIANT);
step++;
}
}
break;
case 2:
solveIK();
yInfo("[doubleTouch] Going to taxel... Desired EE: %s\n",(sol->ee).toString(3,3).c_str());
printMessage(1,"Desired joint configuration: %s\n",(sol->joints*iCub::ctrl::CTRL_RAD2DEG).toString(3,3).c_str());
step++;
recFlag = 1;
break;
case 3:
configureHands();
if (record != 0)
{
Time::delay(2.0);
}
if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
goToTaxelMaster();
}
else
{
goToTaxelSlave();
}
step++;
break;
case 4:
Time::delay(2.0);
step++;
break;
case 5:
if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
goToTaxelSlave();
}
else
{
goToTaxelMaster();
}
step++;
break;
case 6:
recFlag = 0;
bool flag;
if (record == 0)
{
Time::delay(3.0);
flag=1;
if (flag == 1)
{
testAchievement();
step += 2;
}
}
else
{
testAchievement();
printMessage(0,"Waiting for the event to go back.\n");
step++;
}
break;
case 7:
if(skinContacts)
{
if (record == 1)
{
if (testAchievement2(skinContacts))
step++;
else if (robot == "icub" && exitFromDeadlock(skinContacts))
step++;
}
else if (record == 2)
{
if (exitFromDeadlock(skinContacts))
step++;
else
testAchievement2(skinContacts);
}
}
break;
case 8:
if (!dontgoback)
{
printMessage(0,"Going to rest...\n");
clearTask();
steerArmsHomeMasterSlave();
step++;
}
break;
case 9:
printMessage(1,"Switching to position mode..\n");
imodeS -> setInteractionMode(2,VOCAB_IM_STIFF);
imodeS -> setInteractionMode(3,VOCAB_IM_STIFF);
yInfo("[doubleTouch] WAITING FOR CONTACT...\n");
step = 1;
break;
default:
yError("[doubleTouch] doubleTouchThread should never be here!!!\nStep: %d",step);
Time::delay(2.0);
break;
}
}
}
