Difference between revisions of "3. Interfacing with the Actual Allegro Hand"

 function Awake()
 	print("Begin Allegro Hand")
 	version()
 
	-- If you just want to simulate, comment "rtTimer" and "createRTSystem"
	-- and uncomment loadModel
	-- to change CAN interface, change "createRTSystem" line
 
 	--loadModel("hand", "models/Etc/ERHand/ERHand.aml")
 	rtTimer = createTimer("RTTimer", rTimer_MODE_REAL_WIN32)
 	print(rtTimer)
 	createRTSystem("models/Etc/ERHand/ERHandL_RT_NICAN.aml", "RTSys", rtTimer)
 	createController("RTController", "RTSys") --, 1, "controls/control_ERHand.xdl", "")
 	setControlAlgorithm("RTController", "controls/control_ERHandL.xdl")
 	--setNominalSystem("HandController", "")
 end
 
  function Update()
  	-- body
  end
 
 BH_READY = RCMD_USER + 2
 CAN_CMD_RESET_ENC = 4
 
 
  function OnGUI()
 
    if (GUIButton(5, 5, 120, 25, "On")) then
      onDevice("RTSys", "COMM")
      sendCommand("RTController", RESERVED_CMD_SERVO_ON)
    end
 
    if (GUIButton(130, 5, 120, 25, "Off")) then
      --offDevice("RTSys", "COMM")
      --sendCommand("RTController", RESERVED_CMD_SERVO_OFF)
	  sendCommand("RTController", RCMD_USER + 0)
	  print("None")
    end
 
  	if (GUIButton(5, 35, 120, 25, "Home")) then
  		sendCommand("RTController", RCMD_GO_HOME, VK_H)
  		print("Home button pressed")
  	end
  	if (GUIButton(130, 35, 120, 25, "Ready")) then
  		sendCommand("RTController", BH_READY, VK_R)
  		print("Ready button pressed")
  	end
 
    if (GUIButton(5, 65, 120, 25, "GRASP 3")) then
  		sendCommand("RTController", RCMD_USER + 3, VK_G)
  		print("Grasp 3 button pressed")
  	end
    if (GUIButton(130, 65, 120, 25, "GRASP 4")) then
      sendCommand("RTController", RCMD_USER + 4)
    end
 
  	if (GUIButton(5, 95, 120, 25, "PINCHING(I)")) then
  		sendCommand("RTController", RCMD_USER + 5, VK_P)
  		print("Pinching button pressed")
  	end
    if (GUIButton(130, 95, 120, 25, "PINCHING(M)")) then
      sendCommand("RTController", RCMD_USER + 6)
    end
 
    if (GUIButton(5, 125, 245, 25, "Envelop")) then
      sendCommand("RTController", RCMD_USER + 8)
      print("Envelop")
    end
 
    if (GUIButton(5, 155, 245, 25, "Show Off")) then
      sendCommand("RTController", RCMD_USER + 50)
      print("Show Off")
    end
 
 
	if (GUIButton(5, 185, 120, 25, "Test Encoders")) then
 
	  for i=1,16 do
		encName = "enc"..i
 
		if i == 1 then
			print(" ")
			print("Finger 1")
		end
 
		if i == 5 then
			print(" ")
			print("Finger 2")
		end
 
		if i == 9 then
			print(" ")
			print("Finger 3")
		end
 
		if i == 13 then
			print(" ")
			print("Finger 4")
		end
 
		deviceCommand("RTSys", encName, CAN_CMD_RESET_ENC)
		--print("Test Encoders")
	  end
 
    end
 
	if (GUIButton(130, 185, 120, 25, "Test Motors")) then
      sendCommand("RTController", RCMD_USER + 99)
      print("Test Motors")
    end
 
 
    width = (ScreenWidth-10)/4
    --print(width)
    top = ScreenHeight - 35*5;
 
 
    for i=1,4 do
      for j=1,4 do
        context = "Finger"..i.."_Link"..j
        pos = jointPosition("RTSys", context)
        context=context..string.format(": %.3f",math.deg(pos))
        GUILabel(10 + (i-1)*width, top + 35*(j-1), width-10, 15, context)
        GUISlider(10 + (i-1)*width, top+ 15 + 35*(j-1), width-10, 16, math.deg(pos), -180, 180)
      end
    end
 
 
  end

createRTSystem("models/Etc/ERHand/ERHandL_RT_NICAN.aml", "RTSys", rtTimer)