CS_2jack.pmc

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)
 All Classes Files Functions Variables Typedefs Defines