pshell_config/mxsc
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Closedown

Browse Source
This commit is contained in:
gac-S_Changer
2017-03-01 14:58:32 +01:00
parent 92e4756ff8
commit 755914738f
1 changed files with 25 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
robot/MXLAB/Controller1/usr/usrapp/ForceDetection/detect.pgx
View File

@@ -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)