'
'  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<<FF_UNDRVOLT_FLAG)
      if (mot_state = MOT_FSM_OFF_STATE) then
         if (not mot_off) then
            id = 1 ' Start with el stage
            gosub HS_FSM_INIT
            ' Timers seem to get cleared when motor turned off???
            settimercount (tmr1_id, SEND_STAT_TMR_MS)
            settimercount (tmr2_id, SEND_PV_TMR_MS)
            mot_state = MOT_FSM_ON_STATE
            run_state = RUN_FSM_HS_STATE
            'print ("Motor ON\n")
         else
            mot_state = MOT_FSM_OFF_STATE
            run_state = RUN_FSM_OFF_STATE
         end if
      elseif (mot_state = MOT_FSM_ON_STATE) then
         if (mot_off) then
            'print ("Motor OFF\n")
            mot_state = MOT_FSM_OFF_STATE
            run_state = RUN_FSM_OFF_STATE
            gosub RESET
         end if
      end if

      gosub CHK_CAN_BUS

      if (run_state = RUN_FSM_HS_STATE) then
         gosub HS_FSM_EXEC
         if (hs_done) then
            run_state = RUN_FSM_VEL_STATE
            setconfig (_MMOD, 1, 0)
            setconfig (_MMOD, 2, 0)
            ' In open loop now make accel hi so that the
            ' the motor cmds from gimbal ctrl will ramp
            ' up to speed as quickly as possible
            setconfig (_MAC, 1, 8000)
            setconfig (_MAC, 2, 8000)
			setconfig (_MDEC, 1, 8000)
			setconfig (_MDEC, 2, 8000)
			' Setup safety stop when hit hard limits
            setconfig (_DINA, AZ_LIM_DIN, 17)
            setconfig (_DINA, EL_LIM_DIN, 33)

            'run_state = RUN_FSM_IDLE_STATE
         end if
      elseif (run_state = RUN_FSM_IDLE_STATE) then
         ' Use this to keep script from going into vel mode 
         ' after hs_done = true
      end if

      scr_stat = run_state

      tmr_state = gettimerstate (tmr1_id)
      if (tmr_state = 1) then
         settimercount (tmr1_id, SEND_STAT_TMR_MS)
         gosub SEND_STAT
      end if
      tmr_state = gettimerstate (tmr2_id)
      if (tmr_state = 1) then
       	 settimercount (tmr2_id, SEND_PV_TMR_MS)
         gosub SEND_POS
         'gosub send_vel
      end if
   end while
   terminate ' Should never get here

RESET:
   for i = 0 AndWhile i < 2
      'setconfig (_EX, 1)
      setcommand (_G, i+1, 0)
      setconfig (_MMOD, i+1, 0) ' open loop mode
      setconfig (_MAC, i+1, 200)
      setconfig (_MDEC, i+1, 200)
      'setconfig (_MG, 1)
   
      'max_rpm[i] = getconfig (_MVEL, i+1)
      max_rpm[i] = getconfig (_MXRPM, i+1)
      max_ddps[i] = max_rpm[i]*360*10/60
      speed = HS_SPEED*max_rpm[i]*360/60/1000 ' in  deg/sec
      hs_movn_to_ticks[i] = HS_MOVN_DEG*1000/speed/HS_TMR_MS
      cpr[i] = abs(getconfig (_EPPR, i+1)) * 4
      evel[i] = 0
      epos[i] = 0
      'print ("i: ", i, " ddps: ", max_ddps[i], " rpm: ", max_rpm[i], "\n")
      'print ("i: ", i, " ticks: ", hs_movn_to_ticks[i], "\n")
   next
   wait (100)
   return
   
CHK_CAN_BUS:
      ' Check for CAN bus commands
   nf = getvalue (_CF, 1)
   if (nf <> 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