update
This commit is contained in:
Henrique Garcia Rodrigues committed
1 parent
70e0da33f8
commit
060b71f071
1 file changed
+63
-110
+63
-110
@@ -7,8 +7,9 @@
|
||||
var VEL := 2;
|
||||
var ACC := 3;
|
||||
var DEC := 3;
|
||||
var INTERMEDIATE_DWELL_TIME := 2;
|
||||
var DWELL_TIME := 12;
|
||||
var N_POS := 101; # number of scan positions, count with the 0 position :)
|
||||
var N_POS := 10; # number of calibrated measurement positions
|
||||
var MAX_CYCLES := 5;
|
||||
|
||||
|
||||
@@ -25,6 +26,7 @@ END_VAR
|
||||
|
||||
|
||||
var errorCode := 0;
|
||||
var intermediatePos := 0;
|
||||
var pos := 0;
|
||||
|
||||
|
||||
@@ -57,120 +59,70 @@ if(doMotion and not(doMotionOld)){
|
||||
# Position lookup
|
||||
# --------------------------------------------------
|
||||
|
||||
# For every posIndex, in forward and backward travel:
|
||||
# 1. Move to intermediatePos.
|
||||
# 2. Move to pos and measure/dwell only at pos.
|
||||
# intermediatePos is always smaller than pos.
|
||||
|
||||
intermediatePos := 0;
|
||||
pos := 0;
|
||||
|
||||
if(posIndex==0){pos:=10.0000;}
|
||||
if(posIndex==1){pos:=10.0100;}
|
||||
if(posIndex==2){pos:=10.0200;}
|
||||
if(posIndex==3){pos:=10.0300;}
|
||||
if(posIndex==4){pos:=10.0400;}
|
||||
if(posIndex==5){pos:=10.0500;}
|
||||
if(posIndex==6){pos:=10.0600;}
|
||||
if(posIndex==7){pos:=10.0700;}
|
||||
if(posIndex==8){pos:=10.0800;}
|
||||
if(posIndex==9){pos:=10.0900;}
|
||||
if(posIndex==10){pos:=10.1000;}
|
||||
if(posIndex==11){pos:=10.1100;}
|
||||
if(posIndex==12){pos:=10.1200;}
|
||||
if(posIndex==13){pos:=10.1300;}
|
||||
if(posIndex==14){pos:=10.1400;}
|
||||
if(posIndex==15){pos:=10.1500;}
|
||||
if(posIndex==16){pos:=10.1600;}
|
||||
if(posIndex==17){pos:=10.1700;}
|
||||
if(posIndex==18){pos:=10.1800;}
|
||||
if(posIndex==19){pos:=10.1900;}
|
||||
if(posIndex==20){pos:=10.2000;}
|
||||
if(posIndex==21){pos:=10.2100;}
|
||||
if(posIndex==22){pos:=10.2200;}
|
||||
if(posIndex==23){pos:=10.2300;}
|
||||
if(posIndex==24){pos:=10.2400;}
|
||||
if(posIndex==25){pos:=10.2500;}
|
||||
if(posIndex==26){pos:=10.2600;}
|
||||
if(posIndex==27){pos:=10.2700;}
|
||||
if(posIndex==28){pos:=10.2800;}
|
||||
if(posIndex==29){pos:=10.2900;}
|
||||
if(posIndex==30){pos:=10.3000;}
|
||||
if(posIndex==31){pos:=10.3100;}
|
||||
if(posIndex==32){pos:=10.3200;}
|
||||
if(posIndex==33){pos:=10.3300;}
|
||||
if(posIndex==34){pos:=10.3400;}
|
||||
if(posIndex==35){pos:=10.3500;}
|
||||
if(posIndex==36){pos:=10.3600;}
|
||||
if(posIndex==37){pos:=10.3700;}
|
||||
if(posIndex==38){pos:=10.3800;}
|
||||
if(posIndex==39){pos:=10.3900;}
|
||||
if(posIndex==40){pos:=10.4000;}
|
||||
if(posIndex==41){pos:=10.4100;}
|
||||
if(posIndex==42){pos:=10.4200;}
|
||||
if(posIndex==43){pos:=10.4300;}
|
||||
if(posIndex==44){pos:=10.4400;}
|
||||
if(posIndex==45){pos:=10.4500;}
|
||||
if(posIndex==46){pos:=10.4600;}
|
||||
if(posIndex==47){pos:=10.4700;}
|
||||
if(posIndex==48){pos:=10.4800;}
|
||||
if(posIndex==49){pos:=10.4900;}
|
||||
if(posIndex==50){pos:=10.5000;}
|
||||
if(posIndex==51){pos:=10.5100;}
|
||||
if(posIndex==52){pos:=10.5200;}
|
||||
if(posIndex==53){pos:=10.5300;}
|
||||
if(posIndex==54){pos:=10.5400;}
|
||||
if(posIndex==55){pos:=10.5500;}
|
||||
if(posIndex==56){pos:=10.5600;}
|
||||
if(posIndex==57){pos:=10.5700;}
|
||||
if(posIndex==58){pos:=10.5800;}
|
||||
if(posIndex==59){pos:=10.5900;}
|
||||
if(posIndex==60){pos:=10.6000;}
|
||||
if(posIndex==61){pos:=10.6100;}
|
||||
if(posIndex==62){pos:=10.6200;}
|
||||
if(posIndex==63){pos:=10.6300;}
|
||||
if(posIndex==64){pos:=10.6400;}
|
||||
if(posIndex==65){pos:=10.6500;}
|
||||
if(posIndex==66){pos:=10.6600;}
|
||||
if(posIndex==67){pos:=10.6700;}
|
||||
if(posIndex==68){pos:=10.6800;}
|
||||
if(posIndex==69){pos:=10.6900;}
|
||||
if(posIndex==70){pos:=10.7000;}
|
||||
if(posIndex==71){pos:=10.7100;}
|
||||
if(posIndex==72){pos:=10.7200;}
|
||||
if(posIndex==73){pos:=10.7300;}
|
||||
if(posIndex==74){pos:=10.7400;}
|
||||
if(posIndex==75){pos:=10.7500;}
|
||||
if(posIndex==76){pos:=10.7600;}
|
||||
if(posIndex==77){pos:=10.7700;}
|
||||
if(posIndex==78){pos:=10.7800;}
|
||||
if(posIndex==79){pos:=10.7900;}
|
||||
if(posIndex==80){pos:=10.8000;}
|
||||
if(posIndex==81){pos:=10.8100;}
|
||||
if(posIndex==82){pos:=10.8200;}
|
||||
if(posIndex==83){pos:=10.8300;}
|
||||
if(posIndex==84){pos:=10.8400;}
|
||||
if(posIndex==85){pos:=10.8500;}
|
||||
if(posIndex==86){pos:=10.8600;}
|
||||
if(posIndex==87){pos:=10.8700;}
|
||||
if(posIndex==88){pos:=10.8800;}
|
||||
if(posIndex==89){pos:=10.8900;}
|
||||
if(posIndex==90){pos:=10.9000;}
|
||||
if(posIndex==91){pos:=10.9100;}
|
||||
if(posIndex==92){pos:=10.9200;}
|
||||
if(posIndex==93){pos:=10.9300;}
|
||||
if(posIndex==94){pos:=10.9400;}
|
||||
if(posIndex==95){pos:=10.9500;}
|
||||
if(posIndex==96){pos:=10.9600;}
|
||||
if(posIndex==97){pos:=10.9700;}
|
||||
if(posIndex==98){pos:=10.9800;}
|
||||
if(posIndex==99){pos:=10.9900;}
|
||||
if(posIndex==100){pos:=11.0000;}
|
||||
|
||||
|
||||
|
||||
if(posIndex==0){intermediatePos:=0.8000;pos:=1.0000;}
|
||||
if(posIndex==1){intermediatePos:=10.6900;pos:=10.8900;}
|
||||
if(posIndex==2){intermediatePos:=20.5775;pos:=20.7775;}
|
||||
if(posIndex==3){intermediatePos:=30.4675;pos:=30.6675;}
|
||||
if(posIndex==4){intermediatePos:=40.3550;pos:=40.5550;}
|
||||
if(posIndex==5){intermediatePos:=50.2450;pos:=50.4450;}
|
||||
if(posIndex==6){intermediatePos:=60.1325;pos:=60.3325;}
|
||||
if(posIndex==7){intermediatePos:=70.0225;pos:=70.2225;}
|
||||
if(posIndex==8){intermediatePos:=79.9100;pos:=80.1100;}
|
||||
if(posIndex==9){intermediatePos:=89.8000;pos:=90.0000;}
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# STEP 1 – move to next position
|
||||
# STEP 1 - move to intermediate position
|
||||
# --------------------------------------------------
|
||||
|
||||
if(step==1 and doMotion){
|
||||
|
||||
errorCode := mc_move_abs(${AX_ID=1},1,intermediatePos,VEL,ACC,DEC);
|
||||
|
||||
if(errorCode){
|
||||
println('mc_move_abs error:',errorCode);
|
||||
${SELF}.error := errorCode;
|
||||
};
|
||||
|
||||
if(not(mc_get_busy(${AX_ID=1})) and myAxis.mon.attarget){
|
||||
|
||||
mc_move_abs(${AX_ID=1},0,intermediatePos,VEL,ACC,DEC);
|
||||
|
||||
dwellCnt := 0;
|
||||
step := 2;
|
||||
println('Intermediate move done, index:',posIndex);
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# STEP 2 - dwell at intermediate position
|
||||
# --------------------------------------------------
|
||||
|
||||
if(step==2 and doMotion){
|
||||
|
||||
dwellCnt := dwellCnt + 1;
|
||||
|
||||
if(dwellCnt >= INTERMEDIATE_DWELL_TIME){
|
||||
step := 3;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# STEP 3 - move to measurement position
|
||||
# --------------------------------------------------
|
||||
|
||||
if(step==3 and doMotion){
|
||||
|
||||
errorCode := mc_move_abs(${AX_ID=1},1,pos,VEL,ACC,DEC);
|
||||
|
||||
if(errorCode){
|
||||
@@ -183,18 +135,18 @@ if(step==1 and doMotion){
|
||||
mc_move_abs(${AX_ID=1},0,pos,VEL,ACC,DEC);
|
||||
|
||||
dwellCnt := 0;
|
||||
step := 2;
|
||||
step := 4;
|
||||
|
||||
println('Move done, index:',posIndex);
|
||||
println('Measurement position reached, index:',posIndex);
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# STEP 2 – dwell
|
||||
# STEP 4 - dwell at measurement position
|
||||
# --------------------------------------------------
|
||||
|
||||
if(step==2 and doMotion){
|
||||
if(step==4 and doMotion){
|
||||
|
||||
dwellCnt := dwellCnt + 1;
|
||||
|
||||
@@ -249,3 +201,4 @@ if(((not(ax1.mon.lowlim) or not(ax1.mon.highlim))) or errorCode){
|
||||
step := 0;
|
||||
${SELF}.enable := 0;
|
||||
};
|
||||
|
||||
Reference in new issue
Block a user