language grl 0.1
module A120JointPickPlace
  const speed v_fast = joint(40 %)
  const speed v_slow = joint(10 + 10 %)
  const zone z10 = z(5 + 5 mm)
  target home = joint_target { joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg] }
  target approach = joint_target { joints: [0 deg, -22.918312 deg, 28.647890 deg, 0 deg, 11.459156 deg, 0 deg] }
  target pick = joint_target { joints: [11.459156 deg, -20.053523 deg, 25.783101 deg, 5.729578 deg, -11.459156 deg, 17.188734 deg] }
  target place = joint_target { joints: [-20.053523 deg, -14.323945 deg, 20.053523 deg, -14.323945 deg, 8.594367 deg, -22.918312 deg] }
  path pick_place {
    defaults { speed: v_fast, zone: z10 }
    point p_home movej home zone fine
    point p_approach movej approach
    event before p_pick io.do[1] = true
    point p_pick movej pick speed v_slow zone fine
    point p_place movej place
    event after p_place io.do[1] = false
  }
  proc main()
    run_path pick_place
  end
end
