Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Superimposed jog moves
#1
close

/****************************************/
// This PLC program implements a "motor jog" function on a virtual motor
// The virtual motor position acts as a master position to the X, Y, & Z motors
// Depending on the rotation of the XYZ coordinate frame, the master scale
// factor to each of the motors changes.
// With no rotation, Xmsf=0.0, Ymsf=0.0, Zmsf=1.0
// With an XYZ matrix rotation, the following transformation applies:
// [ Xmsf ] [Diag[6] XYZ[0] XYZ[1]] [ 0.0 ]
// [ Ymsf ] = [ XYZ[2] Diag[7] XYZ[3]] x [ 0.0 ]
// [ Zmsf ] [ XYZ[4] XYZ[5] Diag[8]] [ 1.0 ]
//
// This reduces to the following uncoupled equations:
// Xmsf = XYZ[1]
// Ymsf = XYZ[3]
// Zmsf = Diag[8]
// This vector must then be normalized to unit length

ptr GapMotorJogPlusButton->u.io:$A00000.14.1;
ptr GapMotorJogMinusButton->u.io:$A00000.15.1;
global GapJogIncDist = 100;
global GapJogMode;


open plc 29

local ThisCs;
local NormVecLen;
local GapJogDecelDist;
local GapJogPlusLockOut;
local GapJogMinusLockOut;

// Calculate normal vector length for rotated XY plane
ThisCs = 1;
NormVecLen = Tdata[ThisCs].XYZ[1] * Tdata[ThisCs].XYZ[1];
NormVecLen += Tdata[ThisCs].XYZ[3] * Tdata[ThisCs].XYZ[3];
NormVecLen += Tdata[ThisCs].Diag[8] * Tdata[ThisCs].Diag[8];
NormVecLen = sqrt(NormVecLen);
// Use virtual gap motor position as master for X,Y,Z motors
Motor[1].pMasterEnc=EncTable[5].a;
Motor[2].pMasterEnc=EncTable[5].a;
Motor[3].pMasterEnc=EncTable[5].a;

// Calculate master scale factors based on unit normal vector
Motor[1].MasterPosSf = Tdata[ThisCs].XYZ[1] / NormVecLen;
Motor[2].MasterPosSf = Tdata[ThisCs].XYZ[3] / NormVecLen;
Motor[3].MasterPosSf = Tdata[ThisCs].Diag[8] / NormVecLen;
// Turn on following for XYZ motors in offset (superimpose) mode
Motor[1].MasterCtrl = 3;
Motor[2].MasterCtrl = 3;
Motor[3].MasterCtrl = 3;

if (GapJogMode == 1) { // Incremental mode: 1 move per button pressed
if (GapMotorJogPlusButton == 1) { // Plus button pressed
if (GapJogPlusLockOut == 0) { // Action not locked out
jog5: (GapJogIncDist); // Issue incremental plus jog command
GapJogPlusLockOut = 1; // Lock out further plus jogs for now
GapJogMinusLockOut = 1; // Lock out minus jogs for now
}
}
else { // Plus button not pressed
if (Motor[5].DesVel == 0 && GapMotorJogMinusButton == 0) {
GapJogPlusLockOut = 0; // Permit further plus jogs
GapJogMinusLockOut = 0; // Permit minus jogs
}
}

if (GapMotorJogMinusButton == 1) { // Minus button pressed
if (GapJogMinusLockOut == 0) { // Action not locked out
jog5: (-GapJogIncDist); // Issue incremental minus jog command
GapJogMinusLockOut = 1; // Lock out further minus jogs for now
GapJogPlusLockOut = 1; // Lock out plus jogs for now
}
}
else { // Minus button not pressed
if (Motor[5].DesVel == 0 && GapMotorJogPlusButton == 0) {
GapJogMinusLockOut = 0; // Permit further minus jogs
GapJogPlusLockOut = 0; // Permit plus jogs
}
}
}
if (GapJogMode == 2) { // Continuous mode: keep moving until released
// Compute jog decel distance -- assumes JogTa<0 to specify rate of acc/dec
GapJogDecelDist = -(Motor[5].JogSpeed * Motor[5].JogSpeed * Motor[5].JogTa) / 2;

if (GapMotorJogPlusButton == 1) { // Plus button pressed
if (GapJogPlusLockOut == 0) { // Action not locked out
jog5: (GapJogDecelDist); // Command incremental jog from present
GapJogMinusLockOut = 1; // Lock out minus jogs for now
}
}
else GapJogMinusLockOut = 0; // Plus button not pressed; permit minus jogs

if (GapMotorJogMinusButton == 1) { // Minus button pressed
if (GapJogMinusLockOut == 0) { // Action not locaked out
jog5: (-GapJogDecelDist); // Command incremental jog from present
GapJogPlusLockOut = 1; // Lock out plus jogs for now
}
}
else GapJogPlusLockOut = 0; // Minus button not pressed; permit plus jogs
}
close
Reply


Forum Jump:


Users browsing this thread: 1 Guest(s)