' ' Author: Kevin Cox ' Date: 3/15/2017 ' Description ' ' DIN1 = Az (encoder 1) index pin, used for homing sequence ' this ended up not being used ' DIN2 = El (encoder 2) index pin, used for homing sequence ' this ended up not being used ' DIN3 = Az limit switch ' DIN4 = El limit switch option explicit ' Keep the 4 LSBs clear for get/set can id's. These will be used to ' seperate rate vs pos queries. #define POS_CID_SUB 0 #define VEL_CID_SUB 1 #define GET_CID 0x100 #define SET_CID 0x110 #define STAT_CID 0x120 #define FF_OVERHEAT_FLAG 0 #define FF_OVERVOLT_FLAG 1 #define FF_UNDRVOLT_FLAG 2 #define UNK_MODE -1 #define POS_MODE 0 #define VEL_MODE 1 #define AZ_LIM_DIN 3 #define EL_LIM_DIN 4 #define HS_FSM_MOVN_STATE 0 #define HS_FSM_MOVP_STATE 1 #define HS_FSM_FLIM_STATE 2 ' Find limit switch #define HS_FSM_EHLD_STATE 3 ' Find position #define HS_FSM_APAN_STATE 4 #define HS_FSM_BORE_STATE 5 #define MOT_FSM_OFF_STATE 0 #define MOT_FSM_ON_STATE 1 ' NOTE if change these then change in gc_hw_rteq.h #define RUN_FSM_OFF_STATE 0 #define RUN_FSM_HS_STATE 1 #define RUN_FSM_VEL_STATE 2 #define RUN_FSM_IDLE_STATE 3 #define POS_MODE_D1 100 #define POS_MODE_D2 250 #define POS_MODE_D3 500 #define SEND_PV_TMR_MS 1 #define SEND_STAT_TMR_MS 200 #define HS_TMR_MS 1 #define HS_SPEED 100 #define HS_MOVN_DEG 10 #define HS_MOVP_DEG 10 #define HS_MOT_MODE 6 ' This needs to be odd because of the check in the limit check in ' HS_FSM_BORE_STATE #define HS_APAN_MAX_CNT 7 dim d as integer dim i as integer dim f as integer dim v as integer dim p1 as integer dim p2 as integer dim cnt as integer dim speed as integer dim apos as integer ' actual position dim epos[2] as integer ' expected position dim evel[2] as integer dim dir as integer dim bore_oset[2] as integer dim cpr[2] as integer dim ddps as integer ' tenths degrees per sec dim nf as integer dim nB as integer dim hdr as integer dim type as integer dim mode as integer dim data[8] as integer dim run_state as integer dim tmr_state as integer dim tmr0_id as integer dim tmr1_id as integer dim tmr2_id as integer dim tmr3_id as integer dim scr_stat as integer dim max_rpm[2] as integer dim max_ddps[2] as integer dim max_cmd as integer ' Homing sequence variables dim hs_done as boolean dim lim_on as boolean ' limit switch on dim mot_off as boolean dim lim_din as integer dim lim_state as integer dim id as integer dim cmd as integer dim ehld_oset[2] as integer dim hs_state as integer dim mot_state as integer dim hs_ticks as integer dim hs_movn_to_ticks[2] as integer ' BND command is done autmatically. See pg 102. This is important for ' incremental encoders. tmr0_id = 0 ' HOMING_SEQUENCE subroutine tmr1_id = 1 ' SEND_STAT subroutine tmr2_id = 2 ' SEND_POS subroutine tmr3_id = 3 bore_oset[0] = 140 bore_oset[1] = 30 ehld_oset[0] = 180 ehld_oset[1] = 30 gosub reset 'terminate settimercount (tmr1_id, SEND_STAT_TMR_MS) settimercount (tmr2_id, SEND_PV_TMR_MS) mode = UNK_MODE mot_state = MOT_FSM_OFF_STATE run_state = RUN_FSM_OFF_STATE while (true) ' Don't trigger on limit switches until after homing sequence is complete setconfig (_DINA, AZ_LIM_DIN, 16) setconfig (_DINA, EL_LIM_DIN, 32) f = getvalue (_FF, 1) mot_off = f and (1< 0) then hdr = getvalue (_CAN, 1) nB = getvalue (_CAN, 2) data[0] = getvalue (_CAN, 3) data[1] = getvalue (_CAN, 4) data[2] = getvalue (_CAN, 5) data[3] = getvalue (_CAN, 6) data[4] = getvalue (_CAN, 7) data[5] = getvalue (_CAN, 8) data[6] = getvalue (_CAN, 9) data[7] = getvalue (_CAN, 10) type = hdr and 0xFFF0 if (type = SET_CID) then mode = hdr and 0xF if (mode = POS_MODE) then epos[0] = data[0] + (data[1]<<8) + (data[2]<<16) + (data[3]<<24) epos[1] = data[4] + (data[5]<<8) + (data[6]<<16) + (data[7]<<24) elseif (mode = VEL_MODE) then evel[0] = data[0] + (data[1]<<8) + (data[2]<<16) + (data[3]<<24) evel[1] = data[4] + (data[5]<<8) + (data[6]<<16) + (data[7]<<24) if (run_state = RUN_FSM_VEL_STATE) then gosub SET_VEL end if end if end if elseif (nf > 1) then 'print ("Buffering CAN frames:", nf, "\n") end if return SET_VEL: if (mode = VEL_MODE) then for i = 0 AndWhile i < 2 if (evel[i] > 1000) then v = 1000 elseif (evel[i] < -1000) then v = -1000 else v = evel[i] end if 'print ("i: ", i," v: ", v, "\n") 'if (i = 0) then setcommand (_G, i+1, v) 'end if next end if return SEND_POS: setcommand (_CS, 1, GET_CID+POS_CID_SUB) v = getvalue (_C, 1) setcommand (_CS, 3, (v and 0xFF)) setcommand (_CS, 4, (v and 0xFF00)>>8) setcommand (_CS, 5, (v and 0xFF0000)>>16) setcommand (_CS, 6, (v and 0xFF000000)>>24) v = getvalue (_C, 2) setcommand (_CS, 7, (v and 0xFF)) setcommand (_CS, 8, (v and 0xFF00)>>8) setcommand (_CS, 9, (v and 0xFF0000)>>16) setcommand (_CS, 10, (v and 0xFF000000)>>24) setcommand (_CS, 2, 8) return SEND_VEL: setcommand (_CS, 1, GET_CID+VEL_CID_SUB) v = getvalue (_BSR, 1) setcommand (_CS, 3, (v and 0xFF)) setcommand (_CS, 4, (v and 0xFF00)>>8) setcommand (_CS, 5, (v and 0xFF0000)>>16) setcommand (_CS, 6, (v and 0xFF000000)>>24) v = getvalue (_BSR, 2) setcommand (_CS, 7, (v and 0xFF)) setcommand (_CS, 8, (v and 0xFF00)>>8) setcommand (_CS, 9, (v and 0xFF0000)>>16) setcommand (_CS, 10, (v and 0xFF000000)>>24) setcommand (_CS, 2, 8) return SEND_STAT: setcommand (_CS, 1, STAT_CID) setcommand (_CS, 3, scr_stat) v = getvalue (_FF, 1) setcommand (_CS, 4, v) v = getvalue (_FM, 1) setcommand (_CS, 5, v) v = getvalue (_FM, 2) setcommand (_CS, 6, v) v = getvalue (_FS, 1) setcommand (_CS, 7, v) setcommand (_CS, 2, 5) return HS_FSM_INIT: 'print ("HS_INIT called id: ", id, "\n") 'setcommand (_BND, id) 'wait (1000) setcommand (_H, id) setconfig (_MMOD, id, HS_MOT_MODE) if (id = 1) then lim_din = AZ_LIM_DIN else lim_din = EL_LIM_DIN end if cmd = -HS_SPEED hs_state = HS_FSM_MOVN_STATE hs_done = false hs_ticks = 0 p1 = getvalue (_C, id) gosub GCMD settimercount (tmr0_id, HS_TMR_MS) return ' This homing sequence uses closed loop speed position mode HS_FSM_EXEC: if (hs_state = HS_FSM_MOVN_STATE) then tmr_state = gettimerstate (tmr0_id) if (tmr_state = 1) then p2 = getvalue (_C, id) d = p2 - p1 if (abs(d) > cpr[id-1]*HS_MOVN_DEG/360) then 'print ("MOVN p2: ", p2, " p1: ", p1, "\n") hs_state = HS_FSM_FLIM_STATE cmd = HS_SPEED gosub GCMD end if hs_ticks = hs_ticks + 1 if (hs_ticks >= hs_movn_to_ticks[id-1]) then 'print ("ticks: ", hs_ticks, " ", hs_movn_to_ticks[id-1], "\n") hs_state = HS_FSM_MOVP_STATE cmd = HS_SPEED gosub GCMD end if settimercount (tmr0_id, HS_TMR_MS) end if elseif (hs_state = HS_FSM_MOVP_STATE) then tmr_state = gettimerstate (tmr0_id) if (tmr_state = 1) then p2 = getvalue (_C, id) d = p2 - p1 if (abs(d) > cpr[id-1]*HS_MOVP_DEG/360) then 'print ("MOVP p2: ", p2, " p1: ", p1, "\n") hs_state = HS_FSM_FLIM_STATE cmd = HS_SPEED gosub GCMD end if settimercount (tmr0_id, HS_TMR_MS) end if elseif (hs_state = HS_FSM_FLIM_STATE) then tmr_state = gettimerstate (tmr0_id) if (tmr_state = 1) then lim_on = getvalue (_DI, lim_din) if (lim_on) then cmd = 0 gosub GCMD setconfig (_MMOD, id, 0) ' set encoder val to zer at the limit switch setcommand (_H, id) wait (10) setconfig (_MMOD, id, HS_MOT_MODE) cmd = -HS_SPEED gosub GCMD hs_state = HS_FSM_EHLD_STATE 'print ("FLIM p2: ", p2, " p1: ", p1, " id: ", id, "\n") end if settimercount (tmr0_id, HS_TMR_MS) end if elseif (hs_state = HS_FSM_EHLD_STATE) then tmr_state = gettimerstate (tmr0_id) if (tmr_state = 1) then p2 = getvalue (_C, id) if (abs(p2) > cpr[id-1]*ehld_oset[id-1]/360) then 'print ("BORE p2: ", p2, " p1: ", p1, " id: ", id, "\n") if (id = 1) then cmd = 0 gosub GCMD id = 2 gosub HS_FSM_INIT 'Put this is to do id 1 only 'cmd = HS_SPEED 'gosub GCMD 'hs_state = HS_FSM_BORE_STATE else ' az stage pans back/forth for x sweeps cnt = 0 lim_state = 0 hs_state = HS_FSM_APAN_STATE end if end if settimercount (tmr0_id, HS_TMR_MS) end if elseif (hs_state = HS_FSM_APAN_STATE) then tmr_state = gettimerstate (tmr0_id) if (tmr_state = 1) then lim_on = getvalue (_DI, EL_LIM_DIN) if (lim_on) then if (lim_state = 0) then lim_state = 1 cnt = cnt + 1 if (cnt >= HS_APAN_MAX_CNT) then cmd = 0 gosub GCMD id = 1 cmd = HS_SPEED gosub GCMD hs_state = HS_FSM_BORE_STATE else cmd = cmd * -1 gosub GCMD end if end if else lim_state = 0 end if settimercount (tmr0_id, HS_TMR_MS) end if elseif (hs_state = HS_FSM_BORE_STATE) then tmr_state = gettimerstate (tmr0_id) if (tmr_state = 1) then p2 = getvalue (_C, id) if (abs(p2) < cpr[id-1]*bore_oset[id-1]/360) then 'print ("BORE p2: ", p2, " id: ", id, "\n") cmd = 0 gosub GCMD if (id = 1) then id = 2 cmd = HS_SPEED gosub GCMD ' Put this in to do chan 1 only 'hs_done = true else hs_done = true end if end if settimercount (tmr0_id, HS_TMR_MS) end if end if return GCMD: setcommand (_G, id, cmd) return