HOME > RT-C Language
Controller > Sample "Servo Motor Control"
 Japanese |
| （1）EtherCAT® Communication OPEN | |
 |
① Do RSI-ECAT-Master OPEN process |
| （2）ESM goes to OPERATION | |
|
① EtherCAT® state goes to OPERATIONAL ② Then, check whether state change is finished or not. after that go to next step. |
| （3）Detect Servo Slave | |
|
① Detect target Servo Amp ② Get "Alias" and "R/W position on Process Image" of servo to use it as position control parameter settings |
| （4）Settings for servo operation mode | |
|
① Set as "Cycle Synchronous Position Mode" ② You can change by setting object "Modes of Operation" as "8". |
| （5）Initialize command position to current position. | |
|
① Set value of command position as value of current position. ② It is because of prevention for rapid movement to unexpected position when servo turns ON. ③ Read value of "Actual Position" and write this value to "Target position". |
| （6）Go to Servo ON state | |
|
① Go to Servo ON state. When Servo is ON, servo motre is in an overexcitation state. ② Let "PDS (Power Drive System)" move to "Operation Enabled". ③ Set value to "Control word", check state in "Status word" (On more details, please check CiA402 specification.) |
| （7）Waiting process for Servo brake release | |
|
① Wait for 1 sec to wait for Servo brake release |
| （8）Position Controll Process | |
|
① Predefined position information are notified to servo amp in order per the period of __Process()
call. ② It does acceleration and deceleration with right 30 rotation (4 sec) & left 30 rotation (4 sec) - total 60 rotation (8 sec). |
|
namespace MotionDemo
{
[FUNCTION_BLOCK]
public class program
{
/***********************************************************************
* SLAVE DEFINE
************************************************************************/
private readonly static uint VENDOR_ID = 441;
private readonly static uint PRODUCT_CODE = 2;
private readonly static uint REVISION_NO = 3;
// PI/IN
private readonly static uint STATUSWORD_OFFSET = 0;
private readonly static uint POS_ACUTUAL_OFFSET = 4;
// PI/OUT
private readonly static uint CONTROLWORD_OFFSET = 0;
private readonly static uint TARGET_POS_OFFSET = 2;
public Boolean IsError = false;
public Boolean IsDone = false;
private EhApi hEcat = null;
private ushort Alias = 0;
private uint ViosInBaseOffset = 0;
private uint ViosOutBaseOffset = 0;
private uint ActualPos = 0;
private uint TargetPos = 0;
private ushort ControlWord = 0;
private ushort StatusWord = 0;
private uint rcpIdx = 0;
private uint loopCnt = 0;
private Recipe recipeData = null;
/***********************************************************************
* Constructor
************************************************************************/
public program()
{
hEcat = new EhApi();
}
private bool EndProcess()
{
IsDone = true;
return IsDone;
}
private bool ErrorInit()
{
IsError = true;
return IsError;
}
/***********************************************************************
*CiA402 Servo Driver state transition process
************************************************************************/
ushort fnChgMotionGoState(ushort woStWd, ref ushort pCrWd)
{
switch ((0x6F & woStWd))
{
// Not initialized -> Request initialization
case 0x00: pCrWd = (ushort)((0xFF70 & pCrWd) | 0x00); break;
// Initialized -> Request turn off main circuit power
case 0x40: // (0x60と同じ)
case 0x60: pCrWd = (ushort)((0xFF70 & pCrWd) | 0x06); break;
// Main circuit power OFF -> Request servo ready
case 0x21: pCrWd = (ushort)((0xFF70 & pCrWd) | 0x07); break;
// Servo ready -> Request turn on Servo
case 0x23: pCrWd = (ushort)((0xFF70 & pCrWd) | 0x0F); break;
// Servo ON (=Start operation is OK.)
case 0x27: break;
// Abnornal State
case 0x2F: break;
// Abnornal State -> Request initialization
case 0x28: pCrWd = (ushort)((0xFF70 & pCrWd) | 0x80); break;
}
return (ushort)(0x6F & woStWd);
}
/***********************************************************************
*Init Process (__Init() is once execued when program is booted)
************************************************************************/
public void __Init()
{
ushort Status = 0;
Console.WriteLine(\"\\n === Start Program ===\");
// Read recipe data
recipeData = new Recipe(\"C:\\\\ReceipeData.dat\");
//========================
//（1）EtherCAT® communication OPEN
//========================
Status = hEcat.EhOpen();
if (0 != Status)
{
Console.WriteLine(\"EhOpen is failed.\");
ErrorInit();
return;
}
//========================
//（2）Go to ESMをOPERATION
//========================
Status = hEcat.EhRqState(8);
if (0 != Status)
{
Console.WriteLine(\"EhRqState is failed.\");
ErrorInit();
return;
}
//========================
//（3）Detect Servo Slave
//========================
Status = hEcat.EhFindSlave(VENDOR_ID,
PRODUCT_CODE,
REVISION_NO,
out Alias,
out ViosInBaseOffset,
out ViosOutBaseOffset);
if (0 != Status)
{
Console.WriteLine(\"EhFindSlave is failed.\");
ErrorInit();
return;
}
//========================
//（4）Set Servo operation mode(request change to cyclic synchronous position mode)
//========================
Status = hEcat.EhWriteODByAlias(Alias, 0x6060, 0x00, 8, 0, 1);
if (0 != Status)
{
Console.WriteLine(\"EhWriteODByAlias is failed.\");
ErrorInit();
return;
}
//========================
//（5）Initialize command position to current position
//========================
// Get current position
//-----------------------
Status = hEcat.EhReadDword(ViosInBaseOffset + POS_ACUTUAL_OFFSET,
out ActualPos);
if (0 != Status)
{
Console.WriteLine(\"EhReadDword is failed.\");
ErrorInit();
return;
}
// Write current position
//-----------------------
TargetPos = TargetPos + ActualPos;
Status = hEcat.EhWriteDword(ViosOutBaseOffset + TARGET_POS_OFFSET,
TargetPos);
if (0 != Status)
{
Console.WriteLine(\"EhWriteDword is failed.\");
ErrorInit();
return;
}
//========================
//（6）Go to Serve ON state
//========================
// Get current servo driver state
//-----------------------
while (true)
{
ControlWord = 0;
// Wait for cyclic process
Status = hEcat.EhWaitForCyclic(EhApi.NO_WAIT);
if (EhApi.ER_EHAPI_TIMEOUT == Status)
{
return;
}
if (0 != Status)
{
// End since cyclic process is interrupted
Console.WriteLine(\"EhWaitForCyclic is failed.\");
ErrorInit();
return;
}
// Get current servo driver state
Status = hEcat.EhReadWord(ViosInBaseOffset + STATUSWORD_OFFSET,
out StatusWord);
if (0 != Status)
{
Console.WriteLine(\"EhReadWord is failed.\");
ErrorInit();
return;
}
// Get COntrolword value for next PDS state transition
if (0x27 == fnChgMotionGoState(StatusWord, ref ControlWord))
{
// [Servo ON is done]:to waiting for brake release
break;
}
// Write next servo diver transition value
Status = hEcat.EhWriteWord(ViosOutBaseOffset + CONTROLWORD_OFFSET,
ControlWord);
if (0 != Status)
{
Console.WriteLine(\"EhWriteWord is failed.\");
ErrorInit();
return;
}
}
//========================
//（7）Wait process for Servo brake release （according to Sanyo servo specification）
//========================
Thread.Sleep(1000);
}
/***********************************************************************
* Regular interval period method (function called at 1msec interval)
************************************************************************/
public void __Process()
{
// State judgement
if (IsDone) { return; }
if (IsError) { return; }
//========================
//（8）Position Control Process
//========================
// Wait for cyclic process
ushort Status = 0;
Status = hEcat.EhWaitForCyclic(EhApi.NO_WAIT);
if (EhApi.ER_EHAPI_TIMEOUT == Status)
{
return;
}
if (0 != Status)
{
// End since cyclic process is interrupted
Console.WriteLine(\"EhWaitForCyclic is failed.\");
EndProcess();
}
// Request movement per loop
TargetPos = recipeData.Data[rcpIdx] + ActualPos;// Refresh requied position
hEcat.EhWriteDword(ViosOutBaseOffset + TARGET_POS_OFFSET, TargetPos);
// Refresh target position
loopCnt++;
if (loopCnt < recipeData.Data.Length)
{
rcpIdx++; // Forward
}
else
{
rcpIdx--; // Reverse
}
// Completion Judgement
if (loopCnt >= (recipeData.Data.Length * 2) - 1)
{
// All movement (to defined position) is complete.
EndProcess(); // Process End
}
}
}
}