Go to the documentation of this file.00001 CLOSE
00002
00003 ;################################################
00004 ;; \file
00005 ;; Define motion for 2 jack system
00006 ;;
00007 ;; Original Author: Ronaldo Mercado
00008 ;;
00009 ;; Defined axes:
00010 ;; - X (addr 6) = height of surface in EGUs, PIVOT away from J1
00011 ;; - Y (addr 7) = angle of the surface in i15 units
00012 ;;
00013 ;; Macros:
00014 ;; - COORD = $(COORD) CS number, e.g. 2
00015 ;; - PLC = $(PLC) PLC number, should be CS number+15, e.g. 17
00016 ;; - J1 = $(J1) Axisnum for Jack 1, e.g. 1
00017 ;; - J2 = $(J2) Axisnum for Jack 2, e.g. 2
00018 ;; - DIST = $(DIST) Distance between 2 jacks when they are in the zero position
00019 ;; - PIVOT = $(PIVOT) Distance from jack 1 to pivot point of the surface
00020 ;; - DEPTH = $(DEPTH) Depth of the surface on the mount
00021 ;################################################
00022
00023 ; Change to CS$(COORD)
00024 &$(COORD)
00025
00026 ; Set relevant axes to use kinematics
00027 #$(J1)->I ; +ve blade
00028 #$(J2)->I ; -ve blade
00029
00030 ; These are set by motor_in_cs.template
00031 #define J1MOVE P(4700+$(J1))
00032 #define J1MRES P(4800+$(J1))
00033 #define J1OFF P(4900+$(J1))
00034 #define J2MOVE P(4700+$(J2))
00035 #define J2MRES P(4800+$(J2))
00036 #define J2OFF P(4900+$(J2))
00037
00038 ;; This is the distance between the 2 jacks when they are in the zero position
00039 #define DIST Q20
00040 DIST = $(DIST)
00041 ;; This is the distance from jack 1 to pivot point of the surface
00042 #define PIVOT Q21
00043 PIVOT = $(PIVOT)
00044 ;; This is the depth of the surface on the mount
00045 #define DEPTH Q22
00046 DEPTH = $(DEPTH)
00047
00048 ; Calculate height and angle from jack positions
00049 #define J1POS (J1MRES*P$(J1)+J1OFF)
00050 #define J2POS (J2MRES*P$(J2)+J2OFF)
00051 #define HEIGHT Q7
00052 #define ANGLE Q8
00053 ; Local variables
00054 #define RATIO Q128
00055 OPEN FORWARD
00056 CLEAR
00057 ; this is the difference in height of the jacks divided by their distance apart
00058 RATIO=(J2POS-J1POS)/DIST
00059 ; this is the angle of the surface
00060 ANGLE=atan(RATIO)
00061 ; the height of the surface
00062 HEIGHT=J1POS+RATIO*PIVOT+DEPTH/cos(ANGLE)
00063 CLOSE
00064
00065 ; Calculate jack positions from height and angle
00066 #define SURFACE Q228
00067 #define TANTHETA Q229
00068 #define J1POS Q230
00069 #define J2POS Q231
00070 OPEN INVERSE
00071 CLEAR
00072 ; this is the bottom edge of the surface
00073 SURFACE=HEIGHT-DEPTH/cos(ANGLE)
00074 ; store tan theta
00075 TANTHETA=tan(ANGLE)
00076 ; work out the jack positions in EGUs
00077 J1POS = SURFACE-PIVOT*TANTHETA
00078 J2POS = SURFACE+(DIST-PIVOT)*TANTHETA
00079 ; then cts
00080 P$(J1)=(J1POS-J1OFF)/J1MRES
00081 P$(J2)=(J2POS-J2OFF)/J2MRES
00082 CLOSE
00083
00084 ; A PLC(sx+15) needs to be made to do position reporting
00085 ; Readbacks should be in &{axisnum}Q81..89
00086 ; As forward kinematic, but with Px = mx62/(Ix08*32)
00087 #define J1POS (J1MRES*m$(J1)62/(I$(J1)08*32)+J1OFF)
00088 #define J2POS (J2MRES*m$(J2)62/(I$(J2)08*32)+J2OFF)
00089 #define HEIGHT Q87
00090 #define ANGLE Q88
00091 ; Local variables
00092 #define RATIO Q328
00093 OPEN PLC$(PLC)
00094 CLEAR
00095 ADDRESS&$(COORD)
00096 ; this is the difference in height of the jacks divided by their distance apart
00097 RATIO=(J2POS-J1POS)/DIST
00098 ; this is the angle of the surface
00099 ANGLE=atan(RATIO)
00100 ; the height of the surface
00101 HEIGHT=J1POS+RATIO*PIVOT+DEPTH/cos(ANGLE)
00102 ; If jack motor record did the last move, set demands = readbacks
00103 if (J1MOVE = 1)
00104 or (J2MOVE = 1)
00105 J1MOVE = 0
00106 J2MOVE = 0
00107 Q77 = Q87
00108 Q78 = Q88
00109 endif
00110 CLOSE
00111 ENABLE PLC$(PLC)