adding cryojet and other stuff...
This commit is contained in:
@@ -44,4 +44,13 @@
|
||||
!motor(mot=5,dirCur=1800,JogSpeed=40,invDir=False)
|
||||
|
||||
|
||||
!holding_current(m1=[0,1000],m2=[0,1000],m3=[0,1000],m4=[0,1000],m5=[1400,1800])
|
||||
//Cryojet
|
||||
//---------
|
||||
//Max. 1.5A -1024000=-10mm
|
||||
//posSf = userUnits/encoder_steps =10000/1024000 =10./1024
|
||||
//servoSf=motor_u_steps/userUnits = 1024000/10000. = 102.4
|
||||
!encoder_sim(enc=6,posSf=10./1024)
|
||||
!motor(mot=6,dirCur=1500,JogSpeed=10,invDir=False,servoSf=102.4)
|
||||
|
||||
|
||||
!holding_current(m1=[0,1000],m2=[0,1000],m3=[0,1000],m4=[0,1000],m5=[1400,1800],m6=[0,500])
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
def coordTrf(fileParser,kwargs):
|
||||
def MX3_coordTrf(fileParser,kwargs):
|
||||
from math import sqrt
|
||||
'''
|
||||
--- mandatory ---
|
||||
@@ -49,84 +49,69 @@ S=D*X
|
||||
Coord[1].Ta=100
|
||||
Coord[1].Td=100
|
||||
Coord[1].AltFeedRate=1
|
||||
|
||||
open forward
|
||||
define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
||||
define(r='L6',scl='L7')
|
||||
define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
||||
define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
||||
r=$height/$width
|
||||
scl=$exc/$sqrt2
|
||||
S1=scl*sin(AA1*$camSf)
|
||||
S2=scl*sin(AA2*$camSf)
|
||||
S3=scl*sin(AA3*$camSf)
|
||||
S4=scl*sin(AA4*$camSf)
|
||||
S5=scl*sin(AA5*$camSf)
|
||||
|
||||
//debug code
|
||||
P1=S1
|
||||
P2=S2
|
||||
P3=S3
|
||||
P4=S4
|
||||
P5=S5
|
||||
P6=P6+1
|
||||
|
||||
//X=D^-1*S
|
||||
X=+.5*S1 -.5*S2 +(r+.5)*S3 +(r-.5)*S4 +(.5-r)*$sqrt2*S5
|
||||
Y=+.5*S1 +.5*S2 +.25*S3 +.25*S4 +.25*$sqrt2*S5
|
||||
U= -.5*S3 -.5*S4 +.5$sqrt2*S5
|
||||
V=-.5*S1 +1.*S2 +1.5*S3 -.5*S4 -.5*$sqrt2*S5
|
||||
W=-.5*S1 -1.*S2 +.5*S3 +.5*S4 +.5*$sqrt2*S5
|
||||
|
||||
D0=$$000000f8; //U=$$8 V=$$10 W=$$20 X=$$40 Y=$$80 hex(8+int('10',16)+int('20',16)+int('40',16)+int('80',16)) -> '0xf8'
|
||||
close
|
||||
|
||||
open inverse
|
||||
define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
||||
define(r='L6',scl='L7')
|
||||
define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
||||
define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
||||
|
||||
r=$height/$width
|
||||
|
||||
S1=+.5*X +.5*Y +(0) *U -.5*V +.5*W
|
||||
S2=-.5*X +.5*Y +(0) *U +.5*V +.5*W
|
||||
S3=+.5*X +.5*Y -(.5) *U +.5*V -.5*W
|
||||
S4=-.5*X +.5*Y -(.5) *U -.5*V -.5*W
|
||||
S5= .5*$sqrt2*Y +.5*$sqrt2*U -.5*$sqrt2*W
|
||||
|
||||
//debug code
|
||||
P11=S1
|
||||
P12=S2
|
||||
P13=S3
|
||||
P14=S4
|
||||
P15=S5
|
||||
P16=P16+1
|
||||
|
||||
scl=$sqrt2/$exc
|
||||
AA1=asin(S1*scl)/$camSf
|
||||
AA2=asin(S2*scl)/$camSf
|
||||
AA3=asin(S3*scl)/$camSf
|
||||
AA4=asin(S4*scl)/$camSf
|
||||
AA5=asin(S5*scl)/$camSf
|
||||
|
||||
//debug code
|
||||
P21=AA1
|
||||
P22=AA2
|
||||
P23=AA3
|
||||
P24=AA4
|
||||
P25=AA5
|
||||
|
||||
close
|
||||
|
||||
//#1..5j/
|
||||
//#1..5hmz
|
||||
&1
|
||||
|
||||
|
||||
Motor[1].MaxSpeed=Motor[1].JogSpeed
|
||||
Motor[2].MaxSpeed=Motor[2].JogSpeed
|
||||
Motor[3].MaxSpeed=Motor[3].JogSpeed
|
||||
Motor[4].MaxSpeed=Motor[4].JogSpeed
|
||||
Motor[5].MaxSpeed=Motor[5].JogSpeed
|
||||
|
||||
open forward
|
||||
define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
||||
define(r='L6',scl='L7')
|
||||
define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
||||
define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
||||
r=$height/$width
|
||||
scl=$exc/$sqrt2
|
||||
S1=scl*sin(AA1*$camSf)
|
||||
S2=scl*sin(AA2*$camSf)
|
||||
S3=scl*sin(AA3*$camSf)
|
||||
S4=scl*sin(AA4*$camSf)
|
||||
S5=scl*sin(AA5*$camSf)
|
||||
|
||||
send 1"fwd_inp(%f) %f %f %f %f %f\\n",D0,AA1,AA2,AA3,AA4,AA5
|
||||
send 1"fwd_inp(%f) %f %f %f %f %f\\n",D0,S1,S2,S3,S4,S5
|
||||
|
||||
|
||||
//X=D^-1*S
|
||||
X=+.5*S1 -.5*S2 +(r+.5)*S3 +(r-.5)*S4 +(.5-r)*$sqrt2*S5
|
||||
Y=+.5*S1 +.5*S2 +.25*S3 +.25*S4 +.25*$sqrt2*S5
|
||||
U= -.5*S3 -.5*S4 +.5*$sqrt2*S5
|
||||
V=-.5*S1 +1.*S2 +1.5*S3 -.5*S4 -.5*$sqrt2*S5
|
||||
W=-.5*S1 -1.*S2 +.5*S3 +.5*S4 +.5*$sqrt2*S5
|
||||
|
||||
send 1"fwd_res %f %f %f %f %f\\n",X,Y,U,V,W
|
||||
|
||||
D0=$$000000f8; //U=$$8 V=$$10 W=$$20 X=$$40 Y=$$80 hex(8+int('10',16)+int('20',16)+int('40',16)+int('80',16)) -> '0xf8'
|
||||
close
|
||||
|
||||
open inverse
|
||||
define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
||||
define(r='L6',scl='L7')
|
||||
define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
||||
define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
||||
|
||||
//if(D0>0)
|
||||
// send 1"Velocity calculation NOT SUPPORTED\\n"
|
||||
|
||||
send 1"inv_inp(%f) %f %f %f %f %f\\n",D0,X,Y,U,V,W
|
||||
|
||||
r=$height/$width
|
||||
|
||||
S1=+.5*X +.5*Y +(0) *U -.5*V +.5*W
|
||||
S2=-.5*X +.5*Y +(0) *U +.5*V +.5*W
|
||||
S3=+.5*X +.5*Y -(.5) *U +.5*V -.5*W
|
||||
S4=-.5*X +.5*Y -(.5) *U -.5*V -.5*W
|
||||
S5= .5*$sqrt2*Y +.5*$sqrt2*U -.5*$sqrt2*W
|
||||
|
||||
scl=$sqrt2/$exc
|
||||
AA1=asin(S1*scl)/$camSf
|
||||
AA2=asin(S2*scl)/$camSf
|
||||
AA3=asin(S3*scl)/$camSf
|
||||
AA4=asin(S4*scl)/$camSf
|
||||
AA5=asin(S5*scl)/$camSf
|
||||
|
||||
send 1"inv_res %f %f %f %f %f\\n",AA1,AA2,AA3,AA4,AA5
|
||||
|
||||
close
|
||||
|
||||
''',kw)
|
||||
|
||||
@@ -20,3 +20,31 @@ Motor[4].pLimits=0;Motor[4].AmpFaultLevel=0;Motor[4].pAmpEnable=0;Motor[4].pAmpF
|
||||
!encoder_sim(enc=5,tbl=5,mot=5)
|
||||
!motor(mot=5,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[5].pLimits=0;Motor[5].AmpFaultLevel=0;Motor[5].pAmpEnable=0;Motor[5].pAmpFault=0
|
||||
|
||||
|
||||
// //NEW:
|
||||
// //the motor has 512*200*100 usteps per revolution (512 uystep/step, 200 steps/rev 1:100 gear)
|
||||
// //the ssi-encoder has 2**18=262144 steps per revolution
|
||||
// //motor_u_steps/inc_enc_step=39.0625
|
||||
// //JogSpeed=102.4 ustep/ms=102400ustep/s = 512*200= 1 rev/sec
|
||||
// posSf : position scale factor: encoder position to motor u-steps, 1 motor step is 512 motor u-steps
|
||||
// motor_u_steps=posSf*encoder_steps -> posSf = motor_u_steps/encoder_steps
|
||||
// default is 1.0 (1 motor_u_steps = 1 encoder_steps)
|
||||
|
||||
// !encoder_ssi(enc=1,numBits=18,posSf=39.0625)
|
||||
// !encoder_ssi(enc=2,numBits=18,posSf=39.0625)
|
||||
// !encoder_ssi(enc=3,numBits=18,posSf=39.0625)
|
||||
// !encoder_ssi(enc=4,numBits=18,posSf=39.0625)
|
||||
// !encoder_ssi(enc=5,numBits=18,posSf=39.0625)
|
||||
|
||||
// !motor(mot=1,dirCur=600,JogSpeed=102.4)
|
||||
// !motor(mot=2,dirCur=600,JogSpeed=102.4)
|
||||
// !motor(mot=3,dirCur=600,JogSpeed=102.4)
|
||||
// !motor(mot=4,dirCur=600,JogSpeed=102.4)
|
||||
// !motor(mot=5,dirCur=600,JogSpeed=102.4)
|
||||
|
||||
// !cm_brake(mot=1,curOn=600,cs=1,delay=200,gpio=16)
|
||||
// !cm_brake(mot=2,curOn=600,cs=1,delay=200,gpio=17)
|
||||
// !cm_brake(mot=3,curOn=600,cs=1,delay=200,gpio=18)
|
||||
// !cm_brake(mot=4,curOn=600,cs=1,delay=200,gpio=19)
|
||||
// !cm_brake(mot=5,curOn=600,cs=1,delay=200,gpio=20)
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
|
||||
!MX3_setup()
|
||||
//!MX3_home()
|
||||
!MX3_coordTrf()
|
||||
//using 360000 for 1 rev. -> 1.7453292519943296e-05=np.pi/180000
|
||||
!MX3_coordTrf(exc=1000,height=2000,width=3000,length=4000,camSf=1.7453292519943296e-05)
|
||||
Reference in New Issue
Block a user