diff --git a/lwr_bringup/KRL/fri_start.src b/lwr_bringup/KRL/fri_start.src new file mode 100755 index 0000000..848edea --- /dev/null +++ b/lwr_bringup/KRL/fri_start.src @@ -0,0 +1,205 @@ +&ACCESS RVP +&REL 41 +&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe +&PARAM EDITMASK = * +DEF fri_start( ) +;FOLD INI + +DECL FRISTATE ret +DECL FRAME t +DECL REAL force +DECL REAL torque +DECL STIFFNESS USERSTIFF +DECL INT AX +DECL INT IP[4] +DECL E6AXIS START_POSE + + ;FOLD BASISTECH INI + GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) + INTERRUPT ON 3 + BAS (#INITMOV,0 ) + ;ENDFOLD (BASISTECH INI) + ;FOLD USER INI + ;Make your modifications here + + ;ENDFOLD (USER INI) +;ENDFOLD (INI) + +;stop FRI in case it's running +ret=FRISTOP() +ret=FRICLOSE() + +;;; set tool no. 1 +bas(#tool,1) + +;FOLD SETUP + + ;;; not needed if configured correctly + ;IP[1]=192 + ;IP[2]=168 + ;IP[3]=0 + ;IP[4]=10 + + ;; set IP address and port of remote controller + ret=FRISETUP("no",49948,49948) + + ;Set control strategy and initial parameters + USERSTIFF = $STIFFNESS + USERSTIFF.STRATEGY = 30 + USERSTIFF.FRAMETYPE=#TOOL + USERSTIFF.AXISSTIFFNESS = {A1 40, A2 40, A3 40, A4 40, A5 40, A6 40, E1 40} + USERSTIFF.AXISDAMPING = {A1 0.7, A2 0.7, A3 0.7, A4 0.7, A5 0.7, A6 0.7, E1 0.7} + + ;Limits + USERSTIFF.AXISMAXDELTA = {A1 340, A2 340, A3 340, A4 340, A5 340, A6 340, E1 340} + USERSTIFF.AXISMAXDELTATRQ = {A1 400, A2 400, A3 400, A4 400, A5 400, A6 400, E1 400} + USERSTIFF.CPMAXDELTA = {X 2000, Y 2000, Z 2000, A 180, B 180, C 180} + USERSTIFF.MAXFORCE = {X 900, Y 900, Z 900, A 900, B 900, C 900} + + ;commit new settings + $STIFFNESS=USERSTIFF + + FOR AX=1 TO $NUM_AX STEP 1 + $VEL_AXIS[AX]= 100 + $ACC_AXIS[AX]= 50 + ENDFOR + FOR AX=1 TO $EX_AX_NUM STEP 1 + $VEL_EXTAX[AX]= 100 + $ACC_EXTAX[AX]= 50 + ENDFOR + + ; set base + $base = $robroot + +;ENDFOLD (SETUP) + +WAIT SEC 0.2 + +;; check if torques are within limits +t = $torque_tcp_est.ft +force = SQRT(t.x*t.x + t.y*t.y + t.z*t.z) +torque = SQRT(t.a*t.a + t.b*t.b + t.c*t.c) + +IF (force > 8) OR (torque > 8) THEN + ; experiencing too much force. refusing to start! + WHILE TRUE + HALT + ENDWHILE +ENDIF + +; switch on motors +start_pose = TOUCH_AXES($AXIS_ACT) +PTP start_pose + +;set up timer +$TIMER[1] = -3000 +$TIMER_STOP[1] = FALSE + +; open with data rate 2 msec +ret=friopen(2) + +;wait sec 2.5 + +;ret=FRISTART(1.0) + +while true + ;;; here we command from remote + wait for $fri_frm_bool[1] or $TIMER_FLAG[1] + + ; check if we got a command + if $fri_frm_bool[1] then + ;acknowledge command + $fri_to_bool[1] = true + + ; start control mode + if ($fri_frm_int[1] == 1) then + ret=FRISTART(1.0) + wait sec 0.2 + endif + + ; stop control mode + if $FRI_FRM_INT[1]==2 then + PTP CLAMP_AXES($AXIS_ACT) + wait sec 0.2 + ret=FRISTOP() + wait sec 0.2 + endif + + ; restart interface + if $FRI_FRM_INT[1]==3 then + ret=FRISTOP() + ret=FRICLOSE() + ret=FRIOPEN($FRI_FRM_INT[2]) + endif + endif + + ;clear processing flag + $fri_to_bool[1] = false + + IF $FRI_FRM_BOOL[8] THEN + PTP CLAMP_AXES($AXIS_ACT_MES) + ELSE + PTP CLAMP_AXES($AXIS_ACT_CMD) + ENDIF + + $TIMER[1] = -3000 +endwhile + +ret=FRISTOP() +ret=FRICLOSE() + +halt + +END +;FOLD AXIS HELPER FUNCTIONS + +DEFFCT E6AXIS TOUCH_AXES(ax) +E6AXIS ax +DECL E6AXIS an + +an = CLAMP_AXES(ax) + +IF an.A6 < 0 THEN + an.A6 = an.A6 + 0.01 +ELSE + an.A6 = an.A6 - 0.01 +ENDIF + +RETURN an +ENDFCT + + +DEFFCT E6AXIS CLAMP_AXES(ax:IN) +E6AXIS ax +DECL E6AXIS an + +an.A1 = CLAMP(ax.A1, -169., 169.) +an.A2 = CLAMP(ax.A2, -29., 209.) +an.E1 = CLAMP(ax.E1, -169., 169.) +an.A3 = CLAMP(ax.A3, -119., 119.) +an.A4 = CLAMP(ax.A4, -169., 169.) +an.A5 = CLAMP(ax.A5, -119., 119.) +an.A6 = CLAMP(ax.A6, -169., 169.) + +RETURN an +ENDFCT + + +DEFFCT REAL CLAMP(v:IN, minval:IN, maxval:IN) +REAL v, minval, maxval +DECL REAL r + +r = v + +IF v > maxval THEN + r = maxval +ENDIF + +IF v < minval THEN + r = minval +ENDIF + +RETURN r +ENDFCT + +;ENDFOLD (AXIS HELPER FUNCTIONS)