Moving robot to home position

Radek_C

Hi, I want to make a specific homing path:
1. Get current position
2. Move current position 20 mm up
3. Move 2,3,4,5,6 axes  from point 2 to home position
4. Move 1 axis from point 3 to home position

I stucked on point 3...
That's my code for now:

VAR robtarget current_pos;
VAR bool do_home := false;

proc homing()
        current_pos := CRobT(\Tool:=Gripper_1,\WObj:=wobj0);
        current_pos := Offs(current_pos, 0, 0, 20);
        MoveJ current_pos,v200,fine,Gripper_1\WObj:=wobj0;
endproc

What is more I also want to execute this procedure at any moment during the programme (which has few procedures) when digital output do_home get "1". So:

If do_home then
       homing;
endif

Should I put it in the beginning of the programme or in the another specific section?

kioog

hello,
what you can do is calculate your homeposition in axis values with the function CalcJointT.
in your point 3, you look at the actual joint position with CJointT.
then you make a jointtarget with the actual position of axis 1 and for axis 2-6 you use the homeposition axis
if you have stored the position in a jointtarget named current_jpos you get the value of axis 1 with n_ax1:=current_jpos.robax.rax_1
then you use the instruction moveAbsJ to move to your jointtarget.
You can also use the calcJointT function to check if your robot can reach your calculated positions, for exemple when you want to move 1000mm up from a point. 

when you want to execute your routine at any moment you can use an interrupt, check the command CONNECT in the manual.

Radek_C

I made this procedure on another way. I checked it and It's working correct, what do u think about this?

PROC homing()

        current_pos:=CRobT(\Tool:=Gripper_1,\WObj:=wobj0);
        current_pos:=Offs(current_pos,0,0,20);
        home1 :=[[current_pos.trans.x,home.trans.y,home.trans.z],[home.rot.q1,home.rot.q2,home.rot.q3,home.rot.q4],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        MoveJ current_pos,v200,fine,Gripper_1\WObj:=wobj0;
        waittime 2;
        MoveJ home1,v200,fine,Gripper_1\WObj:=wobj0;
        waittime 2;
        MoveJ home,v200,fine,Gripper_1\WObj:=wobj0;
        waittime 2;

ENDPROC

kioog

this way your robot will not move in X direction, but can move in axis 1, but if it works for your situation, it is ok.

Radek_C

I have problem with interrupts. During execution (when I want to restore path) there is an error - missing error handler. How to connect ERROR with TRAP? Or maybe there is another way to solve it ?


TRAP homing
        StopMove;
        StorePath;
        current_pos:=CRobT(\Tool:=Gripper_1,\WObj:=wobj0);
        current_pos:=Offs(current_pos,0,0,20);
        home1:=[[current_pos.trans.x,home.trans.y,home.trans.z],[home.rot.q1,home.rot.q2,home.rot.q3,home.rot.q4],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        MoveJ current_pos,v200,fine,Gripper_1\WObj:=wobj0;
        waittime 2;
        MoveJ home1,v200,fine,Gripper_1\WObj:=wobj0;
        waittime 2;
        MoveJ home,v200,fine,Gripper_1\WObj:=wobj0;
        waittime 2;
        RestoPath;
        StartMove;

ENDTRAP

PROC main()
        CONNECT signal WITH homing;
        ISignalDI di_home0,1,signal;   

Radek_C

I've just changed startmove to stopmovereset and problem doesn't occur anymore.

Radek_C

I have another problem. I'm using Collision detection and don't know how to get digital output when collision occurs.I can't find it in any collision's option.

lemster68

You need to configure a system output for motion supervision triggered.

Radek_C

Is there any option to come back to previous position if I don't know coordinates?

Alberto

I made something like that with an IRB460 robot. When a collision occurs the motion supervision system specifies (or make it TRUE) an error domain called MOTION_ERR, you can link this to a TRAP routine and make high a specific output. This is my code:


PROC main()

    VAR intnum supervisión_movimiento_error;

    WHILE  TRUE  DO

            <<Something else>>

            !Colisión detectada por el sistema: "MOTION SUPERVISION".

            IDelete supervisión_movimiento_error;

            CONNECT supervisión_movimiento_error WITH trap4_FATAL;

            IError MOTION_ERR,TYPE_ERR,supervisión_movimiento_error;
            
            <<Something else>>
    ENDWHILE

TRAP trap4_FATAL

        !Código para alertar una colision del robot.

        TPErase;

        TPWrite "EL SISTEMA DE SUPERVISION DE MOVIMIENTO DETECTO FALLA";

        Set DO10_11;

        Stop;

    ENDTRAP


I was in the same trouble (well, I guess is what you want) because I wanted to send a digital input to a PLC in order to alert it that something went wrong and It was necessary to stop all the systems that belong to the Robotic cell, but I could not find a specific output already defined to this proposal on the IRC5.

This code is one way. Does it work for you?