Hi guys,
I am using KSS 8.6 END V6
I am trying to create a homing sequence by filling up a text file then rereading it backwards to get back to home.
Have created a block of code to record positions and an interrupt to call READCSV.src to read back the positions and send it back for execution in reverse.
After reversing the file will be deleted and an empty text file( CREATECSV.src) is called to create an empty text file for the next homing call.
I have found the post below to be suitable.
I am able to Open a file, Start populating positions and close it.
The issue i am facing is that the positions are recorded at cycle time rate thus too many positions. so i added a timer to lengthen the sample rate. This does not seem to work.
The robot moves in reverses smoothly for awhile and starts to jerk after.
Any assistance would be greatly appreciated.
Code: SPS.SUB
&ACCESS RVO
&COMMENT PLC on control
DEF SPS ( )
;FOLD DECLARATIONS
;FOLD BASISTECH DECL
;Automatik extern
DECL CHAR CURRENT[200]
DECL STATE_T STAT
DECL MODUS_T MODE
DECL INT OFS
DECL INT UPDATETIME
DECL BOOL ran
;DECL INT global_handle
DECL BOOL RECORD
DECL BOOL rob_stopped
DECL BOOL peri_rdy
;DECL BOOL timer1_Q
;ENDFOLD (BASISTECH DECL)
;FOLD USER DECL
; Please insert user defined declarations
;ENDFOLD (USER DECL)
;ENDFOLD (DECLARATIONS)
;FOLD INI
;FOLD AUTOEXT INIT
INTERRUPT DECL 91 WHEN $PRO_STATE1==#P_FREE DO RESET_OUT ()
INTERRUPT ON 91
INTERRUPT DECL 92 WHEN $PRO_MOVE==TRUE DO RESET_LINESEL()
INTERRUPT ON 92
$LOOP_MSG[]=" "
MODE=#SYNC
$H_POS=$H_POS
;Automatic extern
IF $MODE_OP==#EX THEN
CWRITE($CMD,STAT,MODE,"RUN/R1/CELL()")
ENDIF
;ENDFOLD (AUTOEXT INIT)
;FOLD BACKUPMANAGER PLC INIT
BM_ENABLED = FALSE
BM_OUTPUTVALUE = 0
;ENDFOLD (BACKUPMANAGER PLC INIT)
;FOLD USER INIT
; Please insert user defined initialization commands
OFS = 0
ran = false
;global_handle = 0
RECORD = FALSE
; open file
CWRITE($FCT_CALL,STAT,MODE,"krl_fopen","position_log.txt","a", global_handle)
;ENDFOLD (USER INIT)
;ENDFOLD (INI)
;timer1_Q=$timer_flag[1]
;UPDATETIME=-30
LOOP
WAIT FOR NOT($POWER_FAIL)
;FOLD BASISTECH PLC
BasisTech_PLC_LOOP()
;ENDFOLD (BASISTECH PLC)
;FOLD BACKUPMANAGER PLC
IF BM_ENABLED THEN
BM_OUTPUTSIGNAL = BM_OUTPUTVALUE
ENDIF
;ENDFOLD (BACKUPMANAGER PLC)
;FOLD USER PLC
;Make your modifications here
rob_stopped = $ROB_STOPPED
peri_rdy = $PERI_RDY
; $TIMER[1] = -500 ;set timer duration
; $TIMER_STOP[1] = false; start timer
IF NOT rob_stopped THEN
RECORD = TRUE
$TIMER_STOP[1] = FALSE
ELSE
RECORD = FALSE
$TIMER_STOP[1] = TRUE
ENDIF
;AND ($timer[1] >=-1000)
IF RECORD==TRUE AND ($timer[1] >=-2000) THEN
ran = true
OFS = 0
SWRITE(CURRENT[], STAT, OFS, "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f", $POS_ACT.X, $POS_ACT.Y, $POS_ACT.Z, $POS_ACT.A, $POS_ACT.B, $POS_ACT.C)
MODE = #SYNC
; write data
CWRITE($FCT_CALL,STAT,MODE,"krl_fwriteln", global_handle, CURRENT[])
IF (NOT STRCLEAR(CURRENT[])) THEN
halt
ENDIF
;$TIMER_STOP[1] = TRUE ;
ENDIF
IF $TIMER[1] >=-500 then
$TIMER_STOP[1] = TRUE
$TIMER[1] = -2000
$TIMER_STOP[1] = false
ENDIF
; close file
IF NOT RECORD AND ran AND NOT peri_rdy THEN
CWRITE($FCT_CALL, STAT,MODE,"krl_fclose", global_handle)
ran = false
ENDIF
;Set speed limits
IF $MODE_OP == #EX THEN
IF $OV_PRO >10 THEN
$OV_PRO =10
ENDIF
ENDIF
IF $MODE_OP == #AUT THEN
IF $OV_PRO >10 THEN
$OV_PRO =10
ENDIF
ENDIF
IF $MODE_OP == #T1 THEN
IF $OV_PRO >49 THEN
$OV_PRO =30
ENDIF
ENDIF
;ENDFOLD (USER PLC)
ENDLOOP
;FOLD ;%{H}
;FOLD
END
;ENDFOLD
DEF RESET_OUT ( )
INT N
MsgLoop(" ")
IF REFLECT_PROG_NR == 1 THEN
FOR N = 0 TO PGNO_LENGTH - 1
$OUT[PGNO_FBIT_REFL + N] = FALSE
ENDFOR
ENDIF
IF (PGNO_REQ >0) THEN
$OUT[PGNO_REQ]=FALSE
ELSE
IF (PGNO_REQ <0) THEN
$OUT[-PGNO_REQ]=TRUE
ENDIF
ENDIF
END
DEF RESET_LINESEL()
$LINE_SEL_OK=FALSE
END
;FOLD USER SUBROUTINE
; Integrate your user defined subroutines
;ENDFOLD (USER SUBROUTINE)
;ENDFOLDCode: Read_CSV
DEF read_csv ( )
DECL state_t S
DECL modus_t M
DECL int handle,read,n,i
DECL char Buff[256], fmt[64], tmp[10]
DECL BOOL debug
;FOLD INI;%{PE}
;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
$TOOL = TOOL_DATA[2]
$BASE = BASE_DATA[2]
;ENDFOLD (USER INI)
;ENDFOLD (INI)
debug = false
; initialize buffer, it cannot be empty
for n=1 to 256
Buff[n]=32 ; 0x20 is a space character
endfor
for n=1 to 1000
P[n]={X 0,Y 0,Z 0,A 0,B 0,C 0}
endfor
fmt[] = "%f, %f, %f, %f, %f, %f" ; line format
handle=0 ; pick file handle
read=0 ; number of characters read from file
;-------> step 1 - open file <------------
; note: file is stored in C:\KRC\ROBOTER\USERFILE
if debug then
cwrite($FCT_CALL,S,M,"krl_fclose_all",-99)
halt
endif
cwrite($FCT_CALL,S,M,"krl_fopen","position_log.txt","r",handle)
wait for S.ret1==#data_ok ; confirm action
;-------> step 2 - read from file <-------
n=0
repeat
if debug then
halt
endif
n=n+1
MsgNotify("Trying to read line %1",,n)
tmp[]=" "
cwrite($FCT_CALL,S,M,"krl_fscanf",handle,fmt[],P[n].X,P[n].Y,P[n].Z,P[n].A,P[n].B,P[n].C)
;or (n>=100) , (S.msg_no== -4))
until ((S.ret1<>#data_ok) or (n>=1000))
if S.msg_no== -4 then
msgnotify("end of file")
endif
;-------> step 3 - close file <----------
cwrite($FCT_CALL,S,M,"krl_fclose",handle)
wait for S.ret1==#data_ok
SPTP $AXIS_ACT
i = n
REPEAT
$TOOL = TOOL_DATA[2]
$BASE = BASE_DATA[2]
SLIN P[i]
i= i-1
UNTIL (i == 1)
; delete file
cwrite($FCT_CALL,S,M,"krl_remove","position_log.txt")
WAIT SEC 3
Create_CSV();
END
