Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- MODULE Module1
- VAR bool pirmaskartas:=TRUE;
- CONST robtarget Home:=[[679.548518297,-3.475463124,785.190542715],[0.445563926,-0.003982484,0.895208082,-0.007708267],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST robtarget tarpinis:=[[1056.205080757,0,1143],[0.5,0,0.866025404,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST robtarget nulinis:=[[0,0,0],[0,0,0,1],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST robtarget nulinisKampu:=[[0,0,0],[0.766958477,0,0,-0.641120691],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- VAR robtarget point:=nulinis;
- VAR robtarget midpoint:=nulinis;
- VAR robtarget endpoint:=nulinis;
- VAR robtarget pointk:=nulinisKampu;
- VAR robtarget midpointk:=nulinisKampu;
- VAR robtarget endpointk:=nulinisKampu;
- var num start_x;
- var num start_y;
- var num mid_x;
- var num mid_y;
- var num end_x;
- var num end_y;
- VAR num dir;
- PROC main()
- IF pirmaskartas THEN
- SetDO DO_baigtas250,0;
- SetDO DO_pasukti250Home,0;
- SetDO DO_pasukti250L,0;
- pirmaskartas:=FALSE;
- MoveL Home,v1000,z100,pen\WObj:=wobj0;
- SetDO DO_pasukti250L,1;
- WaitTime 1;
- SetDO DO_pasukti250L,0;
- WaitTime 2;
- SetDO DO_pasukti250Home,1;
- WaitTime 1;
- SetDO DO_pasukti250Home,0;
- WaitTime 2;
- SetDO DO_baigtas250,1;
- ENDIF
- MoveL Home,v1000,z100,pen\WObj:=wobj0;
- WaitDI DI_piesti250,1;
- SetDO DO_baigtas250,0;
- Path_10;
- SetDO DO_pasukti250L,1;
- WaitTime 0.5;
- SetDO DO_pasukti250L,0;
- WaitTime 2;
- Path_10kampu;
- SetDO DO_pasukti250Home,1;
- WaitTime 0.5;
- SetDO DO_pasukti250Home,0;
- WaitTime 2;
- SetDO DO_baigtas250,1;
- ENDPROC
- PROC Path_10()
- MoveL tarpinis,v1000,z100,pen\WObj:=wobj0;
- MoveL nulinis,v1000,z100,pen\WObj:=Box250;
- DL 0,120.1,300,156.3;
- DL 300,156.3,175,500;
- DL 175,500,0,343.69;
- DL 0,343.69,300,375;
- DC 75,250,150,-30 \clockwise;
- DC 75,250,150,210;
- DC 150,0,375,90;
- DC 150,500,375,90 \clockwise;
- MoveL tarpinis,v1000,z100,pen\WObj:=wobj0;
- ENDPROC
- PROC Path_10kampu()
- MoveL tarpinis,v1000,z100,pen\WObj:=wobj0;
- MoveL nulinisKampu,v1000,z100,pen\WObj:=Box250kampu;
- DLkampu 0,120.1,300,156.3;
- DLkampu 300,156.3,175,500;
- DLkampu 175,500,0,343.69;
- DLkampu 0,343.69,300,375;
- DCkampu 75,250,150,-30 \clockwise;
- DCkampu 75,250,150,210;
- DCkampu 150,0,375,90;
- DCkampu 150,500,375,90 \clockwise;
- MoveL tarpinis,v1000,z100,pen\WObj:=wobj0;
- MoveL Home,v1000,z100,pen\WObj:=wobj0;
- ENDPROC
- PROC D(num x, num y)
- point.trans:=[x,y,0];
- SingArea\Wrist;
- movel point,v1000,fine,pen\WObj:=Box250;
- setDO DO_draw250,1;
- ENDPROC
- PROC Dkampu(num x, num y)
- point.trans:=[x,y,0];
- SingArea\Wrist;
- movel point,v1000,fine,pen\WObj:=Box250kampu;
- setDO DO_draw250,1;
- ENDPROC
- PROC Ukampu(num x, num y)
- point.trans:=[x,y,-10];
- SingArea\Wrist;
- setDO DO_draw250,0;
- movel point,v1000,fine,pen\WObj:=Box250kampu;
- ENDPROC
- PROC U(num x, num y)
- point.trans:=[x,y,-10];
- SingArea\Wrist;
- setDO DO_draw250,0;
- movel point,v1000,fine,pen\WObj:=Box250;
- ENDPROC
- PROC DL(num x0, num y0, num x1, num y1)
- ! Draws a 2D line from (x0,y0) to (x1,y1)
- U x0,y0;
- D x0, y0;
- D x1,y1;
- U x1,y1;
- ENDPROC
- PROC DLkampu(num x0, num y0, num x1, num y1)
- ! Draws a 2D line from (x0,y0) to (x1,y1)
- Ukampu x0,y0;
- Dkampu x0, y0;
- Dkampu x1,y1;
- Ukampu x1,y1;
- ENDPROC
- PROC DC(num center_x, num center_y, num radius, num start_angle,
- \switch clockwise, \num end_angle)
- ! Draws a half-circle counter-clockwise from start_angle.
- ! Center_x || Define the circle center x coordinate on the workobject (0..210) on paper
- ! || Must be within the workobject!
- ! Center_y || Define the circle center y coordinate on the workobject (0..-290) on paper
- ! || Must be within the workobject!
- ! Radius || Define the circle radius, recommended range for default half-circle (0..145)
- ! || Must be within the workobject!
- ! Start angle || The starting angle for the circle arc relative to the x axis (0..360)
- ! TO DO: (OPTIONAL) || Enter "Clockwise" to draw the circle clockwise.
- ! TO DO: (OPTIONAL) || end_angle (0..360)
- ! TO DO: || Can draw more or less then half circle if end_angle is specified
- IF Present (clockwise)
- THEN dir := 1;
- ELSE dir := -1;
- ENDIF
- start_x := center_x + radius*sin(start_angle);
- start_y := center_y + radius*cos(start_angle);
- mid_x := center_x + radius*sin(start_angle + dir * 90);
- mid_y := center_y + radius*cos(start_angle + dir * 90);
- end_x := center_x + radius*sin(start_angle + dir * 180);
- end_y := center_y + radius*cos(start_angle + dir * 180);
- IF start_x > 300 AND dir=1 THEN start_y:=start_y-Sqrt(Pow(radius,2)-pow((300-center_x),2)); start_x:=300; ENDIF
- IF start_x > 300 AND dir=-1 THEN start_y:=start_y+Sqrt(Pow(radius,2)-pow((300-center_x),2)); start_x:=300; ENDIF
- IF end_x < 0 AND dir=1 then end_y:=end_y-Sqrt(Pow(radius,2)-pow((0-center_x),2)); end_x:=0; ENDIF
- IF end_x < 0 AND dir=-1 then end_y:=end_y+Sqrt(Pow(radius,2)-pow((0-center_x),2)); end_x:=0; ENDIF
- midpoint.trans:=[mid_x,mid_y,0];
- endpoint.trans:=[end_x,end_y,0];
- U start_x, start_y;
- D start_x, start_y;
- MoveC midpoint,endpoint,v100,fine,pen,\WObj:=Box250;
- U end_x, end_y;
- ENDPROC
- PROC DCkampu(num center_x, num center_y, num radius, num start_angle,
- \switch clockwise, \num end_angle)
- ! Draws a half-circle counter-clockwise from start_angle.
- ! Center_x || Define the circle center x coordinate on the workobject (0..210) on paper
- ! || Must be within the workobject!
- ! Center_y || Define the circle center y coordinate on the workobject (0..-290) on paper
- ! || Must be within the workobject!
- ! Radius || Define the circle radius, recommended range for default half-circle (0..145)
- ! || Must be within the workobject!
- ! Start angle || The starting angle for the circle arc relative to the x axis (0..360)
- ! TO DO: (OPTIONAL) || Enter "Clockwise" to draw the circle clockwise.
- ! TO DO: (OPTIONAL) || end_angle (0..360)
- ! TO DO: || Can draw more or less then half circle if end_angle is specified
- IF Present (clockwise)
- THEN dir := 1;
- ELSE dir := -1;
- ENDIF
- start_x := center_x + radius*sin(start_angle);
- start_y := center_y + radius*cos(start_angle);
- mid_x := center_x + radius*sin(start_angle + dir * 90);
- mid_y := center_y + radius*cos(start_angle + dir * 90);
- end_x := center_x + radius*sin(start_angle + dir * 180);
- end_y := center_y + radius*cos(start_angle + dir * 180);
- IF start_x > 300 AND dir=1 THEN start_y:=start_y-Sqrt(Pow(radius,2)-pow((300-center_x),2)); start_x:=300; ENDIF
- IF start_x > 300 AND dir=-1 THEN start_y:=start_y+Sqrt(Pow(radius,2)-pow((300-center_x),2)); start_x:=300; ENDIF
- IF end_x < 0 AND dir=1 then end_y:=end_y-Sqrt(Pow(radius,2)-pow((0-center_x),2)); end_x:=0; ENDIF
- IF end_x < 0 AND dir=-1 then end_y:=end_y+Sqrt(Pow(radius,2)-pow((0-center_x),2)); end_x:=0; ENDIF
- midpointk.trans:=[mid_x,mid_y,0];
- endpointk.trans:=[end_x,end_y,0];
- Ukampu start_x, start_y;
- Dkampu start_x, start_y;
- MoveC midpointk,endpointk,v100,fine,pen,\WObj:=Box250kampu;
- Ukampu end_x, end_y;
- ENDPROC
- PROC kampu()
- MoveL nulinisKampu,v1000,z100,pen\WObj:=Box250kampu;
- ENDPROC
- ENDMODULE
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement