roboforum.ru

' Get turret control variables
flt_base = 10000

rate_v  = GetVariable("RATE_V")/flt_base
turret_v  = GetVariable("TURRET_V")/flt_base
turret_Sv = GetVariable("TURRET_V_SPEED")

rate_h  = GetVariable("RATE_H")/flt_base
turret_h  = GetVariable("TURRET_H")/flt_base
turret_Sh = GetVariable("TURRET_H_SPEED")
turret_f  = GetVariable("TURRET_FIRE")
step_counter = GetVariable("STEP_COUNTER")

dX = 0
dY = 0

status = ""
turret_v_initial = -80

nvObjectsTotal = GetVariable("NV_OBJECTS_TOTAL")

if nvObjectsTotal>0 then   ' If any object was found

   ' Get image size
   img_w = GetVariable("IMAGE_WIDTH")
   img_h = GetVariable("IMAGE_HEIGHT")
   
   ' Get array variables of recognized objects
   nvArrObjRectX = GetArrayVariable("NV_ARR_OBJ_RECT_X")
   nvArrObjRectY = GetArrayVariable("NV_ARR_OBJ_RECT_Y")
   nvArrObjRectW = GetArrayVariable("NV_ARR_OBJ_RECT_W")
   nvArrObjRectH = GetArrayVariable("NV_ARR_OBJ_RECT_H")

   ' Get center coordinates of first object from array
   obj_x = nvArrObjRectX(0) + nvArrObjRectW(0)/2
   obj_y = nvArrObjRectY(0) - nvArrObjRectH(0)/2
   
   ' Get difference between object and screen centers
   dX = img_w/2 - obj_x
   dY = img_h/2 - obj_y
   
   dXr = 1 - abs(dX*4/img_w)
   if dXr < 0 then dXr = 0
   
   dYr = 1 - abs(dY*4/img_h)
   if dYr < 0 then dYr = 0
   
   turret_min = -100
   turret_max = 100
   reaction   = 7
   speed_min  = 1
   speed_max  = 100
   filtering  = 0.7
   decay      = 0.1
   threshold  = round(img_w*0.03)

   sRateH = exp(-dXr*reaction)
   sRateV = exp(-dYr*reaction)
   
   rate_h = rate_h + (sRateH - rate_h)*filtering
   rate_v = rate_v + (sRateV - rate_v)*filtering
   
   turret_Sh = round(speed_min + rate_h*(speed_max - speed_min))
   turret_Sv = round(speed_min + rate_v*(speed_max - speed_min))
   
   delta_h = (img_w/8)*rate_h
   delta_v = (img_h/8)*rate_v

  if step_counter =< 0 then
      step_counter = round(exp(-(dXr*dYr)*reaction*0.7)*15)
      
      if dX > threshold then
         ' The object is at left side
         turret_h = turret_h - delta_h
      
         if turret_h < turret_min then turret_h = turret_min
      end if

      if dX < -threshold then
         ' The object is at right side
         turret_h = turret_h + delta_h
      
         if turret_h > turret_max then turret_h = turret_max
      end if
   
      if dY > threshold then
         ' The object is at the bottom
         turret_v = turret_v - delta_v
      
         if turret_v < turret_min then turret_v = turret_min
      end if
   
      if dY < -threshold then
         ' The object is at the top
         turret_v = turret_v + delta_v
      
         if turret_v > turret_max then turret_v = turret_max
      end if
   else
      step_counter = step_counter - 1
   end if
      
   ' Is the target locked?
   if dX < threshold and dX > -threshold and dY < threshold and dY > -threshold then
      status = "Target is locked"
      turret_f = 1
   else
      status = "Tracking"
      turret_f = 0
   end if
else
   ' Back to the center if object is lost
   if turret_h > 0 then turret_h = turret_h - 1
   if turret_h < 0 then turret_h = turret_h + 1
   if turret_v > turret_v_initial then turret_v = turret_v - 1
   if turret_v < turret_v_initial then turret_v = turret_v + 1
   
   turret_Sh = speed_min
   turret_Sv = speed_min
   
   rate_h = rate_h - rate_h*decay
   rate_v = rate_v - rate_v*decay
   
   turret_f = 0
end if

' Set turret control variables
SetVariable "RATE_V", rate_v*flt_base
SetVariable "TURRET_V", turret_v*flt_base
SetVariable "TURRET_V_CONTROL", round(turret_v)
SetVariable "TURRET_V_SPEED", turret_Sv
SetVariable "RATE_H", rate_h*flt_base
SetVariable "TURRET_H", turret_h*flt_base
SetVariable "TURRET_H_CONTROL", round(turret_h)
SetVariable "TURRET_H_SPEED", turret_Sh
SetVariable "TURRET_FIRE", turret_f
SetVariable "STEP_COUNTER", step_counter
SetVariable "DELTA_X", dX
SetVariable "DELTA_Y", dY
SetVariable "TURRET_STATUS", status