diff --git a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/detect.pgx b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/detect.pgx index 01c30c7..32bafce 100644 --- a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/detect.pgx +++ b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/detect.pgx @@ -44,29 +44,31 @@ // Compare with the trained values for l_nIndex=0 to 5 x_nThreshold = abs(nJntMaxForce[l_nIndex])+abs(x_nTolPercent*nJntMaxForce[l_nIndex]/100) - l_nPerc = abs(nJntForce[l_nIndex]/nJntMaxForce[l_nIndex] *100) - if l_nPerc>nJntMaxMeasured[l_nIndex] - nJntMaxMeasured[l_nIndex]=l_nPerc - endIf - l_bCondition=abs(nJntForce[l_nIndex]) > x_nThreshold - - gotoxy(0,8+l_nIndex) - put(toString(".2",nJntMaxForce[l_nIndex])) - gotoxy(8,8+l_nIndex) - put(toString(".2",x_nThreshold)) - gotoxy(16,8+l_nIndex) - put(toString(".2",abs(nJntForce[l_nIndex]))) - gotoxy(24,8+l_nIndex) - put(toString(".2",l_nPerc)) - gotoxy(32,8+l_nIndex) - put(toString(".2",nJntMaxMeasured[l_nIndex])) - - if l_bCondition - stopMove() - resetMotion() - bContact=true - l_sDisplay="Contact: force on joint "+toString("",l_nIndex+1)+" = "+toString("",abs(nJntForce[l_nIndex])) - logMsg(l_sDisplay) + if nJntMaxForce[l_nIndex]>0 + l_nPerc = abs(nJntForce[l_nIndex]/nJntMaxForce[l_nIndex] *100) + if l_nPerc>nJntMaxMeasured[l_nIndex] + nJntMaxMeasured[l_nIndex]=l_nPerc + endIf + l_bCondition=abs(nJntForce[l_nIndex]) > x_nThreshold + + gotoxy(0,8+l_nIndex) + put(toString(".2",nJntMaxForce[l_nIndex])) + gotoxy(8,8+l_nIndex) + put(toString(".2",x_nThreshold)) + gotoxy(16,8+l_nIndex) + put(toString(".2",abs(nJntForce[l_nIndex]))) + gotoxy(24,8+l_nIndex) + put(toString(".2",l_nPerc)) + gotoxy(32,8+l_nIndex) + put(toString(".2",nJntMaxMeasured[l_nIndex])) + + if l_bCondition + stopMove() + resetMotion() + bContact=true + l_sDisplay="Contact: force on joint "+toString("",l_nIndex+1)+" = "+toString("",abs(nJntForce[l_nIndex])) + logMsg(l_sDisplay) + endIf endIf endFor delay(0)